diff --git a/.cproject b/.cproject index 8eb74b58b..19d3912b0 100644 --- a/.cproject +++ b/.cproject @@ -1293,6 +1293,7 @@ make + testSimulated2DOriented.run true false @@ -1332,6 +1333,7 @@ make + testSimulated2D.run true false @@ -1339,6 +1341,7 @@ make + testSimulated3D.run true false @@ -1442,7 +1445,6 @@ make - testErrors.run true false @@ -1544,6 +1546,14 @@ true true + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + make -j3 @@ -1745,7 +1755,6 @@ cpack - -G DEB true false @@ -1753,7 +1762,6 @@ cpack - -G RPM true false @@ -1761,7 +1769,6 @@ cpack - -G TGZ true false @@ -1769,7 +1776,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1961,6 +1967,7 @@ make + tests/testGaussianISAM2 true false @@ -2112,7 +2119,6 @@ make - tests/testBayesTree.run true false @@ -2120,7 +2126,6 @@ make - testBinaryBayesNet.run true false @@ -2168,7 +2173,6 @@ make - testSymbolicBayesNet.run true false @@ -2176,7 +2180,6 @@ make - tests/testSymbolicFactor.run true false @@ -2184,7 +2187,6 @@ make - testSymbolicFactorGraph.run true false @@ -2200,7 +2202,6 @@ make - tests/testBayesTree true false @@ -3320,7 +3321,6 @@ make - testGraph.run true false @@ -3328,7 +3328,6 @@ make - testJunctionTree.run true false @@ -3336,7 +3335,6 @@ make - testSymbolicBayesNetB.run true false diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..ac7af2e80 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/html/ diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework #include #include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..c3d901507 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,15 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include -#include +#include #include +#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#include -#include - -// Each variable in the system (poses) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use simple integer keys -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. - // We will use another Between Factor to enforce this constraint, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..fce046a59 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,51 +17,22 @@ * @author Frank Dellaert */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" +#include using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -127,7 +98,8 @@ int main(int argc, char* argv[]) { initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); result.print("Final results:\n"); // A smart factor represent the 3D point inside the factor, not as a variable. @@ -136,20 +108,20 @@ int main(int argc, char* argv[]) { Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL landmark_result.insert(j, *point); } } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp new file mode 100644 index 000000000..49482292f --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactorPCG.cpp + * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient + * @author Frank Dellaert + */ + +// For an explanation of these headers, see SFMExample_SmartFactor.cpp +#include "SFMdata.h" +#include + +// These extra headers allow us a LM outer loop with PCG linear solver (inner loop) +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + 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()); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + SimpleCamera 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); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // 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( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, poses[1], poseNoise)); + + // 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)); + + // We will use LM in the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + LevenbergMarquardtParams parameters; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.absoluteErrorTol = 1e-10; + parameters.relativeErrorTol = 1e-10; + parameters.maxIterations = 500; + PCGSolverParameters::shared_ptr pcg = + boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + // Following is crucial: + pcg->setEpsilon_abs(1e-10); + pcg->setEpsilon_rel(1e-10); + parameters.iterativeParams = pcg; + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + + // Display result as in SFMExample_SmartFactor.run + result.print("Final results:\n"); + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smart = // + boost::dynamic_pointer_cast(graph[j]); + if (smart) { + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam.h b/gtsam.h index c5528309e..f8e0af28e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2310,23 +2310,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); SmartProjectionPoseFactor(double rankTol); SmartProjectionPoseFactor(); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..3a34ca1fd --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#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 { + +protected: + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int ZDim = traits::dimension; ///< Measurement dimension + static const int Dim = traits::dimension; ///< Camera dimension + +public: + + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; // F + typedef std::pair FBlock; // Fblocks + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + 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(); + } + + /// equals + virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + if (this->size() != p.size()) + return false; + bool camerasAreEqual = true; + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).equals(p.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * Project a point, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector project(const Point3& point, boost::optional F = + boost::none, boost::optional E = boost::none, + boost::optional H = 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); + + 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; + } + return z; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & (*this); + } +}; + +template +const int CameraSet::ZDim; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 1edb1a496..bfc2bbb93 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,6 +31,14 @@ 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 diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9170f8282..eb83fd10f 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -31,7 +31,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,6> H3) const { + OptionalJacobian<3,0> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -78,8 +78,7 @@ namespace gtsam { } #endif if (H3) - // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet - H3->setZero(); + throw std::runtime_error("StereoCamera::project does not support third derivative yet"); } // finally translate diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 913b1eab3..88ffbcdbd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -28,32 +28,47 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ class GTSAM_EXPORT StereoCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef StereoPoint2 Measurement; + private: Pose3 leftCamPose_; Cal3_S2Stereo::shared_ptr K_; public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -62,26 +77,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -100,10 +117,12 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } @@ -114,19 +133,16 @@ public: * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 6> H3 = boost::none) const; + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 0> H3 = boost::none) const; - /** - * - */ + /// back-project a measurement Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); return world_point; } @@ -134,7 +150,8 @@ public: /// @} private: - /** utility functions */ + + /// utility function Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; @@ -147,8 +164,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..42cf0f299 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * 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; + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(CameraSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + CameraSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + CHECK(assert_equal(set, set)); + CameraSet set2 = set; + set2.push_back(camera); + CHECK(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix46 actualF; + Matrix43 actualE; + Matrix43 actualH; + { + Matrix26 F1; + Matrix23 E1; + Matrix23 H1; + camera.project(p, F1, E1, H1); + 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)); +} + +/* ************************************************************************* */ +#include +TEST(CameraSet, Stereo) { + typedef vector ZZ; + CameraSet set; + StereoCamera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + StereoPoint2 expected(0, -1, 0); + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix66 actualF; + Matrix63 actualE; + { + Matrix36 F1; + Matrix33 E1; + camera.project(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)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 5e7e08f61..a1b9e2d83 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -1,8 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * ConjugateGradientSolver.cpp - * - * Created on: Jun 4, 2014 - * Author: Yong-Dian Jian + * @file ConjugateGradientSolver.cpp + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 */ #include @@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) } /*****************************************************************************/ -ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; @@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } +/*****************************************************************************/ } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 20e0c5262..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,7 +20,6 @@ #pragma once #include -#include namespace gtsam { @@ -87,9 +86,8 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. + * A template for the linear preconditioned conjugate gradient method. * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. @@ -98,8 +96,9 @@ public: * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ -template -V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { +template +V preconditionedConjugateGradient(const S &system, const V &initial, + const ConjugateGradientParameters ¶meters) { V estimate, residual, direction, q1, q2; estimate = residual = direction = q1 = q2 = initial; diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..79ade1c8a 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 IterativeSolver.cpp - * @brief + * @brief Some support classes for iterative solvers * @date Sep 3, 2012 * @author Yong-Dian Jian */ @@ -9,18 +20,22 @@ #include #include #include +#include #include -#include using namespace std; namespace gtsam { /*****************************************************************************/ -string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { + return verbosityTranslator(verbosity_); +} /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { + verbosity_ = verbosityTranslator(src); +} /*****************************************************************************/ void IterativeOptimizationParameters::print() const { @@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const { /*****************************************************************************/ void IterativeOptimizationParameters::print(ostream &os) const { - os << "IterativeOptimizationParameters:" << endl - << "verbosity: " << verbosityTranslator(verbosity_) << endl; + os << "IterativeOptimizationParameters:" << endl << "verbosity: " + << verbosityTranslator(verbosity_) << endl; } /*****************************************************************************/ - ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { +ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); return os; } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { - string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return IterativeOptimizationParameters::SILENT; - else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; - else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( + const string &src) { + string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") + return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") + return IterativeOptimizationParameters::ERROR; /* default is default */ - else return IterativeOptimizationParameters::SILENT; + else + return IterativeOptimizationParameters::SILENT; } /*****************************************************************************/ - string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { - if (verbosity == SILENT) return "SILENT"; - else if (verbosity == COMPLEXITY) return "COMPLEXITY"; - else if (verbosity == ERROR) return "ERROR"; - else return "UNKNOWN"; +string IterativeOptimizationParameters::verbosityTranslator( + IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) + return "SILENT"; + else if (verbosity == COMPLEXITY) + return "COMPLEXITY"; + else if (verbosity == ERROR) + return "ERROR"; + else + return "UNKNOWN"; } /*****************************************************************************/ -VectorValues IterativeSolver::optimize( - const GaussianFactorGraph &gfg, +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) -{ - return optimize( - gfg, - keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ -VectorValues IterativeSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda) -{ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) - : ordering_(ordering) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : + ordering_(ordering) { initialize(fg); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) : + ordering_(Ordering::Natural(fg)) { initialize(fg); } /****************************************************************************/ -void KeyInfo::initialize(const GaussianFactorGraph &fg){ +void KeyInfo::initialize(const GaussianFactorGraph &fg) { const map colspec = fg.getKeyDimMap(); const size_t n = ordering_.size(); size_t start = 0; - for ( size_t i = 0 ; i < n ; ++i ) { + for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; insert(make_pair(key, KeyInfoEntry(i, dim, start))); - start += dim ; + start += dim; } numCols_ = start; } @@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -117,7 +136,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; @@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const { return Vector::Zero(numCols_); } - } - diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index d6e1b6e98..442a5fc6b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -9,131 +9,178 @@ * -------------------------------------------------------------------------- */ +/** + * @file IterativeSolver.h + * @brief Some support classes for iterative solvers + * @date 2010 + * @author Yong-Dian Jian + */ + #pragma once -#include -#include #include +#include + #include -#include -#include +#include +#include + #include -#include #include -#include +#include namespace gtsam { - // Forward declarations - class KeyInfo; - class KeyInfoEntry; - class GaussianFactorGraph; - class Values; - class VectorValues; +// Forward declarations +class KeyInfo; +class KeyInfoEntry; +class GaussianFactorGraph; +class Values; +class VectorValues; - /************************************************************************************/ - /** - * parameters for iterative linear solvers - */ - class GTSAM_EXPORT IterativeOptimizationParameters { +/** + * parameters for iterative linear solvers + */ +class GTSAM_EXPORT IterativeOptimizationParameters { - public: +public: - typedef boost::shared_ptr shared_ptr; - enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; + typedef boost::shared_ptr shared_ptr; + enum Verbosity { + SILENT = 0, COMPLEXITY, ERROR + } verbosity_; - public: +public: - IterativeOptimizationParameters(Verbosity v = SILENT) - : verbosity_(v) {} + IterativeOptimizationParameters(Verbosity v = SILENT) : + verbosity_(v) { + } - virtual ~IterativeOptimizationParameters() {} + virtual ~IterativeOptimizationParameters() { + } - /* utility */ - inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s) ; + /* utility */ + inline Verbosity verbosity() const { + return verbosity_; + } + std::string getVerbosity() const; + void setVerbosity(const std::string &s); - /* matlab interface */ - void print() const ; + /* matlab interface */ + void print() const; - /* virtual print function */ - virtual void print(std::ostream &os) const ; + /* virtual print function */ + virtual void print(std::ostream &os) const; - /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, + const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); - }; + static Verbosity verbosityTranslator(const std::string &s); + static std::string verbosityTranslator(Verbosity v); +}; - /************************************************************************************/ - class GTSAM_EXPORT IterativeSolver { - public: - typedef boost::shared_ptr shared_ptr; - IterativeSolver() {} - virtual ~IterativeSolver() {} +/** + * Base class for Iterative Solvers like SubgraphSolver + */ +class GTSAM_EXPORT IterativeSolver { +public: + typedef boost::shared_ptr shared_ptr; + IterativeSolver() { + } + virtual ~IterativeSolver() { + } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none - ); + boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda - ); + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + const std::map &lambda); - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) = 0; + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) = 0; - }; +}; - /************************************************************************************/ - /* Handy data structure for iterative solvers - * key to (index, dimension, colstart) */ - class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { - public: - typedef boost::tuple Base; - KeyInfoEntry(){} - KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - size_t index() const { return this->get<0>(); } - size_t dim() const { return this->get<1>(); } - size_t colstart() const { return this->get<2>(); } - }; +/** + * Handy data structure for iterative solvers + * key to (index, dimension, colstart) + */ +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - /************************************************************************************/ - class GTSAM_EXPORT KeyInfo : public std::map { - public: - typedef std::map Base; - KeyInfo() : numCols_(0) {} - KeyInfo(const GaussianFactorGraph &fg); - KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); +public: - std::vector colSpec() const ; - VectorValues x0() const; - Vector x0vector() const; + typedef boost::tuple Base; - inline size_t numCols() const { return numCols_; } - inline const Ordering & ordering() const { return ordering_; } + KeyInfoEntry() { + } + KeyInfoEntry(size_t idx, size_t d, Key start) : + Base(idx, d, start) { + } + size_t index() const { + return this->get<0>(); + } + size_t dim() const { + return this->get<1>(); + } + size_t colstart() const { + return this->get<2>(); + } +}; - protected: +/** + * Handy data structure for iterative solvers + */ +class GTSAM_EXPORT KeyInfo: public std::map { - void initialize(const GaussianFactorGraph &fg); +public: - Ordering ordering_; - size_t numCols_; + typedef std::map Base; - }; +protected: + Ordering ordering_; + size_t numCols_; -} + void initialize(const GaussianFactorGraph &fg); + +public: + + /// Default Constructor + KeyInfo() : + numCols_(0) { + } + + /// Construct from Gaussian factor graph, use "Natural" ordering + KeyInfo(const GaussianFactorGraph &fg); + + /// Construct from Gaussian factor graph and a given ordering + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + /// Return the total number of columns (scalar variables = sum of dimensions) + inline size_t numCols() const { + return numCols_; + } + + /// Return the ordering + inline const Ordering & ordering() const { + return ordering_; + } + + /// Return a vector of dimensions ordered by ordering() + std::vector colSpec() const; + + /// Return VectorValues with zeros, of correct dimension + VectorValues x0() const; + + /// Return zero Vector of correct dimension + Vector x0vector() const; + +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3698edc2f..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,16 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.cpp - * - * Created on: Feb 14, 2012 - * Author: Yong-Dian Jian - * Author: Sungtae An + * @file PCGSolver.cpp + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Feb 14, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ -#include #include +#include #include +#include +#include #include + +#include #include #include @@ -21,7 +36,7 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } @@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { } /*****************************************************************************/ -VectorValues PCGSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) -{ +VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); /* apply pcg */ - const Vector sol = preconditionedConjugateGradient( - GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), - initial.vector(keyInfo.ordering()), parameters_); + GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda); + Vector x0 = initial.vector(keyInfo.ordering()); + const Vector sol = preconditionedConjugateGradient(system, x0, parameters_); return buildVectorValues(sol, keyInfo); } /*****************************************************************************/ GaussianFactorGraphSystem::GaussianFactorGraphSystem( - const GaussianFactorGraph &gfg, - const Preconditioner &preconditioner, - const KeyInfo &keyInfo, - const std::map &lambda) - : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, + const KeyInfo &keyInfo, const std::map &lambda) : + gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_( + lambda) { +} /*****************************************************************************/ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { @@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /* substract A*x */ Vector Ax = Vector::Zero(r.rows(), 1); multiply(x, Ax); - r -= Ax ; + r -= Ax; } /*****************************************************************************/ @@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x - VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; @@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-1} x preconditioner_.solve(x, y); } /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-T} x preconditioner_.transposeSolve(x, y); } /**********************************************************************************/ -VectorValues buildVectorValues(const Vector &v, - const Ordering &ordering, - const map & dimensions) { +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const map & dimensions) { VectorValues result; DenseIndex offset = 0; - for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + for (size_t i = 0; i < ordering.size(); ++i) { const Key &key = ordering[i]; map::const_iterator it = dimensions.find(key); - if ( it == dimensions.end() ) { - throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + if (it == dimensions.end()) { + throw invalid_argument( + "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; result.insert(key, v.segment(offset, dim)); @@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { - result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + result.insert(item.first, + v.segment(item.second.colstart(), item.second.dim())); } return result; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 0201000ad..f5b278ae5 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -1,20 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.h - * - * Created on: Jan 31, 2012 - * Author: Yong-Dian Jian + * @file PCGSolver.h + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Jan 31, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ #pragma once -#include #include -#include -#include - -#include -#include -#include #include namespace gtsam { @@ -22,15 +27,19 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; +class VectorValues; struct PreconditionerParameters; -/*****************************************************************************/ +/** + * Parameters for PCG + */ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() {} + PCGSolverParameters() { + } virtual void print(std::ostream &os) const; @@ -42,8 +51,9 @@ public: boost::shared_ptr preconditioner_; }; -/*****************************************************************************/ -/* A virtual base class for the preconditioned conjugate gradient solver */ +/** + * A virtual base class for the preconditioned conjugate gradient solver + */ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; @@ -57,7 +67,8 @@ protected: public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() {} + virtual ~PCGSolver() { + } using IterativeSolver::optimize; @@ -67,7 +78,9 @@ public: }; -/*****************************************************************************/ +/** + * System class needed for calling preconditionedConjugateGradient + */ class GTSAM_EXPORT GaussianFactorGraphSystem { public: @@ -97,13 +110,17 @@ public: void getb(Vector &b) const; }; -/* utility functions */ -/**********************************************************************************/ +/// @name utility functions +/// @{ + +/// Create VectorValues from a Vector VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const std::map & dimensions); -/**********************************************************************************/ +/// Create VectorValues from a Vector and a KeyInfo class VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); +/// @} + } diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 6f10d04ad..3c7256c29 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -9,73 +9,81 @@ * -------------------------------------------------------------------------- */ -#include -#include +/** + * @file SubgraphSolver.cpp + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +#include #include #include #include #include -#include -#include -#include #include -#include -#include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : -parameters_(parameters), ordering_(ordering) -{ + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); + VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } @@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) -{ - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { + GaussianFactorGraph::shared_ptr Ab1 = + boost::make_shared(), Ab2 = boost::make_shared< + GaussianFactorGraph>(); - boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + boost::tie(Ab1, Ab2) = splitGraph(jfg); if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) -{ - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2) { + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -boost::tuple +boost::tuple // SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); @@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - if ( gf->keys().size() > 2 ) { - throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + if (gf->keys().size() > 2) { + throw runtime_error( + "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false ; + bool augment = false; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if (gf->keys().size() == 1) + augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], - u_root = D.findSet(u), v_root = D.findSet(v); - if ( u_root != v_root ) { - t++; augment = true ; + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), + v_root = D.findSet(v); + if (u_root != v_root) { + t++; + augment = true; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if (augment) + At->push_back(gf); + else + Ac->push_back(gf); } return boost::tie(At, Ac); } /****************************************************************************/ -VectorValues SubgraphSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial -) { return VectorValues(); } +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); +} } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index ac8a9da87..318c029f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -9,27 +9,37 @@ * -------------------------------------------------------------------------- */ +/** + * @file SubgraphSolver.h + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + #pragma once - #include -#include -#include -#include namespace gtsam { - // Forward declarations - class GaussianFactorGraph; - class GaussianBayesNet; - class SubgraphPreconditioner; +// Forward declarations +class GaussianFactorGraph; +class GaussianBayesNet; +class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { +class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} - void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + SubgraphSolverParameters() : + Base() { + } + void print() const { + Base::print(); + } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -53,8 +63,7 @@ public: * * \nosubgrouping */ - -class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { +class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { public: typedef SubgraphSolverParameters Parameters; @@ -62,41 +71,64 @@ public: protected: Parameters parameters_; Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: - /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); - /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, + const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering& ordering); + + /** + * The user specify the subgraph part and the constraint part + * may throw exception if A1 is underdetermined + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters, const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering); - virtual ~SubgraphSolver() {} + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); - VectorValues optimize () ; - VectorValues optimize (const VectorValues &initial) ; + /// Destructor + virtual ~SubgraphSolver() { + } - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) ; + /// Optimize from zero + VectorValues optimize(); + + /// Optimize from given initial values + VectorValues optimize(const VectorValues &initial); + + /** Interface that IterativeSolver subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2); - boost::tuple, boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg) ; + boost::tuple, + boost::shared_ptr > + splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..04d4734a4 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &graph_; public: - System(const NonlinearFactorGraph &graph): graph_(graph) {} - double error(const State &state) const ; - Gradient gradient(const State &state) const ; - State advance(const State ¤t, const double alpha, const Gradient &g) const ; + System(const NonlinearFactorGraph &graph) : + graph_(graph) { + } + double error(const State &state) const; + Gradient gradient(const State &state) const; + State advance(const State ¤t, const double alpha, + const Gradient &g) const; }; public: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { /* normalize it such that it becomes a unit vector */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // perform the golden section search algorithm to decide the the optimal step size // detail refer to http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; - double minStep = -1.0/g, maxStep = 0, - newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = + 1e-5; + double minStep = -1.0 / g, maxStep = 0, newStep = minStep + + (maxStep - minStep) / (phi + 1.0); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.error(newValues); while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ; - const double testStep = flag ? - newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep); + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = + flag ? newStep + resphi * (maxStep - newStep) : + newStep - resphi * (newStep - minStep); - if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * * The S (system) class requires three member functions: error(state), gradient(state) and @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = std::max(0.0, + currentGradient.dot(currentGradient - prevGradient) + / prevGradient.dot(prevGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration + && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 923edddb7..ed6213058 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactorQ.h * @date Oct 27, 2013 @@ -6,16 +17,18 @@ #pragma once -#include "JacobianSchurFactor.h" +#include "RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: @@ -24,37 +37,45 @@ public: } /// Empty constructor with keys - JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorQ(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + 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) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianFactorQ(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 < KeyMatrix > QF; + 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 typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); } }; +// end class JacobianFactorQ + +// traits +template struct traits > : public Testable< + JacobianFactorQ > { +}; } diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index ccd6e8756..4c1b0ff14 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,31 +6,38 @@ */ #pragma once -#include +#include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + 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 std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + 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; @@ -38,9 +45,9 @@ public: //gfg.print("gfg"); // eliminate the point - GaussianBayesNet::shared_ptr bn; + boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; - std::vector < Key > variables; + std::vector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); @@ -48,6 +55,6 @@ public: JacobianFactor::operator=(JacobianFactor(*fg)); } }; -// class +// end class JacobianFactorQR -}// gtsam +}// end namespace gtsam diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e0a5f4566..b4389d681 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,50 +5,57 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include "gtsam/slam/RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +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: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrix2D; - typedef std::pair KeyMatrix; - /// Default constructor - JacobianFactorSVD() {} + JacobianFactorSVD() { + } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorSVD(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + JacobianFactorSVD(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 j = 0, m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, + (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 5d28bbada..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; + // Use Eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -61,25 +63,22 @@ public: } // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; mutable std::vector y; /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); } /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); BOOST_FOREACH(DVector & yi, y) yi.setZero(); - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after @@ -104,14 +103,10 @@ public: } } + /// Raw memory version, with offsets TODO document reasoning void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Create a vector of temporary y values, corresponding to rows i std::vector y; y.reserve(size()); @@ -140,50 +135,38 @@ public: // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; } /** Return the diagonal of the Hessian for this factor (raw memory version) */ virtual void hessianDiagonal(double* d) const { - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal const Matrix& B = info_(pos, pos).selfadjointView(); - //DMap(d + 9 * j) += B.diagonal(); DMap(d + D * j) += B.diagonal(); } } - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + /// Add gradient at zero to d TODO: is it really the goal to add ?? virtual void gradientAtZero(double* d) const { - // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos,size()).knownOffDiagonal(); - //DMap(d + 9 * j) += dj; + DVector dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } /* ************************************************************************* */ -}; // end class RegularHessianFactor +}; +// end class RegularHessianFactor // traits template struct traits > : public Testable< diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index bde928723..b1490d465 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,12 +7,10 @@ #pragma once -#include #include #include #include -#include -#include +#include namespace gtsam { diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index bb275ef3f..38e1407f0 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -21,25 +21,34 @@ #include #include #include -#include namespace gtsam { +/** + * JacobianFactor with constant sized blocks + * Provides raw memory access versions of linear operator. + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + */ template class RegularJacobianFactor: public JacobianFactor { private: - /** Use eigen magic to access raw memory */ + // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; typedef Eigen::Map ConstDMap; public: + /// Default constructor + RegularJacobianFactor() {} + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * TODO Verify terms are regular + */ template RegularJacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : @@ -49,91 +58,66 @@ public: /** Constructor with arbitrary number keys, and where the augmented matrix is given all together * instead of in block terms. Note that only the active view of the provided augmented matrix * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ + * factor. + * TODO Verify complies to regular + */ template RegularJacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()) : + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { - - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; Vector Ax = zero(Ab_.rows()); - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) - { - Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - } - } - - /** Raw memory access version of multiplyHessianAdd */ - void multiplyHessianAdd(double alpha, const double* x, double* y) const { - - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; poswhiten(column_k); dj(k) = dot(column_k, column_k); - }else{ + } else { dj(k) = Ab_(j).col(k).squaredNorm(); } } @@ -141,10 +125,13 @@ public: } } - /** Raw memory access version of gradientAtZero - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void gradientAtZero(double* d) const { + /// Expose base class gradientAtZero + virtual VectorValues gradientAtZero() const { + return JacobianFactor::gradientAtZero(); + } + + /// Raw memory access version of gradientAtZero + void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); @@ -156,19 +143,90 @@ public: } // Just iterate over all A matrices - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column - for (size_t k = 0; k < D; ++k){ + for (size_t k = 0; k < D; ++k) { Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b, column_k); + dj(k) = -1.0 * dot(b, column_k); } DMap(d + D * j) += dj; } } + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; + } + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + return model_ ? model_->whiten(Ax) : Ax; + } + + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO Frank asks: why is this here if not regular ???? + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } + } + }; +// end class RegularJacobianFactor -} - +}// end namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 44d6902fa..a2bdfc059 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,30 +25,41 @@ #include #include -#include // for Cheirality exception -#include -#include -#include +#include #include #include #include namespace gtsam { -/// Base class with no internal point, completely functional -template + +/** + * @brief Base class for smart factors + * This base class has no internal point, but it has a measurement, noise model + * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * 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 class SmartFactorBase: public NonlinearFactor { protected: - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views + typedef typename CAMERA::Measurement Z; + + /** + * 2D measurement and noise model for each of the m views + * We keep a copy of measurements for I/O and computing the error. + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; + std::vector noise_; ///< noise model used - ///< (important that the order is the same as the keys that we use to create the factor) - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension; ///< Measurement dimension + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F @@ -63,27 +74,22 @@ protected: typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; - + typedef SmartFactorBase This; public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef CAMERA Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : + SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { } @@ -92,7 +98,7 @@ public: } /** - * add a new measurement and pose key + * 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 @@ -105,9 +111,8 @@ public: } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { @@ -118,9 +123,8 @@ public: } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and uses the same noise model for all of them */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { @@ -134,7 +138,6 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** template void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { @@ -144,12 +147,17 @@ public: } } + /// get the dimension (number of rows!) of the factor + virtual size_t dim() const { + return ZDim * this->measured_.size(); + } + /** return the measurements */ const std::vector& measured() const { return measured_; } - /** return the noise model */ + /** return the noise models */ const std::vector& noise() const { return noise_; } @@ -187,30 +195,38 @@ public: && body_P_sensor_->equals(*e->body_P_sensor_))); } - // **************************************************************************************************** -// /// Calculate vector of re-projection errors, before applying noise model + /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - Vector b = zero(ZDim * cameras.size()); + // 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 + } - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z e(camera.project(point) - zi); - b[ZDim * i] = e.x(); - b[ZDim * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; + // 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; + } + /** * 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. @@ -220,178 +236,233 @@ public: */ double totalReprojectionError(const Cameras& cameras, const Point3& point) const { - + Vector b = reprojectionError(cameras, point); double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - return overallError; + 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; } - // **************************************************************************************************** - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { + /** + * 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 { - int numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(ZDim, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei, boost::none); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim * i, 0) = Ei; + // 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 } - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + // 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 only (called below in both vanilla and SVD versions) - /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { + /** + * 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 { - size_t numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - b = zero(ZDim * numKeys); - double f = 0; + // 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 + } - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); - for (size_t i = 0; i < this->measured_.size(); i++) { + // 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) { - Vector bi; - try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - if(body_P_sensor_){ - 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); - Fi = Fi * J; + // 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; } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + F.block(row, 0) = Fi; + E.block(row, 0) = Ei; } - this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); - - f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); - } else { - Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block(ZDim * i, 0) = Ei; - subInsert(b, bi, ZDim * i); + b.segment(row) = bi; } - return f; + return b; } - // **************************************************************************************************** - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, - Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { + /// Computes Point Covariance P from E + static Matrix3 PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); + } - double f = computeJacobians(Fblocks, E, b, cameras, point); + /// Computes Point Covariance P, with lambda parameter + static Matrix3 PointCov(Matrix& E, double lambda, + bool diagonalDamping = false) { - // Point covariance inv(E'*E) 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{ + } else { EtE(0, 0) += lambda; EtE(1, 1) += lambda; EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE).inverse(); - - return f; + return (EtE).inverse(); } - // **************************************************************************************************** - // TODO, there should not be a Matrix version, really - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const Point3& point, - const double lambda = 0.0) const { + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, + const Point3& point) const { + whitenedError(cameras, point, E); + P = PointCov(E); + } - size_t numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(This::ZDim * numKeys, D * numKeys); + /** + * 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. + */ + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { - for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras + // 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; + } + /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, double lambda = - 0.0, bool diagonalDamping = false) const { + Vector& b, const Cameras& cameras, const Point3& point) const { Matrix E; - Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + double f = computeJacobians(Fblocks, E, b, cameras, point); // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns + 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; } - // **************************************************************************************************** /// 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 { - - int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(ZDim * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - + FillDiagonalF(Fblocks, F); return f; } - // **************************************************************************************************** - /// linearize returns a Hessianfactor that is an approximation of error(p) + /** + * Linearize returns a Hessianfactor that is an approximation of error(p) + */ boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -400,10 +471,9 @@ public: std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS @@ -411,14 +481,13 @@ public: std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Vector > gs(numKeys); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + 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); + 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 @@ -426,17 +495,19 @@ public: 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, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... augmentedHessian(numKeys, numKeys)(0, 0) = f; return boost::make_shared >(this->keys_, augmentedHessian); #endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices + /** + * 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 Matrix& PointCov, const Vector& b, + 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 @@ -446,16 +517,14 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras + Matrix F; + FillDiagonalF(Fblocks, F); Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + 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; @@ -471,9 +540,12 @@ public: } } - // **************************************************************************************************** + /** + * 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 Matrix& P /*Point Covariance*/, const Vector& b, + 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 @@ -490,7 +562,8 @@ public: // 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 + 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) ) @@ -508,9 +581,12 @@ public: } // 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 Matrix& P /*Point Covariance*/, const Vector& b, + 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 @@ -535,7 +611,7 @@ public: { // 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) + - 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() @@ -554,27 +630,12 @@ public: } // end of for over cameras } - // **************************************************************************************************** - void updateAugmentedHessian(const Cameras& cameras, const Point3& point, - const double lambda, bool diagonalDamping, - SymmetricBlockMatrix& augmentedHessian, - const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - } - - // **************************************************************************************************** + /** + * 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 Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const FastVector allKeys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick @@ -584,9 +645,9 @@ public: MatrixDD matrixBlock; typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - FastMap KeySlotMap; - for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + 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 @@ -607,7 +668,8 @@ public: // 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() + 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) @@ -615,8 +677,11 @@ public: // 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) ); + 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 @@ -629,48 +694,76 @@ public: // 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); + 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; } - // **************************************************************************************************** + /** + * Add the contribution of the smart factor to a pre-allocated Hessian, + * using sparse linear algebra. More efficient than the creation of the + * Hessian without preallocation of the SymmetricBlockMatrix + */ + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + 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) = ... + } + + /** + * 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->PointCovariance(), f->b(), - cameras, point, lambda, diagonalDamping); + computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); + f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); f->initKeys(); return f; } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactorQ + */ boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(Fblocks, E, P, b); } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactor + * 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; Vector b; - Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); + Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point); + return boost::make_shared >(Fblocks, Enull, b); } private: @@ -683,9 +776,10 @@ private: ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; +} +; -template -const int SmartFactorBase::ZDim; +template +const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..7fb43152a 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -58,11 +58,11 @@ enum LinearizationMode { }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { +template +class SmartProjectionFactor: public SmartFactorBase, + D> { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -102,9 +102,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + typedef SmartProjectionFactor This; public: @@ -113,7 +111,7 @@ public: /// shorthand for a pinhole camera typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -126,16 +124,16 @@ public: */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : + 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) { + false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -252,11 +250,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_) > landmarkDistanceThreshold_) { degenerate_ = true; break; } @@ -270,8 +268,8 @@ public: i += 1; } // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (dynamicOutlierRejectionThreshold_ > 0 + && totalReprojError / m > dynamicOutlierRejectionThreshold_) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -300,7 +298,8 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) { - std::cout << "createRegularImplicitSchurFactor: degenerate configuration" + std::cout + << "createRegularImplicitSchurFactor: degenerate configuration" << std::endl; } return false; @@ -321,9 +320,9 @@ public: 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 @@ -338,7 +337,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -373,9 +372,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -383,20 +381,20 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * 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; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + 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++) { + 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 * D); GsCount2++; } } @@ -420,16 +418,16 @@ public: } /// 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 >(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 ?? @@ -437,16 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(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 @@ -479,27 +477,27 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { + return Base::computeEP(E, P, cameras, point_); + } + /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + bool computeEP(Matrix& E, Matrix& P, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + computeEP(E, P, myCameras); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras); return nonDegenerate; } @@ -538,7 +536,7 @@ public: 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; + E.block<2, 2>(2 * i, 0) = Ei; subInsert(b, bi, 2 * i); } return f; @@ -548,19 +546,6 @@ public: } // 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 { @@ -585,9 +570,9 @@ public: } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + 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 @@ -709,7 +694,4 @@ private: } }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0801d141f..70476ba3e 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -67,7 +67,7 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + 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, @@ -141,11 +141,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -216,7 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 8dfb753f4..e2b8ac3cf 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -15,11 +15,10 @@ * @date March 4, 2014 */ -#include -#include - -//#include #include +#include + +#include #include #include @@ -29,8 +28,6 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -const double tol = 1e-5; - /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { @@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, zero(2)); + actualVV.insert(1, zero(2)); + actualVV.insert(3, zero(2)); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); double xvalues[8] = {1,2,3,4,0,0,5,6}; - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); // now, do it with non-zero y - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(2*expected_y, fast_y)); // check some expressions diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 29eaf2ac1..8571a345d 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,16 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImplicitSchurFactor.cpp + * @file testRegularImplicitSchurFactor.cpp * @brief unit test implicit jacobian factors * @author Frank Dellaert * @date Oct 20, 2013 */ -//#include -#include -//#include #include -//#include "gtsam_unstable/slam/JacobianFactorQR.h" -#include "gtsam/slam/JacobianFactorQR.h" +#include +#include #include #include diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index b56b65d7b..5803516a1 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularJacobianFactor.cpp * @brief unit test regular jacobian factors @@ -5,13 +16,13 @@ * @date Nov 12, 2014 */ -#include -#include - #include #include #include #include +#include + +#include #include #include diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..b5ef18842 --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include "../SmartFactorBase.h" +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeFactor: public SmartFactorBase, 9> { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f; + f.add(Point2(), 1, SharedNoiseModel()); + f.add(Point2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 2, f.dim()); +} + +/* ************************************************************************* */ +#include +class StereoFactor: public SmartFactorBase { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Stereo) { + StereoFactor f; + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 38722d88b..474009cfb 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone @@ -20,11 +20,12 @@ #include "../SmartProjectionPoseFactor.h" -#include #include #include -#include +#include +#include #include +#include #include using namespace std; @@ -35,37 +36,38 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +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 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; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +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)); +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; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; -void projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ +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); @@ -101,7 +103,8 @@ TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -117,21 +120,22 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // 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), gtsam::Point3(0,0,1)); + 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)); + 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 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); @@ -139,93 +143,114 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); + SmartFactor factor; + factor.add(level_uv, x1, model, K); + factor.add(level_uv_right, x2, model, K); - double actualError = factor1.error(values); + double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); - double actualError2 = factor1.totalReprojectionError(cameras); + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // test vector of errors - //Vector actual = factor1.unwhitenedError(values); + //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); + + // Calculate using computeEP + Matrix actualE, PointCov; + factor.computeEP(actualE, PointCov, cameras); + 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)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ +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), gtsam::Point3(0,0,1)); + 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)); + 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 - Point2 pixelError(0.2,0.2); + Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + 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)); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor()); + factor->add(level_uv, x1, model, K); + factor->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -240,7 +265,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -263,21 +288,25 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); @@ -287,25 +316,29 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // 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),1e-8)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ +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), 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)); + 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)); 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), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -325,7 +358,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -335,13 +368,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ bool manageDegeneracy = false; bool enableEPI = false; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -363,12 +402,13 @@ 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), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + 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; Values result; @@ -376,8 +416,34 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(bodyPose3,result.at(x3))); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3, result.at(x3))); + + // Check derivatives + + // 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); + + // Calculate using computeEP + Matrix actualE, PointCov; + smartFactor1->computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, + actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + + // 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)); } /* *************************************************************************/ @@ -394,8 +460,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ vector measurements_cam1; // Project 2 landmarks into 2 cameras - cout << cam1.project(landmark1) << endl; - cout << cam2.project(landmark1) << endl; measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1)); @@ -419,65 +483,87 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(p); EXPECT(assert_equal(landmark1,*p)); + // 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; { - // RegularHessianFactor<6> - Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; - Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; - Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; + // createHessianFactor + Matrix66 G11 = 0.5*A1.transpose()*A1; + Matrix66 G12 = 0.5*A1.transpose()*A2; + Matrix66 G22 = 0.5*A2.transpose()*A2; Vector6 g1; g1.setZero(); Vector6 g2; g2.setZero(); double f = 0; - RegularHessianFactor<6> expected(x1, x2, 50 * G11, 50 * G12, g1, 50 * G22, g2, f); + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); 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)); + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(), actual->information(), 1e-8)); + CHECK(assert_equal(expected, *actual, 1e-8)); } { - // RegularImplicitSchurFactor<6> Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10; + Matrix43 E; 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, F1))(make_pair(x2, F2)); + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); Matrix3 P = (E.transpose() * E).inverse(); Vector4 b; b.setZero(); + // createRegularImplicitSchurFactor RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expected,*actual)); + CHECK(assert_equal(expected, *actual)); + + // createJacobianQFactor + JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expectedQ, *actualQ)); } + { + // createJacobianSVDFactor + Vector1 b; b.setZero(); + double s = sin(M_PI_4); + JacobianFactor expected(x1, s*A1, x2, s*A2, b); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected, *actual)); + } } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -510,48 +596,54 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // 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), 1e-7)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -566,13 +658,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -584,38 +679,39 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -630,13 +726,19 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -648,40 +750,41 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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(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(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -690,29 +793,34 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark3(3, 0, 3.0); Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; // 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); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -725,7 +833,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + 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); @@ -736,25 +845,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -769,13 +878,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); 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, boost::none, 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, boost::none, 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, boost::none, JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -787,39 +899,40 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -831,60 +944,74 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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), gtsam::Point3(0.5,0.1,0.3)); + 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(x3, pose3 * 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: "); + if (isDebugTest) + values.at(x3).print("Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + 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), 1e-7)); + if (isDebugTest) + result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, CheckHessian){ +TEST( SmartProjectionPoseFactor, CheckHessian) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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 - Pose3 pose3 = pose2 * 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 @@ -915,57 +1042,65 @@ TEST( SmartProjectionPoseFactor, CheckHessian){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // 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)); + 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 @@ -979,68 +1114,81 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + 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(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + 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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); - values.insert(x2, pose2*noise_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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + 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; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + 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_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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 - Pose3 pose3 = pose2 * 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 @@ -1057,68 +1205,82 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ double rankTol = 10; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + 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(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + 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; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + 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_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ +TEST( SmartProjectionPoseFactor, Hessian ) { // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // three landmarks ~5 meters infront of camera @@ -1132,15 +1294,18 @@ TEST( SmartProjectionPoseFactor, Hessian ){ measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); + smartFactor1->add(measurements_cam1, views, model, K2); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + 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); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor1->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -1148,26 +1313,25 @@ TEST( SmartProjectionPoseFactor, Hessian ){ // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); Point3 landmark1(5, 0.5, 1.2); @@ -1184,54 +1348,62 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + 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)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + 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)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->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(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); Point3 landmark1(5, 0.5, 1.2); @@ -1243,62 +1415,72 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + 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), gtsam::Point3(0,0,0)); + 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)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + 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)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + 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(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); + SmartFactorBundler factor(rankTol, linThreshold); + boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor.add(measurement1, poseKey1, model, Kbundler); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ +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), gtsam::Point3(0,0,1)); + 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(), Point3(1,0,0)); + 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)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1330,7 +1512,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ measurements_cam3.push_back(cam2_uv3); measurements_cam3.push_back(cam3_uv3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -1353,50 +1535,56 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // 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), 1e-6)); - if(isDebugTest) tictoc_print_(); - } + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); + if (isDebugTest) + tictoc_print_(); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + 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)); + 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 - Pose3 pose3 = pose2 * 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); // three landmarks ~5 meters infront of camera @@ -1430,56 +1618,72 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ double rankTol = 10; - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + 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)); + 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)); + SmartFactorBundler::shared_ptr smartFactor3( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, Kbundler); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + 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(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + 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; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + 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_(); + if (isDebugTest) + tictoc_print_(); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c8a4e7123..dc921a7da 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 770c5f18c..cfed9ae0c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5d2ba3323..11513d19f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -62,10 +62,9 @@ enum LinearizationMode { /** * SmartStereoProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -95,7 +94,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -105,7 +104,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; enum {ZDim = 3}; ///< Dimension trait of measurement type @@ -118,7 +117,7 @@ public: typedef StereoCamera Camera; /// Vector of cameras - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -131,7 +130,7 @@ public: */ SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : @@ -370,7 +369,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -408,9 +407,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -418,9 +416,9 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * 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; if (isDebug) std::cout << "H:\n" << H << std::endl; @@ -515,6 +513,11 @@ 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; @@ -524,18 +527,13 @@ public: return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras, 0.0); return nonDegenerate; } @@ -621,9 +619,9 @@ public: // } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + 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 @@ -746,9 +744,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : + public Testable > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2e3bc866b..3e0c6476f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,10 +50,10 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -69,7 +69,7 @@ public: */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + 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, @@ -143,11 +143,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -216,9 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..4cc8e66ff 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,42 +32,44 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = true; +static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; static double b = 1; -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); -static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); +static Cal3_S2Stereo::shared_ptr K2( + new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +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; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +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; +typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; -vector stereo_projectToMultipleCameras( - const StereoCamera& cam1, const StereoCamera& cam2, - const StereoCamera& cam3, Point3 landmark){ +vector stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { vector measurements_cam; @@ -83,7 +85,7 @@ vector stereo_projectToMultipleCameras( /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr,"Test 1 Complete"); + fprintf(stderr, "Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -108,7 +110,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -124,15 +127,16 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ +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), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera 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)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera @@ -164,75 +168,78 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, noisy ){ +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), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera 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)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera 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 - StereoPoint2 pixelError(0.2,0.2,0); + StereoPoint2 pixelError(0.2, 0.2, 0); StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; StereoPoint2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + 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)); SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(level_uv, x1, model, K); factor1->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K2); // three landmarks ~5 meters infront of camera @@ -241,11 +248,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -268,21 +278,25 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartStereoProjectionPoseFactor); + gttic_ (SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); @@ -292,28 +306,29 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // 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_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3))); + if (isDebugTest) + tictoc_print_(); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -322,17 +337,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -344,38 +365,39 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -384,18 +406,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -407,40 +437,41 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // 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(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(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -450,28 +481,35 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark4(5, -0.5, 1); // 1. Project four landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark4); + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier - 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, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, 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, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -484,7 +522,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + 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); @@ -495,19 +534,19 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ // -// std::vector views; +// 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), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -546,8 +585,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // 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); @@ -564,13 +603,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ // // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // -// std::vector views; +// 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), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -606,7 +645,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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), gtsam::Point3(0.5,0.1,0.3)); +// 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); @@ -627,23 +666,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //} // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +TEST( SmartStereoProjectionPoseFactor, CheckHessian) { - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera 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)); + // create second camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam2(pose2, K); - // 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)); + // create third camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -651,12 +690,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - + // Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + // Create smartfactors double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); @@ -668,38 +710,47 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); smartFactor3->add(measurements_cam3, views, model, K); + // Create graph to optimize NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::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); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + 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: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + // TODO: next line throws Cheirality exception on Mac + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); @@ -709,13 +760,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// 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), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -745,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -754,7 +805,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // 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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -775,7 +826,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -784,13 +835,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// 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), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -826,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -836,8 +887,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // 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); @@ -858,7 +909,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // -// std::vector views; +// 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), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -892,7 +943,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // SmartFactor::shared_ptr smartFactor1(new SmartFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// 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); @@ -908,29 +959,30 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); smartFactorInstance->add(measurements_cam1, views, model, K); @@ -940,46 +992,54 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + 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)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + 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)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->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(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + 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), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -990,49 +1050,60 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + 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), gtsam::Point3(0,0,0)); + 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)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + 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)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + 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(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..e45f234aa 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -18,89 +25,74 @@ #include #include - using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it - NonlinearFactorGraph graph ; + NonlinearFactorGraph graph; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); + Pose2 x1(0.5, 0.0, 0.2); + initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1, -0.2); + initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); + initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI); + initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1, -M_PI_2); + initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; +// cout << "initial error = " << graph.error(initialEstimate) << endl; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); // cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index c90b09db1..66b022c82 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -17,21 +17,11 @@ */ #include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include #include -#include #include #include diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 89c3d5cf8..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,11 +18,12 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam;