diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 4b3af9810..3e5cadd32 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${target_name} COMMAND ${target_name}) add_dependencies(check.${groupName} ${target_name}) add_dependencies(check ${target_name}) - add_dependencies(all.tests ${script_name}) + if(NOT XCODE_VERSION) + add_dependencies(all.tests ${script_name}) + endif() # Add TOPSRCDIR set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore new file mode 100644 index 000000000..2211df63d --- /dev/null +++ b/examples/Data/.gitignore @@ -0,0 +1 @@ +*.txt diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index fce046a59..97d646552 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -34,6 +34,9 @@ using namespace gtsam; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; +// create a typedef to the camera type +typedef PinholePose Camera; + /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -55,12 +58,12 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor()); + SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: @@ -68,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise); } // insert the smart factor in the graph @@ -77,15 +80,15 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(1, Camera(poses[1],K), noise)); // add directly to graph graph.print("Factor Graph:\n"); @@ -94,7 +97,7 @@ int main(int argc, char* argv[]) { Values initialEstimate; Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 49482292f..c1b18a946 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -30,6 +30,9 @@ using namespace gtsam; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; +// create a typedef to the camera type +typedef PinholePose Camera; + /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -51,16 +54,16 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor()); + SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise); } // insert the smart factor in the graph @@ -69,18 +72,18 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[1], poseNoise)); + graph.push_back(PriorFactor(1, Camera(poses[0],K), noise)); // Create the initial estimate to the solution Values initialEstimate; Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(i, Camera(poses[i].compose(delta),K)); // We will use LM in the outer optimization loop, but by specifying "Iterative" below // We indicate that an iterative linear solver should be used. diff --git a/gtsam.h b/gtsam.h index c39b2005c..46e0ffa28 100644 --- a/gtsam.h +++ b/gtsam.h @@ -812,7 +812,7 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; @@ -848,7 +848,7 @@ class PinholeCamera { static size_t Dim(); // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; @@ -884,7 +884,7 @@ virtual class SimpleCamera { static size_t Dim(); // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; @@ -2352,18 +2352,31 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +#include +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + #include template -virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); - - SmartProjectionPoseFactor(double rankTol); - SmartProjectionPoseFactor(); + SmartProjectionPoseFactor(const CALIBRATION* K); + SmartProjectionPoseFactor(const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); void add(const gtsam::Point2& measured_i, size_t poseKey_i, - const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); + const gtsam::noiseModel::Base* noise_i); // enabling serialization functionality //void serialize() const; diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 33f2c84eb..2d27b4dc7 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -85,8 +85,7 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { } /* ************************************************************************* */ -Point2 PinholeBase::project_to_camera(const Point3& pc, - OptionalJacobian<2, 3> Dpoint) { +Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { double d = 1.0 / pc.z(); const double u = pc.x() * d, v = pc.y() * d; if (Dpoint) @@ -94,10 +93,22 @@ Point2 PinholeBase::project_to_camera(const Point3& pc, return Point2(u, v); } +/* ************************************************************************* */ +Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { + if (Dpoint) { + Matrix32 Dpoint3_pc; + Matrix23 Duv_point3; + Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); + *Dpoint = Duv_point3 * Dpoint3_pc; + return uv; + } else + return Project(pc.point3()); +} + /* ************************************************************************* */ pair PinholeBase::projectSafe(const Point3& pw) const { const Point3 pc = pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); + const Point2 pn = Project(pc); return make_pair(pn, pc.z() > 0); } @@ -109,9 +120,9 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) - throw CheiralityException(); + throw CheiralityException(); #endif - const Point2 pn = project_to_camera(q); + const Point2 pn = Project(q); if (Dpose || Dpoint) { const double d = 1.0 / q.z(); @@ -123,6 +134,32 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, return pn; } +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint) const { + + // world to camera coordinate + Matrix23 Dpc_rot; + Matrix2 Dpc_point; + const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, + Dpose ? &Dpc_point : 0); + + // camera to normalized image coordinate + Matrix2 Dpn_pc; + const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); + + // chain the Jacobian matrices + if (Dpose) { + // only rotation is important + Matrix26 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; + *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6 + } + if (Dpoint) + *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2 + return pn; +} /* ************************************************************************* */ Point3 PinholeBase::backproject_from_camera(const Point2& p, const double depth) { diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 0b278bade..4d605ef4e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -44,6 +44,18 @@ public: */ class GTSAM_EXPORT PinholeBase { +public: + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + private: Pose3 pose_; ///< 3D pose of camera @@ -135,6 +147,16 @@ public: return pose_; } + /// get rotation + const Rot3& rotation() const { + return pose_.rotation(); + } + + /// get translation + const Point3& translation() const { + return pose_.translation(); + } + /// return pose, with derivative const Pose3& getPose(OptionalJacobian<6, 6> H) const; @@ -147,14 +169,21 @@ public: * Does *not* throw a CheiralityException, even if pc behind image plane * @param pc point in camera coordinates */ - static Point2 project_to_camera(const Point3& pc, // + static Point2 Project(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); + /** + * Project from 3D point at infinity in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + */ + static Point2 Project(const Unit3& pc, // + OptionalJacobian<2, 2> Dpoint = boost::none); + /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const; - /** - * Project point into the image + /** Project point into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point @@ -162,9 +191,33 @@ public: Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /** Project point at infinity into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION + * @param point 3D point in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Unit3& point, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; + /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 backproject_from_camera(const Point2& p, const double depth); + /// @} + /// @name Advanced interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { + return std::make_pair(3, 5); + } + + /// @} + private: /** Serialization function */ diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 3e40d11a0..a60528909 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -21,13 +21,14 @@ #include #include // for Cheirality exception #include +#include +#include #include namespace gtsam { /** * @brief A set of cameras, all with their own calibration - * Assumes that a camera is laid out as 6 Pose3 parameters then calibration */ template class CameraSet: public std::vector { @@ -40,28 +41,46 @@ protected: */ typedef typename CAMERA::Measurement Z; + static const int D = traits::dimension; ///< Camera dimension static const int ZDim = traits::dimension; ///< Measurement dimension - static const int Dim = traits::dimension; ///< Camera dimension + + /// Make a vector of re-projection errors + static Vector ErrorVector(const std::vector& predicted, + const std::vector& measured) { + + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); + + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Z e = predicted[i] - measured[i]; + b.segment(row) = e.vector(); + } + return b; + } public: /// Definitions for blocks of F - typedef Eigen::Matrix MatrixZD; // F - typedef std::pair FBlock; // Fblocks + typedef Eigen::Matrix MatrixZD; + typedef std::vector FBlocks; /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols */ - void print(const std::string& s = "") const { + virtual void print(const std::string& s = "") const { std::cout << s << "CameraSet, cameras = \n"; for (size_t k = 0; k < this->size(); ++k) - this->at(k).print(); + this->at(k).print(s); } /// equals - virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + bool equals(const CameraSet& p, double tol = 1e-9) const { if (this->size() != p.size()) return false; bool camerasAreEqual = true; @@ -74,31 +93,227 @@ public: } /** - * Project a point, with derivatives in this, point, and calibration + * Project a point (possibly Unit3 at infinity), with derivatives + * Note that F is a sparse block-diagonal matrix, so instead of a large dense + * matrix this function returns the diagonal blocks. * throws CheiralityException */ - std::vector project(const Point3& point, boost::optional F = - boost::none, boost::optional E = boost::none, - boost::optional H = boost::none) const { + template + std::vector project2(const POINT& point, // + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { - size_t nrCameras = this->size(); - if (F) F->resize(ZDim * nrCameras, 6); - if (E) E->resize(ZDim * nrCameras, 3); - if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6); - std::vector z(nrCameras); + static const int N = FixedDimension::value; - for (size_t i = 0; i < nrCameras; i++) { - Eigen::Matrix Fi; - Eigen::Matrix Ei; - Eigen::Matrix Hi; - z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); - if (F) F->block(ZDim * i, 0) = Fi; - if (E) E->block(ZDim * i, 0) = Ei; - if (H) H->block(ZDim * i, 0) = Hi; + // Allocate result + size_t m = this->size(); + std::vector z(m); + + // Allocate derivatives + if (E) + E->resize(ZDim * m, N); + if (Fs) + Fs->resize(m); + + // Project and fill derivatives + for (size_t i = 0; i < m; i++) { + MatrixZD Fi; + Eigen::Matrix Ei; + z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); + if (Fs) + (*Fs)[i] = Fi; + if (E) + E->block(ZDim * i, 0) = Ei; } + return z; } + /// Calculate vector [project2(point)-z] of re-projection errors + template + Vector reprojectionError(const POINT& point, const std::vector& measured, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + return ErrorVector(project2(point, Fs, E), measured); + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * Fixed size version + */ + template // N = 2 or 3 + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix + size_t M1 = D * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = // + E.block(ZDim * i, 0, ZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian(i, m) = Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian(i, i) = Fi.transpose() + * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i, j) = -Fi.transpose() + * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(m, m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + + /// Computes Point Covariance P, with lambda parameter + template // N = 2 or 3 + static void ComputePointCovariance(Eigen::Matrix& P, + const Matrix& E, double lambda, bool diagonalDamping = false) { + + Matrix EtE = E.transpose() * E; + + if (diagonalDamping) { // diagonal of the hessian + EtE.diagonal() += lambda * EtE.diagonal(); + } else { + DenseIndex n = E.cols(); + EtE += lambda * Eigen::MatrixXd::Identity(n, n); + } + + P = (EtE).inverse(); + } + + /// Computes Point Covariance P, with lambda parameter, dynamic version + static Matrix PointCov(const Matrix& E, const double lambda = 0.0, + bool diagonalDamping = false) { + if (E.cols() == 2) { + Matrix2 P2; + ComputePointCovariance(P2, E, lambda, diagonalDamping); + return P2; + } else { + Matrix3 P3; + ComputePointCovariance(P3, E, lambda, diagonalDamping); + return P3; + } + } + + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * Dynamic version + */ + static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, + const Matrix& E, const Vector& b, const double lambda = 0.0, + bool diagonalDamping = false) { + SymmetricBlockMatrix augmentedHessian; + if (E.cols() == 2) { + Matrix2 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } else { + Matrix3 P; + ComputePointCovariance(P, E, lambda, diagonalDamping); + augmentedHessian = SchurComplement(Fblocks, E, P, b); + } + return augmentedHessian; + } + + /** + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + */ + template // N = 2 or 3 + static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, + const Eigen::Matrix& P, const Vector& b, + const FastVector& allKeys, const FastVector& keys, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + + assert(keys.size()==Fs.size()); + assert(keys.size()<=allKeys.size()); + + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); + + // Schur complement trick + // G = F' * F - F' * E * P * E' * F + // g = F' * (b - E * P * E' * b) + + Eigen::Matrix matrixBlock; + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + + // a single point is observed in m cameras + size_t m = Fs.size(); // cameras observing current point + size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group + assert(allKeys.size()==M); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera in the current factor + + const MatrixZD& Fi = Fs[i]; + const Eigen::Matrix Ei_P = E.template block( + ZDim * i, 0) * P; + + // D = (DxZDim) * (ZDim) + // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) + // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) + // Key cameraKey_i = this->keys_[i]; + DenseIndex aug_i = KeySlotMap.at(keys[i]); + + // information vector - store previous vector + // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, M) = augmentedHessian(aug_i, M).knownOffDiagonal() + + Fi.transpose() * b.segment(ZDim * i) // F' * b + - Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + // main block diagonal - store previous block + matrixBlock = augmentedHessian(aug_i, aug_i); + // add contribution of current factor + augmentedHessian(aug_i, aug_i) = matrixBlock + + (Fi.transpose() + * (Fi - Ei_P * E.template block(ZDim * i, 0).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const MatrixZD& Fj = Fs[j]; + + DenseIndex aug_j = KeySlotMap.at(keys[j]); + + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) + // off diagonal block - store previous block + // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); + // add contribution of current factor + augmentedHessian(aug_i, aug_j) = + augmentedHessian(aug_i, aug_j).knownOffDiagonal() + - Fi.transpose() + * (Ei_P * E.template block(ZDim * j, 0).transpose() * Fj); + } + } // end of for over cameras + + augmentedHessian(M, M)(0, 0) += b.squaredNorm(); + } + private: /// Serialization function @@ -109,6 +324,9 @@ private: } }; +template +const int CameraSet::D; + template const int CameraSet::ZDim; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index fb57b0a69..12e9f023b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,14 +31,6 @@ namespace gtsam { template class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { -public: - - /** - * Some classes template on either PinholeCamera or StereoCamera, - * and this typedef informs those classes what "project" returns. - */ - typedef Point2 Measurement; - private: typedef PinholeBaseK Base; ///< base class has 3D pose as private member @@ -153,7 +145,7 @@ public: const Pose3& getPose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); - H->block(0, 0, 6, 6) = I_6x6; + H->template block<6, 6>(0, 0) = I_6x6; } return Base::pose(); } @@ -184,16 +176,15 @@ public: if ((size_t) d.size() == 6) return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(this->pose().retract(d.head(6)), + return PinholeCamera(this->pose().retract(d.head<6>()), calibration().retract(d.tail(calibration().dim()))); } /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; - // TODO: why does d.head<6>() not compile?? - d.head(6) = this->pose().localCoordinates(T2.pose()); - d.tail(DimK) = calibration().localCoordinates(T2.calibration()); + d.template head<6>() = this->pose().localCoordinates(T2.pose()); + d.template tail() = calibration().localCoordinates(T2.calibration()); return d; } @@ -208,101 +199,34 @@ public: typedef Eigen::Matrix Matrix2K; - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration + /** Templated projection of a 3D point or a point at infinity into the image + * @param pw either a Point3 or a Unit3, in world coordinates */ - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - // project to normalized coordinates - const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - - // uncalibrate to pixel coordinates - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, Dcal, - Dpose || Dpoint ? &Dpi_pn : 0); - - // If needed, apply chain rule - if (Dpose) - *Dpose = Dpi_pn * *Dpose; - if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; - - return pi; - } - - /** project a point at infinity from world coordinate to the image - * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = Base::project_to_camera(pc); // project the point to the camera - return K_.uncalibrate(pn); - } - - // world to camera coordinate - Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); - - Matrix36 Dpc_pose; - Dpc_pose.setZero(); - Dpc_pose.leftCols<3>() = Dpc_rot; - - // camera to normalized image coordinate - Matrix23 Dpn_pc; // 2*3 - const Point2 pn = Base::project_to_camera(pc, Dpn_pc); - - // uncalibration - Matrix2 Dpi_pn; // 2*2 - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; - if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; - if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) - return pi; - } - - /** project a point from world coordinate to the image, fixed Jacobians - * @param pw is a point in the world coordinate - */ - Point2 project2( - const Point3& pw, // - OptionalJacobian<2, dimension> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { - - // project to normalized coordinates + template + Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, + OptionalJacobian<2, FixedDimension::value> Dpoint) const { + // We just call 3-derivative version in Base Matrix26 Dpose; - const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - - // uncalibrate to pixel coordinates - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0, - Dcamera || Dpoint ? &Dpi_pn : 0); - - // If needed, calculate derivatives + Eigen::Matrix Dcal; + Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, + Dcamera ? &Dcal : 0); if (Dcamera) - *Dcamera << Dpi_pn * Dpose, Dcal; - if (Dpoint) - *Dpoint = Dpi_pn * (*Dpoint); - + *Dcamera << Dpose, Dcal; return pi; } + /// project a 3D point from world coordinates into the image + Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); + } + + /// project a point at infinity from world coordinates into the image + Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + return _project2(pw, Dcamera, Dpoint); + } + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -350,7 +274,7 @@ public: if (Dother) { Dother->resize(1, 6 + CalibrationB::dimension); Dother->setZero(); - Dother->block(0, 0, 1, 6) = Dother_; + Dother->block<1, 6>(0, 0) = Dother_; } return result; } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index bfb336f9a..ac453e048 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -30,14 +30,19 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -template +template class GTSAM_EXPORT PinholeBaseK: public PinholeBase { - GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) +private: -public : + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); - typedef Calibration CalibrationType; + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; + +public: + + typedef CALIBRATION CalibrationType; /// @name Standard Constructors /// @{ @@ -67,7 +72,7 @@ public : } /// return calibration - virtual const Calibration& calibration() const = 0; + virtual const CALIBRATION& calibration() const = 0; /// @} /// @name Transformations and measurement functions @@ -80,27 +85,65 @@ public : return pn; } - /** project a point from world coordinate to the image, fixed Jacobians + /** project a point from world coordinate to the image * @param pw is a point in the world coordinates */ - Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { + Point2 project(const Point3& pw) const { + const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** project a point from world coordinate to the image + * @param pw is a point at infinity in the world coordinates + */ + Point2 project(const Unit3& pw) const { + const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame + const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates + return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates + } + + /** Templated projection of a point (possibly at infinity) from world coordinate to the image + * @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + template + Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, FixedDimension::value> Dpoint, + OptionalJacobian<2, DimK> Dcal) const { // project to normalized coordinates const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); // uncalibrate to pixel coordinates Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, boost::none, + const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpose || Dpoint ? &Dpi_pn : 0); // If needed, apply chain rule - if (Dpose) *Dpose = Dpi_pn * (*Dpose); - if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint); + if (Dpose) + *Dpose = Dpi_pn * *Dpose; + if (Dpoint) + *Dpoint = Dpi_pn * *Dpoint; return pi; } + /// project a 3D point from world coordinates into the image + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + + /// project a point at infinity from world coordinates into the image + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + return _project(pw, Dpose, Dpoint, Dcal); + } + /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& p, double depth) const { const Point2 pn = calibration().calibrate(p); @@ -108,9 +151,9 @@ public : } /// backproject a 2-dimensional point to a 3-dimensional point at infinity - Point3 backprojectPointAtInfinity(const Point2& p) const { + Unit3 backprojectPointAtInfinity(const Point2& p) const { const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 return pose().rotation().rotate(pc); } @@ -178,13 +221,13 @@ private: * @addtogroup geometry * \nosubgrouping */ -template -class GTSAM_EXPORT PinholePose: public PinholeBaseK { +template +class GTSAM_EXPORT PinholePose: public PinholeBaseK { private: - typedef PinholeBaseK Base; ///< base class has 3D pose as private member - boost::shared_ptr K_; ///< shared pointer to fixed calibration + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + boost::shared_ptr K_; ///< shared pointer to fixed calibration public: @@ -201,11 +244,11 @@ public: /** constructor with pose, uses default calibration */ explicit PinholePose(const Pose3& pose) : - Base(pose), K_(new Calibration()) { + Base(pose), K_(new CALIBRATION()) { } /** constructor with pose and calibration */ - PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : Base(pose), K_(K) { } @@ -220,14 +263,14 @@ public: * (theta 0 = looking in direction of positive X axis) * @param height camera height */ - static PinholePose Level(const boost::shared_ptr& K, + static PinholePose Level(const boost::shared_ptr& K, const Pose2& pose2, double height) { return PinholePose(Base::LevelPose(pose2, height), K); } /// PinholePose::level with default calibration static PinholePose Level(const Pose2& pose2, double height) { - return PinholePose::Level(boost::make_shared(), pose2, height); + return PinholePose::Level(boost::make_shared(), pose2, height); } /** @@ -240,8 +283,8 @@ public: * @param K optional calibration parameter */ static PinholePose Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const boost::shared_ptr& K = - boost::make_shared()) { + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { return PinholePose(Base::LookatPose(eye, target, upVector), K); } @@ -251,12 +294,12 @@ public: /// Init from 6D vector explicit PinholePose(const Vector &v) : - Base(v), K_(new Calibration()) { + Base(v), K_(new CALIBRATION()) { } /// Init from Vector and calibration PinholePose(const Vector &v, const Vector &K) : - Base(v), K_(new Calibration(K)) { + Base(v), K_(new CALIBRATION(K)) { } /// @} @@ -286,10 +329,26 @@ public: } /// return calibration - virtual const Calibration& calibration() const { + virtual const CALIBRATION& calibration() const { return *K_; } + /** project a point from world coordinate to the image, 2 derivatives only + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + + /// project2 version for point at infinity + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + /// @} /// @name Manifold /// @{ @@ -336,14 +395,14 @@ private: }; // end of class PinholePose -template -struct traits > : public internal::Manifold< - PinholePose > { +template +struct traits > : public internal::Manifold< + PinholePose > { }; -template -struct traits > : public internal::Manifold< - PinholePose > { +template +struct traits > : public internal::Manifold< + PinholePose > { }; } // \ gtsam diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h new file mode 100644 index 000000000..5101e9fc8 --- /dev/null +++ b/gtsam/geometry/PinholeSet.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PinholeSet.h + * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * PinholeSet: triangulates point and keeps an estimate of it around. + */ +template +class PinholeSet: public CameraSet { + +private: + typedef CameraSet Base; + typedef PinholeSet This; + +protected: + +public: + + /** Virtual destructor */ + virtual ~PinholeSet() { + } + + /// @name Testable + /// @{ + + /// print + virtual void print(const std::string& s = "") const { + Base::print(s); + } + + /// equals + bool equals(const PinholeSet& p, double tol = 1e-9) const { + return Base::equals(p, tol); // TODO all flags + } + + /// @} + + /// triangulateSafe + TriangulationResult triangulateSafe( + const std::vector& measured, + const TriangulationParameters& params) const { + return gtsam::triangulateSafe(*this, measured, params); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index c02a11928..199ae30ce 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -88,13 +88,30 @@ TEST( CalibratedCamera, project) } /* ************************************************************************* */ -TEST( CalibratedCamera, Dproject_to_camera1) { - Point3 pp(155,233,131); - Matrix actual; - CalibratedCamera::project_to_camera(pp, actual); - Matrix expected_numerical = numericalDerivative11( - boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); - CHECK(assert_equal(expected_numerical, actual)); +static Point2 Project1(const Point3& point) { + return PinholeBase::Project(point); +} + +TEST( CalibratedCamera, DProject1) { + Point3 pp(155, 233, 131); + Matrix test1; + CalibratedCamera::Project(pp, test1); + Matrix test2 = numericalDerivative11(Project1, pp); + CHECK(assert_equal(test1, test2)); +} + +/* ************************************************************************* */ +static Point2 Project2(const Unit3& point) { + return PinholeBase::Project(point); +} + +Unit3 pointAtInfinity(0, 0, 1000); +TEST( CalibratedCamera, DProjectInfinity) { + Matrix test1; + CalibratedCamera::Project(pointAtInfinity, test1); + Matrix test2 = numericalDerivative11(Project2, + pointAtInfinity); + CHECK(assert_equal(test1, test2)); } /* ************************************************************************* */ @@ -128,6 +145,36 @@ TEST( CalibratedCamera, Dproject_point_pose2) CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +static Point2 projectAtInfinity(const CalibratedCamera& camera, const Unit3& point) { + return camera.project2(point); +} + +TEST( CalibratedCamera, Dproject_point_pose_infinity) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + CHECK(assert_equal(Point2(), result)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject_point_pose2_infinity) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); + Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 42cf0f299..0afa04411 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -31,43 +31,105 @@ using namespace gtsam; #include TEST(CameraSet, Pinhole) { typedef PinholeCamera Camera; + typedef CameraSet Set; typedef vector ZZ; - CameraSet set; + Set set; Camera camera; set.push_back(camera); set.push_back(camera); Point3 p(0, 0, 1); - CHECK(assert_equal(set, set)); - CameraSet set2 = set; + EXPECT(assert_equal(set, set)); + Set set2 = set; set2.push_back(camera); - CHECK(!set.equals(set2)); + EXPECT(!set.equals(set2)); // Check measurements Point2 expected; - ZZ z = set.project(p); - CHECK(assert_equal(expected, z[0])); - CHECK(assert_equal(expected, z[1])); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); // Calculate expected derivatives using Pinhole - Matrix46 actualF; - Matrix43 actualE; - Matrix43 actualH; + Matrix actualE; + Matrix29 F1; { - Matrix26 F1; Matrix23 E1; - Matrix23 H1; - camera.project(p, F1, E1, H1); + camera.project2(p, F1, E1); + actualE.resize(4,3); actualE << E1, E1; - actualF << F1, F1; - actualH << H1, H1; } // Check computed derivatives - Matrix F, E, H; - set.project(p, F, E, H); - CHECK(assert_equal(actualF, F)); - CHECK(assert_equal(actualE, E)); - CHECK(assert_equal(actualH, H)); + Set::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // Check Schur complement + Matrix F(4, 18); + F << F1, Matrix29::Zero(), Matrix29::Zero(), F1; + Matrix Ft = F.transpose(); + Matrix34 Et = E.transpose(); + Matrix3 P = Et * E; + Matrix schur(19, 19); + Vector4 b = actualV; + Vector v = Ft * (b - E * P * Et * b); + schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30; + SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b); + EXPECT(assert_equal(schur, actualReduced.matrix())); + + // Check Schur complement update, same order, should just double + FastVector allKeys, keys; + allKeys.push_back(1); + allKeys.push_back(2); + keys.push_back(1); + keys.push_back(2); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced); + EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.matrix())); + + // Check Schur complement update, keys reversed + FastVector keys2; + keys2.push_back(2); + keys2.push_back(1); + Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); + Vector4 reverse_b; + reverse_b << b.tail<2>(), b.head<2>(); + Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b); + Matrix A(19, 19); + A << Ft * F - Ft * E * P * Et * F, reverse_v, reverse_v.transpose(), 30; + EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix())); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(expectedV, actualV)); + LONGS_EQUAL(2, Fs.size()); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4,2); + actualE << E1, E1; + } + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); } /* ************************************************************************* */ @@ -83,26 +145,27 @@ TEST(CameraSet, Stereo) { // Check measurements StereoPoint2 expected(0, -1, 0); - ZZ z = set.project(p); - CHECK(assert_equal(expected, z[0])); - CHECK(assert_equal(expected, z[1])); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); // Calculate expected derivatives using Pinhole - Matrix66 actualF; Matrix63 actualE; + Matrix F1; { - Matrix36 F1; Matrix33 E1; - camera.project(p, F1, E1); + camera.project2(p, F1, E1); actualE << E1, E1; - actualF << F1, F1; } // Check computed derivatives - Matrix F, E; - set.project(p, F, E); - CHECK(assert_equal(actualF, F)); - CHECK(assert_equal(actualE, E)); + CameraSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 0e610d8d6..74bc4ca2a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -45,10 +45,10 @@ static const Point3 point2(-0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0); -static const Point3 point1_inf(-0.16,-0.16, -1.0); -static const Point3 point2_inf(-0.16, 0.16, -1.0); -static const Point3 point3_inf( 0.16, 0.16, -1.0); -static const Point3 point4_inf( 0.16,-0.16, -1.0); +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) @@ -154,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 1., 0.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 1., 0.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -169,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 0., 1.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 0., 1.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -197,17 +197,17 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { - return Camera(pose,cal).projectPointAtInfinity(point3D); +static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).project(point3D); } TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 + Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection - Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); EXPECT(assert_equal(actual, expected, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 411273c1f..dc294899f 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -46,10 +46,10 @@ static const Point3 point2(-0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0); -static const Point3 point1_inf(-0.16,-0.16, -1.0); -static const Point3 point2_inf(-0.16, 0.16, -1.0); -static const Point3 point3_inf( 0.16, 0.16, -1.0); -static const Point3 point4_inf( 0.16,-0.16, -1.0); +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholePose, constructor) @@ -144,11 +144,11 @@ TEST( PinholePose, Dproject) { Matrix Dpose, Dpoint; Point2 result = camera.project2(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); - Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix expectedDcamera = numericalDerivative31(project3, pose, point1, K); + Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K); EXPECT(assert_equal(Point2(-100, 100), result)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); - EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } /* ************************************************************************* */ @@ -161,11 +161,11 @@ TEST( PinholePose, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); - Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix expectedDpoint = numericalDerivative22(project4, camera, point1); EXPECT(assert_equal(result, Point2(-100, 100) )); - EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); - EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } /* ************************************************************************* */ @@ -176,12 +176,31 @@ TEST( CalibratedCamera, Dproject3) static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(expectedDcamera, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project(pointAtInfinity); +} + +/* ************************************************************************* */ +TEST( PinholePose, DprojectAtInfinity2) +{ + Unit3 pointAtInfinity(0,0,1000); + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); + Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K); + EXPECT(assert_equal(Point2(0,0), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); @@ -191,12 +210,12 @@ static double range0(const Camera& camera, const Point3& point) { TEST( PinholePose, range0) { Matrix D1; Matrix D2; double result = camera.range(point1, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); - Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); + Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -208,11 +227,11 @@ static double range1(const Camera& camera, const Pose3& pose) { TEST( PinholePose, range1) { Matrix D1; Matrix D2; double result = camera.range(pose1, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); - Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1); + Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -228,11 +247,11 @@ static double range2(const Camera& camera, const Camera2& camera2) { TEST( PinholePose, range2) { Matrix D1; Matrix D2; double result = camera.range(camera2, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); - Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2); + Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -245,11 +264,11 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) { TEST( PinholePose, range3) { Matrix D1; Matrix D2; double result = camera.range(camera3, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); - Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3); + Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp new file mode 100644 index 000000000..b8f001f1c --- /dev/null +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +TEST(PinholeSet, Stereo) { + typedef vector ZZ; + PinholeSet set; + CalibratedCamera camera; + set.push_back(camera); + set.push_back(camera); + // set.print("set: "); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + Point2 expected(0, 0); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix43 actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + EXPECT(assert_equal(actualE, E)); + + // Instantiate triangulateSafe + // TODO triangulation does not work yet for CalibratedCamera + // PinholeSet::Result actual = set.triangulateSafe(z); +} + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(PinholeSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + PinholeSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + PinholeSet set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE.resize(4, 3); + actualE << E1, E1; + } + + // Check computed derivatives + { + PinholeSet::FBlocks Fs; + Matrix E; + set.project2(p, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // reprojectionErrorAtInfinity + Unit3 pointAtInfinity(0, 0, 1000); + { + Matrix22 E1; + camera.project2(pointAtInfinity, F1, E1); + actualE.resize(4, 2); + actualE << E1, E1; + } + EXPECT( + assert_equal(pointAtInfinity, + camera.backprojectPointAtInfinity(Point2()))); + { + PinholeSet::FBlocks Fs; + Matrix E; + actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); + EXPECT(assert_equal(actualE, E)); + LONGS_EQUAL(2, Fs.size()); + EXPECT(assert_equal(F1, Fs[0])); + EXPECT(assert_equal(F1, Fs[1])); + } + EXPECT(assert_equal(expectedV, actualV)); + + // Instantiate triangulateSafe + TriangulationParameters params; + TriangulationResult actual = set.triangulateSafe(z, params); + CHECK(actual.degenerate()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f986cca1d..352493683 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include @@ -49,6 +50,7 @@ Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); //****************************************************************************** +// Simple test with a well-behaved two camera situation TEST( triangulation, twoPoses) { vector poses; @@ -57,24 +59,37 @@ TEST( triangulation, twoPoses) { poses += pose1, pose2; measurements += z1, z2; - bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual1, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual2, 1e-7)); + + // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); + optimize = false; + boost::optional actual3 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } //****************************************************************************** - +// Similar, but now with Bundler calibration TEST( triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // @@ -95,17 +110,17 @@ TEST( triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional triangulated_landmark = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *actual, 1e-7)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // Add some noise and try again measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + boost::optional actual2 = // + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); } //****************************************************************************** @@ -116,17 +131,17 @@ TEST( triangulation, fourPoses) { poses += pose1, pose2; measurements += z1, z2; - boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + boost::optional actual = triangulatePoint3(poses, sharedCal, + measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(poses, sharedCal, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -150,7 +165,7 @@ TEST( triangulation, fourPoses) { SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); poses += pose4; measurements += Point2(400, 400); @@ -180,17 +195,17 @@ TEST( triangulation, fourPoses_distinct_Ks) { cameras += camera1, camera2; measurements += z1, z2; - boost::optional triangulated_landmark = // + boost::optional actual = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + EXPECT(assert_equal(landmark, *actual, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = // + boost::optional actual2 = // triangulatePoint3(cameras, measurements); - EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); + EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); @@ -216,7 +231,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { SimpleCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); cameras += camera4; measurements += Point2(400, 400); @@ -244,23 +259,19 @@ TEST( triangulation, twoIdenticalPoses) { } //****************************************************************************** -/* - TEST( triangulation, onePose) { - // we expect this test to fail with a TriangulationUnderconstrainedException - // because there's only one camera observation +TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation - Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480); + vector poses; + vector measurements; - vector poses; - vector measurements; + poses += Pose3(); + measurements += Point2(); - poses += Pose3(); - measurements += Point2(); - - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), - TriangulationUnderconstrainedException); - } - */ + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), + TriangulationUnderconstrainedException); +} //****************************************************************************** int main() { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c0f69217c..c92aa8237 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -46,7 +46,6 @@ Vector4 triangulateHomogeneousDLT( double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - // std::cout << "s " << s.transpose() << std:endl; if (rank < 3) throw(TriangulationUnderconstrainedException()); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index f4f40ccba..4ac634f03 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -64,7 +65,6 @@ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, const std::vector& measurements, double rank_tol = 1e-9); -/// /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses @@ -86,8 +86,9 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; - PinholeCamera camera_i(pose_i, *sharedCal); - graph.push_back(TriangulationFactor // + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -114,13 +115,22 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } -/// +/// PinholeCamera specific version +template +std::pair triangulationGraph( + const std::vector >& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + return triangulationGraph > // + (cameras, measurements, landmarkKey, initialEstimate); +} + /** * Optimize for triangulation * @param graph nonlinear factors for projection @@ -147,8 +157,8 @@ Point3 triangulateNonlinear(const std::vector& poses, // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } @@ -168,12 +178,21 @@ Point3 triangulateNonlinear( // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph(cameras, measurements, - Symbol('p', 0), initialEstimate); + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); return optimize(graph, values, Symbol('p', 0)); } +/// PinholeCamera specific version +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + return triangulateNonlinear > // + (cameras, measurements, initialEstimate); +} + /** * Create a 3*4 camera projection matrix from calibration and pose. * Functor for partial application on calibration @@ -224,12 +243,13 @@ Point3 triangulatePoint3(const std::vector& poses, // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // The n refine using non-linear optimization + // Then refine using non-linear optimization if (optimize) - point = triangulateNonlinear(poses, sharedCal, measurements, point); + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras + // verify that the triangulated point lies in front of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) @@ -265,9 +285,8 @@ Point3 triangulatePoint3( throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - typedef PinholeCamera Camera; std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) + BOOST_FOREACH(const CAMERA& camera, cameras) projection_matrices.push_back( CameraProjectionMatrix(camera.calibration())( camera.pose())); @@ -275,11 +294,11 @@ Point3 triangulatePoint3( // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Camera& camera, cameras) { + // verify that the triangulated point lies in front of all cameras + BOOST_FOREACH(const CAMERA& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); @@ -289,5 +308,160 @@ Point3 triangulatePoint3( return point; } +/// Pinhole-specific version +template +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); +} + +struct TriangulationParameters { + + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + bool enableEPI; ///< if set to true, will refine triangulation using LM + + /** + * if the landmark is triangulated at distance larger than this, + * result is flagged as degenerate. + */ + double landmarkDistanceThreshold; // + + /** + * If this is nonnegative the we will check if the average reprojection error + * is smaller than this threshold after triangulation, otherwise result is + * flagged as degenerate. + */ + double dynamicOutlierRejectionThreshold; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param enableEPI if true refine triangulation with embedded LM iterations + * @param landmarkDistanceThreshold flag as degenerate if point further than this + * @param dynamicOutlierRejectionThreshold or if average error larger than this + * + */ + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { + } + + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } +}; + +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const std::vector& cameras, + const std::vector& measured, + const TriangulationParameters& params) { + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double totalReprojError = 0.0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + const Pose3& pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 + && pose.translation().distance(point) + > params.landmarkDistanceThreshold) + return TriangulationResult::Degenerate(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3& p_local = pose.transform_to(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // This exception is thrown if + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } +} + } // \namespace gtsam diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 06c5fe7fb..609c03acf 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -506,7 +507,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar /* ************************************************************************* */ void Isotropic::print(const string& name) const { - cout << name << "isotropic sigma " << " " << sigma_ << endl; + cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl; } /* ************************************************************************* */ @@ -534,6 +535,11 @@ void Isotropic::WhitenInPlace(Matrix& H) const { H *= invsigma_; } +/* ************************************************************************* */ +void Isotropic::whitenInPlace(Vector& v) const { + v *= invsigma_; +} + /* ************************************************************************* */ void Isotropic::WhitenInPlace(Eigen::Block H) const { H *= invsigma_; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a6c63da50..f2f8a01cf 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -550,6 +550,7 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; virtual void WhitenInPlace(Matrix& H) const; + virtual void whitenInPlace(Vector& v) const; virtual void WhitenInPlace(Eigen::Block H) const; /** diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h similarity index 99% rename from gtsam/slam/RegularHessianFactor.h rename to gtsam/linear/RegularHessianFactor.h index be14067db..e5e545c36 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include #include #include diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h similarity index 100% rename from gtsam/slam/RegularJacobianFactor.h rename to gtsam/linear/RegularJacobianFactor.h diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 664fcf3b7..33c62cfb6 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -66,6 +66,18 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::insert(const std::pair& key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + std::pair result = values_.insert(key_value); + if(!result.second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 968fc1adb..d04d9faac 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -181,23 +181,14 @@ namespace gtsam { * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector + return insert(std::make_pair(j, value)); } /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. - std::pair result = values_.insert(key_value); - if(!result.second) - throw std::invalid_argument( - "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) - + "' already in this VectorValues."); - return result.first; - } + iterator insert(const std::pair& key_value); /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp similarity index 98% rename from gtsam/slam/tests/testRegularHessianFactor.cpp rename to gtsam/linear/tests/testRegularHessianFactor.cpp index 6457e45fe..f53803dd1 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -15,7 +15,7 @@ * @date March 4, 2014 */ -#include +#include #include #include diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/linear/tests/testRegularJacobianFactor.cpp similarity index 99% rename from gtsam/slam/tests/testRegularJacobianFactor.cpp rename to gtsam/linear/tests/testRegularJacobianFactor.cpp index 5803516a1..b8c4aa689 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/linear/tests/testRegularJacobianFactor.cpp @@ -16,7 +16,7 @@ * @date Nov 12, 2014 */ -#include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 3e86bcb8c..75af5f634 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -175,7 +175,8 @@ using namespace binary; Expression K(3); // Create expression tree -Expression projection(PinholeCamera::project_to_camera, p_cam); +Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; +Expression projection(f, p_cam); Expression uv_hat(uncalibrate, K, projection); } /* ************************************************************************* */ diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9d4a8e6e5..da22225e5 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -173,7 +173,7 @@ public: Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) - pn = SimpleCamera::project_to_camera(dP2); + pn = PinholeBase::Project(dP2); } else { @@ -186,7 +186,7 @@ public: Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Matrix23 Dpn_dP2; - pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + pn = PinholeBase::Project(dP2, Dpn_dP2); if (DE) { Matrix DdP2_E(3, 5); diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index ed6213058..16560a43e 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -17,7 +17,7 @@ #pragma once -#include "RegularJacobianFactor.h" +#include namespace gtsam { /** @@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; - typedef std::pair KeyMatrixZD; + typedef std::pair KeyMatrix; public: @@ -42,7 +42,6 @@ public: Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) @@ -51,24 +50,25 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, const SharedDiagonal& model = - SharedDiagonal()) : + JacobianFactorQ(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; QF.push_back( - KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); + } // Which is then passed to the normal JacobianFactor constructor - JacobianFactor::fillTerms(QF, Q * b, model); + JacobianFactor::fillTerms(QF, - Q * b, model); } }; // end class JacobianFactorQ diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 4c1b0ff14..77102c24b 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,8 +6,8 @@ */ #pragma once -#include #include +#include #include namespace gtsam { @@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; - typedef std::pair KeyMatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, // + JacobianFactorQR(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix3& P, + const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); - size_t i = 0; - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) { - gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, - b.segment(ZDim * i), model); - i += 1; + for (size_t k = 0; k < FBlocks.size(); ++k) { + Key key = keys[k]; + gfg.add(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], + b.segment < ZDim > (ZDim * k), model); } //gfg.print("gfg"); diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index b4389d681..86636c38f 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,18 +5,17 @@ */ #pragma once -#include "gtsam/slam/RegularJacobianFactor.h" +#include namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement */ template class JacobianFactorSVD: public RegularJacobianFactor { typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrixZD; typedef std::pair KeyMatrix; public: @@ -38,13 +37,21 @@ public: JacobianFactor::fillTerms(QF, zeroVector, model); } - /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, - const Matrix& Enull, const Vector& b, // + /** + * @brief Constructor + * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) + * and a reduced point derivative, Enull + * and creates a reduced-rank Jacobian factor on the CameraSet + * + * @Fblocks: + */ + JacobianFactorSVD(const FastVector& keys, + const std::vector& Fblocks, const Matrix& Enull, + const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t numKeys = Enull.rows() / ZDim; - size_t j = 0, m2 = ZDim * numKeys - 3; + size_t m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) @@ -52,10 +59,12 @@ public: // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; QF.push_back( - KeyMatrix(it.first, - (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); + KeyMatrix(key, + (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 71944c670..c713eff72 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -17,7 +18,7 @@ namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -26,14 +27,20 @@ public: protected: - typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian - typedef std::pair KeyMatrix2D; ///< named F block + // This factor is closely related to a CameraSet + typedef CameraSet Set; - std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) - Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) - Matrix E_; ///< The 2m*3 E Jacobian with respect to the point - Vector b_; ///< 2m-dimensional RHS vector + typedef typename CAMERA::Measurement Z; + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + typedef Eigen::Matrix MatrixZD; ///< type of an F block + typedef Eigen::Matrix MatrixDD; ///< camera hessian + + const std::vector FBlocks_; ///< All ZDim*D F blocks (one for each camera) + const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point + const Vector b_; ///< 2m-dimensional RHS vector public: @@ -41,54 +48,40 @@ public: RegularImplicitSchurFactor() { } - /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b) : - Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { - initKeys(); - } - - /// initialize keys from Fblocks - void initKeys() { - keys_.reserve(Fblocks_.size()); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) - keys_.push_back(it.first); + /// Construct from blocks of F, E, inv(E'*E), and RHS vector b + RegularImplicitSchurFactor(const FastVector& keys, + const std::vector& FBlocks, const Matrix& E, const Matrix& P, + const Vector& b) : + GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { } /// Destructor virtual ~RegularImplicitSchurFactor() { } - // Write access, only use for construction! - - inline std::vector& Fblocks() { - return Fblocks_; + std::vector& FBlocks() const { + return FBlocks_; } - inline Matrix3& PointCovariance() { - return PointCovariance_; - } - - inline Matrix& E() { + const Matrix& E() const { return E_; } - inline Vector& b() { + const Vector& b() const { return b_; } - /// Get matrix P - inline const Matrix3& getPointCovariance() const { + const Matrix& getPointCovariance() const { return PointCovariance_; } /// print - void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); for (size_t pos = 0; pos < size(); ++pos) { - std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl; } std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; std::cout << "E:\n" << E_ << std::endl; @@ -100,10 +93,11 @@ public: const This* f = dynamic_cast(&lf); if (!f) return false; - for (size_t pos = 0; pos < size(); ++pos) { - if (keys_[pos] != f->keys_[pos]) return false; - if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; - if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; + for (size_t k = 0; k < FBlocks_.size(); ++k) { + if (keys_[k] != f->keys_[k]) + return false; + if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol)) + return false; } return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) && equal_with_abs_tol(E_, f->E_, tol) @@ -126,18 +120,26 @@ public: return Matrix(); } virtual std::pair jacobian() const { - throw std::runtime_error("RegularImplicitSchurFactor::jacobian non implemented"); + throw std::runtime_error( + "RegularImplicitSchurFactor::jacobian non implemented"); return std::make_pair(Matrix(), Vector()); } + + /// *Compute* full augmented information matrix virtual Matrix augmentedInformation() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::augmentedInformation non implemented"); - return Matrix(); + + // Do the Schur complement + SymmetricBlockMatrix augmentedHessian = // + Set::SchurComplement(FBlocks_, E_, b_); + return augmentedHessian.matrix(); } + + /// *Compute* full information matrix virtual Matrix information() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::information non implemented"); - return Matrix(); + Matrix augmented = augmentedInformation(); + int m = this->keys_.size(); + size_t M = D * m; + return augmented.block(0, 0, M, M); } /// Return the diagonal of the Hessian for this factor @@ -145,17 +147,17 @@ public: // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); VectorValues d; - for (size_t pos = 0; pos < size(); ++pos) { // for each camera - Key j = keys_[pos]; + for (size_t k = 0; k < size(); ++k) { // for each camera + Key j = keys_[k]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; - Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[k]; + Eigen::Matrix FtE = Fj.transpose() + * E_.block(ZDim * k, 0); Eigen::Matrix dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian // Vector column_k_Fj = Fj.col(k); dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); // Vector column_k_FtE = FtE.row(k); @@ -181,13 +183,13 @@ public: Key j = keys_[pos]; // Calculate Fj'*Ej for the current camera (observing a single point) - // D x 3 = (D x 2) * (2 x 3) - const Matrix2D& Fj = Fblocks_[pos].second; + // D x 3 = (D x ZDim) * (ZDim x 3) + const MatrixZD& Fj = FBlocks_[pos]; Eigen::Matrix FtE = Fj.transpose() - * E_.block<2, 3>(2 * pos, 0); + * E_.block(ZDim * pos, 0); DVector dj; - for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian dj(k) = Fj.col(k).squaredNorm(); // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); @@ -202,38 +204,41 @@ public: // F'*(I - E*P*E')*F for (size_t pos = 0; pos < size(); ++pos) { Key j = keys_[pos]; - // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) - const Matrix2D& Fj = Fblocks_[pos].second; + // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + const MatrixZD& Fj = FBlocks_[pos]; // Eigen::Matrix FtE = Fj.transpose() - // * E_.block<2, 3>(2 * pos, 0); + // * E_.block(ZDim * pos, 0); // blocks[j] = Fj.transpose() * Fj // - FtE * PointCovariance_ * FtE.transpose(); - const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0); - blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); + const Matrix23& Ej = E_.block(ZDim * pos, 0); + blocks[j] = Fj.transpose() + * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( - // static const Eigen::Matrix I2 = eye(2); + // static const Eigen::Matrix I2 = eye(ZDim); // Matrix2 Q = // - // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); + // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * E_.block(ZDim * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } return blocks; } virtual GaussianFactor::shared_ptr clone() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::clone non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::clone non implemented"); } virtual bool empty() const { return false; } virtual GaussianFactor::shared_ptr negate() const { - return boost::make_shared >(Fblocks_, - PointCovariance_, E_, b_); - throw std::runtime_error("RegularImplicitSchurFactor::negate non implemented"); + return boost::make_shared >(keys_, + FBlocks_, PointCovariance_, E_, b_); + throw std::runtime_error( + "RegularImplicitSchurFactor::negate non implemented"); } // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing @@ -251,22 +256,24 @@ public: typedef std::vector Error2s; /** - * @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b) + * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) */ void projectError2(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * (e1-2*b) = (3*2m)*2m + // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m Vector3 d1; d1.setZero(); for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2)); + d1 += E_.block(ZDim * k, 0).transpose() + * (e1[k] - ZDim * b_.segment(k * ZDim)); // d2 = E.transpose() * e1 = (3*2m)*2m Vector3 d2 = PointCovariance_ * d1; // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2; + e2[k] = e1[k] - ZDim * b_.segment(k * ZDim) + - E_.block(ZDim * k, 0) * d2; } /* @@ -286,7 +293,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError2(e1, e2); double result = 0; @@ -308,7 +315,7 @@ public: // e1 = F * x - b = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment(k * ZDim); projectError(e1, e2); double result = 0; @@ -321,21 +328,21 @@ public: /** * @brief Calculate corrected error Q*e = (I - E*P*E')*e */ - void projectError(const Error2s& e1, Error2s& e2) const { + void projectError(const Error2s& e1, Error2s& e2) const { - // d1 = E.transpose() * e1 = (3*2m)*2m - Vector3 d1; - d1.setZero(); - for (size_t k = 0; k < size(); k++) - d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block(ZDim * k, 0).transpose() * e1[k]; - // d2 = E.transpose() * e1 = (3*2m)*2m - Vector3 d2 = PointCovariance_ * d1; + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; - // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] - for (size_t k = 0; k < size(); k++) - e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; - } + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block(ZDim * k, 0) * d2; + } /// Scratch space for multiplyHessianAdd mutable Error2s e1, e2; @@ -356,19 +363,17 @@ public: e2.resize(size()); // e1 = F * x = (2m*dm)*dm - size_t k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - e1[k++] = it.second * ConstDMap(x + D * key); + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + e1[k] = FBlocks_[k] * ConstDMap(x + D * key); } projectError(e1, e2); // y += F.transpose()*e2 = (2d*2m)*2m - k = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { - Key key = it.first; - DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -389,7 +394,7 @@ public: // e1 = F * x = (2m*dm)*dm for (size_t k = 0; k < size(); ++k) - e1[k] = Fblocks_[k].second * x.at(keys_[k]); + e1[k] = FBlocks_[k] * x.at(keys_[k]); projectError(e1, e2); @@ -401,8 +406,8 @@ public: Vector& yi = it.first->second; // Create the value as a zero vector if it does not exist. if (it.second) - yi = Vector::Zero(Fblocks_[k].second.cols()); - yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + yi = Vector::Zero(FBlocks_[k].cols()); + yi += FBlocks_[k].transpose() * alpha * e2[k]; } } @@ -412,9 +417,9 @@ public: void multiplyHessianDummy(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + for (size_t k = 0; k < size(); ++k) { static const Vector empty; - Key key = Fi.first; + Key key = keys_[k]; std::pair it = y.tryInsert(key, empty); Vector& yi = it.first->second; yi = x.at(key); @@ -429,14 +434,14 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); // g = F.transpose()*e2 VectorValues g; for (size_t k = 0; k < size(); ++k) { Key key = keys_[k]; - g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + g.insert(key, -FBlocks_[k].transpose() * e2[k]); } // return it @@ -456,27 +461,33 @@ public: e1.resize(size()); e2.resize(size()); for (size_t k = 0; k < size(); k++) - e1[k] = b_.segment < 2 > (2 * k); + e1[k] = b_.segment(ZDim * k); projectError(e1, e2); for (size_t k = 0; k < size(); ++k) { // for each camera in the factor Key j = keys_[k]; - DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; + DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k]; } } /// Gradient wrt a key at any values Vector gradient(Key key, const VectorValues& x) const { - throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet"); + throw std::runtime_error( + "gradient for RegularImplicitSchurFactor is not implemented yet"); } - }; // end class RegularImplicitSchurFactor +template +const int RegularImplicitSchurFactor::D; + +template +const int RegularImplicitSchurFactor::ZDim; + // traits -template struct traits > : public Testable< - RegularImplicitSchurFactor > { +template struct traits > : public Testable< + RegularImplicitSchurFactor > { }; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index c448dbed4..b147c2721 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -22,9 +22,9 @@ #include #include #include -#include #include +#include #include #include @@ -41,13 +41,23 @@ namespace gtsam { * The methods take a Cameras argument, which should behave like PinholeCamera, and * the value of a point, which is kept in the base class. */ -template +template class SmartFactorBase: public NonlinearFactor { -protected: - +private: + typedef NonlinearFactor Base; + typedef SmartFactorBase This; typedef typename CAMERA::Measurement Z; + /** + * As of Feb 22, 2015, the noise model is the same for all measurements and + * is isotropic. This allows for moving most calculations of Schur complement + * etc to be moved to CameraSet very easily, and also agrees pragmatically + * with what is normally done. + */ + SharedIsotropic noiseModel_; + +protected: /** * 2D measurement and noise model for each of the m views * We keep a copy of measurements for I/O and computing the error. @@ -55,45 +65,54 @@ protected: */ std::vector measured_; - std::vector noise_; ///< noise model used - - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + /// @name Pose of the camera in the body frame + const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + /// @} + static const int Dim = traits::dimension; ///< Camera dimension static const int ZDim = traits::dimension; ///< Measurement dimension - /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' - typedef std::pair KeyMatrix2D; // Fblocks - typedef Eigen::Matrix MatrixDD; // camera hessian block + // Definitions for block matrices used internally + typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix Matrix23; - typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; - /// shorthand for base class type - typedef NonlinearFactor Base; - - /// shorthand for this class - typedef SmartFactorBase This; + // check that noise model is isotropic and the same + // TODO, this is hacky, we should just do this via constructor, not add + void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { + if (!sharedNoiseModel) + return; + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + if (!noiseModel_) + noiseModel_ = sharedIsotropic; + else if (!sharedIsotropic->equals(*noiseModel_)) + throw std::runtime_error( + "SmartFactorBase: cannot add measurements with different noise model"); + } public: + // Definitions for blocks of F, externally visible + typedef Eigen::Matrix MatrixZD; // F + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; - /** - * Constructor - * @param body_P_sensor is the transform from sensor to body frame (default identity) - */ + /// Constructor SmartFactorBase(boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor) { - } + body_P_sensor_(body_P_sensor){} - /** Virtual destructor */ + /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { } @@ -101,36 +120,36 @@ public: * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark - * @param noise_i is the measurement noise + * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& poseKey_i, - const SharedNoiseModel& noise_i) { + void add(const Z& measured_i, const Key& cameraKey_i, + const SharedNoiseModel& sharedNoiseModel) { this->measured_.push_back(measured_i); - this->keys_.push_back(poseKey_i); - this->noise_.push_back(noise_i); + this->keys_.push_back(cameraKey_i); + maybeSetNoiseModel(sharedNoiseModel); } /** * Add a bunch of measurements, together with the camera keys and noises */ - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noises.at(i)); + this->keys_.push_back(cameraKeys.at(i)); + maybeSetNoiseModel(noises.at(i)); } } /** - * Add a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and use the same noise model for all of them */ - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noise); + this->keys_.push_back(cameraKeys.at(i)); + maybeSetNoiseModel(noise); } } @@ -143,7 +162,7 @@ public: for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); - this->noise_.push_back(noise); + maybeSetNoiseModel(noise); } } @@ -157,9 +176,12 @@ public: return measured_; } - /** return the noise models */ - const std::vector& noise() const { - return noise_; + /// Collect all cameras: important that in key order + virtual Cameras cameras(const Values& values) const { + Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) + cameras.push_back(values.at(k)); + return cameras; } /** @@ -172,11 +194,11 @@ public: std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { std::cout << "measurement, p = " << measured_[k] << "\t"; - noise_[k]->print("noise model = "); + noiseModel_->print("noise model = "); } - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); + print("", keyFormatter); } /// equals @@ -189,518 +211,105 @@ public: areMeasurementsEqual = false; break; } - return e && Base::equals(p, tol) && areMeasurementsEqual - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e && Base::equals(p, tol) && areMeasurementsEqual; } - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - - // Project into CameraSet - std::vector predicted; - try { - predicted = cameras.project(point); - } catch (CheiralityException&) { - std::cout << "reprojectionError: Cheirality exception " << std::endl; - exit(EXIT_FAILURE); // TODO: throw exception + /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives + template + Vector unwhitenedError(const Cameras& cameras, const POINT& point, + boost::optional Fs = boost::none, // + boost::optional E = boost::none) const { + Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + if(body_P_sensor_){ + for(size_t i=0; i < Fs->size(); i++){ + Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + Matrix J(6, 6); + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + Fs->at(i) = Fs->at(i) * J; + } } - - // Calculate vector of errors - size_t nrCameras = cameras.size(); - Vector b(ZDim * nrCameras); - for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) { - Z e = predicted[i] - measured_.at(i); - b.segment(row) = e.vector(); - } - - return b; - } - - /// Calculate vector of re-projection errors, noise model applied - Vector whitenedError(const Cameras& cameras, const Point3& point) const { - Vector b = reprojectionError(cameras, point); - size_t nrCameras = cameras.size(); - for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) - b.segment(row) = noise_.at(i)->whiten(b.segment(row)); - return b; + return ue; } /** - * Calculate the error of the factor. + * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] + * Noise model applied + */ + template + Vector whitenedError(const Cameras& cameras, const POINT& point) const { + Vector e = cameras.reprojectionError(point, measured_); + if (noiseModel_) + noiseModel_->whitenInPlace(e); + return e; + } + + /** Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - * This is different from reprojectionError(cameras,point) as each point is whitened + * Will be used in "error(Values)" function required by NonlinearFactor base class */ + template double totalReprojectionError(const Cameras& cameras, - const Point3& point) const { - Vector b = reprojectionError(cameras, point); - double overallError = 0; - size_t nrCameras = cameras.size(); - for (size_t i = 0; i < nrCameras; i++) - overallError += noise_.at(i)->distance(b.segment(i * ZDim)); - return 0.5 * overallError; - } - - /** - * Compute whitenedError, returning only the derivative E, i.e., - * the stacked ZDim*3 derivatives of project with respect to the point. - * Assumes non-degenerate ! TODO explain this - */ - Vector whitenedError(const Cameras& cameras, const Point3& point, - Matrix& E) const { - - // Project into CameraSet, calculating derivatives - std::vector predicted; - try { - using boost::none; - predicted = cameras.project(point, none, E, none); - } catch (CheiralityException&) { - std::cout << "whitenedError(E): Cheirality exception " << std::endl; - exit(EXIT_FAILURE); // TODO: throw exception - } - - // if needed, whiten - size_t m = keys_.size(); - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - - // Calculate error - const Z& zi = measured_.at(i); - Vector bi = (zi - predicted[i]).vector(); - - // if needed, whiten - SharedNoiseModel model = noise_.at(i); - if (model) { - // TODO: re-factor noiseModel to take any block/fixed vector/matrix - Vector dummy; - Matrix Ei = E.block(row, 0); - model->WhitenSystem(Ei, dummy); - E.block(row, 0) = Ei; - } - b.segment(row) = bi; - } - return b; - } - - /** - * Compute F, E, and optionally H, where F and E are the stacked derivatives - * with respect to the cameras, point, and calibration, respectively. - * The value of cameras/point are passed as parameters. - * Returns error vector b - * TODO: the treatment of body_P_sensor_ is weird: the transformation - * is applied in the caller, but the derivatives are computed here. - */ - Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, - Matrix& E, boost::optional G = boost::none) const { - - // Project into CameraSet, calculating derivatives - std::vector predicted; - try { - predicted = cameras.project(point, F, E, G); - } catch (CheiralityException&) { - std::cout << "whitenedError(E,F): Cheirality exception " << std::endl; - exit(EXIT_FAILURE); // TODO: throw exception - } - - // Calculate error and whiten derivatives if needed - size_t m = keys_.size(); - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - - // Calculate error - const Z& zi = measured_.at(i); - Vector bi = (zi - predicted[i]).vector(); - - // if we have a sensor offset, correct camera derivatives - if (body_P_sensor_) { - // TODO: no simpler way ?? - const Pose3& pose_i = cameras[i].pose(); - Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); - Matrix66 J; - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - F.block(row, 0) *= J; - } - - // if needed, whiten - SharedNoiseModel model = noise_.at(i); - if (model) { - // TODO, refactor noiseModel so we can take blocks - Matrix Fi = F.block(row, 0); - Matrix Ei = E.block(row, 0); - if (!G) - model->WhitenSystem(Fi, Ei, bi); - else { - Matrix Gi = G->block(row, 0); - model->WhitenSystem(Fi, Ei, Gi, bi); - G->block(row, 0) = Gi; - } - F.block(row, 0) = Fi; - E.block(row, 0) = Ei; - } - b.segment(row) = bi; - } - return b; + const POINT& point) const { + Vector e = whitenedError(cameras, point); + return 0.5 * e.dot(e); } /// Computes Point Covariance P from E - static Matrix3 PointCov(Matrix& E) { + static Matrix PointCov(Matrix& E) { return (E.transpose() * E).inverse(); } - /// Computes Point Covariance P, with lambda parameter - static Matrix3 PointCov(Matrix& E, double lambda, - bool diagonalDamping = false) { - - Matrix3 EtE = E.transpose() * E; - - if (diagonalDamping) { // diagonal of the hessian - EtE(0, 0) += lambda * EtE(0, 0); - EtE(1, 1) += lambda * EtE(1, 1); - EtE(2, 2) += lambda * EtE(2, 2); - } else { - EtE(0, 0) += lambda; - EtE(1, 1) += lambda; - EtE(2, 2) += lambda; - } - - return (EtE).inverse(); - } - - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, - const Point3& point) const { - whitenedError(cameras, point, E); - P = PointCov(E); - } - /** * Compute F, E, and b (called below in both vanilla and SVD versions), where * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * with respect to the point. The value of cameras/point are passed as parameters. + * TODO: Kill this obsolete method */ - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { - + template + void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras, const POINT& point) const { // Project into Camera set and calculate derivatives - // TODO: if D==6 we optimize only camera pose. That is fairly hacky! - Matrix F, G; - using boost::none; - boost::optional optionalG(G); - b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG); - - // Now calculate f and divide up the F derivatives into Fblocks - double f = 0.0; - size_t m = keys_.size(); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - - // Accumulate normalized error - f += b.segment(row).squaredNorm(); - - // Get piece of F and possibly G - Matrix2D Fi; - if (D == 6) - Fi << F.block(row, 0); - else - Fi << F.block(row, 0), G.block(row, 0); - - // Push it onto Fblocks - Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); - } - return f; - } - - /// Create BIG block-diagonal matrix F from Fblocks - static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { - size_t m = Fblocks.size(); - F.resize(ZDim * m, D * m); - F.setZero(); - for (size_t i = 0; i < m; ++i) - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; - } - - /** - * Compute F, E, and b, where F and E are the stacked derivatives - * with respect to the point. The value of cameras/point are passed as parameters. - */ - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras, const Point3& point) const { - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, b, cameras, point); - FillDiagonalF(Fblocks, F); - return f; + // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) + // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| + // = |A*dx - (z-h(x_bar))| + b = -unwhitenedError(cameras, point, Fblocks, E); } /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point) const { + template + void computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + Vector& b, const Cameras& cameras, const POINT& point) const { Matrix E; - double f = computeJacobians(Fblocks, E, b, cameras, point); + computeJacobians(Fblocks, E, b, cameras, point); + + static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); size_t m = this->keys_.size(); - // Enull = zeros(ZDim * m, ZDim * m - 3); - Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns - - return f; + Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns } - /// Matrix version of SVD - // TODO, there should not be a Matrix version, really - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras, const Point3& point) const { - std::vector Fblocks; - double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - FillDiagonalF(Fblocks, F); - return f; - } - - /** - * Linearize returns a Hessianfactor that is an approximation of error(p) - */ - boost::shared_ptr > createHessianFactor( + /// Linearize to a Hessianfactor + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - int numKeys = this->keys_.size(); - - std::vector Fblocks; + std::vector Fblocks; Matrix E; Vector b; - double f = computeJacobians(Fblocks, E, b, cameras, point); - Matrix3 P = PointCov(E, lambda, diagonalDamping); + computeJacobians(Fblocks, E, b, cameras, point); -//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix -#ifdef HESSIAN_BLOCKS - // Create structures for Hessian Factors - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, + b); - sparseSchurComplement(Fblocks, E, P, b, Gs, gs); - // schurComplement(Fblocks, E, P, b, Gs, gs); - - //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); - //std::vector < Vector > gs2(gs.begin(), gs.end()); - - return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); -#else // we create directly a SymmetricBlockMatrix - size_t n1 = D * numKeys + 1; - std::vector dims(numKeys + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, D); - dims.back() = 1; - - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) - sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - augmentedHessian(numKeys, numKeys)(0, 0) = f; - return boost::make_shared >(this->keys_, + return boost::make_shared >(keys_, augmentedHessian); -#endif - } - - /** - * Do Schur complement, given Jacobian as F,E,P. - * Slow version - works on full matrices - */ - void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * inv(E'*E) * E' * F - // gs = F' * (b - E * inv(E'*E) * E' * b) - // This version uses full matrices - - int numKeys = this->keys_.size(); - - /// Compute Full F ???? - Matrix F; - FillDiagonalF(Fblocks, F); - - Matrix H(D * numKeys, D * numKeys); - Vector gs_vector; - - H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); - - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); - GsCount2++; - } - } - } - } - - /** - * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix - * Fast version - works on with sparsity - */ - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // D = (Dx2) * (2) - // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() - * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - } - - /** - * Do Schur complement, given Jacobian as F,E,P, return Gs/gs - * Fast version - works on with sparsity - */ - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); - - int GsIndex = 0; - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - // GsIndex points to the upper triangular blocks - // 0 1 2 3 4 - // X 5 6 7 8 - // X X 9 10 11 - // X X X 12 13 - // X X X X 14 - const Matrix2D& Fi1 = Fblocks.at(i1).second; - - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - { // for i1 = i2 - // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); - GsIndex++; - } - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - GsIndex++; - } - } // end of for over cameras - } - - /** - * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, - * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. - */ - void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, - const double f, const FastVector allKeys, - /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { - // Schur complement trick - // Gs = F' * F - F' * E * P * E' * F - // gs = F' * (b - E * P * E' * b) - - MatrixDD matrixBlock; - - FastMap KeySlotMap; - for (size_t slot = 0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); - - // a single point is observed in numKeys cameras - size_t numKeys = this->keys_.size(); // cameras observing current point - size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group - - // Blockwise Schur complement - for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor - - const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - - // D = (DxZDim) * (ZDim) - // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) - // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) - // Key cameraKey_i1 = this->keys_[i1]; - DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]]; - - // information vector - store previous vector - // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, - aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - // main block diagonal - store previous block - matrixBlock = augmentedHessian(aug_i1, aug_i1); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = - matrixBlock - + (Fi1.transpose() - * (Fi1 - - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1)); - - // upper triangular part of the hessian - for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera - const Matrix2D& Fi2 = Fblocks.at(i2).second; - - //Key cameraKey_i2 = this->keys_[i2]; - DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - - // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) - // off diagonal block - store previous block - // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = - augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() - * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); - } - } // end of for over cameras - - augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; } /** @@ -712,73 +321,100 @@ public: const double lambda, bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian, const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; Matrix E; Vector b; - double f = computeJacobians(Fblocks, E, b, cameras, point); - Matrix3 P = PointCov(E, lambda, diagonalDamping); - updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + std::vector Fblocks; + computeJacobians(Fblocks, E, b, cameras, point); + Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, + augmentedHessian); } - /** - * Return Jacobians as RegularImplicitSchurFactor with raw access - */ - boost::shared_ptr > createRegularImplicitSchurFactor( - const Cameras& cameras, const Point3& point, double lambda = 0.0, - bool diagonalDamping = false) const { - typename boost::shared_ptr > f( - new RegularImplicitSchurFactor()); - computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); - f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); - f->initKeys(); - return f; + /// Whiten the Jacobians computed by computeJacobians using noiseModel_ + void whitenJacobians(std::vector& F, Matrix& E, Vector& b) const { + noiseModel_->WhitenSystem(E, b); + // TODO make WhitenInPlace work with any dense matrix type + for (size_t i = 0; i < F.size(); i++) + F[i] = noiseModel_->Whiten(F[i]); + } + + /// Return Jacobians as RegularImplicitSchurFactor with raw access + boost::shared_ptr > // + createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { + Matrix E; + Vector b; + std::vector F; + computeJacobians(F, E, b, cameras, point); + whitenJacobians(F, E, b); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(keys_, F, E, + P, b); } /** * Return Jacobians as JacobianFactorQ */ - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; Matrix E; Vector b; - computeJacobians(Fblocks, E, b, cameras, point); - Matrix3 P = PointCov(E, lambda, diagonalDamping); - return boost::make_shared >(Fblocks, E, P, b); + std::vector F; + computeJacobians(F, E, b, cameras, point); + const size_t M = b.size(); + Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E, P, b, n); } /** - * Return Jacobians as JacobianFactor + * Return Jacobians as JacobianFactorSVD * TODO lambda is currently ignored */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { - size_t numKeys = this->keys_.size(); - std::vector Fblocks; + size_t m = this->keys_.size(); + std::vector F; Vector b; - Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - return boost::make_shared >(Fblocks, Enull, b); + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + computeJacobiansSVD(F, E0, b, cameras, point); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, + noiseModel_->sigma()); + return boost::make_shared >(keys_, F, E0, b, n); + } + + /// Create BIG block-diagonal matrix F from Fblocks + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + size_t m = Fblocks.size(); + F.resize(ZDim * m, Dim * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(ZDim * i, Dim * i) = Fblocks.at(i); + } + + + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity } private: - /// Serialization function +/// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -} -; +}; +// end class SmartFactorBase -template -const int SmartFactorBase::ZDim; +// Definitions need to avoid link errors (above are only declarations) +template const int SmartFactorBase::Dim; +template const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index a28482583..d6b549acb 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -32,108 +31,135 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartProjectionFactorState { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; -protected: +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart projection factors + */ +class GTSAM_EXPORT SmartProjectionParams { public: - SmartProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q + /// @name Parameters governing the triangulation + /// @{ + mutable TriangulationParameters triangulation; + const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + 1e-5), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } }; /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, - D> { +template +class SmartProjectionFactor: public SmartFactorBase { + +public: + +private: + typedef SmartFactorBase Base; + typedef SmartProjectionFactor This; + typedef SmartProjectionFactor SmartProjectionCameraFactor; + protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ - const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + /// @name Parameters + /// @{ + const SmartProjectionParams params_; + /// @} + + /// @name Caching triangulation + /// @{ + mutable TriangulationResult result_; ///< result from triangulateSafe mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses - - const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases - - const bool enableEPI_; ///< if set to true, will refine triangulation using LM - - const double linearizationThreshold_; ///< threshold to decide whether to re-linearize - mutable std::vector cameraPosesLinearization_; ///< current linearization poses - - mutable Point3 point_; ///< Current estimate of the 3D point - - mutable bool degenerate_; - mutable bool cheiralityException_; - - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; - - /// shorthand for base class type - typedef SmartFactorBase, D> Base; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large - - /// shorthand for this class - typedef SmartProjectionFactor This; + /// @} public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef PinholeCamera Camera; - typedef CameraSet Cameras; + /// shorthand for a set of cameras + typedef CameraSet Cameras; /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors */ - SmartProjectionFactor(const double rankTol, const double linThreshold, - const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = - SmartFactorStatePtr(new SmartProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( - linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( - landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( - dynamicOutlierRejectionThreshold) { + SmartProjectionFactor( + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(body_P_sensor), params_(params), // + result_(TriangulationResult::Degenerate()) { } /** Virtual destructor */ @@ -147,24 +173,33 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << s << "SmartProjectionFactor\n"; + std::cout << "linearizationMode:\n" << params_.linearizationMode + << std::endl; + std::cout << "triangulationParameters:\n" << params_.triangulation + << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } - /// Check if the new linearization point_ is the same as the one used for previous triangulation + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && params_.linearizationMode == e->params_.linearizationMode + && Base::equals(p, tol); + } + + /// Check if the new linearization point is the same as the one used for previous triangulation bool decideIfTriangulate(const Cameras& cameras) const { - // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate // Note that this is not yet "selecting linearization", that will come later, and we only check if the - // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point size_t m = cameras.size(); bool retriangulate = false; - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + // if we do not have a previous linearization point or the new linearization point includes more poses if (cameraPosesTriangulation_.empty() || cameras.size() != cameraPosesTriangulation_.size()) retriangulate = true; @@ -172,7 +207,7 @@ public: if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], - retriangulationThreshold_)) { + params_.retriangulationThreshold)) { retriangulate = true; // at least two poses are different, hence we retriangulate break; } @@ -187,137 +222,34 @@ public: cameraPosesTriangulation_.push_back(cameras[i].pose()); } - return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation - } - - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization - bool decideIfLinearize(const Cameras& cameras) const { - // "selective linearization" - // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose - // (we only care about the "rigidity" of the poses, not about their absolute pose) - - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize - return true; - - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses - if (cameraPosesLinearization_.empty() - || (cameras.size() != cameraPosesLinearization_.size())) - return true; - - Pose3 firstCameraPose, firstCameraPoseOld; - for (size_t i = 0; i < cameras.size(); i++) { - - if (i == 0) { // we store the initial pose, this is useful for selective re-linearization - firstCameraPose = cameras[i].pose(); - firstCameraPoseOld = cameraPosesLinearization_[i]; - continue; - } - - // we compare the poses in the frame of the first pose - Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); - Pose3 localCameraPoseOld = firstCameraPoseOld.between( - cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) - return true; // at least two "relative" poses are different, hence we re-linearize - } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation } /// triangulateSafe - size_t triangulateSafe(const Values& values) const { - return triangulateSafe(this->cameras(values)); - } - - /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); - if (m < 2) { // if we have a single pose the corresponding factor is uninformative - degenerate_ = true; - return m; - } + if (m < 2) // if we have a single pose the corresponding factor is uninformative + return TriangulationResult::Degenerate(); + bool retriangulate = decideIfTriangulate(cameras); - - if (retriangulate) { - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, - rankTolerance_, enableEPI_); - degenerate_ = false; - cheiralityException_ = false; - - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.0; - size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - Point3 cameraTranslation = camera.pose().translation(); - // we discard smart factors corresponding to points that are far away - if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { - degenerate_ = true; - break; - } - const Point2& zi = this->measured_.at(i); - try { - Point2 reprojectionError(camera.project(point_) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - cheiralityException_ = true; - } - i += 1; - } - // we discard smart factors that have large reprojection error - if (dynamicOutlierRejectionThreshold_ > 0 - && totalReprojError / m > dynamicOutlierRejectionThreshold_) - degenerate_ = true; - - } catch (TriangulationUnderconstrainedException&) { - // if TriangulationUnderconstrainedException can be - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - // in the second case we want to use a rotation-only smart factor - degenerate_ = true; - cheiralityException_ = false; - } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - cheiralityException_ = true; - } - } - return m; + if (retriangulate) + result_ = gtsam::triangulateSafe(cameras, this->measured_, + params_.triangulation); + return result_; } /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { - - bool isDebug = false; - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) { - std::cout - << "createRegularImplicitSchurFactor: degenerate configuration" - << std::endl; - } - return false; - } else { - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - return true; - } + triangulateSafe(cameras); // imperative, might reset result_ + return (result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0) const { + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = + false) const { - bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors std::vector js; @@ -331,264 +263,198 @@ public: exit(1); } - this->triangulateSafe(cameras); + triangulateSafe(cameras); - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - // std::cout << "In linearize: exception" << std::endl; + if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { + // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(D, D); + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + v = zero(Base::Dim); + return boost::make_shared >(this->keys_, + Gs, gs, 0.0); } - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - - bool doLinearize = this->decideIfLinearize(cameras); - - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize - for (size_t i = 0; i < cameras.size(); i++) - this->cameraPosesLinearization_[i] = cameras[i].pose(); - - if (!doLinearize) { // return the previous Hessian factor - std::cout << "=============================" << std::endl; - std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - std::cout - << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" - << std::endl; - exit(1); - return boost::make_shared >(this->keys_, - this->state_->Gs, this->state_->gs, this->state_->f); - } - - // ================================================================== - Matrix F, E; + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). + std::vector Fblocks; + Matrix E; Vector b; - double f = computeJacobians(F, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); - // Schur complement trick - // Frank says: should be possible to do this more efficiently? - // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); - Vector gs_vector; + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); - Matrix3 P = Base::PointCov(E, lambda); - H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); - if (isDebug) - std::cout << "gs_vector size " << gs_vector.size() << std::endl; + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); - GsCount2++; - } - } - } - // ================================================================== - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables - this->state_->Gs = Gs; - this->state_->gs = gs; - this->state_->f = f; - } - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, + augmentedHessian); } // create factor - boost::shared_ptr > createRegularImplicitSchurFactor( + boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createRegularImplicitSchurFactor(cameras, point_, lambda); + return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else - return boost::shared_ptr >(); + // failed: return empty + return boost::shared_ptr >(); } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianQFactor(cameras, point_, lambda); + return Base::createJacobianQFactor(cameras, *result_, lambda); else - return boost::make_shared >(this->keys_); + // failed: return empty + return boost::make_shared >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { - Cameras myCameras; - // TODO triangulate twice ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - return createJacobianQFactor(myCameras, lambda); - else - return boost::make_shared >(this->keys_); + return createJacobianQFactor(this->cameras(values), lambda); } /// different (faster) way to compute Jacobian factor boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianSVDFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, *result_, lambda); else - return boost::make_shared >(this->keys_); + // failed: return empty + return boost::make_shared >(this->keys_); } - /// Returns true if nonDegenerate - bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { - Values valuesFactor; + /// linearize to a Hessianfactor + virtual boost::shared_ptr > linearizeToHessian( + const Values& values, double lambda = 0.0) const { + return createHessianFactor(this->cameras(values), lambda); + } - // Select only the cameras - BOOST_FOREACH(const Key key, this->keys_) - valuesFactor.insert(key, values.at(key)); + /// linearize to an Implicit Schur factor + virtual boost::shared_ptr > linearizeToImplicit( + const Values& values, double lambda = 0.0) const { + return createRegularImplicitSchurFactor(this->cameras(values), lambda); + } - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + /// linearize to a JacobianfactorQ + virtual boost::shared_ptr > linearizeToJacobian( + const Values& values, double lambda = 0.0) const { + return createJacobianQFactor(this->cameras(values), lambda); + } - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) - return false; - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - - if (this->degenerate_) { - std::cout << "SmartProjectionFactor: this is not ready" << std::endl; - std::cout << "this->cheiralityException_ " << this->cheiralityException_ - << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Cameras& cameras, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(cameras, lambda); + case IMPLICIT_SCHUR: + return createRegularImplicitSchurFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); + case JACOBIAN_Q: + return createJacobianQFactor(cameras, lambda); + default: + throw std::runtime_error("SmartFactorlinearize: unknown mode"); } - return true; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { - return Base::computeEP(E, P, cameras, point_); + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Values& values, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + Cameras cameras = this->cameras(values); + return linearizeDamped(cameras, lambda); } - /// Takes values - bool computeEP(Matrix& E, Matrix& P, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /// linearize + virtual boost::shared_ptr linearize( + const Values& values) const { + return linearizeDamped(values); + } + + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeEP(E, P, myCameras); + cameras.project2(*result_, boost::none, E); return nonDegenerate; } - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, b, myCameras); - return nonDegenerate; + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateAndComputeE(E, cameras); } /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - double computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Cameras& cameras) const { + void computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { - if (this->degenerate_) { - std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; - std::cout << "point " << point_ << std::endl; - std::cout - << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" - << std::endl; - if (D > 6) { - std::cout - << "Management of degeneracy is not yet ready when one also optimizes for the calibration " - << std::endl; - } - - int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 2); - b = zero(2 * numKeys); - double f = 0; - for (size_t i = 0; i < this->measured_.size(); i++) { - if (i == 0) { // first pose - this->point_ = cameras[i].backprojectPointAtInfinity( - this->measured_.at(i)); - // 3D parametrization of point at infinity: [px py 1] - } - Matrix Fi, Ei; - Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) - - this->measured_.at(i)).vector(); - - this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); - f += bi.squaredNorm(); - Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); - E.block<2, 2>(2 * i, 0) = Ei; - subInsert(b, bi, 2 * i); - } - return f; + if (!result_) { + // Handle degeneracy + // TODO check flag whether we should do this + Unit3 backProjected = cameras[0].backprojectPointAtInfinity( + this->measured_.at(0)); + Base::computeJacobians(Fblocks, E, b, cameras, backProjected); } else { - // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } + } + + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; } /// takes values - bool computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras myCameras; - double good = computeCamerasAndTriangulate(values, myCameras); - if (good) - computeJacobiansSVD(Fblocks, Enull, b, myCameras); - return true; - } - - /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Cameras& cameras) const { - return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - // TODO should there be a lambda? - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobians(F, E, b, cameras, point_); - } - - /// Calculate vector of re-projection errors, before applying noise model - /// Assumes triangulation was done and degeneracy handled - Vector reprojectionError(const Cameras& cameras) const { - return Base::reprojectionError(cameras, point_); - } - - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return reprojectionError(myCameras); + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + return nonDegenerate; + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + return Base::unwhitenedError(cameras, *result_); else - return zero(myCameras.size() * 2); + return zero(cameras.size() * 2); } /** @@ -600,86 +466,57 @@ public: double totalReprojectionError(const Cameras& cameras, boost::optional externalPoint = boost::none) const { - size_t nrCameras; - if (externalPoint) { - nrCameras = this->keys_.size(); - point_ = *externalPoint; - degenerate_ = false; - cheiralityException_ = false; - } else { - nrCameras = this->triangulateSafe(cameras); - } + if (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { + if (result_) + // All good, just use version in base class + return Base::totalReprojectionError(cameras, *result_); + else if (params_.degeneracyMode == HANDLE_INFINITY) { + // Otherwise, manage the exceptions with rotation-only factors + const Point2& z0 = this->measured_.at(0); + Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); + return Base::totalReprojectionError(cameras, backprojected); + } else // if we don't want to manage the exceptions we discard the factor - // std::cout << "In error evaluation: exception" << std::endl; return 0.0; - } + } - if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors - std::cout - << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" - << std::endl; - this->degenerate_ = true; - } - - if (this->degenerate_) { - // return 0.0; // TODO: this maybe should be zero? - std::cout - << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" - << std::endl; - size_t i = 0; - double overallError = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); - if (i == 0) // first pose - this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity - Point2 reprojectionError( - camera.projectPointAtInfinity(this->point_) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - i += 1; - } - return overallError; - } else { - // Just use version in base class - return Base::totalReprojectionError(cameras, point_); + /// Calculate total reprojection error + virtual double error(const Values& values) const { + if (this->active(values)) { + return totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ - boost::optional point() const { - return point_; + TriangulationResult point() const { + return result_; } /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } + + /// Is result valid? + bool isValid() const { + return result_; } /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); + bool isDegenerate() const { + return result_.degenerate(); } /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; - } - /** return cheirality verbosity */ - inline bool verboseCheirality() const { - return verboseCheirality_; - } - - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { - return throwCheirality_; + bool isPointBehindCamera() const { + return result_.behindCamera(); } private: @@ -687,11 +524,18 @@ private: /// Serialization function friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(throwCheirality_); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); + ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); } +} +; + +/// traits +template +struct traits > : public Testable< + SmartProjectionFactor > { }; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 127bf284f..93a4449f5 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -38,87 +38,37 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { + +private: + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; + protected: - LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) - - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + boost::shared_ptr K_; ///< calibration object (one for all cameras) public: - /// shorthand for base class type - typedef SmartProjectionFactor Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param body_P_sensor pose of the camera in the body frame + * @param params internal parameters of the smart factors */ - SmartProjectionPoseFactor(const double rankTol = 1, - const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + SmartProjectionPoseFactor(const boost::shared_ptr K, + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams& params = SmartProjectionParams()) : + Base(body_P_sensor, params), K_(K) { + } /** Virtual destructor */ - virtual ~SmartProjectionPoseFactor() {} - - /** - * add a new measurement and pose key - * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is key corresponding to the camera observing the same landmark - * @param noise_i is the measurement noise - * @param K_i is the (known) camera calibration - */ - void add(const Point2 measured_i, const Key poseKey_i, - const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { - Base::add(measured_i, poseKey_i, noise_i); - K_all_.push_back(K_i); - } - - /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noises vector of measurement noises - * @param Ks vector of calibration objects - */ - void add(std::vector measurements, std::vector poseKeys, - std::vector noises, - std::vector > Ks) { - Base::add(measurements, poseKeys, noises); - for (size_t i = 0; i < measurements.size(); i++) { - K_all_.push_back(Ks.at(i)); - } - } - - /** - * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noise measurement noise (same for all measurements) - * @param K the (known) camera calibration (same for all measurements) - */ - void add(std::vector measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); - K_all_.push_back(K); - } + virtual ~SmartProjectionPoseFactor() { } /** @@ -129,59 +79,15 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionPoseFactor, z = \n "; - BOOST_FOREACH(const boost::shared_ptr& K, K_all_) - K->print("calibration = "); Base::print("", keyFormatter); } /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol); } - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding - * to keys involved in this factor - * @return vector of Values - */ - typename Base::Cameras cameras(const Values& values) const { - typename Base::Cameras cameras; - size_t i=0; - BOOST_FOREACH(const Key& k, this->keys_) { - Pose3 pose = values.at(k); - if(Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - typename Base::Camera camera(pose, *K_all_[i++]); - cameras.push_back(camera); - } - return cameras; - } - - /** - * Linearize to Gaussian Factor - * @param values Values structure which must contain camera poses for this factor - * @return - */ - virtual boost::shared_ptr linearize( - const Values& values) const { - // depending on flag set on construction we may linearize to different linear factors - switch(linearizeTo_){ - case JACOBIAN_SVD : - return this->createJacobianSVDFactor(cameras(values), 0.0); - break; - case JACOBIAN_Q : - return this->createJacobianQFactor(cameras(values), 0.0); - break; - default: - return this->createHessianFactor(cameras(values)); - break; - } - } - /** * error calculates the error of the factor. */ @@ -193,9 +99,28 @@ public: } } - /** return the calibration object */ - inline const std::vector > calibration() const { - return K_all_; + /** return calibration shared pointers */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ + typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + + Camera camera(pose, K_); + cameras.push_back(camera); + } + return cameras; } private: @@ -205,10 +130,11 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(K_); } -}; // end of class declaration +}; +// end of class declaration /// traits template diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index d0371d1f8..b94eafba7 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -16,8 +16,7 @@ */ #include -#include -#include +#include #include namespace gtsam { @@ -27,18 +26,24 @@ namespace gtsam { * The calibration and pose are assumed known. * @addtogroup SLAM */ -template +template class TriangulationFactor: public NoiseModelFactor1 { public: - /// Camera type - typedef PinholeCamera Camera; + /// CAMERA type + typedef CAMERA Camera; protected: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + // Keep a copy of measurement and calibration for I/O - const Camera camera_; ///< Camera in which this landmark was seen + const CAMERA camera_; ///< CAMERA in which this landmark was seen const Point2 measured_; ///< 2D measurement // verbosity handling for Cheirality Exceptions @@ -47,12 +52,6 @@ protected: public: - /// shorthand for base class type - typedef NoiseModelFactor1 Base; - - /// shorthand for this class - typedef TriangulationFactor This; - /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -70,7 +69,7 @@ public: * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TriangulationFactor(const Camera& camera, const Point2& measured, + TriangulationFactor(const CAMERA& camera, const Point2& measured, const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( @@ -114,7 +113,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); + Point2 error(camera_.project2(point, boost::none, H2) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -154,7 +153,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); + b = -(camera_.project2(point, boost::none, A) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index b819993ef..fac2cf03d 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -28,6 +28,7 @@ inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { // 3D Geometry typedef Expression Point3_; +typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; @@ -40,33 +41,52 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { typedef Expression Cal3_S2_; typedef Expression Cal3Bundler_; +/// Expression version of PinholeBase::Project inline Point2_ project(const Point3_& p_cam) { - return Point2_(PinholeCamera::project_to_camera, p_cam); + Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + return Point2_(f, p_cam); } -template -Point2 project4(const CAMERA& camera, const Point3& p, - OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { +inline Point2_ project(const Unit3_& p_cam) { + Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project; + return Point2_(f, p_cam); +} + +namespace internal { +// Helper template for project2 expression below +template +Point2 project4(const CAMERA& camera, const POINT& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, + OptionalJacobian<2, FixedDimension::value> Dpoint) { return camera.project2(p, Dcam, Dpoint); } - -template -Point2_ project2(const Expression& camera_, const Point3_& p_) { - return Point2_(project4, camera_, p_); } +template +Point2_ project2(const Expression& camera_, + const Expression& p_) { + return Point2_(internal::project4, camera_, p_); +} + +namespace internal { +// Helper template for project3 expression below +template inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, + OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } - -inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { - return Point2_(project6, x, p, K); } -template -Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { - return Point2_(K, &CAL::uncalibrate, xy_hat); +template +inline Point2_ project3(const Pose3_& x, const Expression& p, + const Expression& K) { + return Point2_(internal::project6, x, p, K); +} + +template +Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { + return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } } // \namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h new file mode 100644 index 000000000..8e83ec503 --- /dev/null +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartFactorScenarios.h + * @brief Scenarios for testSmart*.cpp + * @author Frank Dellaert + * @date Feb 2015 + */ + +#pragma once +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// three landmarks ~5 meters infront of camera +Point3 landmark1(5, 0.5, 1.2); +Point3 landmark2(5, -0.5, 1.2); +Point3 landmark3(3, 0, 3.0); +Point3 landmark4(10, 0.5, 1.2); +Point3 landmark5(10, -0.5, 1.2); + +// First camera pose, looking along X-axis, 1 meter above ground plane (x-y) +Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +// Second camera 1 meter to the right of first camera +Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Third camera 1 meter above the first camera +Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); + +// Create a noise unit2 for the pixel error +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + +static double fov = 60; // degrees +static size_t w = 640, h = 480; + +/* ************************************************************************* */ +// default Cal3_S2 cameras +namespace vanilla { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3_S2 K(fov, w, h); +static Cal3_S2 K2(1500, 1200, 0, w, h); +Camera level_camera(level_pose, K2); +Camera level_camera_right(pose_right, K2); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Camera cam1(level_pose, K2); +Camera cam2(pose_right, K2); +Camera cam3(pose_above, K2); +typedef GeneralSFMFactor SFMFactor; +SmartProjectionParams params; +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Camera level_camera(level_pose, sharedK); +Camera level_camera_right(pose_right, sharedK); +Camera cam1(level_pose, sharedK); +Camera cam2(pose_right, sharedK); +Camera cam3(pose_above, sharedK); +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose2 { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +Camera level_camera(level_pose, sharedK2); +Camera level_camera_right(pose_right, sharedK2); +Camera cam1(level_pose, sharedK2); +Camera cam2(pose_right, sharedK2); +Camera cam3(pose_above, sharedK2); +} + +/* *************************************************************************/ +// Cal3Bundler cameras +namespace bundler { +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SmartFactor; +static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +Camera level_camera(level_pose, K); +Camera level_camera_right(pose_right, K); +Point2 level_uv = level_camera.project(landmark1); +Point2 level_uv_right = level_camera_right.project(landmark1); +Pose3 pose1 = level_pose; +Camera cam1(level_pose, K); +Camera cam2(pose_right, K); +Camera cam3(pose_above, K); +typedef GeneralSFMFactor SFMFactor; +} +/* *************************************************************************/ +// Cal3Bundler poses +namespace bundlerPose { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static boost::shared_ptr sharedBundlerK( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +Camera level_camera(level_pose, sharedBundlerK); +Camera level_camera_right(pose_right, sharedBundlerK); +Camera cam1(level_pose, sharedBundlerK); +Camera cam2(pose_right, sharedBundlerK); +Camera cam3(pose_above, sharedBundlerK); +} +/* *************************************************************************/ + +template +CAMERA perturbCameraPose(const CAMERA& camera) { + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + return CAMERA(perturbedCameraPose, camera.calibration()); +} + +template +void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, + const CAMERA& cam3, Point3 landmark, vector& measurements_cam) { + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e0e26ecff..3bcc3eccd 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -160,7 +160,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); - const Point2 pi = SimpleCamera::project_to_camera(P2); + const Point2 pi = PinholeBase::Project(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 8571a345d..77944da83 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -19,11 +19,13 @@ #include #include #include +#include +#include -#include #include #include #include +#include #include #include @@ -39,8 +41,8 @@ using namespace gtsam; const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); -const vector > Fblocks = list_of > // - (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); +const vector FBlocks = list_of(F0)(F1)(F3); +const FastVector keys = list_of(0)(1)(3); // RHS and sigmas const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); @@ -51,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) { E.block<2,2>(0, 0) = eye(2); E.block<2,3>(2, 0) = 2 * ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); EXPECT(assert_equal(expectedP, P)); } @@ -84,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x - FastVector keys; - keys += 0,1,2,3; - Vector x = xvalues.vector(keys); + FastVector keys2; + keys2 += 0,1,2,3; + Vector x = xvalues.vector(keys2); Vector expected = zero(24); - RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); - EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); + RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); + EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); // Create ImplicitSchurFactor - RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); + RegularImplicitSchurFactor implicitFactor(keys, FBlocks, E, P, b); VectorValues zero = 0 * yExpected;// quick way to get zero w right structure { // First Version @@ -122,32 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model); - { // error - double expectedError = jf.error(xvalues); - double actualError = implicitFactor.errorJF(xvalues); - DOUBLES_EQUAL(expectedError,actualError,1e-7) + // error + double expectedError = 11875.083333333334; + { + EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7) + EXPECT_DOUBLES_EQUAL(11903.500000000007,implicitFactor.error(xvalues),1e-7) } - { // JacobianFactor with same error + { VectorValues yActual = zero; - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(alpha, xvalues, yActual); + jfQ.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jf.multiplyHessianAdd(-1, xvalues, yActual); + jfQ.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // check hessian Diagonal - VectorValues diagExpected = jf.hessianDiagonal(); + VectorValues diagExpected = jfQ.hessianDiagonal(); VectorValues diagActual = implicitFactor.hessianDiagonal(); EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); } { // check hessian Block Diagonal - map BD = jf.hessianBlockDiagonal(); + map BD = jfQ.hessianBlockDiagonal(); map actualBD = implicitFactor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); EXPECT(assert_equal(BD[0],actualBD[0])); @@ -157,40 +161,46 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jf.multiplyHessianAdd(alpha, xdata, y); + jfQ.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jf.multiplyHessianAdd(-1, xdata, y); + jfQ.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } + VectorValues expectedVV; + expectedVV.insert(0,-3.5*ones(6)); + expectedVV.insert(1,10*ones(6)/3); + expectedVV.insert(3,-19.5*ones(6)); { // Check gradientAtZero - VectorValues expected = jf.gradientAtZero(); VectorValues actual = implicitFactor.gradientAtZero(); - EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8)); + EXPECT(assert_equal(expectedVV, implicitFactor.gradientAtZero(), 1e-8)); } // Create JacobianFactorQR - JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model); + EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7) + EXPECT(assert_equal(expectedVV, jfQR.gradientAtZero(), 1e-8)); { const SharedDiagonal model; VectorValues yActual = zero; - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(alpha, xvalues, yActual); + jfQR.multiplyHessianAdd(alpha, xvalues, yActual); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); - jfq.multiplyHessianAdd(-1, xvalues, yActual); + jfQR.multiplyHessianAdd(-1, xvalues, yActual); EXPECT(assert_equal(zero, yActual, 1e-8)); } { // Raw memory Version std::fill(y, y + 24, 0);// zero y ! - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(expected, XMap(y), 1e-8)); - jfq.multiplyHessianAdd(alpha, xdata, y); + jfQR.multiplyHessianAdd(alpha, xdata, y); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); - jfq.multiplyHessianAdd(-1, xdata, y); + jfQR.multiplyHessianAdd(-1, xdata, y); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); } delete [] y; @@ -214,7 +224,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; Matrix3 P = (E.transpose() * E).inverse(); - RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); + RegularImplicitSchurFactor factor(keys, FBlocks, E, P, b); // hessianDiagonal VectorValues expected; @@ -255,6 +265,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // augmentedInformation (test just checks diagonals) + Matrix actualInfo = factor.augmentedInformation(); + EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12))); + + // information (test just checks diagonals) + Matrix actualInfo2 = factor.information(); + EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12))); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index b5ef18842..373d482fe 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -26,7 +26,7 @@ using namespace gtsam; /* ************************************************************************* */ #include #include -class PinholeFactor: public SmartFactorBase, 9> { +class PinholeFactor: public SmartFactorBase > { virtual double error(const Values& values) const { return 0.0; } @@ -45,7 +45,7 @@ TEST(SmartFactorBase, Pinhole) { /* ************************************************************************* */ #include -class StereoFactor: public SmartFactorBase { +class StereoFactor: public SmartFactorBase { virtual double error(const Values& values) const { return 0.0; } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp new file mode 100644 index 000000000..533e16bec --- /dev/null +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -0,0 +1,856 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSmartProjectionCameraFactor.cpp + * @brief Unit tests for SmartProjectionCameraFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + * @date Sept 2013 + */ + +#include "smartFactorScenarios.h" +#include +#include +#include +#include +#include + +using namespace boost::assign; + +static bool isDebugTest = false; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +static Key x1(1); +Symbol l1('l', 1), l2('l', 2), l3('l', 3); +Key c1 = 1, c2 = 2, c3 = 3; + +static Point2 measurement1(323.0, 240.0); + +static double rankTol = 1.0; + +template +PinholeCamera perturbCameraPoseAndCalibration( + const PinholeCamera& camera) { + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + typename gtsam::traits::TangentVector d; + d.setRandom(); + d *= 0.1; + CALIBRATION perturbedCalibration = camera.calibration().retract(d); + return PinholeCamera(perturbedCameraPose, perturbedCalibration); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, perturbCameraPose) { + using namespace vanilla; + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 perturbed_level_pose = level_pose.compose(noise_pose); + Camera actualCamera(perturbed_level_pose, K2); + + Camera expectedCamera = perturbCameraPose(level_camera); + CHECK(assert_equal(expectedCamera, actualCamera)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor2) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor3) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, x1, unit2); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Constructor4) { + using namespace vanilla; + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); + factor1.add(measurement1, x1, unit2); +} + +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, Equals ) { + using namespace vanilla; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, x1, unit2); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + factor2->add(measurement1, x1, unit2); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiseless ) { + using namespace vanilla; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); + CHECK( + assert_equal(zero(4), + factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noisy ) { + + using namespace vanilla; + + // Project one landmark into two cameras and add noise on first + Point2 level_uv = level_camera.project(landmark1) + Point2(0.2, 0.2); + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(c1, level_camera); + Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); + values.insert(c2, perturbed_level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + // Point is now uninitialized before a triangulation event + EXPECT(!factor1->point()); + + double expectedError = 58640; + double actualError1 = factor1->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError1, 1); + + // Check triangulated point + CHECK(factor1->point()); + EXPECT( + assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4)); + + // Check whitened errors + Vector expected(4); + expected << -7, 235, 58, -242; + SmartFactor::Cameras cameras1 = factor1->cameras(values); + Point3 point1 = *factor1->point(); + Vector actual = factor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + vector noises; + noises.push_back(unit2); + noises.push_back(unit2); + + vector views; + views.push_back(c1); + views.push_back(c2); + + factor2->add(measurements, views, noises); + + double actualError2 = factor2->error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create and fill smartfactors + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + smartFactor1->add(measurements_cam1, views, unit2); + smartFactor2->add(measurements_cam2, views, unit2); + smartFactor3->add(measurements_cam3, views, unit2); + + // Create factor graph and add priors on two cameras + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + // Create initial estimate + Values initial; + initial.insert(c1, cam1); + initial.insert(c2, cam2); + initial.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + initial.at(c3).print("Smart: Pose3 before optimization: "); + + // Points are now uninitialized before a triangulation event + EXPECT(!smartFactor1->point()); + EXPECT(!smartFactor2->point()); + EXPECT(!smartFactor3->point()); + + EXPECT_DOUBLES_EQUAL(75711, smartFactor1->error(initial), 1); + EXPECT_DOUBLES_EQUAL(58524, smartFactor2->error(initial), 1); + EXPECT_DOUBLES_EQUAL(77564, smartFactor3->error(initial), 1); + + // Error should trigger this and initialize the points, abort if not so + CHECK(smartFactor1->point()); + CHECK(smartFactor2->point()); + CHECK(smartFactor3->point()); + + EXPECT( + assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(), + 1e-4)); + + // Check whitened errors + Vector expected(6); + expected << 256, 29, -26, 29, -206, -202; + Point3 point1 = *smartFactor1->point(); + SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); + Vector reprojectionError = cameras1.reprojectionError(point1, + measurements_cam1); + EXPECT(assert_equal(expected, reprojectionError, 1)); + Vector actual = smartFactor1->whitenedError(cameras1, point1); + EXPECT(assert_equal(expected, actual, 1)); + + // Optimize + LevenbergMarquardtParams lmParams; + if (isDebugTest) { + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + } + LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7)); + EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7)); + EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7)); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { + + using namespace vanilla; + + // Project three landmarks into three cameras + vector measurements_cam1, measurements_cam2, measurements_cam3; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SfM_Track track1; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam1.at(i); + track1.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(track1, unit2); + + SfM_Track track2; + for (size_t i = 0; i < 3; ++i) { + SfM_Measurement measures; + measures.first = i + 1; // cameras are from 1 to 3 + measures.second = measurements_cam2.at(i); + track2.measurements.push_back(measures); + } + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(track2, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { + + using namespace vanilla; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(smartFactor5); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); + // VectorValues delta = GFG->optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 20)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPose(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 1)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { + + using namespace bundler; + + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4, measurements_cam5; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); + + vector views; + views.push_back(c1); + views.push_back(c2); + views.push_back(c3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, unit2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, unit2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, unit2); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor()); + smartFactor4->add(measurements_cam4, views, unit2); + + SmartFactor::shared_ptr smartFactor5(new SmartFactor()); + smartFactor5->add(measurements_cam5, views, unit2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); + + Values values; + values.insert(c1, cam1); + values.insert(c2, cam2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(c3, perturbCameraPoseAndCalibration(cam3)); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; + if (isDebugTest) + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + lmParams.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, noiselessBundler ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + + double actualError = factor1->error(values); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-3); + + Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + if (factor1->point()) + oldPoint = *(factor1->point()); + + Point3 expectedPoint; + if (factor1->point(values)) + expectedPoint = *(factor1->point(values)); + + EXPECT(assert_equal(expectedPoint, landmark1, 1e-3)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + smartGraph.push_back(factor1); + double expectedError = factor1->error(values); + double expectedErrorGraph = smartGraph.error(values); + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + // cout << "expectedPoint " << expectedPoint.vector() << endl; + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); + Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); + double actualError = 0.5 + * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + double actualErrorGraph = generalGraph.error(values); + + DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); + DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7); + DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7); + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + NonlinearFactorGraph smartGraph; + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + smartGraph.push_back(factor1); + Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; + Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; + Point3 expectedPoint; + if (factor1->point()) + expectedPoint = *(factor1->point()); + + // COMMENTS: + // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as + // value in the generalGrap + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second; + Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullHessian.block(18, 0, 3, 18); + Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullInfoVector.block(18, 0, 3, 1); + + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7)); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7)); +} + +/* *************************************************************************/ +// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors +//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ +// +// Values values; +// values.insert(c1, level_camera); +// values.insert(c2, level_camera_right); +// +// NonlinearFactorGraph smartGraph; +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// factor1->add(level_uv, c1, unit2); +// factor1->add(level_uv_right, c2, unit2); +// smartGraph.push_back(factor1); +// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); +// +// Point3 expectedPoint; +// if(factor1->point()) +// expectedPoint = *(factor1->point()); +// +// // COMMENTS: +// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as +// // value in the generalGrap +// NonlinearFactorGraph generalGraph; +// SFMFactor sfm1(level_uv, unit2, c1, l1); +// SFMFactor sfm2(level_uv_right, unit2, c2, l1); +// generalGraph.push_back(sfm1); +// generalGraph.push_back(sfm2); +// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation +// GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values); +// +// double alpha = 1.0; +// +// VectorValues yExpected, yActual, ytmp; +// VectorValues xtmp = map_list_of +// (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// gfg ->multiplyHessianAdd(alpha, xtmp, ytmp); +// +// VectorValues x = map_list_of +// (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9)) +// (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19)) +// (l1, (Vec(3) << 5.5, 0.5, 1.2)); +// +// gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual); +// gfg ->multiplyHessianAdd(alpha, x, yExpected); +// +// EXPECT(assert_equal(yActual,yExpected, 1e-7)); +//} +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { + + using namespace bundler; + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, c1, unit2); + factor1->add(level_uv_right, c2, unit2); + Matrix expectedE; + Vector expectedb; + + CameraSet cameras; + cameras.push_back(level_camera); + cameras.push_back(level_camera_right); + + factor1->error(values); // this is important to have a triangulation of the point + Point3 point; + if (factor1->point()) + point = *(factor1->point()); + vector Fblocks; + factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); + + NonlinearFactorGraph generalGraph; + SFMFactor sfm1(level_uv, unit2, c1, l1); + SFMFactor sfm2(level_uv_right, unit2, c2, l1); + generalGraph.push_back(sfm1); + generalGraph.push_back(sfm2); + values.insert(l1, point); // note: we get rid of possible errors in the triangulation + Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; + Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse(); + + Matrix3 expectedVinv = factor1->PointCov(expectedE); + EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { + + using namespace bundler; + + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + double rankTol = 1; + bool useEPI = false; + + // Hessian version + SmartProjectionParams params; + params.setRankTolerance(rankTol); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(useEPI); + + SmartFactor::shared_ptr explicitFactor( + new SmartFactor(boost::none, params)); + explicitFactor->add(level_uv, c1, unit2); + explicitFactor->add(level_uv_right, c2, unit2); + + GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( + values); + HessianFactor& hessianFactor = + dynamic_cast(*gaussianHessianFactor); + + // Implicit Schur version + params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); + SmartFactor::shared_ptr implicitFactor( + new SmartFactor(boost::none, params)); + implicitFactor->add(level_uv, c1, unit2); + implicitFactor->add(level_uv_right, c2, unit2); + GaussianFactor::shared_ptr gaussianImplicitSchurFactor = + implicitFactor->linearize(values); + CHECK(gaussianImplicitSchurFactor); + typedef RegularImplicitSchurFactor Implicit9; + Implicit9& implicitSchurFactor = + dynamic_cast(*gaussianImplicitSchurFactor); + + VectorValues x = map_list_of(c1, + (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, + (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); + + VectorValues yExpected, yActual; + double alpha = 1.0; + hessianFactor.multiplyHessianAdd(alpha, x, yActual); + implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected); + EXPECT(assert_equal(yActual, yExpected, 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 07c49008d..72147ff35 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -15,39 +15,25 @@ * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira + * @author Frank Dellaert * @date Sept 2013 */ -#include "../SmartProjectionPoseFactor.h" - -#include +#include "smartFactorScenarios.h" #include +#include #include #include #include -#include +#include #include -using namespace std; using namespace boost::assign; -using namespace gtsam; -static bool isDebugTest = false; - -// make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; - -static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); -static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler( - new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - -static double rankTol = 1.0; -static double linThreshold = -1.0; -static bool manageDegeneracy = true; +static const double rankTol = 1.0; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys using symbol_shorthand::X; @@ -58,94 +44,70 @@ static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); -static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; - -void projectToMultipleCameras(SimpleCamera cam1, SimpleCamera cam2, - SimpleCamera cam3, Point3 landmark, vector& measurements_cam) { - - Point2 cam1_uv1 = cam1.project(landmark); - Point2 cam2_uv1 = cam2.project(landmark); - Point2 cam3_uv1 = cam3.project(landmark); - measurements_cam.push_back(cam1_uv1); - measurements_cam.push_back(cam2_uv1); - measurements_cam.push_back(cam3_uv1); -} +LevenbergMarquardtParams lmParams; +// Make more verbose like so (in tests): +// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + factor1->add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { - SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, - body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); + factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + factor1->add(measurement1, x1, model); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartFactor::shared_ptr factor2(new SmartFactor(sharedK)); + factor2->add(measurement1, x1, model); CHECK(assert_equal(*factor1, *factor2)); } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; +TEST( SmartProjectionPoseFactor, noiseless ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); - SimpleCamera level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + using namespace vanillaPose; // Project two landmarks into two cameras - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); - Values values; - values.insert(x1, level_pose); - values.insert(x2, level_pose_right); + SmartFactor factor(sharedK); + factor.add(level_uv, x1, model); + factor.add(level_uv_right, x2, model); - SmartFactor factor; - factor.add(level_uv, x1, model, K); - factor.add(level_uv_right, x2, model, K); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); double actualError = factor.error(values); double expectedError = 0.0; @@ -155,65 +117,62 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - // test vector of errors - //Vector actual = factor.unwhitenedError(values); - //EXPECT(assert_equal(zero(4),actual,1e-8)); - - // Check derivatives - // Calculate expected derivative for point (easiest to check) boost::function f = // - boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); - boost::optional point = factor.point(); - CHECK(point); - Matrix expectedE = numericalDerivative11(f, *point); + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); // Calculate using computeEP - Matrix actualE, PointCov; - factor.computeEP(actualE, PointCov, cameras); + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); EXPECT(assert_equal(expectedE, actualE, 1e-7)); - // Calculate using whitenedError - Matrix F, actualE2; - Vector actualErrors = factor.whitenedError(cameras, *point, F, actualE2); - EXPECT(assert_equal(expectedE, actualE2, 1e-7)); - EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + vector Fblocks; + factor.computeJacobians(Fblocks, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-8); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, noisy ) { - // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); - SimpleCamera level_camera(level_pose, *K2); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate + // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); Values values; - values.insert(x1, level_pose); + values.insert(x1, cam1.pose()); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); + values.insert(x2, pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor(new SmartFactor()); - factor->add(level_uv, x1, model, K); - factor->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor((sharedK))); + factor->add(level_uv, x1, model); + factor->add(level_uv_right, x2, model); double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor((sharedK))); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); @@ -222,125 +181,34 @@ TEST( SmartProjectionPoseFactor, noisy ) { noises.push_back(model); noises.push_back(model); - vector > Ks; ///< shared pointer to calibration object (one for each camera) - Ks.push_back(K); - Ks.push_back(K); - vector views; views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises, Ks); - + factor2->add(measurements, views, noises); double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); - - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); - if (isDebugTest) - tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); // body poses - Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1, 0, 0)); - Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0, -1, 0)); + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); SimpleCamera cam1(cameraPose1, *K); // with camera poses SimpleCamera cam2(cameraPose2, *K); SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -360,30 +228,24 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - vector views; + std::vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartProjectionPoseFactor smartFactor1(K, sensor_to_body, params); + smartFactor1.add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartProjectionPoseFactor smartFactor2(K, sensor_to_body, params); + smartFactor2.add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartProjectionPoseFactor smartFactor3(K, sensor_to_body, params); + smartFactor3.add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -404,56 +266,91 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, bodyPose3 * noise_pose); + values.insert(x3, bodyPose3*noise_pose); - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} - // result.print("results of 3 camera, 3 landmark optimization \n"); - EXPECT(assert_equal(bodyPose3, result.at(x3))); +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { - // Check derivatives + using namespace vanillaPose2; + vector measurements_cam1, measurements_cam2, measurements_cam3; - // Calculate expected derivative for point (easiest to check) - SmartFactor::Cameras cameras = smartFactor1->cameras(values); - boost::function f = // - boost::bind(&SmartFactor::whitenedError, *smartFactor1, cameras, _1); - boost::optional point = smartFactor1->point(); - CHECK(point); - Matrix expectedE = numericalDerivative11(f, *point); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - // Calculate using computeEP - Matrix actualE, PointCov; - smartFactor1->computeEP(actualE, PointCov, cameras); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); - // Calculate using whitenedError - Matrix F, actualE2; - Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, - actualE2); - EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); + smartFactor1->add(measurements_cam1, views, model); - // TODO the derivatives agree with f, but returned errors are -f :-( - // TODO We should merge the two whitenedErrors functions and make derivatives optional - EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2)); + smartFactor2->add(measurements_cam2, views, model); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2)); + smartFactor3->add(measurements_cam3, views, model); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Factors ) { + using namespace vanillaPose; + // Default cameras for simple derivatives Rot3 R; - static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); - SimpleCamera cam1(Pose3(R, Point3(0, 0, 0)), *K), cam2( - Pose3(R, Point3(1, 0, 0)), *K); + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -465,29 +362,44 @@ TEST( SmartProjectionPoseFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); // Make sure triangulation works - LONGS_EQUAL(2, smartFactor1->triangulateSafe(cameras)); + CHECK(smartFactor1->triangulateSafe(cameras)); CHECK(!smartFactor1->isDegenerate()); CHECK(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); CHECK(p); EXPECT(assert_equal(landmark1, *p)); + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: Matrix16 A1, A2; - A1 << -1000, 0, 0, 0, 100, 0; - A2 << 1000, 0, 100, 0, -100, 0; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below { // createHessianFactor Matrix66 G11 = 0.5 * A1.transpose() * A1; @@ -502,11 +414,14 @@ TEST( SmartProjectionPoseFactor, Factors ) { double f = 0; RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); - CHECK(assert_equal(expected.information(), actual->information(), 1e-8)); - CHECK(assert_equal(expected, *actual, 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual, 1e-8)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); } { @@ -524,91 +439,99 @@ TEST( SmartProjectionPoseFactor, Factors ) { F2(1, 0) = 100; F2(1, 2) = 10; F2(1, 4) = -10; - Matrix43 E; + Matrix E(4, 3); E.setZero(); - E(0, 0) = 100; - E(1, 1) = 100; - E(2, 0) = 100; - E(2, 2) = 10; - E(3, 1) = 100; - const vector > Fblocks = list_of > // - (make_pair(x1, 10 * F1))(make_pair(x2, 10 * F2)); - Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + vector Fblocks = list_of(F1)(F2); + Vector b(4); b.setZero(); - // createRegularImplicitSchurFactor - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); - - boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); - CHECK(actual); - CHECK(assert_equal(expected, *actual)); + // Create smart factors + FastVector keys; + keys.push_back(x1); + keys.push_back(x2); // createJacobianQFactor - JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fblocks, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8)); boost::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-8); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fblocks[0] = model->Whiten(Fblocks[0]); + Fblocks[1] = model->Whiten(Fblocks[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fblocks, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expectedQ, *actualQ)); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); } { // createJacobianSVDFactor Vector1 b; b.setZero(); - double s = sin(M_PI_4); - JacobianFactor expected(x1, s * A1, x2, s * A2, b); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); boost::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expected, *actual)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-8); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-8); } } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + + using namespace vanillaPose; vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -616,17 +539,17 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3 * noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); EXPECT( assert_equal( Pose3( @@ -635,66 +558,47 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); - if (isDebugTest) - tictoc_print_(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, jacobianSVD ) { + using namespace vanillaPose; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(sharedK, boost::none, params); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor1->add(measurements_cam1, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -702,27 +606,28 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3 * noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, landmarkDistance ) { + using namespace vanillaPose; + double excludeLandmarksFutherThanDist = 2; vector views; @@ -730,42 +635,31 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -773,21 +667,20 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3 * noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(values.at(x3), result.at(x3))); } @@ -795,6 +688,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { + using namespace vanillaPose; + double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error @@ -803,51 +698,39 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + // add fourth landmark Point3 landmark4(5, -0.5, 1); vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; - // 1. Project three landmarks into three cameras and triangulate + // Project 4 landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -856,65 +739,52 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, jacobianQ ) { + using namespace vanillaPose; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_Q); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor2->add(measurements_cam2, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -922,102 +792,79 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3 * noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); - LevenbergMarquardtParams params; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { - // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + + using namespace vanillaPose2; vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - typedef GenericProjectionFactor ProjectionFactor; NonlinearFactorGraph graph; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x2, pose_right, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3 * noise_pose); + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if (isDebugTest) - values.at(x3).print("Pose3 before optimization: "); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Values result = optimizer.optimize(); - if (isDebugTest) - result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } /* *************************************************************************/ @@ -1028,40 +875,36 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); + using namespace vanillaPose; - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, model); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, model); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -1072,9 +915,9 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( @@ -1085,15 +928,12 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize( - values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize( - values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize( - values); + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); - Matrix CumulativeInformation = hessianFactor1->information() - + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); boost::shared_ptr GaussianGraph = graph.linearize( values); @@ -1102,12 +942,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() - + hessianFactor2->augmentedInformation() - + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian @@ -1116,149 +954,102 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; + using namespace vanillaPose2; vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Camera cam2(pose2, sharedK2); + Camera cam3(pose3, sharedK2); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K2); + vector measurements_cam1, measurements_cam2; - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - double rankTol = 50; + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K2); + new SmartFactor(sharedK2, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K2); + new SmartFactor(sharedK2, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); + values.insert(x1, cam1.pose()); values.insert(x2, pose2 * noise_pose); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3 * noise_pose * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, - -0.572308662, -0.324093872, 0.639358349, -0.521617766, - -0.564921063), - Point3(0.145118171, -0.252907438, 0.819956033)), - values.at(x3))); + values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - cout - << "TEST COMMENTED: rotation only version of smart factors has been deprecated " - << endl; - EXPECT( - assert_equal( - Pose3( - Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814, - -0.572308662, -0.324093872, 0.639358349, -0.521617766, - -0.564921063), - Point3(0.145118171, -0.252907438, 0.819956033)), - result.at(x3))); - if (isDebugTest) - tictoc_print_(); + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; + + using namespace vanillaPose; vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera + // Two different cameras, at the same position, but different rotations + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, K); + new SmartFactor(sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1269,7 +1060,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( @@ -1279,9 +1070,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( @@ -1292,78 +1082,46 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); - cout - << "TEST COMMENTED: rotation only version of smart factors has been deprecated " - << endl; - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - result.at(x3))); - if (isDebugTest) - tictoc_print_(); + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Hessian ) { - // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; + + using namespace vanillaPose2; vector views; views.push_back(x1); views.push_back(x2); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - - // 1. Project three landmarks into three cameras and triangulate + // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); Point2 cam2_uv1 = cam2.project(landmark1); vector measurements_cam1; measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); + smartFactor1->add(measurements_cam1, views, model); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - boost::shared_ptr hessianFactor = smartFactor1->linearize( - values); - if (isDebugTest) - hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor1->linearize(values); // compute triangulation from linearization point // compute reprojection errors (sum squared) - // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } @@ -1371,214 +1129,148 @@ TEST( SmartProjectionPoseFactor, Hessian ) { TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; + using namespace vanillaPose; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - SimpleCamera cam3(pose3, *K); - - Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1, measurements_cam2, measurements_cam3; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK)); + smartFactorInstance->add(measurements_cam1, views, model); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = - smartFactorInstance->linearize(values); - // hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactorInstance->linearize( + values); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); - boost::shared_ptr hessianFactorRot = - smartFactorInstance->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = smartFactorInstance->linearize( + rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT( - assert_equal(hessianFactor->information(), - hessianFactorRot->information(), 1e-7)); + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); - boost::shared_ptr hessianFactorRotTran = + boost::shared_ptr factorRotTran = smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT( - assert_equal(hessianFactor->information(), - hessianFactorRotTran->information(), 1e-7)); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; + + using namespace vanillaPose2; vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); - SimpleCamera cam3(pose3, *K2); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; + // All cameras have the same pose so will be degenerate ! + Camera cam2(level_pose, sharedK2); + Camera cam3(level_pose, sharedK2); + vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, K2); + SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2)); + smartFactor->add(measurements_cam1, views, model); Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); - boost::shared_ptr hessianFactor = smartFactor->linearize( - values); - if (isDebugTest) - hessianFactor->print("Hessian factor \n"); + boost::shared_ptr factor = smartFactor->linearize(values); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize( - rotValues); - if (isDebugTest) - hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr factorRot = // + smartFactor->linearize(rotValues); // Hessian is invariant to rotations in the nondegenerate case - EXPECT( - assert_equal(hessianFactor->information(), - hessianFactorRot->information(), 1e-8)); + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); - boost::shared_ptr hessianFactorRotTran = - smartFactor->linearize(tranValues); + boost::shared_ptr factorRotTran = smartFactor->linearize( + tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT( - assert_equal(hessianFactor->information(), - hessianFactorRotTran->information(), 1e-8)); + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartFactorBundler factor(rankTol, linThreshold); - boost::shared_ptr Kbundler( - new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor.add(measurement1, poseKey1, model, Kbundler); + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(sharedBundlerK, boost::none, params); + factor.add(measurement1, x1, model); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera cam1(pose1, *Kbundler); + using namespace bundlerPose; - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK)); + smartFactor1->add(measurements_cam1, views, model); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1586,103 +1278,72 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); + graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3 * noise_pose); + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); EXPECT( assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - - EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); - if (isDebugTest) - tictoc_print_(); + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); } /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { + using namespace bundlerPose; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - PinholeCamera cam1(pose1, *Kbundler); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera + // Two different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - PinholeCamera cam3(pose3, *Kbundler); + Camera cam2(pose2, sharedBundlerK); + Camera cam3(pose3, sharedBundlerK); - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); + // landmark3 at 3 meters now Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor1->add(measurements_cam1, views, model); - double rankTol = 10; + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor2->add(measurements_cam2, views, model); - SmartFactorBundler::shared_ptr smartFactor1( - new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor2( - new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor3( - new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, Kbundler); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(sharedBundlerK, boost::none, params)); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1693,7 +1354,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( @@ -1703,9 +1364,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose3 * noise_pose); EXPECT( assert_equal( @@ -1716,20 +1377,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; - Values result; - gttic_(SmartProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); - gttoc_(SmartProjectionPoseFactor); - tictoc_finishedIteration_(); - // result.print("results of 3 camera, 3 landmark optimization \n"); EXPECT( assert_equal( Pose3( @@ -1738,12 +1389,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - cout - << "TEST COMMENTED: rotation only version of smart factors has been deprecated " - << endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if (isDebugTest) - tictoc_print_(); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 6b79171df..2c3a64495 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -35,7 +36,7 @@ static const boost::shared_ptr sharedCal = // // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -PinholeCamera camera1(pose1, *sharedCal); +SimpleCamera camera1(pose1, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); @@ -48,7 +49,7 @@ TEST( triangulation, TriangulationFactor ) { // Create the factor with a measurement that is 3 pixels off in x Key pointKey(1); SharedNoiseModel model; - typedef TriangulationFactor<> Factor; + typedef TriangulationFactor Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index dc921a7da..e00dcee57 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -26,16 +26,14 @@ * -makes monocular observations of many landmarks */ -#include +#include +#include #include #include #include #include #include #include -#include - -#include #include #include @@ -46,6 +44,7 @@ using namespace gtsam; int main(int argc, char** argv){ + typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; @@ -68,18 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int pose_id; + int i; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses - while (pose_file >> pose_id) { - for (int i = 0; i < 16; i++) { + while (pose_file >> i) { + for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(i, Pose3(m)); } - // camera and landmark keys - size_t x, l; + // landmark keys + size_t l; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -89,24 +87,24 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(K)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); + factor = SmartFactor::shared_ptr(new SmartFactor(K)); current_l = l; } - factor->add(Point2(uL,v), Symbol('x',x), model, K); + factor->add(Point2(uL,v), i, model); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 firstPose = initial_estimate.at(1); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.push_back(NonlinearEquality(1,firstPose)); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 1358e1349..f4e82d98d 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -33,49 +33,25 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartStereoProjectionFactorState { - -protected: - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - SmartStereoProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; - -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q -}; - /** * SmartStereoProjectionFactor: triangulates point */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: - // Some triangulation parameters - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + /// @name Caching triangulation + /// @{ + const TriangulationParameters parameters_; + // TODO: +// mutable TriangulationResult result_; ///< result from triangulateSafe + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + /// @} const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases - const bool enableEPI_; ///< if set to true, will refine triangulation using LM - const double linearizationThreshold_; ///< threshold to decide whether to re-linearize mutable std::vector cameraPosesLinearization_; ///< current linearization poses @@ -84,29 +60,22 @@ protected: mutable bool degenerate_; mutable bool cheiralityException_; - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large + typedef SmartFactorBase Base; /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; - enum {ZDim = 3}; ///< Dimension trait of measurement type + enum { + ZDim = 3 + }; ///< Dimension trait of measurement type + + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} public: @@ -128,18 +97,15 @@ public: * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - SmartStereoProjectionFactor(const double rankTol, const double linThreshold, - const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + SmartStereoProjectionFactor(const double rankTolerance, + const double linThreshold, const bool manageDegeneracy, + const bool enableEPI, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, + dynamicOutlierRejectionThreshold), // + retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false) { } /** Virtual destructor */ @@ -153,11 +119,12 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "SmartStereoProjectionFactor, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << s << "SmartStereoProjectionFactor\n"; + std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "degenerate_ = " << degenerate_ << std::endl; std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; - std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl; + std::cout << "linearizationThreshold_ = " << linearizationThreshold_ + << std::endl; Base::print("", keyFormatter); } @@ -197,40 +164,6 @@ public: return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation } - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization - bool decideIfLinearize(const Cameras& cameras) const { - // "selective linearization" - // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose - // (we only care about the "rigidity" of the poses, not about their absolute pose) - - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize - return true; - - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses - if (cameraPosesLinearization_.empty() - || (cameras.size() != cameraPosesLinearization_.size())) - return true; - - Pose3 firstCameraPose, firstCameraPoseOld; - for (size_t i = 0; i < cameras.size(); i++) { - - if (i == 0) { // we store the initial pose, this is useful for selective re-linearization - firstCameraPose = cameras[i].pose(); - firstCameraPoseOld = cameraPosesLinearization_[i]; - continue; - } - - // we compare the poses in the frame of the first pose - Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); - Pose3 localCameraPoseOld = firstCameraPoseOld.between( - cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) - return true; // at least two "relative" poses are different, hence we re-linearize - } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize - } - /// triangulateSafe size_t triangulateSafe(const Values& values) const { return triangulateSafe(this->cameras(values)); @@ -268,7 +201,7 @@ public: mono_cameras.push_back(PinholeCamera(pose, K)); } point_ = triangulatePoint3 >(mono_cameras, mono_measurements, - rankTolerance_, enableEPI_); + parameters_.rankTolerance, parameters_.enableEPI); // // // End temporary hack @@ -281,11 +214,11 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { degenerate_ = true; break; } @@ -300,8 +233,8 @@ public: } //std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (parameters_.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -345,15 +278,15 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -362,138 +295,123 @@ public: exit(1); } - this->triangulateSafe(cameras); - if (isDebug) std::cout << "point_ = " << point_ << std::endl; + triangulateSafe(cameras); + if (isDebug) + std::cout << "point_ = " << point_ << std::endl; if (numKeys < 2 || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) std::cout << "In linearize: exception" << std::endl; + if (isDebug) + std::cout << "In linearize: exception" << std::endl; BOOST_FOREACH(Matrix& m, Gs) - m = zeros(D, D); + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, + v = zero(Base::Dim); + return boost::make_shared >(this->keys_, Gs, gs, 0.0); } // instead, if we want to manage the exception.. if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors this->degenerate_ = true; - if (isDebug) std::cout << "degenerate_ = true" << std::endl; + if (isDebug) + std::cout << "degenerate_ = true" << std::endl; } - bool doLinearize = this->decideIfLinearize(cameras); - - if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl; - - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize for (size_t i = 0; i < cameras.size(); i++) this->cameraPosesLinearization_[i] = cameras[i].pose(); - if (!doLinearize) { // return the previous Hessian factor - std::cout << "=============================" << std::endl; - std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - std::cout - << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" - << std::endl; - exit(1); - return boost::make_shared >(this->keys_, - this->state_->Gs, this->state_->gs, this->state_->f); - } - // ================================================================== + std::vector Fblocks; Matrix F, E; Vector b; - double f = computeJacobians(F, E, b, cameras); + computeJacobians(Fblocks, E, b, cameras); + Base::FillDiagonalF(Fblocks, F); // expensive !!! // Schur complement trick // Frank says: should be possible to do this more efficiently? // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); + Matrix H(Base::Dim * numKeys, Base::Dim * numKeys); Vector gs_vector; - Matrix3 P = Base::PointCov(E,lambda); + Matrix3 P = Cameras::PointCov(E, lambda); H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); - if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; - if (isDebug) std::cout << "H:\n" << H << std::endl; + if (isDebug) + std::cout << "gs_vector size " << gs_vector.size() << std::endl; + if (isDebug) + std::cout << "H:\n" << H << std::endl; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * Base::Dim; + gs.at(i1) = gs_vector.segment(i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * Base::Dim); GsCount2++; } } } // ================================================================== - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables - this->state_->Gs = Gs; - this->state_->gs = gs; - this->state_->f = f; - } - return boost::make_shared >(this->keys_, Gs, gs, f); + double f = b.squaredNorm(); + return boost::make_shared >(this->keys_, Gs, gs, f); } // // create factor -// boost::shared_ptr > createImplicitSchurFactor( +// boost::shared_ptr > createImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createImplicitSchurFactor(cameras, point_, lambda); // else -// return boost::shared_ptr >(); +// return boost::shared_ptr >(); // } // // /// create factor -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createJacobianQFactor(cameras, point_, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return boost::make_shared< JacobianFactorQ >(this->keys_); // } // // /// Create a factor, takes values -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Values& values, double lambda) const { -// Cameras myCameras; +// Cameras cameras; // // TODO triangulate twice ?? -// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); +// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); // if (nonDegenerate) -// return createJacobianQFactor(myCameras, lambda); +// return createJacobianQFactor(cameras, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return boost::make_shared< JacobianFactorQ >(this->keys_); // } // /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, - Cameras& myCameras) const { + Cameras& cameras) const { Values valuesFactor; // Select only the cameras BOOST_FOREACH(const Key key, this->keys_) valuesFactor.insert(key, values.at(key)); - myCameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(myCameras); + cameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(cameras); if (nrCameras < 2 || (!this->manageDegeneracy_ @@ -505,7 +423,8 @@ public: this->degenerate_ = true; if (this->degenerate_) { - std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl; + std::cout << "SmartStereoProjectionFactor: this is not ready" + << std::endl; std::cout << "this->cheiralityException_ " << this->cheiralityException_ << std::endl; std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; @@ -513,34 +432,32 @@ public: return true; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - Base::computeEP(E, PointCov, cameras, point_); - } - - /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + cameras.project2(point_, boost::none, E); return nonDegenerate; } /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, + bool computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - computeJacobians(Fblocks, E, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, cameras, 0.0); return nonDegenerate; } /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - double computeJacobians(std::vector& Fblocks, + void computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { if (this->degenerate_) { throw("FIXME: computeJacobians degenerate case commented out!"); @@ -571,73 +488,36 @@ public: // // this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); // f += bi.squaredNorm(); -// Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); +// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi)); // E.block < 2, 2 > (2 * i, 0) = Ei; // subInsert(b, bi, 2 * i); // } // return f; } else { // nondegenerate: just return Base version - return Base::computeJacobians(Fblocks, E, b, cameras, point_); + Base::computeJacobians(Fblocks, E, b, cameras, point_); } // end else } -// /// Version that computes PointCov, with optional lambda parameter -// double computeJacobians(std::vector& Fblocks, -// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, -// const double lambda = 0.0) const { -// -// double f = computeJacobians(Fblocks, E, b, cameras); -// -// // Point covariance inv(E'*E) -// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); -// -// return f; -// } -// -// /// takes values -// bool computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Values& values) const { -// typename Base::Cameras myCameras; -// double good = computeCamerasAndTriangulate(values, myCameras); -// if (good) -// computeJacobiansSVD(Fblocks, Enull, b, myCameras); -// return true; -// } -// -// /// SVD version -// double computeJacobiansSVD(std::vector& Fblocks, -// Matrix& Enull, Vector& b, const Cameras& cameras) const { -// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); -// } -// -// /// Returns Matrix, TODO: maybe should not exist -> not sparse ! -// // TODO should there be a lambda? -// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, -// const Cameras& cameras) const { -// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); -// } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobians(F, E, b, cameras, point_); + /// takes values + bool triangulateAndComputeJacobiansSVD( + std::vector& Fblocks, Matrix& Enull, Vector& b, + const Values& values) const { + typename Base::Cameras cameras; + double good = computeCamerasAndTriangulate(values, cameras); + if (good) + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + return true; } /// Calculate vector of re-projection errors, before applying noise model - /// Assumes triangulation was done and degeneracy handled - Vector reprojectionError(const Cameras& cameras) const { - return Base::reprojectionError(cameras, point_); - } - - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Values& values) const { - Cameras myCameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + Vector reprojectionErrorAfterTriangulation(const Values& values) const { + Cameras cameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return reprojectionError(myCameras); + return Base::unwhitenedError(cameras, point_); else - return zero(myCameras.size() * 3); + return zero(cameras.size() * 3); } /** @@ -675,7 +555,7 @@ public: } if (this->degenerate_) { - return 0.0; // TODO: this maybe should be zero? + return 0.0; // TODO: this maybe should be zero? // std::cout // << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" // << std::endl; @@ -744,9 +624,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 3db1c883e..bc4d3ccfb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once @@ -38,7 +39,14 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { + +public: + /// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,7 +58,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class typedef SmartStereoProjectionPoseFactor This; @@ -65,15 +73,16 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, + double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_( + linearizeTo) { + } /** Virtual destructor */ virtual ~SmartStereoProjectionPoseFactor() {} diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index eac63006d..258c8d0eb 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,8 +18,8 @@ * @date Sept 2013 */ +// TODO #include #include - #include #include #include @@ -32,8 +32,6 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = false; - // make a realistic calibration matrix static double fov = 60; // degrees static size_t w = 640, h = 480; @@ -66,7 +64,6 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); typedef SmartStereoProjectionPoseFactor SmartFactor; -typedef SmartStereoProjectionPoseFactor SmartFactorBundler; vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -83,9 +80,10 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, return measurements_cam; } +LevenbergMarquardtParams params; + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr, "Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -106,15 +104,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { factor1.add(measurement1, poseKey1, model, K); } -/* ************************************************************************* */ -TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, - body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); -} - /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { SmartFactor::shared_ptr factor1(new SmartFactor()); @@ -128,8 +117,6 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { /* *************************************************************************/ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -169,8 +156,6 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); @@ -226,10 +211,6 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { - cout - << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" - << endl; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); @@ -286,31 +267,31 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); - LevenbergMarquardtParams params; - if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; + EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1); Values result; - gttic_ (SmartStereoProjectionPoseFactor); + gttic_(SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6); + + GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); + VectorValues delta = GFG->optimize(); + VectorValues expected = VectorValues::Zero(delta); + EXPECT(assert_equal(expected, delta, 1e-6)); // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); EXPECT(assert_equal(pose3, result.at(x3))); - if (isDebugTest) - tictoc_print_(); } /* *************************************************************************/ @@ -345,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -373,7 +354,6 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { values.insert(x2, pose2); values.insert(x3, pose3 * noise_pose); - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -414,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); @@ -446,7 +426,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -493,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); @@ -530,7 +509,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { values.insert(x3, pose3); // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -567,13 +545,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -592,8 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(x2, pose2); // values.insert(x3, pose3*noise_pose); // -// LevenbergMarquardtParams params; -// Values result; +//// Values result; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // result = optimizer.optimize(); // EXPECT(assert_equal(pose3,result.at(x3))); @@ -601,7 +578,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ -// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // // vector views; // views.push_back(x1); @@ -653,15 +629,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(L(1), landmark1); // values.insert(L(2), landmark2); // values.insert(L(3), landmark3); -// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); // -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // LevenbergMarquardtOptimizer optimizer(graph, values, params); // Values result = optimizer.optimize(); // -// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); // EXPECT(assert_equal(pose3,result.at(x3))); //} // @@ -723,8 +694,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x3, pose3 * noise_pose); - if (isDebugTest) - values.at(x3).print("Smart: Pose3 before optimization: "); // TODO: next line throws Cheirality exception on Mac boost::shared_ptr hessianFactor1 = smartFactor1->linearize( @@ -749,7 +718,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { + hessianFactor3->augmentedInformation(); // Check Information vector - // cout << AugInformationMatrix.size() << endl; Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian @@ -758,7 +726,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // // vector views; // views.push_back(x1); @@ -811,11 +778,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2*noise_pose); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -825,15 +787,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // // vector views; // views.push_back(x1); @@ -894,11 +852,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2); // // initialize third pose with some noise, we expect it to move back to original pose3 // values.insert(x3, pose3*noise_pose); -// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); -// -// LevenbergMarquardtParams params; -// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; -// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; // // Values result; // gttic_(SmartStereoProjectionPoseFactor); @@ -908,15 +861,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // tictoc_finishedIteration_(); // // // result.print("results of 3 camera, 3 landmark optimization \n"); -// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); -// if(isDebugTest) tictoc_print_(); //} // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ -// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // // vector views; // views.push_back(x1); @@ -949,7 +898,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x2, pose2); // // boost::shared_ptr hessianFactor = smartFactor1->linearize(values); -// if(isDebugTest) hessianFactor->print("Hessian factor \n"); // // // compute triangulation from linearization point // // compute reprojection errors (sum squared) @@ -960,8 +908,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - vector views; views.push_back(x1); views.push_back(x2); @@ -1031,8 +977,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - vector views; views.push_back(x1); views.push_back(x2); @@ -1063,8 +1007,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); - if (isDebugTest) - hessianFactor->print("Hessian factor \n"); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); @@ -1075,8 +1017,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); - if (isDebugTest) - hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case EXPECT( diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp similarity index 98% rename from gtsam/nonlinear/tests/testExpressionFactor.cpp rename to tests/testExpressionFactor.cpp index 2fb544edf..312ad89eb 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -37,6 +37,10 @@ using namespace gtsam; Point2 measured(-17, 30); SharedNoiseModel model = noiseModel::Unit::Create(2); +// This deals with the overload problem and makes the expressions factor +// understand that we work on Point3 +Point2 (*Project)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; + namespace leaf { // Create some values struct MyValues: public Values { @@ -313,7 +317,7 @@ TEST(ExpressionFactor, tree) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); // Create factor and check value, dimension, linearization @@ -331,8 +335,6 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - Expression::TernaryFunction::type fff = project6; - // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); @@ -489,7 +491,7 @@ TEST(ExpressionFactor, tree_finite_differences) { // Create expression tree Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); const double fd_step = 1e-5; diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h new file mode 100644 index 000000000..ff9732909 --- /dev/null +++ b/timing/DummyFactor.h @@ -0,0 +1,56 @@ +/** + * @file DummyFactor.h + * @brief Just to help in timing overhead + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * DummyFactor + */ +template // +class DummyFactor: public RegularImplicitSchurFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + DummyFactor() { + } + + DummyFactor(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b) :RegularImplicitSchurFactor(Fblocks,E,P,b) + { + } + + virtual ~DummyFactor() { + } + +public: + + /** + * @brief Dummy version to measure overhead of key access + */ + void multiplyHessian(double alpha, const VectorValues& x, + VectorValues& y) const { + + BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { + static const Vector empty; + Key key = Fi.first; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + yi = x.at(key); + } + } + +}; +// DummyFactor + +} + diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp new file mode 100644 index 000000000..06a526567 --- /dev/null +++ b/timing/timeSchurFactors.cpp @@ -0,0 +1,152 @@ +/** + * @file timeSchurFactors.cpp + * @brief Time various Schur-complement Jacobian factors + * @author Frank Dellaert + * @date Oct 27, 2013 + */ + +#include "DummyFactor.h" +#include + +#include +#include "gtsam/slam/JacobianFactorQR.h" +#include + +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +#define SLOW +#define RAW +#define HESSIAN +#define NUM_ITERATIONS 1000 + +// Create CSV file for results +ofstream os("timeSchurFactors.csv"); + +/*************************************************************************************/ +template +void timeAll(size_t m, size_t N) { + + cout << m << endl; + + // create F + typedef Eigen::Matrix Matrix2D; + typedef pair KeyMatrix2D; + vector < pair > Fblocks; + for (size_t i = 0; i < m; i++) + Fblocks.push_back(KeyMatrix2D(i, (i + 1) * Matrix::Ones(2, D))); + + // create E + Matrix E(2 * m, 3); + for (size_t i = 0; i < m; i++) + E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3); + + // Calculate point covariance + Matrix P = (E.transpose() * E).inverse(); + + // RHS and sigmas + const Vector b = gtsam::repeat(2 * m, 1); + const SharedDiagonal model; + + // parameters for multiplyHessianAdd + double alpha = 0.5; + VectorValues xvalues, yvalues; + for (size_t i = 0; i < m; i++) + xvalues.insert(i, gtsam::repeat(D, 2)); + + // Implicit + RegularImplicitSchurFactor implicitFactor(Fblocks, E, P, b); + // JacobianFactor with same error + JacobianFactorQ jf(Fblocks, E, P, b, model); + // JacobianFactorQR with same error + JacobianFactorQR jqr(Fblocks, E, P, b, model); + // Hessian + HessianFactor hessianFactor(jqr); + +#define TIME(label,factor,xx,yy) {\ + for (size_t t = 0; t < N; t++) \ + factor.multiplyHessianAdd(alpha, xx, yy);\ + gttic_(label);\ + for (size_t t = 0; t < N; t++) {\ + factor.multiplyHessianAdd(alpha, xx, yy);\ + }\ + gttoc_(label);\ + tictoc_getNode(timer, label)\ + os << timer->secs()/NUM_ITERATIONS << ", ";\ + } + +#ifdef SLOW + TIME(Implicit, implicitFactor, xvalues, yvalues) + TIME(Jacobian, jf, xvalues, yvalues) + TIME(JacobianQR, jqr, xvalues, yvalues) +#endif + +#ifdef HESSIAN + TIME(Hessian, hessianFactor, xvalues, yvalues) +#endif + +#ifdef OVERHEAD + DummyFactor dummy(Fblocks, E, P, b); + TIME(Overhead,dummy,xvalues,yvalues) +#endif + +#ifdef RAW + { // Raw memory Version + FastVector < Key > keys; + for (size_t i = 0; i < m; i++) + keys += i; + Vector x = xvalues.vector(keys); + double* xdata = x.data(); + + // create a y + Vector y = zero(m * D); + TIME(RawImplicit, implicitFactor, xdata, y.data()) + TIME(RawJacobianQ, jf, xdata, y.data()) + TIME(RawJacobianQR, jqr, xdata, y.data()) + } +#endif + + os << m << endl; + +} // timeAll + +/*************************************************************************************/ +int main(void) { +#ifdef SLOW + os << "Implicit,"; + os << "JacobianQ,"; + os << "JacobianQR,"; +#endif +#ifdef HESSIAN + os << "Hessian,"; +#endif +#ifdef OVERHEAD + os << "Overhead,"; +#endif +#ifdef RAW + os << "RawImplicit,"; + os << "RawJacobianQ,"; + os << "RawJacobianQR,"; +#endif + os << "m" << endl; + // define images + vector < size_t > ms; + // ms += 2; + // ms += 3, 4, 5, 6, 7, 8, 9, 10; + // ms += 11,12,13,14,15,16,17,18,19; + // ms += 20, 30, 40, 50; + // ms += 20,30,40,50,60,70,80,90,100; + for (size_t m = 2; m <= 50; m += 2) + ms += m; + //for (size_t m=10;m<=100;m+=10) ms += m; + // loop over number of images + BOOST_FOREACH(size_t m,ms) + timeAll<6>(m, NUM_ITERATIONS); +} + +//*************************************************************************************