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

Merged 'develop'.

release/4.3a0
Paul Drews 2015-02-25 11:00:46 -05:00
parent 9991ae04f3 ae5994c262
commit abfcfa1a17
65 changed files with 3978 additions and 2099 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -1027,6 +1027,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholePose.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testPinholePose.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1546,6 +1554,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j3</buildArguments>

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;

1
examples/Pose2SLAMExample_g2o.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>

10
examples/Pose2SLAMExample_graph.cpp
View File

@ -16,11 +16,15 @@
* @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/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.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;

1
examples/Pose2SLAMExample_lago.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.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>

52
gtsam/discrete/DecisionTree-inl.h
View File

@ -462,15 +462,17 @@ namespace gtsam {
// cannot just create a root Choice node on the label: if the label is not the
// highest label, we need to do a complicated and expensive recursive call.
template<typename L, typename Y> template<typename Iterator>
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(
Iterator begin, Iterator end, const L& label) const {
typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(Iterator begin,
Iterator end, const L& label) const {
// find highest label among branches
boost::optional<L> highestLabel;
boost::optional<size_t> nrChoices;
for (Iterator it = begin; it != end; it++) {
if (it->root_->isLeaf()) continue;
boost::shared_ptr<const Choice> c = boost::dynamic_pointer_cast<const Choice> (it->root_);
if (it->root_->isLeaf())
continue;
boost::shared_ptr<const Choice> c =
boost::dynamic_pointer_cast<const Choice>(it->root_);
if (!highestLabel || c->label() > *highestLabel) {
highestLabel.reset(c->label());
nrChoices.reset(c->nrChoices());
@ -478,30 +480,31 @@ namespace gtsam {
}
// if label is already in correct order, just put together a choice on label
if (!highestLabel || label > *highestLabel) {
if (!highestLabel || !nrChoices || label > *highestLabel) {
boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
for (Iterator it = begin; it != end; it++)
choiceOnLabel->push_back(it->root_);
return Choice::Unique(choiceOnLabel);
}
// Set up a new choice on the highest label
boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices));
// now, for all possible values of highestLabel
for (size_t index = 0; index < *nrChoices; index++) {
// make a new set of functions for composing by iterating over the given
// functions, and selecting the appropriate branch.
std::vector<DecisionTree> functions;
for (Iterator it = begin; it != end; it++) {
// by restricting the input functions to value i for labelBelow
DecisionTree chosen = it->choose(*highestLabel, index);
functions.push_back(chosen);
} else {
// Set up a new choice on the highest label
boost::shared_ptr<Choice> choiceOnHighestLabel(
new Choice(*highestLabel, *nrChoices));
// now, for all possible values of highestLabel
for (size_t index = 0; index < *nrChoices; index++) {
// make a new set of functions for composing by iterating over the given
// functions, and selecting the appropriate branch.
std::vector<DecisionTree> functions;
for (Iterator it = begin; it != end; it++) {
// by restricting the input functions to value i for labelBelow
DecisionTree chosen = it->choose(*highestLabel, index);
functions.push_back(chosen);
}
// We then recurse, for all values of the highest label
NodePtr fi = compose(functions.begin(), functions.end(), label);
choiceOnHighestLabel->push_back(fi);
}
// We then recurse, for all values of the highest label
NodePtr fi = compose(functions.begin(), functions.end(), label);
choiceOnHighestLabel->push_back(fi);
return Choice::Unique(choiceOnHighestLabel);
}
return Choice::Unique(choiceOnHighestLabel);
}
/*********************************************************************************/
@ -667,9 +670,10 @@ namespace gtsam {
void DecisionTree<L, Y>::dot(const std::string& name, bool showZero) const {
std::ofstream os((name + ".dot").c_str());
dot(os, showZero);
system(
int result = system(
("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str());
}
if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed");
}
/*********************************************************************************/

168
gtsam/geometry/CalibratedCamera.cpp
View File

@ -19,88 +19,134 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/CalibratedCamera.h>
using namespace std;
namespace gtsam {
#ifndef PINHOLEBASE_LINKING_FIX
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
pose_(pose) {
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
return Dpn_pose;
}
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) :
pose_(Pose3::Expmap(v)) {
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
/**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
Dpn_point *= d;
return Dpn_point;
}
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
OptionalJacobian<2,3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
return Pose3(wRc, t);
}
/* ************************************************************************* */
Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
return Pose3(Rot3(xc, yc, zc), eye);
}
/* ************************************************************************* */
bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void PinholeBase::print(const string& s) const {
pose_.print(s + ".pose");
}
/* ************************************************************************* */
const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return Point2(P.x() / P.z(), P.y() / P.z());
return pose_;
}
/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
Point2 PinholeBase::project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
return make_pair(pn, pc.z() > 0);
}
/* ************************************************************************* */
Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt);
}
return pn;
}
/* ************************************************************************* */
Point3 PinholeBase::backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
#endif
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
double st = sin(pose2.theta()), ct = cos(pose2.theta());
Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
Rot3 wRc(x, y, z);
Point3 t(pose2.x(), pose2.y(), height);
Pose3 pose3(wRc, t);
return CalibratedCamera(pose3);
return CalibratedCamera(LevelPose(pose2, height));
}
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
const Point3& target, const Point3& upVector) {
return CalibratedCamera(LookatPose(eye, target, upVector));
}
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Matrix36 Dpose_;
Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else
Point3 q = pose_.transform_to(point);
#endif
Point2 intrinsic = project_to_camera(q);
// Check if point is in front of camera
if (q.z() <= 0)
throw CheiralityException();
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
if(Dpose && Dpoint) {
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
}
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v;
if (Dpoint) {
const Matrix3 R(pose_.rotation().matrix());
Matrix23 Dpoint_;
Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
*Dpoint = d * Dpoint_;
}
#endif
}
return intrinsic;
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint);
}
/* ************************************************************************* */

416
gtsam/geometry/CalibratedCamera.h
View File

@ -19,10 +19,14 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
#define PINHOLEBASE_LINKING_FIX
#ifdef PINHOLEBASE_LINKING_FIX
#include <gtsam/geometry/Pose2.h>
#endif
namespace gtsam {
class Point2;
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> {
public:
@ -31,6 +35,275 @@ public:
}
};
/**
* A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT PinholeBase {
private:
Pose3 pose_; ///< 3D pose of camera
#ifndef PINHOLEBASE_LINKING_FIX
protected:
/// @name Derivatives
/// @{
/**
* Calculate Jacobian with respect to pose
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
*/
static Matrix26 Dpose(const Point2& pn, double d);
/**
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Rt transposed rotation matrix
*/
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
/// @}
public:
/// @name Static functions
/// @{
/**
* Create a level pose at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static Pose3 LevelPose(const Pose2& pose2, double height);
/**
* Create a camera pose at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector);
/// @}
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBase() {
}
/** constructor with pose */
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const PinholeBase &camera, double tol = 1e-9) const;
/// print
void print(const std::string& s = "PinholeBase") const;
/// @}
/// @name Standard Interface
/// @{
/// return pose, constant version
const Pose3& pose() const {
return pose_;
}
/// return pose, with derivative
const Pose3& getPose(OptionalJacobian<6, 6> H) const;
/// @}
/// @name Transformations and measurement functions
/// @{
/**
* Project from 3D point in camera coordinates into image
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 project_to_camera(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
/// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
/**
* Project point into the image
* Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
static Point3 backproject_from_camera(const Point2& p, const double depth);
#else
public:
PinholeBase() {
}
explicit PinholeBase(const Pose3& pose) :
pose_(pose) {
}
explicit PinholeBase(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
const Pose3& pose() const {
return pose_;
}
/* ************************************************************************* */
static Matrix26 Dpose(const Point2& pn, double d) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
return Dpn_pose;
}
/* ************************************************************************* */
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
/**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
Dpn_point *= d;
return Dpn_point;
}
/* ************************************************************************* */
static Pose3 LevelPose(const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
return Pose3(wRc, t);
}
/* ************************************************************************* */
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
return Pose3(Rot3(xc, yc, zc), eye);
}
/* ************************************************************************* */
bool equals(const PinholeBase &camera, double tol=1e-9) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void print(const std::string& s) const {
pose_.print(s + ".pose");
}
/* ************************************************************************* */
const Pose3& getPose(OptionalJacobian<6, 6> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return pose_;
}
/* ************************************************************************* */
static Point2 project_to_camera(const Point3& pc,
OptionalJacobian<2, 3> Dpoint = boost::none) {
double d = 1.0 / pc.z();
const double u = pc.x() * d, v = pc.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/* ************************************************************************* */
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw);
const Point2 pn = project_to_camera(pc);
return std::make_pair(pn, pc.z() > 0);
}
/* ************************************************************************* */
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix3 Rt; // calculated by transform_to if needed
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (q.z() <= 0)
throw CheiralityException();
#endif
const Point2 pn = project_to_camera(q);
if (Dpose || Dpoint) {
const double d = 1.0 / q.z();
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt);
}
return pn;
}
/* ************************************************************************* */
static Point3 backproject_from_camera(const Point2& p,
const double depth) {
return Point3(p.x() * depth, p.y() * depth, depth);
}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
}
};
// end of class PinholeBase
/**
* A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient
@ -38,13 +311,13 @@ public:
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT CalibratedCamera {
private:
Pose3 pose_; // 6DOF pose
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
public:
enum { dimension = 6 };
enum {
dimension = 6
};
/// @name Standard Constructors
/// @{
@ -54,26 +327,40 @@ public:
}
/// construct with pose
explicit CalibratedCamera(const Pose3& pose);
explicit CalibratedCamera(const Pose3& pose) :
PinholeBase(pose) {
}
/// @}
/// @name Named Constructors
/// @{
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
* @param height specifies the height of the camera (along the positive Z-axis)
* (theta 0 = looking in direction of positive X axis)
*/
static CalibratedCamera Level(const Pose2& pose2, double height);
/**
* Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector);
/// @}
/// @name Advanced Constructors
/// @{
/// construct from vector
explicit CalibratedCamera(const Vector &v);
/// @}
/// @name Testable
/// @{
virtual void print(const std::string& s = "") const {
pose_.print(s);
}
/// check equality to another camera
bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol);
explicit CalibratedCamera(const Vector &v) :
PinholeBase(v) {
}
/// @}
@ -84,19 +371,6 @@ public:
virtual ~CalibratedCamera() {
}
/// return pose
inline const Pose3& pose() const {
return pose_;
}
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
* @param height specifies the height of the camera (along the positive Z-axis)
* (theta 0 = looking in direction of positive X axis)
*/
static CalibratedCamera Level(const Pose2& pose2, double height);
/// @}
/// @name Manifold
/// @{
@ -107,87 +381,68 @@ public:
/// Return canonical coordinate
Vector localCoordinates(const CalibratedCamera& T2) const;
/// Lie group dimensionality
/// @deprecated
inline size_t dim() const {
return 6;
}
/// Lie group dimensionality
/// @deprecated
inline static size_t Dim() {
return 6;
}
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
/// @}
/// @name Transformations and mesaurement functions
/// @{
/**
* This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image
* @param point a 3D point to be projected
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the 3D point
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const Point3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative
* @deprecated
* Use project2, which is more consistently named across Pinhole cameras
*/
static Point2 project_to_camera(const Point3& cameraPoint,
OptionalJacobian<2, 3> H1 = boost::none);
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
static Point3 backproject_from_camera(const Point2& p, const double scale);
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& pn, double depth) const {
return pose().transform_from(backproject_from_camera(pn, depth));
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
return pose().range(point, Dcamera, Dpoint);
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2);
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
return this->pose().range(pose, Dcamera, Dpose);
}
/**
* Calculate range to another camera
* @param camera Other camera
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2);
double range(const CalibratedCamera& camera, //
OptionalJacobian<1, 6> H1 = boost::none, //
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose().range(camera.pose(), H1, H2);
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{
@ -195,17 +450,22 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
}
/// @}
};
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
}

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

319
gtsam/geometry/PinholeCamera.h
View File

@ -18,32 +18,40 @@
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
#include <gtsam/geometry/PinholePose.h>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* Use PinholePose if you will not be optimizing for Calibration
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera {
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:
Pose3 pose_;
Calibration K_;
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
Calibration K_; ///< Calibration, part of class now
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value;
public:
enum { dimension = 6 + DimK };
enum {
dimension = 6 + DimK
}; ///< Dimension depends on calibration
/// @name Standard Constructors
/// @{
@ -54,12 +62,12 @@ public:
/** constructor with pose */
explicit PinholeCamera(const Pose3& pose) :
pose_(pose) {
Base(pose) {
}
/** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& K) :
pose_(pose), K_(K) {
Base(pose), K_(K) {
}
/// @}
@ -75,12 +83,7 @@ public:
*/
static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
const Pose3 pose3(wRc, t);
return PinholeCamera(pose3, K);
return PinholeCamera(Base::LevelPose(pose2, height), K);
}
/// PinholeCamera::level with default calibration
@ -99,28 +102,23 @@ public:
*/
static PinholeCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const Calibration& K = Calibration()) {
Point3 zc = target - eye;
zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
xc = xc / xc.norm();
Point3 yc = zc.cross(xc);
Pose3 pose3(Rot3(xc, yc, zc), eye);
return PinholeCamera(pose3, K);
return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeCamera(const Vector &v) {
pose_ = Pose3::Expmap(v.head(6));
if (v.size() > 6) {
K_ = Calibration(v.tail(DimK));
}
/// Init from vector, can be 6D (default calibration) or dim
explicit PinholeCamera(const Vector &v) :
Base(v.head<6>()) {
if (v.size() > 6)
K_ = Calibration(v.tail<DimK>());
}
/// Init from Vector and calibration
PinholeCamera(const Vector &v, const Vector &K) :
pose_(Pose3::Expmap(v)), K_(K) {
Base(v), K_(K) {
}
/// @}
@ -128,14 +126,14 @@ public:
/// @{
/// assert equality up to a tolerance
bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol)
&& K_.equals(camera.calibration(), tol);
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
}
/// print
void print(const std::string& s = "PinholeCamera") const {
pose_.print(s + ".pose");
Base::print(s);
K_.print(s + ".calibration");
}
@ -147,31 +145,21 @@ public:
}
/// return pose
inline Pose3& pose() {
return pose_;
}
/// return pose, constant version
inline const Pose3& pose() const {
return pose_;
const Pose3& pose() const {
return Base::pose();
}
/// return pose, with derivative
inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->block(0, 0, 6, 6) = I_6x6;
}
return pose_;
return Base::pose();
}
/// return calibration
inline Calibration& calibration() {
return K_;
}
/// return calibration
inline const Calibration& calibration() const {
const Calibration& calibration() const {
return K_;
}
@ -179,13 +167,13 @@ public:
/// @name Manifold
/// @{
/// Manifold dimension
inline size_t dim() const {
/// @deprecated
size_t dim() const {
return dimension;
}
/// Manifold dimension
inline static size_t Dim() {
/// @deprecated
static size_t Dim() {
return dimension;
}
@ -194,9 +182,9 @@ public:
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == 6)
return PinholeCamera(pose().retract(d), calibration());
return PinholeCamera(this->pose().retract(d), calibration());
else
return PinholeCamera(pose().retract(d.head(6)),
return PinholeCamera(this->pose().retract(d.head(6)),
calibration().retract(d.tail(calibration().dim())));
}
@ -204,7 +192,7 @@ public:
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = pose().localCoordinates(T2.pose());
d.head(6) = this->pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
return d;
}
@ -218,32 +206,6 @@ public:
/// @name Transformations and measurement functions
/// @{
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/
static Point2 project_to_camera(const Point3& P, //
OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
#endif
double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d;
if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v);
}
/// Project a point into the image and check depth
inline std::pair<Point2, bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
}
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image
@ -252,31 +214,25 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
if (Dpose || Dpoint) {
const double z = pc.z(), d = 1.0 / z;
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
// uncalibration
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
if (Dpose)
calculateDpose(pn, d, Dpi_pn, *Dpose);
if (Dpoint)
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
return pi;
} else
return K_.uncalibrate(pn, Dcal);
return pi;
}
/** project a point at infinity from world coordinate to the image
@ -285,20 +241,19 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 projectPointAtInfinity(const Point3& pw,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = project_to_camera(pc); // project the point to the camera
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix36 Dpc_pose;
Dpc_pose.setZero();
@ -306,7 +261,7 @@ public:
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix2 Dpi_pn; // 2*2
@ -323,65 +278,40 @@ public:
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
// project to normalized coordinates
Matrix26 Dpose;
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
if (!Dcamera && !Dpoint) {
return K_.uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibrate to pixel coordinates
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
Dcamera || Dpoint ? &Dpi_pn : 0);
// uncalibration
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// If needed, calculate derivatives
if (Dcamera)
*Dcamera << Dpi_pn * Dpose, Dcal;
if (Dpoint)
*Dpoint = Dpi_pn * (*Dpoint);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
(*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
inline Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
return pose_.transform_from(pc);
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
inline Point3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = K_.calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose_.rotation().rotate(pc);
return pi;
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double)
*/
double range(
const Point3& point, //
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
@ -390,16 +320,12 @@ public:
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double)
*/
double range(
const Pose3& pose, //
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
@ -408,26 +334,21 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
template<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const {
boost::optional<Matrix&> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6+CalibrationB::dimension);
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
}
@ -437,12 +358,9 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
double range(
const CalibratedCamera& camera, //
double range(const CalibratedCamera& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
@ -450,65 +368,26 @@ public:
private:
/**
* Calculate Jacobian with respect to pose
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpose Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
Eigen::MatrixBase<Derived> const & Dpose) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v;
Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
Dpi_pn * Dpn_pose;
}
/**
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @param d disparity (inverse depth)
* @param Dpi_pn derivative of uncalibrate with respect to pn
* @param Dpoint Output argument, can be matrix or block, assumed right size !
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/
template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point;
Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn * Dpn_point;
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
template<typename Calibration>
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
template<typename Calibration>
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
template<typename Calibration>
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
} // \ gtsam

344
gtsam/geometry/PinholePose.h Normal file
View File

@ -0,0 +1,344 @@
/* ----------------------------------------------------------------------------
* 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 PinholePose.h
* @brief Pinhole camera with known calibration
* @author Yong-Dian Jian
* @author Frank Dellaert
* @date Feb 20, 2015
*/
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
public :
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeBaseK() {
}
/** constructor with pose */
explicit PinholeBaseK(const Pose3& pose) :
PinholeBase(pose) {
}
/// @}
/// @name Advanced Constructors
/// @{
explicit PinholeBaseK(const Vector &v) :
PinholeBase(v) {
}
/// @}
/// @name Standard Interface
/// @{
virtual ~PinholeBaseK() {
}
/// return calibration
virtual const Calibration& calibration() const = 0;
/// @}
/// @name Transformations and measurement functions
/// @{
/// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
pn.first = calibration().uncalibrate(pn.first);
return pn;
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinates
*/
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, boost::none,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose) *Dpose = Dpi_pn * (*Dpose);
if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
return pi;
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = calibration().calibrate(p);
return pose().transform_from(backproject_from_camera(pn, depth));
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Point3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose().rotation().rotate(pc);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
return pose().range(point, Dcamera, Dpoint);
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
return this->pose().range(pose, Dcamera, Dpose);
}
/**
* Calculate range to a CalibratedCamera
* @param camera Other camera
* @return range (double)
*/
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
/**
* Calculate range to a PinholePoseK derived class
* @param camera Other camera
* @return range (double)
*/
template<class CalibrationB>
double range(const PinholeBaseK<CalibrationB>& camera,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
}
};
// end of class PinholeBaseK
/**
* A pinhole camera class that has a Pose3 and a *fixed* Calibration.
* Instead of using this class, one might consider calibrating the measurements
* and using CalibratedCamera, which would then be faster.
* @addtogroup geometry
* \nosubgrouping
*/
template<typename Calibration>
class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
public:
enum {
dimension = 6
}; ///< There are 6 DOF to optimize for
/// @name Standard Constructors
/// @{
/** default constructor */
PinholePose() {
}
/** constructor with pose, uses default calibration */
explicit PinholePose(const Pose3& pose) :
Base(pose), K_(new Calibration()) {
}
/** constructor with pose and calibration */
PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
Base(pose), K_(K) {
}
/// @}
/// @name Named Constructors
/// @{
/**
* Create a level camera at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static PinholePose Level(const boost::shared_ptr<Calibration>& K,
const Pose2& pose2, double height) {
return PinholePose(Base::LevelPose(pose2, height), K);
}
/// PinholePose::level with default calibration
static PinholePose Level(const Pose2& pose2, double height) {
return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
}
/**
* Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
* @param K optional calibration parameter
*/
static PinholePose Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const boost::shared_ptr<Calibration>& K =
boost::make_shared<Calibration>()) {
return PinholePose(Base::LookatPose(eye, target, upVector), K);
}
/// @}
/// @name Advanced Constructors
/// @{
/// Init from 6D vector
explicit PinholePose(const Vector &v) :
Base(v), K_(new Calibration()) {
}
/// Init from Vector and calibration
PinholePose(const Vector &v, const Vector &K) :
Base(v), K_(new Calibration(K)) {
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
}
/// print
void print(const std::string& s = "PinholePose") const {
Base::print(s);
K_->print(s + ".calibration");
}
/// @}
/// @name Standard Interface
/// @{
virtual ~PinholePose() {
}
/// return calibration
virtual const Calibration& calibration() const {
return *K_;
}
/// @}
/// @name Manifold
/// @{
/// @deprecated
size_t dim() const {
return 6;
}
/// @deprecated
static size_t Dim() {
return 6;
}
/// move a cameras according to d
PinholePose retract(const Vector6& d) const {
return PinholePose(Base::pose().retract(d), K_);
}
/// return canonical coordinate
Vector6 localCoordinates(const PinholePose& p) const {
return Base::pose().localCoordinates(p.Base::pose());
}
/// for Canonical
static PinholePose identity() {
return PinholePose(); // assumes that the default constructor is valid
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
// end of class PinholePose
template<typename Calibration>
struct traits<PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
};
template<typename Calibration>
struct traits<const PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
};
} // \ gtsam

51
gtsam/geometry/StereoCamera.cpp
View File

@ -29,16 +29,15 @@ namespace gtsam {
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,6> H3) const {
StereoPoint2 StereoCamera::project(const Point3& point) const {
return project2(point);
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project2(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
#else
// omit derivatives
const Point3 q = leftCamPose_.transform_to(point);
#endif
if ( q.z() <= 0 ) throw StereoCheiralityException();
@ -56,12 +55,6 @@ namespace gtsam {
// check if derivatives need to be computed
if (H1 || H2) {
#ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule
Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2);
#else
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
@ -76,10 +69,6 @@ namespace gtsam {
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
*H2 << d * (*H2);
}
#endif
if (H3)
// TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
H3->setZero();
}
// finally translate
@ -87,15 +76,23 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
Matrix3 m;
m << f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y();
return m;
StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,0> H3) const {
if (H3)
throw std::runtime_error(
"StereoCamera::project does not support third derivative - BTW use project2");
return project2(point,H1,H2);
}
/* ************************************************************************* */
Point3 StereoCamera::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();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
}

86
gtsam/geometry/StereoCamera.h
View File

@ -17,9 +17,6 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>
@ -28,32 +25,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 +74,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,42 +114,46 @@ public:
/// @name Standard Interface
/// @{
/// pose
const Pose3& pose() const {
return leftCamPose_;
}
/// baseline
double baseline() const {
return K_->baseline();
}
/*
* project 3D point and compute optional derivatives
/// Project 3D point to StereoPoint2 (uL,uR,v)
StereoPoint2 project(const Point3& point) const;
/** Project 3D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
*/
StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const;
/// @}
/// @name Deprecated
/// @{
/** Project 3D point and compute optional derivatives
* @deprecated, use project2 - this class has fixed calibration
* @param H1 derivative with respect to pose
* @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;
/**
*
*/
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();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
boost::none) const;
/// @}
private:
/** utility functions */
Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
template<class Archive>
@ -147,8 +165,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> {
};
}

24
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -29,6 +29,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) {
}
/* ************************************************************************* */
static Point2 project2(const Pose3& pose, const Point3& point) {
return CalibratedCamera(pose).project(point);
static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
return camera.project(point);
}
TEST( CalibratedCamera, Dproject_point_pose)
{
Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
CHECK(assert_equal(Point2(-.16, .16), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint;
camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

22
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -15,12 +15,14 @@
* @brief test PinholeCamera class
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2)
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( PinholeCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);

259
gtsam/geometry/tests/testPinholePose.cpp Normal file
View File

@ -0,0 +1,259 @@
/* ----------------------------------------------------------------------------
* 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 testPinholePose.cpp
* @author Frank Dellaert
* @brief test PinholePose class
* @date Feb 20, 2015
*/
#include <gtsam/geometry/PinholePose.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
typedef PinholePose<Cal3_S2> Camera;
static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1, K);
static const Point3 point1(-0.08,-0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
static const Point3 point3( 0.08, 0.08, 0.0);
static const Point3 point4( 0.08,-0.08, 0.0);
static const Point3 point1_inf(-0.16,-0.16, -1.0);
static const Point3 point2_inf(-0.16, 0.16, -1.0);
static const Point3 point3_inf( 0.16, 0.16, -1.0);
static const Point3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholePose, constructor)
{
EXPECT(assert_equal( pose, camera.pose()));
}
//******************************************************************************
TEST(PinholeCamera, Pose) {
Matrix actualH;
EXPECT(assert_equal(pose, camera.getPose(actualH)));
// Check derivative
boost::function<Pose3(Camera)> f = //
boost::bind(&Camera::getPose,_1,boost::none);
Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
/* ************************************************************************* */
TEST( PinholePose, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
}
/* ************************************************************************* */
TEST( PinholePose, project)
{
EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) ));
EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) ));
EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) ));
EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) ));
}
/* ************************************************************************* */
TEST( PinholePose, backproject)
{
EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
}
/* ************************************************************************* */
TEST( PinholePose, backprojectInfinity)
{
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
}
/* ************************************************************************* */
TEST( PinholePose, backproject2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(x.second);
}
/* ************************************************************************* */
static Point2 project3(const Pose3& pose, const Point3& point,
const Cal3_S2::shared_ptr& cal) {
return Camera(pose, cal).project2(point);
}
/* ************************************************************************* */
TEST( PinholePose, Dproject)
{
Matrix Dpose, Dpoint;
Point2 result = camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
EXPECT(assert_equal(Point2(-100, 100), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point);
}
/* ************************************************************************* */
TEST( PinholePose, Dproject2)
{
Matrix Dcamera, Dpoint;
Point2 result = camera.project2(point1, Dcamera, Dpoint);
Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
EXPECT(assert_equal(result, Point2(-100, 100) ));
EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);
}
/* ************************************************************************* */
TEST( PinholePose, range0) {
Matrix D1; Matrix D2;
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static double range1(const Camera& camera, const Pose3& pose) {
return camera.range(pose);
}
/* ************************************************************************* */
TEST( PinholePose, range1) {
Matrix D1; Matrix D2;
double result = camera.range(pose1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
typedef PinholePose<Cal3Bundler> Camera2;
static const boost::shared_ptr<Cal3Bundler> K2 =
boost::make_shared<Cal3Bundler>(625, 1e-3, 1e-3);
static const Camera2 camera2(pose1, K2);
static double range2(const Camera& camera, const Camera2& camera2) {
return camera.range<Cal3Bundler>(camera2);
}
/* ************************************************************************* */
TEST( PinholePose, range2) {
Matrix D1; Matrix D2;
double result = camera.range<Cal3Bundler>(camera2, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static const CalibratedCamera camera3(pose1);
static double range3(const Camera& camera, const CalibratedCamera& camera3) {
return camera.range(camera3);
}
/* ************************************************************************* */
TEST( PinholePose, range3) {
Matrix D1; Matrix D2;
double result = camera.range(camera3, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -16,8 +16,6 @@
* @date Feb 7, 2012
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/EssentialMatrix.h>

10
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -15,13 +15,13 @@
* @brief test SimpleCamera class
*/
#include <cmath>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;

8
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_
TEST( StereoCamera, Dproject)
{
Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
CHECK(assert_equal(expected1,actual1,1e-7));
Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
CHECK(assert_equal(expected2,actual2,1e-7));
Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
// CHECK(assert_equal(expected3,actual3,1e-8));
}
/* ************************************************************************* */

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

1
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>

49
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
@ -6,16 +17,18 @@
#pragma once
#include "JacobianSchurFactor.h"
#include "RegularJacobianFactor.h"
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorQ: public RegularJacobianFactor<D> {
typedef JacobianSchurFactor<D, ZDim> Base;
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public:
@ -24,37 +37,45 @@ public:
}
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
JacobianFactorQ(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() {
JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, const SharedDiagonal& model =
SharedDiagonal()) :
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
// TODO: can we do better ?
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector < KeyMatrix > QF;
std::vector<KeyMatrix> QF;
QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
QF.push_back(
KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
// Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model);
}
};
// end class JacobianFactorQ
// traits
template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
JacobianFactorQ<D, ZDim> > {
};
}

29
gtsam/slam/JacobianFactorQR.h
View File

@ -6,31 +6,38 @@
*/
#pragma once
#include <gtsam/slam/JacobianSchurFactor.h>
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam {
class GaussianBayesNet;
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorQR: public RegularJacobianFactor<D> {
typedef JacobianSchurFactor<D, ZDim> Base;
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public:
/**
* Constructor
*/
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
const Matrix3& P, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D, ZDim>() {
Base() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
b.segment<ZDim>(ZDim * i), model);
i += 1;
@ -38,9 +45,9 @@ public:
//gfg.print("gfg");
// eliminate the point
GaussianBayesNet::shared_ptr bn;
boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg;
std::vector < Key > variables;
std::vector<Key> variables;
variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg");
@ -48,6 +55,6 @@ public:
JacobianFactor::operator=(JacobianFactor(*fg));
}
};
// class
// end class JacobianFactorQR
}// gtsam
}// end namespace gtsam

41
gtsam/slam/JacobianFactorSVD.h
View File

@ -5,50 +5,57 @@
*/
#pragma once
#include "gtsam/slam/JacobianSchurFactor.h"
#include "gtsam/slam/RegularJacobianFactor.h"
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
class JacobianFactorSVD: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
/// Default constructor
JacobianFactorSVD() {}
JacobianFactorSVD() {
}
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
JacobianFactorSVD(const FastVector<Key>& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
const Matrix& Enull, const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t j = 0, m2 = ZDim*numKeys-3;
size_t j = 0, m2 = ZDim * numKeys - 3;
// PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
// BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
QF.push_back(
KeyMatrix(it.first,
(Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
}
};

93
gtsam/slam/JacobianSchurFactor.h
View File

@ -1,93 +0,0 @@
/*
* @file JacobianSchurFactor.h
* @brief Jacobianfactor that combines and eliminates points
* @date Oct 27, 2013
* @uthor Frank Dellaert
*/
#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 {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D, size_t ZDim>
class JacobianSchurFactor: public JacobianFactor {
public:
typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
/**
* @brief double* Matrix-vector multiply, i.e. y = A*x
* RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
*/
Vector operator*(const double* x) const {
Vector Ax = zero(Ab_.rows());
if (empty()) return Ax;
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
return model_ ? model_->whiten(Ax) : Ax;
}
/**
* @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
* RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
*/
void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const
{
Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos)
DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const {
JacobianFactor::multiplyHessianAdd(alpha,x,y);
}
/**
* @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
* RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
*/
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
// Vector Ax = (*this)*x;
// this->transposeMultiplyAdd(alpha,Ax,y);
if (empty()) return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; 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); }
// 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)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
}; // class
} // gtsam

75
gtsam/slam/RegularHessianFactor.h
View File

@ -10,10 +10,10 @@
* -------------------------------------------------------------------------- */
/**
* @file RegularHessianFactor.h
* @brief HessianFactor class with constant sized blcoks
* @author Richard Roberts
* @date Dec 8, 2010
* @file RegularHessianFactor.h
* @brief HessianFactor class with constant sized blocks
* @author Sungtae An
* @date March 2014
*/
#pragma once
@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor {
private:
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
typedef Eigen::Matrix<double, D, 1> VectorD;
// Use Eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
@ -43,6 +45,15 @@ public:
HessianFactor(js, Gs, gs, f) {
}
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
}
/** Constructor with an arbitrary number of keys and with the augmented information matrix
* specified as a block matrix. */
template<typename KEYS>
@ -52,25 +63,22 @@ public:
}
// Scratch space for multiplyHessianAdd
typedef Eigen::Matrix<double, D, 1> DVector;
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)
yi.setZero();
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
@ -95,14 +103,10 @@ public:
}
}
/// Raw memory version, with offsets TODO document reasoning
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// Create a vector of temporary y values, corresponding to rows i
std::vector<Vector> y;
y.reserve(size());
@ -131,47 +135,42 @@ 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;
DVector dj = -info_(pos, size()).knownOffDiagonal();
DMap(d + D * j) += dj;
}
}
/* ************************************************************************* */
};
// end class RegularHessianFactor
// traits
template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
RegularHessianFactor<D> > {
};
}

32
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 {
@ -89,18 +87,27 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
std::cout << " E_ \n" << E_ << std::endl;
std::cout << " b_ \n" << b_.transpose() << std::endl;
for (size_t pos = 0; pos < size(); ++pos) {
std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl;
}
std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
std::cout << "E:\n" << E_ << std::endl;
std::cout << "b:\n" << b_.transpose() << std::endl;
}
/// equals
bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
return false;
else {
const This* f = dynamic_cast<const This*>(&lf);
if (!f)
return false;
for (size_t pos = 0; pos < size(); ++pos) {
if (keys_[pos] != f->keys_[pos]) return false;
if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
}
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
&& equal_with_abs_tol(E_, f->E_, tol)
&& equal_with_abs_tol(b_, f->b_, tol);
}
/// Degrees of freedom of camera
@ -460,7 +467,12 @@ public:
};
// RegularImplicitSchurFactor
// end class RegularImplicitSchurFactor
// traits
template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
RegularImplicitSchurFactor<D> > {
};
}

192
gtsam/slam/RegularJacobianFactor.h
View File

@ -21,25 +21,34 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <vector>
namespace gtsam {
/**
* JacobianFactor with constant sized blocks
* Provides raw memory access versions of linear operator.
* Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
*/
template<size_t D>
class RegularJacobianFactor: public JacobianFactor {
private:
/** Use eigen magic to access raw memory */
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
/// Default constructor
RegularJacobianFactor() {}
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
* collection of keys and matrices making up the factor.
* TODO Verify terms are regular
*/
template<typename TERMS>
RegularJacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
@ -49,91 +58,66 @@ public:
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
* factor.
* TODO Verify complies to regular
*/
template<typename KEYS>
RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal()) :
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
SharedDiagonal()) :
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
* then accumulatedDims is [0 3 9 11 13]
* NOTE: size of accumulatedDims is size of keys + 1!! */
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const {
/// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
typedef Eigen::Map<VectorD> MapD;
typedef Eigen::Map<const VectorD> ConstMapD;
/** y += alpha * A'*A*x */
virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const {
JacobianFactor::multiplyHessianAdd(alpha, x, y);
}
/**
* @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
* RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
*/
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty())
return;
Vector Ax = zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
{
Ax += Ab_(pos)
* ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
}
/// Deal with noise properly, need to Double* whiten as we are dividing by variance
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
/// multiply with alpha
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos){
MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
}
/** Raw memory access version of multiplyHessianAdd */
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty()) return;
Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; 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); }
// 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;
}
/** Raw memory access version of hessianDiagonal
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void hessianDiagonal(double* d) const {
/// Expose base class hessianDiagonal
virtual VectorValues hessianDiagonal() const {
return JacobianFactor::hessianDiagonal();
}
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const {
// Loop over all variables in the factor
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();
}
}
@ -141,10 +125,13 @@ public:
}
}
/** Raw memory access version of gradientAtZero
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void gradientAtZero(double* d) const {
/// Expose base class gradientAtZero
virtual VectorValues gradientAtZero() const {
return JacobianFactor::gradientAtZero();
}
/// Raw memory access version of gradientAtZero
void gradientAtZero(double* d) const {
// Get vector b not weighted
Vector b = getb();
@ -156,19 +143,90 @@ public:
}
// Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DVector dj;
// gradient -= A'*b/sigma^2
// Computing with each column
for (size_t k = 0; k < D; ++k){
for (size_t k = 0; k < D; ++k) {
Vector column_k = Ab_(j).col(k);
dj(k) = -1.0*dot(b, column_k);
dj(k) = -1.0 * dot(b, column_k);
}
DMap(d + D * j) += dj;
}
}
/**
* @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
* RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
*/
void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
}
/**
* @brief double* Matrix-vector multiply, i.e. y = A*x
* RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
*/
Vector operator*(const double* x) const {
Vector Ax = zero(Ab_.rows());
if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
return model_ ? model_->whiten(Ax) : Ax;
}
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
* then accumulatedDims is [0 3 9 11 13]
* NOTE: size of accumulatedDims is size of keys + 1!!
* TODO Frank asks: why is this here if not regular ????
*/
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const {
/// Use Eigen magic to access raw memory
typedef Eigen::Map<Vector> VectorMap;
typedef Eigen::Map<const Vector> ConstVectorMap;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
for (size_t pos = 0; pos < size(); ++pos) {
size_t offset = accumulatedDims[keys_[pos]];
size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
}
/// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
/// multiply with alpha
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos) {
size_t offset = accumulatedDims[keys_[pos]];
size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
}
}
};
// end class RegularJacobianFactor
}
}// end namespace gtsam

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

1
gtsam/slam/expressions.h
View File

@ -8,6 +8,7 @@
#pragma once
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>

14
gtsam/slam/tests/testDataset.cpp
View File

@ -15,16 +15,16 @@
* @author Richard Roberts, Luca Carlone
*/
#include <CppUnitLite/TestHarness.h>
#include <boost/algorithm/string.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/algorithm/string.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam::symbol_shorthand;
using namespace std;

1
gtsam/slam/tests/testLago.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>

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

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

3
tests/testManifold.cpp
View File

@ -17,9 +17,8 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/VectorSpace.h>

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;