12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/SmartCT (pull request #107) 

Refactoring of Smart Factors
release/4.3a0
dellaert 2015-02-22 06:14:19 +01:00
parent a9d8a915ac
commit 64bb6b77d7
46 changed files with 2489 additions and 1440 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
doc/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/html/

10
examples/Pose2SLAMExampleExpressions.cpp
View File

@ -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;

9
examples/Pose2SLAMExample_graph.cpp
View File

@ -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;

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -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;

69
examples/Pose2SLAMwSPCG.cpp
View File

@ -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;
}

8
examples/SFMExample.cpp
View File

@ -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).

58
examples/SFMExample_SmartFactor.cpp
View File

@ -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;
}

126
examples/SFMExample_SmartFactorPCG.cpp Normal file
View File

@ -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
View File

@ -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>

123
gtsam/geometry/CameraSet.h Normal file
View File

@ -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

8
gtsam/geometry/PinholeCamera.h
View File

@ -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

5
gtsam/geometry/StereoCamera.cpp
View File

@ -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

61
gtsam/geometry/StereoCamera.h
View File

@ -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> {
};
}

114
gtsam/geometry/tests/testCameraSet.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

24
gtsam/linear/ConjugateGradientSolver.cpp
View File

@ -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;
}
/*****************************************************************************/
}

9
gtsam/linear/ConjugateGradientSolver.h
View File

@ -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 &parameters) {
template<class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial,
const ConjugateGradientParameters &parameters) {
V estimate, residual, direction, q1, q2;
estimate = residual = direction = q1 = q2 = initial;

101
gtsam/linear/IterativeSolver.cpp
View File

@ -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_);
}
}

231
gtsam/linear/IterativeSolver.h
View File

@ -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

79
gtsam/linear/PCGSolver.cpp
View File

@ -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;
}

57
gtsam/linear/PCGSolver.h
View File

@ -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);
/// @}
}

128
gtsam/linear/SubgraphSolver.cpp
View File

@ -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 &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(gfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(*jfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering) {
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
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 &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering) {
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
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 &parameters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
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 &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering)
{
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
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

104
gtsam/linear/SubgraphSolver.h
View File

@ -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 &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A, const Parameters &parameters, 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 &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, 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 &parameters,
const Ordering& ordering);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
const Parameters &parameters, 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 &parameters, const Ordering& ordering);
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, 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 &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering);
virtual ~SubgraphSolver() {}
/// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, 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

48
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -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 &current, const double alpha, const Gradient &g) const {
NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(
const State &current, 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;
}

163
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -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 &current, 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 &current, 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 &params, 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 &params,
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

11
gtsam/slam/JacobianFactorQ.h
View File

@ -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
gtsam/slam/JacobianFactorQR.h
View File

@ -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);

15
gtsam/slam/JacobianSchurFactor.h
View File

@ -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 {

27
gtsam/slam/RegularHessianFactor.h
View File

@ -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;
}
}

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -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 {

45
gtsam/slam/RegularJacobianFactor.h
View File

@ -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;
}

538
gtsam/slam/SmartFactorBase.h
View File

@ -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

120
gtsam/slam/SmartProjectionFactor.h
View File

@ -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

21
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -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

22
gtsam/slam/tests/testRegularHessianFactor.cpp
View File

@ -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

20
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -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>

17
gtsam/slam/tests/testRegularJacobianFactor.cpp
View File

@ -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>

71
gtsam/slam/tests/testSmartFactorBase.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

858
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

File diff suppressed because it is too large Load Diff

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -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;

2
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -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");

50
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -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

21
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -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

427
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */

84
tests/testGradientDescentOptimizer.cpp
View File

@ -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);
}
/* ************************************************************************* */

10
tests/testPCGSolver.cpp
View File

@ -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>

7
tests/testPreconditioner.cpp
View File

@ -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;