parent
							
								
									a9d8a915ac
								
							
						
					
					
						commit
						64bb6b77d7
					
				|  | @ -0,0 +1 @@ | |||
| /html/ | ||||
|  | @ -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 <gtsam/slam/expressions.h> | ||||
| #include <gtsam/nonlinear/ExpressionFactorGraph.h> | ||||
| 
 | ||||
| // Header order is close to far
 | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| // For an explanation of headers below, please see Pose2SLAMExample.cpp
 | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/inference/Key.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  |  | |||
|  | @ -16,11 +16,14 @@ | |||
|  * @author Frank Dellaert | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| // For an explanation of headers below, please see Pose2SLAMExample.cpp
 | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| 
 | ||||
| // This new header allows us to read examples easily from .graph files
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  |  | |||
|  | @ -16,11 +16,11 @@ | |||
|  * @author Frank Dellaert | ||||
|  */ | ||||
| 
 | ||||
| // For an explanation of headers below, please see Pose2SLAMExample.cpp
 | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <fstream> | ||||
| 
 | ||||
| using namespace std; | ||||
|  |  | |||
|  | @ -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 <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| 
 | ||||
| // 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 <gtsam/inference/Key.h> | ||||
| 
 | ||||
| // 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 <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| 
 | ||||
| // 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 <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| 
 | ||||
| // 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 <gtsam/nonlinear/Values.h> | ||||
| 
 | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| 
 | ||||
| // In contrast to that example, however, we will use a PCG solver here
 | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| 
 | ||||
| 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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise)); | ||||
|   graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); | ||||
|   graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); | ||||
|   graph.push_back(BetweenFactor<Pose2>(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<Pose2>(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<SubgraphSolverParameters>(); | ||||
|     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<SubgraphSolverParameters>(); | ||||
| 
 | ||||
|   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; | ||||
| } | ||||
|  |  | |||
|  | @ -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).
 | ||||
|  |  | |||
|  | @ -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 <gtsam/geometry/Point2.h> | ||||
| 
 | ||||
| // 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 <gtsam/slam/SmartProjectionPoseFactor.h> | ||||
| 
 | ||||
| // Also, we will initialize the robot at some location using a Prior factor.
 | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| 
 | ||||
| // 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 <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| 
 | ||||
| // 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 <gtsam/nonlinear/DoglegOptimizer.h> | ||||
| 
 | ||||
| // 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 <gtsam/nonlinear/Values.h> | ||||
| 
 | ||||
| #include <vector> | ||||
| // For an explanation of these headers, see SFMExample.cpp
 | ||||
| #include "SFMdata.h" | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| // Make the typename short so it looks much cleaner
 | ||||
| typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor; | ||||
| typedef SmartProjectionPoseFactor<Cal3_S2> 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<gtsam::Point3>, as sometimes
 | ||||
|     // the triangulation operation inside smart factor will encounter degeneracy.
 | ||||
|     boost::optional<Point3> point; | ||||
| 
 | ||||
|     // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
 | ||||
|     SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]); | ||||
|     if (smart) { | ||||
|       point = smart->point(result); | ||||
|       // The output of point() is in boost::optional<Point3>, as sometimes
 | ||||
|       // the triangulation operation inside smart factor will encounter degeneracy.
 | ||||
|       boost::optional<Point3> 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; | ||||
| } | ||||
|  |  | |||
|  | @ -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 <gtsam/slam/SmartProjectionPoseFactor.h> | ||||
| 
 | ||||
| // These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
 | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| // Make the typename short so it looks much cleaner
 | ||||
| typedef SmartProjectionPoseFactor<Cal3_S2> 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<Point3> points = createPoints(); | ||||
|   vector<Pose3> 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<Pose3>(0, poses[0], poseNoise)); | ||||
| 
 | ||||
|   // Fix the scale ambiguity by adding a prior
 | ||||
|   graph.push_back(PriorFactor<Pose3>(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<PCGSolverParameters>(); | ||||
|   pcg->preconditioner_ = | ||||
|       boost::make_shared<BlockJacobiPreconditionerParameters>(); | ||||
|   // 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<SmartFactor>(graph[j]); | ||||
|     if (smart) { | ||||
|       boost::optional<Point3> 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; | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
							
								
								
									
										10
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										10
									
								
								gtsam.h
								
								
								
								
							|  | @ -2310,23 +2310,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { | |||
| }; | ||||
| 
 | ||||
| #include <gtsam/slam/SmartProjectionPoseFactor.h> | ||||
| template<POSE, CALIBRATION> | ||||
| template<CALIBRATION> | ||||
| 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<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor; | ||||
| typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor; | ||||
| 
 | ||||
| 
 | ||||
| #include <gtsam/slam/StereoFactor.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 <gtsam/geometry/Point3.h> | ||||
| #include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <vector> | ||||
| 
 | ||||
| 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 CAMERA> | ||||
| class CameraSet: public std::vector<CAMERA> { | ||||
| 
 | ||||
| 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<Z>::dimension; ///< Measurement dimension
 | ||||
|   static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /// Definitions for blocks of F
 | ||||
|   typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
 | ||||
|   typedef std::pair<Key, MatrixZD> 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<Z> project(const Point3& point, boost::optional<Matrix&> F = | ||||
|       boost::none, boost::optional<Matrix&> E = boost::none, | ||||
|       boost::optional<Matrix&> 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> z(nrCameras); | ||||
| 
 | ||||
|     for (size_t i = 0; i < nrCameras; i++) { | ||||
|       Eigen::Matrix<double, ZDim, 6> Fi; | ||||
|       Eigen::Matrix<double, ZDim, 3> Ei; | ||||
|       Eigen::Matrix<double, ZDim, Dim - 6> Hi; | ||||
|       z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); | ||||
|       if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi; | ||||
|       if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei; | ||||
|       if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi; | ||||
|     } | ||||
|     return z; | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /// Serialization function
 | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & (*this); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| template<class CAMERA> | ||||
| const int CameraSet<CAMERA>::ZDim; | ||||
| 
 | ||||
| template<class CAMERA> | ||||
| struct traits<CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > { | ||||
| }; | ||||
| 
 | ||||
| template<class CAMERA> | ||||
| struct traits<const CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  | @ -31,6 +31,14 @@ namespace gtsam { | |||
| template<typename Calibration> | ||||
| class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> { | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Some classes template on either PinholeCamera or StereoCamera, | ||||
|    *  and this typedef informs those classes what "project" returns. | ||||
|    */ | ||||
|   typedef Point2 Measurement; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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<StereoCamera> : public internal::Manifold<StereoCamera> {}; | ||||
| struct traits<StereoCamera> : public internal::Manifold<StereoCamera> { | ||||
| }; | ||||
| 
 | ||||
| template<> | ||||
| struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {}; | ||||
| struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> { | ||||
| }; | ||||
| } | ||||
|  |  | |||
|  | @ -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 <gtsam/geometry/CameraSet.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Cal3Bundler test
 | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| TEST(CameraSet, Pinhole) { | ||||
|   typedef PinholeCamera<Cal3Bundler> Camera; | ||||
|   typedef vector<Point2> ZZ; | ||||
|   CameraSet<Camera> set; | ||||
|   Camera camera; | ||||
|   set.push_back(camera); | ||||
|   set.push_back(camera); | ||||
|   Point3 p(0, 0, 1); | ||||
|   CHECK(assert_equal(set, set)); | ||||
|   CameraSet<Camera> 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 <gtsam/geometry/StereoCamera.h> | ||||
| TEST(CameraSet, Stereo) { | ||||
|   typedef vector<StereoPoint2> ZZ; | ||||
|   CameraSet<StereoCamera> set; | ||||
|   StereoCamera camera; | ||||
|   set.push_back(camera); | ||||
|   set.push_back(camera); | ||||
|   Point3 p(0, 0, 1); | ||||
|   EXPECT_LONGS_EQUAL(6, traits<StereoCamera>::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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  | @ -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 <gtsam/linear/ConjugateGradientSolver.h> | ||||
|  | @ -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; | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -20,7 +20,6 @@ | |||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/linear/IterativeSolver.h> | ||||
| #include <iosfwd> | ||||
| 
 | ||||
| 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 <class S, class V> | ||||
| V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { | ||||
| template<class S, class V> | ||||
| V preconditionedConjugateGradient(const S &system, const V &initial, | ||||
|     const ConjugateGradientParameters ¶meters) { | ||||
| 
 | ||||
|   V estimate, residual, direction, q1, q2; | ||||
|   estimate = residual = direction = q1 = q2 = initial; | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <boost/algorithm/string.hpp> | ||||
| #include <boost/foreach.hpp> | ||||
| #include <iostream> | ||||
| #include <string> | ||||
| 
 | ||||
| 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<const KeyInfo&> keyInfo, | ||||
|     boost::optional<const std::map<Key, Vector>&> lambda) | ||||
| { | ||||
|   return optimize( | ||||
|            gfg, | ||||
|            keyInfo ? *keyInfo : KeyInfo(gfg), | ||||
|            lambda ? *lambda : std::map<Key,Vector>()); | ||||
|     boost::optional<const std::map<Key, Vector>&> lambda) { | ||||
|   return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), | ||||
|       lambda ? *lambda : std::map<Key, Vector>()); | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| VectorValues IterativeSolver::optimize ( | ||||
|   const GaussianFactorGraph &gfg, | ||||
|   const KeyInfo &keyInfo, | ||||
|   const std::map<Key, Vector> &lambda) | ||||
| { | ||||
| VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &keyInfo, const std::map<Key, Vector> &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<Key, size_t> 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<size_t> KeyInfo::colSpec() const { | ||||
|   std::vector<size_t> 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<size_t> 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_); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -9,131 +9,178 @@ | |||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file   IterativeSolver.h | ||||
|  * @brief  Some support classes for iterative solvers | ||||
|  * @date   2010 | ||||
|  * @author Yong-Dian Jian | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/Vector.h> | ||||
| #include <gtsam/global_includes.h> | ||||
| #include <gtsam/inference/Ordering.h> | ||||
| #include <gtsam/base/Vector.h> | ||||
| 
 | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| #include <boost/optional/optional.hpp> | ||||
| #include <boost/none.hpp> | ||||
| #include <boost/shared_ptr.hpp> | ||||
| #include <boost/optional.hpp> | ||||
| 
 | ||||
| #include <iosfwd> | ||||
| #include <map> | ||||
| #include <string> | ||||
| #include <vector> | ||||
| #include <map> | ||||
| 
 | ||||
| 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<IterativeOptimizationParameters> shared_ptr; | ||||
|     enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; | ||||
|   typedef boost::shared_ptr<IterativeOptimizationParameters> 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<IterativeSolver> shared_ptr; | ||||
|     IterativeSolver() {} | ||||
|     virtual ~IterativeSolver() {} | ||||
| /**
 | ||||
|  * Base class for Iterative Solvers like SubgraphSolver | ||||
|  */ | ||||
| class GTSAM_EXPORT IterativeSolver { | ||||
| public: | ||||
|   typedef boost::shared_ptr<IterativeSolver> 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<const KeyInfo&> = boost::none, | ||||
|       boost::optional<const std::map<Key, Vector>&> lambda = boost::none | ||||
|     ); | ||||
|       boost::optional<const std::map<Key, Vector>&> lambda = boost::none); | ||||
| 
 | ||||
|     /* interface to the nonlinear optimizer, without initial estimate */ | ||||
|     VectorValues optimize ( | ||||
|       const GaussianFactorGraph &gfg, | ||||
|       const KeyInfo &keyInfo, | ||||
|       const std::map<Key, Vector> &lambda | ||||
|     ); | ||||
|   /* interface to the nonlinear optimizer, without initial estimate */ | ||||
|   VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, | ||||
|       const std::map<Key, Vector> &lambda); | ||||
| 
 | ||||
|     /* interface to the nonlinear optimizer that the subclasses have to implement */ | ||||
|     virtual VectorValues optimize ( | ||||
|       const GaussianFactorGraph &gfg, | ||||
|       const KeyInfo &keyInfo, | ||||
|       const std::map<Key, Vector> &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<Key, Vector> &lambda, | ||||
|       const VectorValues &initial) = 0; | ||||
| 
 | ||||
|   }; | ||||
| }; | ||||
| 
 | ||||
|   /************************************************************************************/ | ||||
|   /* Handy data structure for iterative solvers
 | ||||
|    * key to (index, dimension, colstart) */ | ||||
|   class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> { | ||||
|   public: | ||||
|     typedef boost::tuple<Key,size_t,Key> 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<size_t, size_t, size_t> { | ||||
| 
 | ||||
|   /************************************************************************************/ | ||||
|   class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> { | ||||
|   public: | ||||
|     typedef std::map<Key, KeyInfoEntry> Base; | ||||
|     KeyInfo() : numCols_(0) {} | ||||
|     KeyInfo(const GaussianFactorGraph &fg); | ||||
|     KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); | ||||
| public: | ||||
| 
 | ||||
|     std::vector<size_t> colSpec() const ; | ||||
|     VectorValues x0() const; | ||||
|     Vector x0vector() const; | ||||
|   typedef boost::tuple<Key, size_t, Key> 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<Key, KeyInfoEntry> { | ||||
| 
 | ||||
|     void initialize(const GaussianFactorGraph &fg); | ||||
| public: | ||||
| 
 | ||||
|     Ordering ordering_; | ||||
|     size_t numCols_; | ||||
|   typedef std::map<Key, KeyInfoEntry> 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<size_t> colSpec() const; | ||||
| 
 | ||||
|   /// Return VectorValues with zeros, of correct dimension
 | ||||
|   VectorValues x0() const; | ||||
| 
 | ||||
|   /// Return zero Vector of correct dimension
 | ||||
|   Vector x0vector() const; | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| 
 | ||||
| #include <boost/foreach.hpp> | ||||
| #include <boost/algorithm/string.hpp> | ||||
| 
 | ||||
| #include <algorithm> | ||||
| #include <iostream> | ||||
| #include <stdexcept> | ||||
| 
 | ||||
|  | @ -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<Key, Vector> &lambda, | ||||
|   const VectorValues &initial) | ||||
| { | ||||
| VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda, | ||||
|     const VectorValues &initial) { | ||||
|   /* build preconditioner */ | ||||
|   preconditioner_->build(gfg, keyInfo, lambda); | ||||
| 
 | ||||
|   /* apply pcg */ | ||||
|   const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>( | ||||
|         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<Key, Vector> &lambda) | ||||
|   : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} | ||||
|     const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, | ||||
|     const KeyInfo &keyInfo, const std::map<Key, Vector> &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<Key, size_t>  & dimensions) { | ||||
| VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, | ||||
|     const map<Key, size_t> & 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<Key, size_t>::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; | ||||
| } | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/IterativeSolver.h> | ||||
| #include <gtsam/linear/ConjugateGradientSolver.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <boost/shared_ptr.hpp> | ||||
| 
 | ||||
| #include <algorithm> | ||||
| #include <iosfwd> | ||||
| #include <map> | ||||
| #include <string> | ||||
| 
 | ||||
| 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<PCGSolverParameters> shared_ptr; | ||||
| 
 | ||||
|   PCGSolverParameters() {} | ||||
|   PCGSolverParameters() { | ||||
|   } | ||||
| 
 | ||||
|   virtual void print(std::ostream &os) const; | ||||
| 
 | ||||
|  | @ -42,8 +51,9 @@ public: | |||
|   boost::shared_ptr<PreconditionerParameters> 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<Key, size_t> & dimensions); | ||||
| 
 | ||||
| /**********************************************************************************/ | ||||
| /// Create VectorValues from a Vector and a KeyInfo class
 | ||||
| VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); | ||||
| 
 | ||||
| /// @}
 | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -9,73 +9,81 @@ | |||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| #include <gtsam/linear/Errors.h> | ||||
| #include <gtsam/linear/GaussianConditional.h> | ||||
| /**
 | ||||
|  * @file   SubgraphSolver.cpp | ||||
|  * @brief  Subgraph Solver from IROS 2010 | ||||
|  * @date   2010 | ||||
|  * @author Frank Dellaert | ||||
|  * @author Yong Dian Jian | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/iterative-inl.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/SubgraphPreconditioner.h> | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/inference/graph-inl.h> | ||||
| #include <gtsam/base/DSFVector.h> | ||||
| #include <boost/foreach.hpp> | ||||
| #include <boost/shared_ptr.hpp> | ||||
| #include <list> | ||||
| 
 | ||||
| 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<GaussianFactorGraph>(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<GaussianFactorGraph>(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<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_); | ||||
|   VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, | ||||
|       Errors>(*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<GaussianFactorGraph>(), | ||||
|                                   Ab2 = boost::make_shared<GaussianFactorGraph>(); | ||||
| void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { | ||||
|   GaussianFactorGraph::shared_ptr Ab1 = | ||||
|       boost::make_shared<GaussianFactorGraph>(), 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<VectorValues>(Rc1->optimize()); | ||||
|   GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, | ||||
|       EliminateQR); | ||||
|   VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>( | ||||
|       Rc1->optimize()); | ||||
|   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); | ||||
| } | ||||
| 
 | ||||
| /**************************************************************************************************/ | ||||
| void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) | ||||
| { | ||||
|   VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize()); | ||||
| void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, | ||||
|     const GaussianFactorGraph::shared_ptr &Ab2) { | ||||
|   VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>( | ||||
|       Rc1->optimize()); | ||||
|   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); | ||||
| } | ||||
| 
 | ||||
| /**************************************************************************************************/ | ||||
| boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> | ||||
| boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
 | ||||
| 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<Key, Vector> &lambda, | ||||
|   const VectorValues &initial | ||||
| ) { return VectorValues(); } | ||||
| VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda, | ||||
|     const VectorValues &initial) { | ||||
|   return VectorValues(); | ||||
| } | ||||
| } // \namespace gtsam
 | ||||
|  |  | |||
|  | @ -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 <gtsam/linear/ConjugateGradientSolver.h> | ||||
| #include <gtsam/inference/Ordering.h> | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| #include <iosfwd> | ||||
| 
 | ||||
| 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<SubgraphPreconditioner> pc_;  ///< preconditioner object
 | ||||
|   boost::shared_ptr<SubgraphPreconditioner> 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<GaussianFactorGraph> &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<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &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<GaussianFactorGraph> &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<GaussianFactorGraph> &Ab1, | ||||
|       const boost::shared_ptr<GaussianFactorGraph> &Ab2, | ||||
|       const Parameters ¶meters, const Ordering& ordering); | ||||
| 
 | ||||
|   /* The same as above, but the A1 is solved before */ | ||||
|   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); | ||||
|   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters ¶meters, const Ordering& ordering); | ||||
|   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, | ||||
|       const GaussianFactorGraph &Ab2, const Parameters ¶meters, | ||||
|       const Ordering& ordering); | ||||
| 
 | ||||
|   virtual ~SubgraphSolver() {} | ||||
|   /// Shared pointer version
 | ||||
|   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, | ||||
|       const boost::shared_ptr<GaussianFactorGraph> &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<Key, Vector> &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<Key, Vector> &lambda, | ||||
|       const VectorValues &initial); | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|   void initialize(const GaussianFactorGraph &jfg); | ||||
|   void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2); | ||||
|   void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, | ||||
|       const boost::shared_ptr<GaussianFactorGraph> &Ab2); | ||||
| 
 | ||||
|   boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> > | ||||
|   splitGraph(const GaussianFactorGraph &gfg) ; | ||||
|   boost::tuple<boost::shared_ptr<GaussianFactorGraph>, | ||||
|       boost::shared_ptr<GaussianFactorGraph> > | ||||
|   splitGraph(const GaussianFactorGraph &gfg); | ||||
| }; | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -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, Values>(System(graph_), state_.values, params_, true /* single iterations */); | ||||
|   int dummy; | ||||
|   boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>( | ||||
|       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, Values>(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; | ||||
| } | ||||
|  |  | |||
|  | @ -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<NonlinearConjugateGradientOptimizer> 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 <class S, class V, class W> | ||||
| template<class S, class V, class W> | ||||
| 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 <class S, class V> | ||||
| boost::tuple<V, int> nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { | ||||
| template<class S, class V> | ||||
| boost::tuple<V, int> 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<V, int> 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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -7,8 +7,13 @@ | |||
| 
 | ||||
| #pragma once | ||||
| #include <gtsam/slam/JacobianSchurFactor.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class GaussianBayesNet; | ||||
| 
 | ||||
| /**
 | ||||
|  * JacobianFactor for Schur complement that uses Q noise model | ||||
|  */ | ||||
|  | @ -38,7 +43,7 @@ public: | |||
|     //gfg.print("gfg");
 | ||||
| 
 | ||||
|     // eliminate the point
 | ||||
|     GaussianBayesNet::shared_ptr bn; | ||||
|     boost::shared_ptr<GaussianBayesNet> bn; | ||||
|     GaussianFactorGraph::shared_ptr fg; | ||||
|     std::vector < Key > variables; | ||||
|     variables.push_back(pointKey); | ||||
|  |  | |||
|  | @ -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  JacobianSchurFactor.h | ||||
|  * @brief Jacobianfactor that combines and eliminates points | ||||
|  | @ -7,11 +18,7 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <boost/foreach.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
|  |  | |||
|  | @ -65,13 +65,14 @@ public: | |||
|   mutable std::vector<DVector> 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) | ||||
|  | @ -104,6 +105,7 @@ public: | |||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Raw memory version, with offsets TODO document reasoning
 | ||||
|   void multiplyHessianAdd(double alpha, const double* x, double* yvalues, | ||||
|       std::vector<size_t> offsets) const { | ||||
| 
 | ||||
|  | @ -140,43 +142,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<double, 9, 1> DVector;
 | ||||
|     typedef Eigen::Matrix<double, D, 1> DVector; | ||||
|     typedef Eigen::Map<DVector> 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<double, 9, 1> DVector;
 | ||||
|     typedef Eigen::Matrix<double, D, 1> DVector; | ||||
|     typedef Eigen::Map<DVector> 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;
 | ||||
|       VectorD dj = -info_(pos, size()).knownOffDiagonal(); | ||||
|       DMap(d + D * j) += dj; | ||||
|     } | ||||
|   } | ||||
|  |  | |||
|  | @ -7,12 +7,10 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <boost/foreach.hpp> | ||||
| #include <boost/make_shared.hpp> | ||||
| #include <iostream> | ||||
| #include <iosfwd> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  |  | |||
|  | @ -52,11 +52,17 @@ public: | |||
|    *  factor. */ | ||||
|   template<typename KEYS> | ||||
|   RegularJacobianFactor(const KEYS& keys, | ||||
|       const VerticalBlockMatrix& augmentedMatrix, | ||||
|       const SharedDiagonal& sigmas = SharedDiagonal()) : | ||||
|       const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = | ||||
|           SharedDiagonal()) : | ||||
|       JacobianFactor(keys, augmentedMatrix, sigmas) { | ||||
|   } | ||||
| 
 | ||||
|   /** y += alpha * A'*A*x */ | ||||
|   virtual void multiplyHessianAdd(double alpha, const VectorValues& x, | ||||
|       VectorValues& y) const { | ||||
|     JacobianFactor::multiplyHessianAdd(alpha, x, y); | ||||
|   } | ||||
| 
 | ||||
|   /** 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 | ||||
|  | @ -78,10 +84,10 @@ public: | |||
|     /// 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) | ||||
|     { | ||||
|     for (size_t pos = 0; pos < size(); ++pos) { | ||||
|       Ax += Ab_(pos) | ||||
|           * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[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
 | ||||
|     if (model_) { | ||||
|  | @ -93,8 +99,9 @@ public: | |||
|     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_( | ||||
|     for (size_t pos = 0; pos < size(); ++pos) { | ||||
|       MapD(y + accumulatedDims[keys_[pos]], | ||||
|           accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( | ||||
|           pos).transpose() * Ax; | ||||
|     } | ||||
|   } | ||||
|  | @ -102,21 +109,25 @@ public: | |||
|   /** Raw memory access version of multiplyHessianAdd */ | ||||
|   void multiplyHessianAdd(double alpha, const double* x, double* y) const { | ||||
| 
 | ||||
|     if (empty()) return; | ||||
|     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; pos<size(); ++pos) | ||||
|     for (size_t pos = 0; pos < size(); ++pos) | ||||
|       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); } | ||||
|     if (model_) { | ||||
|       model_->whitenInPlace(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<size(); ++pos) | ||||
|     for (size_t pos = 0; pos < size(); ++pos) | ||||
|       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; | ||||
|   } | ||||
| 
 | ||||
|  | @ -128,12 +139,12 @@ public: | |||
|     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { | ||||
|       // Get the diagonal block, and insert its diagonal
 | ||||
|       DVector dj; | ||||
|       for (size_t k = 0; k < D; ++k){ | ||||
|         if (model_){ | ||||
|       for (size_t k = 0; k < D; ++k) { | ||||
|         if (model_) { | ||||
|           Vector column_k = Ab_(j).col(k); | ||||
|           column_k = model_->whiten(column_k); | ||||
|           dj(k) = dot(column_k, column_k); | ||||
|         }else{ | ||||
|         } else { | ||||
|           dj(k) = Ab_(j).col(k).squaredNorm(); | ||||
|         } | ||||
|       } | ||||
|  | @ -156,13 +167,13 @@ 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; | ||||
|     } | ||||
|  |  | |||
|  | @ -25,30 +25,41 @@ | |||
| #include <gtsam/slam/RegularHessianFactor.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h>  // for Cheirality exception | ||||
| #include <gtsam/geometry/StereoCamera.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/geometry/CameraSet.h> | ||||
| 
 | ||||
| #include <boost/optional.hpp> | ||||
| #include <boost/make_shared.hpp> | ||||
| #include <vector> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| /// Base class with no internal point, completely functional
 | ||||
| template<class POSE, class Z, class CAMERA, size_t D> | ||||
| 
 | ||||
| /**
 | ||||
|  * @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 CAMERA, size_t D> | ||||
| class SmartFactorBase: public NonlinearFactor { | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|   // Keep a copy of measurement and calibration for I/O
 | ||||
|   std::vector<Z> 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<Z> measured_; | ||||
| 
 | ||||
|   std::vector<SharedNoiseModel> noise_; ///< noise model used
 | ||||
|   ///< (important that the order is the same as the keys that we use to create the factor)
 | ||||
| 
 | ||||
|   boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
 | ||||
|   boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
 | ||||
| 
 | ||||
|   static const int ZDim = traits<Z>::dimension;    ///< Measurement dimension
 | ||||
|   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
 | ||||
| 
 | ||||
|   /// Definitions for blocks of F
 | ||||
|   typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F
 | ||||
|  | @ -63,27 +74,22 @@ protected: | |||
|   typedef NonlinearFactor Base; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef SmartFactorBase<POSE, Z, CAMERA, D> This; | ||||
| 
 | ||||
|   typedef SmartFactorBase<CAMERA, D> This; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> shared_ptr; | ||||
| 
 | ||||
|   /// shorthand for a pinhole camera
 | ||||
|   typedef CAMERA Camera; | ||||
|   typedef std::vector<CAMERA> Cameras; | ||||
|   typedef CameraSet<CAMERA> Cameras; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|    * @param body_P_sensor is the transform from sensor to body frame (default identity) | ||||
|    */ | ||||
|   SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) : | ||||
|   SmartFactorBase(boost::optional<Pose3> 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<Z>& measurements, std::vector<Key>& poseKeys, | ||||
|       std::vector<SharedNoiseModel>& 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<Z>& measurements, std::vector<Key>& 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<class SFM_TRACK> | ||||
|   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<Z>& measured() const { | ||||
|     return measured_; | ||||
|   } | ||||
| 
 | ||||
|   /** return the noise model */ | ||||
|   /** return the noise models */ | ||||
|   const std::vector<SharedNoiseModel>& 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<Z> 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<ZDim>(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<ZDim>(row) = noise_.at(i)->whiten(b.segment<ZDim>(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<ZDim>(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, 3>(ZDim * i, 0) = Ei; | ||||
|     // Project into CameraSet, calculating derivatives
 | ||||
|     std::vector<Z> 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<ZDim, 3>(row, 0); | ||||
|         model->WhitenSystem(Ei, dummy); | ||||
|         E.block<ZDim, 3>(row, 0) = Ei; | ||||
|       } | ||||
|       b.segment<ZDim>(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<KeyMatrix2D>& 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<Matrix&> 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<Z> 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<ZDim, 6>(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<ZDim, 6>(row, 0); | ||||
|         Matrix Ei = E.block<ZDim, 3>(row, 0); | ||||
|         if (!G) | ||||
|           model->WhitenSystem(Fi, Ei, bi); | ||||
|         else { | ||||
|           Matrix Gi = G->block<ZDim, D - 6>(row, 0); | ||||
|           model->WhitenSystem(Fi, Ei, Gi, bi); | ||||
|           G->block<ZDim, D - 6>(row, 0) = Gi; | ||||
|         } | ||||
|       } catch (CheiralityException&) { | ||||
|         std::cout << "Cheirality exception " << std::endl; | ||||
|         exit(EXIT_FAILURE); | ||||
|         F.block<ZDim, 6>(row, 0) = Fi; | ||||
|         E.block<ZDim, 3>(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<ZDim, 6>(0, 0) = Fi; // ZDim x 6 block for the cameras
 | ||||
|         Hcam.block<ZDim, D - 6>(0, 6) = Hcali; // ZDim x nrCal block for the cameras
 | ||||
|         Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); | ||||
|       } | ||||
|       E.block<ZDim, 3>(ZDim * i, 0) = Ei; | ||||
|       subInsert(b, bi, ZDim * i); | ||||
|       b.segment<ZDim>(row) = bi; | ||||
|     } | ||||
|     return f; | ||||
|     return b; | ||||
|   } | ||||
| 
 | ||||
|   // ****************************************************************************************************
 | ||||
|   /// Version that computes PointCov, with optional lambda parameter
 | ||||
|   double computeJacobians(std::vector<KeyMatrix2D>& 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<KeyMatrix2D> 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<KeyMatrix2D>& 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, D>(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<Matrix&> 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<ZDim>(row).squaredNorm(); | ||||
| 
 | ||||
|       // Get piece of F and possibly G
 | ||||
|       Matrix2D Fi; | ||||
|       if (D == 6) | ||||
|         Fi << F.block<ZDim, 6>(row, 0); | ||||
|       else | ||||
|         Fi << F.block<ZDim, 6>(row, 0), G.block<ZDim, D - 6>(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<KeyMatrix2D>& 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, D>(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<KeyMatrix2D> Fblocks; | ||||
|     double f = computeJacobians(Fblocks, E, b, cameras, point); | ||||
|     FillDiagonalF(Fblocks, F); | ||||
|     return f; | ||||
|   } | ||||
| 
 | ||||
|   /// SVD version
 | ||||
|   double computeJacobiansSVD(std::vector<KeyMatrix2D>& 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<Matrix> 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<KeyMatrix2D> 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, D>(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<RegularHessianFactor<D> > createHessianFactor( | ||||
|       const Cameras& cameras, const Point3& point, const double lambda = 0.0, | ||||
|       bool diagonalDamping = false) const { | ||||
|  | @ -400,10 +471,9 @@ public: | |||
| 
 | ||||
|     std::vector<KeyMatrix2D> 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<D> | ||||
|     > (this->keys_, Gs, gs, f); | ||||
|     return boost::make_shared < RegularHessianFactor<D> > (this->keys_, Gs, gs, f); | ||||
| #else // we create directly a SymmetricBlockMatrix
 | ||||
|     size_t n1 = D * numKeys + 1; | ||||
|     std::vector<DenseIndex> 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<D,D> (i1,i2) = ...
 | ||||
|     sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
 | ||||
|     augmentedHessian(numKeys, numKeys)(0, 0) = f; | ||||
|     return boost::make_shared<RegularHessianFactor<D> >(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<KeyMatrix2D>& Fblocks, const Matrix& E, | ||||
|       const Matrix& PointCov, const Vector& b, | ||||
|       const Matrix3& P, const Vector& b, | ||||
|       /*output ->*/std::vector<Matrix>& Gs, std::vector<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, D>(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<KeyMatrix2D>& 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<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
 | ||||
|       augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
 | ||||
|       augmentedHessian(i1, numKeys) = Fi1.transpose() | ||||
|           * b.segment<ZDim>(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<KeyMatrix2D>& 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<Matrix>& Gs, std::vector<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>(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<Key> allKeys) const { | ||||
| 
 | ||||
|     // int numKeys = this->keys_.size();
 | ||||
| 
 | ||||
|     std::vector<KeyMatrix2D> 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<D,D> (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<KeyMatrix2D>& 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<Key> 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<Key,size_t> KeySlotMap; | ||||
|     for (size_t slot=0; slot < allKeys.size(); slot++) | ||||
|       KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); | ||||
|     FastMap<Key, size_t> 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>(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, 3>(ZDim * i1, 0).transpose() * Fi1)  ); | ||||
|       augmentedHessian(aug_i1, aug_i1) = | ||||
|           matrixBlock | ||||
|               + (Fi1.transpose() | ||||
|                   * (Fi1 | ||||
|                       - Ei1_P * E.block<ZDim, 3>(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, 3>(ZDim * i2, 0).transpose() * Fi2); | ||||
|         augmentedHessian(aug_i1, aug_i2) = | ||||
|             augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() | ||||
|                 - Fi1.transpose() | ||||
|                     * (Ei1_P * E.block<ZDim, 3>(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<Key> allKeys) const { | ||||
| 
 | ||||
|     // int numKeys = this->keys_.size();
 | ||||
| 
 | ||||
|     std::vector<KeyMatrix2D> 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<D,D> (i1,i2) = ...
 | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Return Jacobians as RegularImplicitSchurFactor with raw access | ||||
|    */ | ||||
|   boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor( | ||||
|       const Cameras& cameras, const Point3& point, double lambda = 0.0, | ||||
|       bool diagonalDamping = false) const { | ||||
|     typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f( | ||||
|         new RegularImplicitSchurFactor<D>()); | ||||
|     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<JacobianFactorQ<D, ZDim> > createJacobianQFactor( | ||||
|       const Cameras& cameras, const Point3& point, double lambda = 0.0, | ||||
|       bool diagonalDamping = false) const { | ||||
|     std::vector<KeyMatrix2D> Fblocks; | ||||
|     Matrix E; | ||||
|     Matrix3 PointCov; | ||||
|     Vector b; | ||||
|     computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, | ||||
|         diagonalDamping); | ||||
|     return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b); | ||||
|     computeJacobians(Fblocks, E, b, cameras, point); | ||||
|     Matrix3 P = PointCov(E, lambda, diagonalDamping); | ||||
|     return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b); | ||||
|   } | ||||
| 
 | ||||
|   // ****************************************************************************************************
 | ||||
|   /**
 | ||||
|    * Return Jacobians as JacobianFactor | ||||
|    * TODO lambda is currently ignored | ||||
|    */ | ||||
|   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( | ||||
|       const Cameras& cameras, const Point3& point, double lambda = 0.0) const { | ||||
|     size_t numKeys = this->keys_.size(); | ||||
|     std::vector<KeyMatrix2D> 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<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
|  | @ -683,9 +776,10 @@ private: | |||
|     ar & BOOST_SERIALIZATION_NVP(measured_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|   } | ||||
| }; | ||||
| } | ||||
| ; | ||||
| 
 | ||||
| template<class POSE, class Z, class CAMERA, size_t D> | ||||
| const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim; | ||||
| template<class CAMERA, size_t D> | ||||
| const int SmartFactorBase<CAMERA, D>::ZDim; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -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 POSE, class CALIBRATION, size_t D> | ||||
| class SmartProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> { | ||||
| template<class CALIBRATION, size_t D> | ||||
| class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>, | ||||
|     D> { | ||||
| protected: | ||||
| 
 | ||||
|   // Some triangulation parameters
 | ||||
|  | @ -92,7 +92,7 @@ protected: | |||
|   typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr; | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base; | ||||
|   typedef SmartFactorBase<PinholeCamera<CALIBRATION>, 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<POSE, CALIBRATION, D> This; | ||||
| 
 | ||||
|   static const int ZDim = 2;    ///< Measurement dimension
 | ||||
|   typedef SmartProjectionFactor<CALIBRATION, D> This; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|  | @ -113,7 +111,7 @@ public: | |||
| 
 | ||||
|   /// shorthand for a pinhole camera
 | ||||
|   typedef PinholeCamera<CALIBRATION> Camera; | ||||
|   typedef std::vector<Camera> Cameras; | ||||
|   typedef CameraSet<Camera> Cameras; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|  | @ -126,16 +124,16 @@ public: | |||
|    */ | ||||
|   SmartProjectionFactor(const double rankTol, const double linThreshold, | ||||
|       const bool manageDegeneracy, const bool enableEPI, | ||||
|       boost::optional<POSE> body_P_sensor = boost::none, | ||||
|       boost::optional<Pose3> 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<Key> js; | ||||
|     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); | ||||
|     std::vector<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<D>(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<D, D>(i1D, i2 * D); | ||||
|           GsCount2++; | ||||
|         } | ||||
|       } | ||||
|  | @ -420,16 +418,16 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// create factor
 | ||||
|   boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor( | ||||
|   boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor( | ||||
|       const Cameras& cameras, double lambda) const { | ||||
|     if (triangulateForLinearize(cameras)) | ||||
|       return Base::createJacobianQFactor(cameras, point_, lambda); | ||||
|     else | ||||
|       return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_); | ||||
|       return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_); | ||||
|   } | ||||
| 
 | ||||
|   /// Create a factor, takes values
 | ||||
|   boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor( | ||||
|   boost::shared_ptr<JacobianFactorQ<D, 2> > 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<D, ZDim> >(this->keys_); | ||||
|       return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_); | ||||
|   } | ||||
| 
 | ||||
|   /// different (faster) way to compute Jacobian factor
 | ||||
|   boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, | ||||
|       double lambda) const { | ||||
|   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( | ||||
|       const Cameras& cameras, double lambda) const { | ||||
|     if (triangulateForLinearize(cameras)) | ||||
|       return Base::createJacobianSVDFactor(cameras, point_, lambda); | ||||
|     else | ||||
|       return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_); | ||||
|       return boost::make_shared<JacobianFactorSVD<D, 2> >(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<typename Base::KeyMatrix2D>& 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<typename Base::KeyMatrix2D>& 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<typename Base::KeyMatrix2D>& 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<class POSE, class CALIBRATION, size_t D> | ||||
| const int SmartProjectionFactor<POSE, CALIBRATION, D>::ZDim; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -37,8 +37,8 @@ namespace gtsam { | |||
|  * The calibration is known here. The factor only constraints poses (variable dimension is 6) | ||||
|  * @addtogroup SLAM | ||||
|  */ | ||||
| template<class POSE, class CALIBRATION> | ||||
| class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, CALIBRATION, 6> { | ||||
| template<class CALIBRATION> | ||||
| class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> { | ||||
| 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<POSE, CALIBRATION, 6> Base; | ||||
|   typedef SmartProjectionFactor<CALIBRATION, 6> Base; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef SmartProjectionPoseFactor<POSE, CALIBRATION> This; | ||||
|   typedef SmartProjectionPoseFactor<CALIBRATION> This; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> 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<POSE> body_P_sensor = boost::none, | ||||
|       const bool enableEPI = false, boost::optional<Pose3> 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<class T1, class T2> | ||||
| struct traits<SmartProjectionPoseFactor<T1, T2> > : public Testable<SmartProjectionPoseFactor<T1, T2> > {}; | ||||
| template<class CALIBRATION> | ||||
| struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable< | ||||
|     SmartProjectionPoseFactor<CALIBRATION> > { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -15,11 +15,10 @@ | |||
|  * @date    March 4, 2014 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| //#include <gtsam_unstable/slam/RegularHessianFactor.h>
 | ||||
| #include <gtsam/slam/RegularHessianFactor.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> | ||||
| #include <boost/assign/std/map.hpp> | ||||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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 <gtsam_unstable/slam/ImplicitSchurFactor.h>
 | ||||
| #include <gtsam/slam/RegularImplicitSchurFactor.h> | ||||
| //#include <gtsam_unstable/slam/JacobianFactorQ.h>
 | ||||
| #include <gtsam/slam/JacobianFactorQ.h> | ||||
| //#include "gtsam_unstable/slam/JacobianFactorQR.h"
 | ||||
| #include "gtsam/slam/JacobianFactorQR.h" | ||||
| #include <gtsam/slam/JacobianFactorQR.h> | ||||
| #include <gtsam/slam/RegularImplicitSchurFactor.h> | ||||
| 
 | ||||
| #include <gtsam/base/timing.h> | ||||
| #include <gtsam/linear/VectorValues.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    testRegularJacobianFactor.cpp | ||||
|  * @brief   unit test regular jacobian factors | ||||
|  | @ -5,13 +16,13 @@ | |||
|  * @date    Nov 12, 2014 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/slam/RegularJacobianFactor.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/GaussianConditional.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> | ||||
| #include <boost/assign/list_of.hpp> | ||||
|  |  | |||
|  | @ -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 <gtsam/geometry/Pose3.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler>, 9> { | ||||
|   virtual double error(const Values& values) const { | ||||
|     return 0.0; | ||||
|   } | ||||
|   virtual boost::shared_ptr<GaussianFactor> linearize( | ||||
|       const Values& values) const { | ||||
|     return boost::shared_ptr<GaussianFactor>(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 <gtsam/geometry/StereoCamera.h> | ||||
| class StereoFactor: public SmartFactorBase<StereoCamera, 9> { | ||||
|   virtual double error(const Values& values) const { | ||||
|     return 0.0; | ||||
|   } | ||||
|   virtual boost::shared_ptr<GaussianFactor> linearize( | ||||
|       const Values& values) const { | ||||
|     return boost::shared_ptr<GaussianFactor>(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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
										
											
												File diff suppressed because it is too large
												Load Diff
											
										
									
								
							|  | @ -46,7 +46,7 @@ using namespace gtsam; | |||
| 
 | ||||
| int main(int argc, char** argv){ | ||||
| 
 | ||||
|   typedef SmartProjectionPoseFactor<Pose3, Cal3_S2> SmartFactor; | ||||
|   typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; | ||||
| 
 | ||||
|   Values initial_estimate; | ||||
|   NonlinearFactorGraph graph; | ||||
|  |  | |||
|  | @ -46,7 +46,7 @@ using namespace gtsam; | |||
| 
 | ||||
| int main(int argc, char** argv){ | ||||
| 
 | ||||
|   typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor; | ||||
|   typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor; | ||||
| 
 | ||||
|   bool output_poses = true; | ||||
|   string poseOutput("../../../examples/data/optimized_poses.txt"); | ||||
|  |  | |||
|  | @ -62,10 +62,9 @@ enum LinearizationMode { | |||
| 
 | ||||
| /**
 | ||||
|  * SmartStereoProjectionFactor: triangulates point | ||||
|  * TODO: why LANDMARK parameter? | ||||
|  */ | ||||
| template<class POSE, class LANDMARK, class CALIBRATION, size_t D> | ||||
| class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> { | ||||
| template<class CALIBRATION, size_t D> | ||||
| class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> { | ||||
| protected: | ||||
| 
 | ||||
|   // Some triangulation parameters
 | ||||
|  | @ -95,7 +94,7 @@ protected: | |||
|   typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr; | ||||
| 
 | ||||
|   /// shorthand for base class type
 | ||||
|   typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> Base; | ||||
|   typedef SmartFactorBase<StereoCamera, D> 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<POSE, LANDMARK, CALIBRATION, D> This; | ||||
|   typedef SmartStereoProjectionFactor<CALIBRATION, D> This; | ||||
| 
 | ||||
|   enum {ZDim = 3};    ///< Dimension trait of measurement type
 | ||||
| 
 | ||||
|  | @ -118,7 +117,7 @@ public: | |||
|   typedef StereoCamera Camera; | ||||
| 
 | ||||
|   /// Vector of cameras
 | ||||
|   typedef std::vector<Camera> Cameras; | ||||
|   typedef CameraSet<Camera> Cameras; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|  | @ -131,7 +130,7 @@ public: | |||
|    */ | ||||
|   SmartStereoProjectionFactor(const double rankTol, const double linThreshold, | ||||
|       const bool manageDegeneracy, const bool enableEPI, | ||||
|       boost::optional<POSE> body_P_sensor = boost::none, | ||||
|       boost::optional<Pose3> 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<typename Base::KeyMatrix2D>& 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<class POSE, class LANDMARK, class CALIBRATION, size_t D> | ||||
| struct traits<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > : | ||||
|     public Testable<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > { | ||||
| template<class CALIBRATION, size_t D> | ||||
| struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > : | ||||
|     public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -37,8 +37,8 @@ namespace gtsam { | |||
|  * The calibration is known here. The factor only constraints poses (variable dimension is 6) | ||||
|  * @addtogroup SLAM | ||||
|  */ | ||||
| template<class POSE, class LANDMARK, class CALIBRATION> | ||||
| class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> { | ||||
| template<class CALIBRATION> | ||||
| class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION, 6> { | ||||
| 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<POSE, LANDMARK, CALIBRATION, 6> Base; | ||||
|   typedef SmartStereoProjectionFactor<CALIBRATION, 6> Base; | ||||
| 
 | ||||
|   /// shorthand for this class
 | ||||
|   typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This; | ||||
|   typedef SmartStereoProjectionPoseFactor<CALIBRATION> This; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> 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<POSE> body_P_sensor = boost::none, | ||||
|       const bool enableEPI = false, boost::optional<Pose3> 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<class POSE, class LANDMARK, class CALIBRATION> | ||||
| struct traits<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > : | ||||
|     public Testable<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > { | ||||
| template<class CALIBRATION> | ||||
| struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable< | ||||
|     SmartStereoProjectionPoseFactor<CALIBRATION> > { | ||||
| }; | ||||
| 
 | ||||
| } // \ namespace gtsam
 | ||||
|  |  | |||
|  | @ -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<Cal3Bundler> 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<Cal3Bundler> 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<Pose3,Point3,Cal3_S2Stereo> SmartFactor; | ||||
| typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler; | ||||
| typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor; | ||||
| typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler; | ||||
| 
 | ||||
| vector<StereoPoint2> stereo_projectToMultipleCameras( | ||||
|     const StereoCamera& cam1, const StereoCamera& cam2, | ||||
|     const StereoCamera& cam3, Point3 landmark){ | ||||
| vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1, | ||||
|     const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { | ||||
| 
 | ||||
|   vector<StereoPoint2> measurements_cam; | ||||
| 
 | ||||
|  | @ -83,7 +85,7 @@ vector<StereoPoint2> 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<StereoPoint2> measurements; | ||||
|   measurements.push_back(level_uv); | ||||
|   measurements.push_back(level_uv_right); | ||||
| 
 | ||||
|   std::vector< SharedNoiseModel > noises; | ||||
|   vector<SharedNoiseModel> noises; | ||||
|   noises.push_back(model); | ||||
|   noises.push_back(model); | ||||
| 
 | ||||
|   std::vector< boost::shared_ptr<Cal3_S2Stereo> > Ks;  ///< shared pointer to calibration object (one for each camera)
 | ||||
|   vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
 | ||||
|   Ks.push_back(K); | ||||
|   Ks.push_back(K); | ||||
| 
 | ||||
|   std::vector<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); | ||||
|   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark3); | ||||
| 
 | ||||
|   std::vector<Key> views; | ||||
|   vector<Key> 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<Pose3>(x1, pose1, noisePrior)); | ||||
|   graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3).print("Smart: Pose3 before optimization: "); | ||||
|   values.insert(x3, pose3 * noise_pose); | ||||
|   if (isDebugTest) | ||||
|     values.at<Pose3>(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<Pose3>(x3).print("Smart: Pose3 after optimization: "); | ||||
|   EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); | ||||
|   if(isDebugTest) tictoc_print_(); | ||||
|   if (isDebugTest) | ||||
|     result.at<Pose3>(x3).print("Smart: Pose3 after optimization: "); | ||||
|   EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); | ||||
|   if (isDebugTest) | ||||
|     tictoc_print_(); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ | ||||
| TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { | ||||
| 
 | ||||
|   std::vector<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); | ||||
|   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> 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<Pose3>(x1, pose1, noisePrior)); | ||||
|   graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3))); | ||||
|   EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ | ||||
| TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { | ||||
| 
 | ||||
|   double excludeLandmarksFutherThanDist = 2; | ||||
| 
 | ||||
|   std::vector<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); | ||||
|   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> 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<Pose3>(x1, pose1, noisePrior)); | ||||
|   graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3),result.at<Pose3>(x3))); | ||||
|   EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3))); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ | ||||
| TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { | ||||
| 
 | ||||
|   double excludeLandmarksFutherThanDist = 1e10; | ||||
|   double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
 | ||||
| 
 | ||||
|   std::vector<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|    vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); | ||||
|    vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); | ||||
|    vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); | ||||
|   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark3); | ||||
|   vector<StereoPoint2> 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<Pose3>(x1, pose1, noisePrior)); | ||||
|   graph.push_back(PriorFactor<Pose3>(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<Pose3>(x3))); | ||||
|   EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); | ||||
| } | ||||
| //
 | ||||
| ///* *************************************************************************/
 | ||||
| //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
 | ||||
| //
 | ||||
| //  std::vector<Key> views;
 | ||||
| //  vector<Key> 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<Pose3>(x1, pose1, noisePrior));
 | ||||
| //  graph.push_back(PriorFactor<Pose3>(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<Key> views;
 | ||||
| //  vector<Key> 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<Pose3>(x1, pose1, noisePrior));
 | ||||
| //  graph.push_back(PriorFactor<Pose3>(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<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); | ||||
| 
 | ||||
|   // Project three landmarks into three cameras and triangulate
 | ||||
|   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> 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<Pose3>(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<Pose3>(x3).print("Smart: Pose3 before optimization: "); | ||||
| 
 | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values); | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values); | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values); | ||||
|   // TODO: next line throws Cheirality exception on Mac
 | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize( | ||||
|       values); | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize( | ||||
|       values); | ||||
|   boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize( | ||||
|       values); | ||||
| 
 | ||||
|   Matrix CumulativeInformation = hessianFactor1->information() +  hessianFactor2->information() + hessianFactor3->information(); | ||||
|   Matrix CumulativeInformation = hessianFactor1->information() | ||||
|       + hessianFactor2->information() + hessianFactor3->information(); | ||||
| 
 | ||||
|   boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values); | ||||
|   boost::shared_ptr<GaussianFactorGraph> 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<Key> views;
 | ||||
| //  vector<Key> 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<Pose3>(x2, positionPrior, noisePriorTranslation));
 | ||||
| //  graph.push_back(PoseTranslationPrior<Pose3>(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<Pose3>(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<Pose3>(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<Key> views;
 | ||||
| //  vector<Key> 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<Pose3>(x2, positionPrior, noisePriorTranslation));
 | ||||
| //  graph.push_back(PoseTranslationPrior<Pose3>(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<Pose3>(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<Pose3>(x3)));
 | ||||
| //  if(isDebugTest) tictoc_print_();
 | ||||
| //}
 | ||||
|  | @ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ | |||
| //TEST( SmartStereoProjectionPoseFactor, Hessian ){
 | ||||
| //  // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
 | ||||
| //
 | ||||
| //  std::vector<Key> views;
 | ||||
| //  vector<Key> 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<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> 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<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values); | ||||
|   boost::shared_ptr<GaussianFactor> 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<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues); | ||||
|   boost::shared_ptr<GaussianFactor> 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<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues); | ||||
|   boost::shared_ptr<GaussianFactor> 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<Key> views; | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> 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<GaussianFactor> hessianFactor = smartFactor->linearize(values); | ||||
|   if(isDebugTest)  hessianFactor->print("Hessian factor \n"); | ||||
|   boost::shared_ptr<GaussianFactor> 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<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues); | ||||
|   if(isDebugTest)  hessianFactorRot->print("Hessian factor \n"); | ||||
|   boost::shared_ptr<GaussianFactor> 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<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues); | ||||
|   boost::shared_ptr<GaussianFactor> 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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 <boost/shared_ptr.hpp> | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| 
 | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| 
 | ||||
| // Generate a small PoseSLAM problem
 | ||||
| boost::tuple<NonlinearFactorGraph, Values> 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<Pose2>(1, priorMean, priorNoise); | ||||
| 
 | ||||
|   // 2b. Add odometry factors
 | ||||
|   SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); | ||||
|   graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0   ), odometryNoise); | ||||
|   SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( | ||||
|       Vector3(0.2, 0.2, 0.1)); | ||||
|   graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); | ||||
|   graph += BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); | ||||
|   graph += BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); | ||||
|   graph += BetweenFactor<Pose2>(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<Pose2>(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<Pose2>(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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -17,21 +17,11 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <tests/smallExample.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/DoglegOptimizer.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| #include <gtsam/linear/SubgraphPreconditioner.h> | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
|  |  | |||
|  | @ -18,11 +18,12 @@ | |||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue