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

Merge branch 'develop' into feature/cleanup_ImuFactor

release/4.3a0
Duy-Nguyen Ta 2015-09-17 17:42:38 -04:00
parent 6fb453e725 8e2915c4f3
commit aa2ffcd118
21 changed files with 1037 additions and 885 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
README.md
View File

@ -31,6 +31,7 @@ Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

4
examples/SFMExample_SmartFactor.cpp
View File

@ -58,7 +58,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
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);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

14
gtsam.h
View File

@ -916,7 +916,7 @@ class StereoCamera {
// Standard Interface
gtsam::Pose3 pose() const;
double baseline() const;
gtsam::Cal3_S2Stereo* calibration() const;
gtsam::Cal3_S2Stereo calibration() const;
// Manifold
gtsam::StereoCamera retract(const Vector& d) const;
@ -2364,15 +2364,17 @@ class SmartProjectionParams {
template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const CALIBRATION* K);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i);
void add(const gtsam::Point2& measured_i, size_t poseKey_i);
// enabling serialization functionality
//void serialize() const;

2
gtsam/geometry/CameraSet.h
View File

@ -161,7 +161,7 @@ public:
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = //
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim

4
gtsam/geometry/StereoCamera.h
View File

@ -66,8 +66,8 @@ public:
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
/// Return shared pointer to calibration
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
const Cal3_S2Stereo& calibration() const {
return *K_;
}
/// @}

106
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -18,9 +18,15 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
@ -273,6 +279,106 @@ TEST( triangulation, onePose) {
TriangulationUnderconstrainedException);
}
//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {
Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
// two camera poses m1, m2
Matrix4 m1, m2;
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779,
0.592783835, -0.77156583, 0.230856632, 66.2186159,
0.116517574, -0.201470143, -0.9725393, -4.28382528,
0, 0, 0, 1;
m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799,
-0.29277519, 0.947083213, 0.131587097, 65.843136,
-0.0206094928, 0.131334858, -0.991123524, -4.3525033,
0, 0, 0, 1;
typedef CameraSet<StereoCamera> Cameras;
Cameras cameras;
cameras.push_back(StereoCamera(Pose3(m1), stereoK));
cameras.push_back(StereoCamera(Pose3(m2), stereoK));
vector<StereoPoint2> measurements;
measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973);
Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
Point3 actual = triangulateNonlinear(cameras, measurements, initial);
Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187
EXPECT(assert_equal(expected, actual, 1e-4));
// regular stereo factor comparison - expect very similar result as above
{
typedef GenericStereoFactor<Pose3,Point3> StereoFactor;
Values values;
values.insert(Symbol('x', 1), Pose3(m1));
values.insert(Symbol('x', 2), Pose3(m2));
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK)));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK)));
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.push_back(PriorFactor<Pose3>(Symbol('x',1), Pose3(m1), posePrior));
graph.push_back(PriorFactor<Pose3>(Symbol('x',2), Pose3(m2), posePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
// use Triangulation Factor directly - expect same result as above
{
Values values;
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[0], measurements[0], unit, Symbol('l',1)));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[1], measurements[1], unit, Symbol('l',1)));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
// use ExpressionFactor - expect same result as above
{
Values values;
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
Expression<Point3> point_(Symbol('l',1));
Expression<StereoCamera> camera0_(cameras[0]);
Expression<StereoCamera> camera1_(cameras[1]);
Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);
graph.addExpressionFactor(unit, measurements[0], project0_);
graph.addExpressionFactor(unit, measurements[1], project1_);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
}
//******************************************************************************
int main() {
TestResult tr;

19
gtsam/geometry/triangulation.h
View File

@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
/**
* Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses
* @param sharedCal shared pointer to single calibration object
* @param sharedCal shared pointer to single calibration object (monocular only!)
* @param measurements 2D measurements
* @param landmarkKey to refer to landmark
* @param initialEstimate
@ -97,7 +97,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
/**
* Create a factor graph with projection factors from pinhole cameras
* (each camera has a pose and calibration)
* @param cameras pinhole cameras
* @param cameras pinhole cameras (monocular or stereo)
* @param measurements 2D measurements
* @param landmarkKey to refer to landmark
* @param initialEstimate
@ -106,22 +106,21 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measurements, Key landmarkKey,
const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
const Point3& initialEstimate) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
(camera_i, measurements[i], unit2, landmarkKey));
(camera_i, measurements[i], unit, landmarkKey));
}
return std::make_pair(graph, values);
}
/// PinholeCamera specific version
/// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
/**
* Given an initial estimate , refine a point using measurements in several cameras
* @param cameras pinhole cameras
* @param cameras pinhole cameras (monocular or stereo)
* @param measurements 2D measurements
* @param initialEstimate
* @return refined Point3
@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
template<class CAMERA>
Point3 triangulateNonlinear(
const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
@ -184,7 +183,7 @@ Point3 triangulateNonlinear(
return optimize(graph, values, Symbol('p', 0));
}
/// PinholeCamera specific version
/// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION>
Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,

61
gtsam/slam/SmartFactorBase.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
*/
#pragma once
@ -79,22 +80,6 @@ protected:
typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
// check that noise model is isotropic and the same
// TODO, this is hacky, we should just do this via constructor, not add
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
if (!sharedNoiseModel)
return;
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
if (!noiseModel_)
noiseModel_ = sharedIsotropic;
else if (!sharedIsotropic->equals(*noiseModel_))
throw std::runtime_error(
"SmartFactorBase: cannot add measurements with different noise model");
}
public:
// Definitions for blocks of F, externally visible
@ -109,8 +94,21 @@ public:
typedef CameraSet<CAMERA> Cameras;
/// Constructor
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor){}
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor){
if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
noiseModel_ = sharedIsotropic;
}
/// Virtual destructor, subclasses from NonlinearFactor
virtual ~SmartFactorBase() {
@ -122,34 +120,18 @@ public:
* @param poseKey is the index corresponding to the camera observing the landmark
* @param sharedNoiseModel is the measurement noise
*/
void add(const Z& measured_i, const Key& cameraKey_i,
const SharedNoiseModel& sharedNoiseModel) {
void add(const Z& measured_i, const Key& cameraKey_i) {
this->measured_.push_back(measured_i);
this->keys_.push_back(cameraKey_i);
maybeSetNoiseModel(sharedNoiseModel);
}
/**
* Add a bunch of measurements, together with the camera keys and noises
* Add a bunch of measurements, together with the camera keys
*/
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
std::vector<SharedNoiseModel>& noises) {
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noises.at(i));
}
}
/**
* Add a bunch of measurements and use the same noise model for all of them
*/
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noise);
}
}
@ -158,11 +140,10 @@ public:
* The noise is assumed to be the same for all measurements
*/
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
maybeSetNoiseModel(noise);
}
}
@ -198,7 +179,7 @@ public:
}
if(body_P_sensor_)
body_P_sensor_->print("body_P_sensor_:\n");
print("", keyFormatter);
Base::print("", keyFormatter);
}
/// equals

16
gtsam/slam/SmartProjectionFactor.h
View File

@ -44,23 +44,21 @@ enum DegeneracyMode {
/*
* Parameters for the smart projection factors
*/
class GTSAM_EXPORT SmartProjectionParams {
public:
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
mutable TriangulationParameters triangulation;
const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
@ -161,10 +159,10 @@ public:
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionFactor(
SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor), params_(params), //
Base(sharedNoiseModel, body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) {
}

6
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -61,14 +61,16 @@ public:
/**
* Constructor
* @param Isotropic measurement noise
* @param K (fixed) calibration, assumed to be the same for all cameras
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor, params), K_(K) {
Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
}
/** Virtual destructor */

29
gtsam/slam/TriangulationFactor.h
View File

@ -18,6 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
namespace gtsam {
@ -42,9 +43,12 @@ protected:
/// shorthand for this class
typedef TriangulationFactor<CAMERA> This;
/// shorthand for measurement type, e.g. Point2 or StereoPoint2
typedef typename CAMERA::Measurement Measurement;
// Keep a copy of measurement and calibration for I/O
const CAMERA camera_; ///< CAMERA in which this landmark was seen
const Point2 measured_; ///< 2D measurement
const Measurement measured_; ///< 2D measurement
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@ -69,14 +73,17 @@ public:
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
TriangulationFactor(const CAMERA& camera, const Point2& measured,
TriangulationFactor(const CAMERA& camera, const Measurement& measured,
const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
throwCheirality), verboseCheirality_(verboseCheirality) {
if (model && model->dim() != 2)
if (model && model->dim() != Measurement::dimension)
throw std::invalid_argument(
"TriangulationFactor must be created with 2-dimensional noise model.");
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) Measurement::dimension)
+ "-dimensional noise model.");
}
/** Virtual destructor */
@ -113,18 +120,18 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Point2 error(camera_.project2(point, boost::none, H2) - measured_);
Measurement error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
} catch (CheiralityException& e) {
if (H2)
*H2 = zeros(2, 3);
*H2 = zeros(Measurement::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return ones(2) * 2.0 * camera_.calibration().fx();
return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
}
}
@ -146,9 +153,9 @@ public:
// Allocate memory for Jacobian factor, do only once
if (Ab.rows() == 0) {
std::vector<size_t> dimensions(1, 3);
Ab = VerticalBlockMatrix(dimensions, 2, true);
A.resize(2,3);
b.resize(2);
Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true);
A.resize(Measurement::dimension,3);
b.resize(Measurement::dimension);
}
// Would be even better if we could pass blocks to project
@ -164,7 +171,7 @@ public:
}
/** return the measurement */
const Point2& measured() const {
const Measurement& measured() const {
return measured_;
}

25
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -16,17 +16,24 @@
* @date Feb 2015
*/
#include "../SmartFactorBase.h"
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
};
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f;
f.add(Point2(), 1, SharedNoiseModel());
f.add(Point2(), 2, SharedNoiseModel());
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
f.add(Point2(), 2);
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase<StereoCamera> {
};
TEST(SmartFactorBase, Stereo) {
StereoFactor f;
f.add(StereoPoint2(), 1, SharedNoiseModel());
f.add(StereoPoint2(), 2, SharedNoiseModel());
StereoFactor f(unit3);
f.add(StereoPoint2(), 1);
f.add(StereoPoint2(), 2);
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}

160
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
SmartFactor factor1(unit2, boost::none, params);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor3) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
factor1.add(measurement1, x1, unit2);
SmartFactor factor1(unit2, boost::none, params);
factor1.add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Equals ) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1);
SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
factor2->add(measurement1, x1);
}
/* *************************************************************************/
@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) {
Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
values.insert(c2, perturbed_level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
// Point is now uninitialized before a triangulation event
EXPECT(!factor1->point());
@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) {
Vector actual = factor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
vector<Point2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(unit2);
noises.push_back(unit2);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
factor2->add(measurements, views, noises);
factor2->add(measurements, views);
double actualError2 = factor2->error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create and fill smartfactors
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
smartFactor1->add(measurements_cam1, views, unit2);
smartFactor2->add(measurements_cam2, views, unit2);
smartFactor3->add(measurements_cam3, views, unit2);
smartFactor1->add(measurements_cam1, views);
smartFactor2->add(measurements_cam2, views);
smartFactor3->add(measurements_cam3, views);
// Create factor graph and add priors on two cameras
NonlinearFactorGraph graph;
@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(track1, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(track2, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(track2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
double actualError = factor1->error(values);
@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
smartGraph.push_back(factor1);
double expectedError = factor1->error(values);
double expectedErrorGraph = smartGraph.error(values);
@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
smartGraph.push_back(factor1);
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
Matrix expectedE;
Vector expectedb;
@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
params.setEnableEPI(useEPI);
SmartFactor::shared_ptr explicitFactor(
new SmartFactor(boost::none, params));
explicitFactor->add(level_uv, c1, unit2);
explicitFactor->add(level_uv_right, c2, unit2);
new SmartFactor(unit2, boost::none, params));
explicitFactor->add(level_uv, c1);
explicitFactor->add(level_uv_right, c2);
GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
values);
@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
// Implicit Schur version
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
SmartFactor::shared_ptr implicitFactor(
new SmartFactor(boost::none, params));
implicitFactor->add(level_uv, c1, unit2);
implicitFactor->add(level_uv_right, c2, unit2);
new SmartFactor(unit2, boost::none, params));
implicitFactor->add(level_uv, c1);
implicitFactor->add(level_uv_right, c2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
implicitFactor->linearize(values);
CHECK(gaussianImplicitSchurFactor);

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

@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams;
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
}
/* ************************************************************************* */
@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, boost::none, params);
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor3) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
factor1->add(measurement1, x1, model);
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
}
/* ************************************************************************* */
@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(sharedK, boost::none, params);
factor1.add(measurement1, x1, model);
SmartFactor factor1(model, sharedK, boost::none, params);
factor1.add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Equals ) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
factor1->add(measurement1, x1, model);
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
SmartFactor::shared_ptr factor2(new SmartFactor(sharedK));
factor2->add(measurement1, x1, model);
SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
factor2->add(measurement1, x1);
CHECK(assert_equal(*factor1, *factor2));
}
@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
SmartFactor factor(sharedK);
factor.add(level_uv, x1, model);
factor.add(level_uv_right, x2, model);
SmartFactor factor(model, sharedK);
factor.add(level_uv, x1);
factor.add(level_uv_right, x2);
Values values; // it's a pose factor, hence these are poses
values.insert(x1, cam1.pose());
@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) {
Point3(0.5, 0.1, 0.3));
values.insert(x2, pose_right.compose(noise_pose));
SmartFactor::shared_ptr factor(new SmartFactor((sharedK)));
factor->add(level_uv, x1, model);
factor->add(level_uv_right, x2, model);
SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
factor->add(level_uv, x1);
factor->add(level_uv_right, x2);
double actualError1 = factor->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor((sharedK)));
SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
vector<Point2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(model);
noises.push_back(model);
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
factor2->add(measurements, views, noises);
factor2->add(measurements, views);
double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}
@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, sensor_to_body, params);
smartFactor1.add(measurements_cam1, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
smartFactor1.add(measurements_cam1, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, sensor_to_body, params);
smartFactor2.add(measurements_cam2, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
smartFactor2.add(measurements_cam2, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, sensor_to_body, params);
smartFactor3.add(measurements_cam3, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
smartFactor3.add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) {
views.push_back(x1);
views.push_back(x2);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(sharedK);
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
smartFactor1->add(measurements_cam1, views);
SmartFactor::Cameras cameras;
cameras.push_back(cam1);
@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartFactor factor1(sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, boost::none, params);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
params.setEnableEPI(false);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(sharedK, boost::none, params));
smartFactor4->add(measurements_cam4, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor4->add(measurements_cam4, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
params.setLinearizationMode(gtsam::JACOBIAN_Q);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
params.setRankTolerance(10);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK2, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK2, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK2, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK2, boost::none, params));
smartFactor2->add(measurements_cam2, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
measurements_cam1.push_back(cam1_uv1);
measurements_cam1.push_back(cam2_uv1);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
smartFactor1->add(measurements_cam1, views);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK));
smartFactorInstance->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
smartFactorInstance->add(measurements_cam1, views);
Values values;
values.insert(x1, cam1.pose());
@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<Point2> measurements_cam1;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2));
smartFactor->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
smartFactor->add(measurements_cam1, views);
Values values;
values.insert(x1, cam1.pose());
@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose;
SmartProjectionParams params;
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor factor(sharedBundlerK, boost::none, params);
factor.add(measurement1, x1, model);
SmartFactor factor(model, sharedBundlerK, boost::none, params);
factor.add(measurement1, x1);
}
/* *************************************************************************/
@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,

45
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -18,7 +18,10 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -46,7 +49,7 @@ Point2 z1 = camera1.project(landmark);
//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor;
@ -63,6 +66,46 @@ TEST( triangulation, TriangulationFactor ) {
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
TEST( triangulation, TriangulationFactorStereo ) {
Key pointKey(1);
SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5);
Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
StereoCamera camera2(pose1, stereoCal);
StereoPoint2 z2 = camera2.project(landmark);
typedef TriangulationFactor<StereoCamera> Factor;
Factor factor(camera2, z2, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
// compare same problem against expression factor
Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);
ExpressionFactor<StereoPoint2> eFactor(model, z2, project2_);
Values values;
values.insert(pointKey, landmark);
vector<Matrix> HActual1(1), HActual2(1);
Vector error1 = factor.unwhitenedError(values, HActual1);
Vector error2 = eFactor.unwhitenedError(values, HActual2);
EXPECT(assert_equal(error1, error2));
EXPECT(assert_equal(HActual1[0], HActual2[0]));
}
//******************************************************************************
int main() {
TestResult tr;

6
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -87,17 +87,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor(K));
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(K));
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l;
}
factor->add(Point2(uL,v), i, model);
factor->add(Point2(uL,v), i);
}
Pose3 firstPose = initial_estimate.at<Pose3>(1);

8
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -46,7 +46,7 @@ using namespace gtsam;
int main(int argc, char** argv){
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor SmartFactor;
bool output_poses = true;
string poseOutput("../../../examples/data/optimized_poses.txt");
@ -109,17 +109,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor());
SmartFactor::shared_ptr factor(new SmartFactor(model));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor());
factor = SmartFactor::shared_ptr(new SmartFactor(model));
current_l = l;
}
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));

775
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -11,10 +11,11 @@
/**
* @file SmartStereoProjectionFactor.h
* @brief Base class to create smart factors on poses or cameras
* @brief Smart stereo factor on StereoCameras (pose + calibration)
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
*/
#pragma once
@ -24,6 +25,7 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
@ -33,79 +35,136 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart stereo projection factors
*/
struct GTSAM_EXPORT SmartStereoProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
/// Constructor
SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
1e-5), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartStereoProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
};
/**
* SmartStereoProjectionFactor: triangulates point
*/
template<class CALIBRATION>
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with StereoCamera. This factor requires that values
* contains the involved StereoCameras. Calibration is assumed to be fixed, as this
* is also assumed in StereoCamera.
* If you'd like to store poses in values instead of cameras, use
* SmartStereoProjectionPoseFactor instead
*/
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
private:
typedef SmartFactorBase<StereoCamera> Base;
protected:
/// @name Parameters
/// @{
const SmartStereoProjectionParams params_;
/// @}
/// @name Caching triangulation
/// @{
const TriangulationParameters parameters_;
// TODO:
// mutable TriangulationResult result_; ///< result from triangulateSafe
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
/// @}
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_;
mutable bool cheiralityException_;
/// shorthand for base class type
typedef SmartFactorBase<StereoCamera> Base;
/// shorthand for this class
typedef SmartStereoProjectionFactor<CALIBRATION> This;
enum {
ZDim = 3
}; ///< Dimension trait of measurement type
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a StereoCamera // TODO: Get rid of this?
typedef StereoCamera Camera;
typedef boost::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
/// Vector of cameras
typedef CameraSet<Camera> Cameras;
typedef CameraSet<StereoCamera> Cameras;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionFactor(const double rankTolerance,
const double linThreshold, const bool manageDegeneracy,
const bool enableEPI, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false) {
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel), //
params_(params), //
result_(TriangulationResult::Degenerate()) {
}
/** Virtual destructor */
@ -120,14 +179,20 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionFactor\n";
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
<< std::endl;
std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const SmartStereoProjectionFactor *e =
dynamic_cast<const SmartStereoProjectionFactor*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol);
}
/// Check if the new linearization point_ is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
@ -146,7 +211,7 @@ public:
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) {
params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
@ -164,124 +229,99 @@ public:
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
}
// /// triangulateSafe
// size_t triangulateSafe(const Values& values) const {
// return triangulateSafe(this->cameras(values));
// }
/// triangulateSafe
size_t triangulateSafe(const Cameras& cameras) const {
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true;
return m;
}
bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate)
// std::cout << "retriangulate = false" << std::endl;
//
// bool retriangulate = true;
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
//TODO: Chris will replace this with something else for stereo
// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
// rankTolerance_, enableEPI_);
// // // Temporary hack to use monocular triangulation
std::vector<Point2> mono_measurements;
BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
mono_measurements.push_back(sp.point2());
}
std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
BOOST_FOREACH(const Camera& camera, cameras) {
const Pose3& pose = camera.pose();
const Cal3_S2& K = camera.calibration()->calibration();
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
}
point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
parameters_.rankTolerance, parameters_.enableEPI);
// // // End temporary hack
// FIXME: temporary: triangulation using only first camera
// const StereoPoint2& z0 = this->measured_.at(0);
// point_ = cameras[0].backproject(z0);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
degenerate_ = true;
break;
}
const StereoPoint2& zi = this->measured_.at(i);
try {
StereoPoint2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// we discard smart factors that have large reprojection error
if (parameters_.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
degenerate_ = true;
cheiralityException_ = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
// std::cout << "Retriangulate " << std::endl;
std::vector<Point3> reprojections;
reprojections.reserve(m);
for(size_t i = 0; i < m; i++) {
reprojections.push_back(cameras[i].backproject(measured_[i]));
}
}
return m;
Point3 pw_sum;
BOOST_FOREACH(const Point3& pw, reprojections) {
pw_sum = pw_sum + pw;
}
// average reprojected landmark
Point3 pw_avg = pw_sum / double(m);
double totalReprojError = 0;
// check if it lies in front of all cameras
for(size_t i = 0; i < m; i++) {
const Pose3& pose = cameras[i].pose();
const Point3& pl = pose.transform_to(pw_avg);
if (pl.z() <= 0) {
result_ = TriangulationResult::BehindCamera();
return result_;
}
// check landmark distance
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
const StereoPoint2& zi = measured_[i];
StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
totalReprojError += reprojectionError.vector().norm();
}
} // for
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if(params_.triangulation.enableEPI) {
try {
pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
} catch(StereoCheiralityException& e) {
if(params_.verboseCheirality)
std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl;
if(params_.throwCheirality)
throw;
result_ = TriangulationResult::BehindCamera();
return TriangulationResult::BehindCamera();
}
}
result_ = TriangulationResult(pw_avg);
} // if retriangulate
return result_;
}
/// triangulate
bool triangulateForLinearize(const Cameras& cameras) const {
bool isDebug = false;
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
} else {
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
return true;
}
triangulateSafe(cameras); // imperative, might reset result_
return (result_);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0) const {
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
false) const {
bool isDebug = false;
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
std::vector<Key> js;
@ -290,146 +330,142 @@ public:
if (this->measured_.size() != cameras.size()) {
std::cout
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< std::endl;
exit(1);
}
triangulateSafe(cameras);
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug)
std::cout << "In linearize: exception" << std::endl;
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
0.0);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (isDebug)
std::cout << "degenerate_ = true" << std::endl;
}
if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
// ==================================================================
std::vector<typename Base::MatrixZD> Fblocks;
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<Base::MatrixZD> Fblocks;
Matrix F, E;
Vector b;
computeJacobians(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks, F); // expensive !!!
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
// And we care, as in grouped factors this is called repeatedly
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
Vector gs_vector;
// Whiten using noise model
Base::whitenJacobians(Fblocks, E, b);
Matrix3 P = Cameras::PointCov(E, lambda);
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug)
std::cout << "H:\n" << H << std::endl;
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * Base::Dim;
gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
GsCount2++;
}
}
}
// ==================================================================
double f = b.squaredNorm();
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
augmentedHessian);
}
// // create factor
// boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
// create factor
// boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createImplicitSchurFactor(cameras, point_, lambda);
// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
// else
// return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
// // failed: return empty
// return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
// }
//
// /// create factor
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createJacobianQFactor(cameras, point_, lambda);
// return Base::createJacobianQFactor(cameras, *result_, lambda);
// else
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// // failed: return empty
// return boost::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
// }
//
// /// Create a factor, takes values
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Values& values, double lambda) const {
// Cameras cameras;
// // TODO triangulate twice ??
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
// if (nonDegenerate)
// return createJacobianQFactor(cameras, lambda);
// else
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// return createJacobianQFactor(this->cameras(values), lambda);
// }
//
/// different (faster) way to compute Jacobian factor
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& cameras) const {
Values valuesFactor;
// /// linearize to a Hessianfactor
// virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
// const Values& values, double lambda = 0.0) const {
// return createHessianFactor(this->cameras(values), lambda);
// }
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
// /// linearize to an Implicit Schur factor
// virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
// const Values& values, double lambda = 0.0) const {
// return createRegularImplicitSchurFactor(this->cameras(values), lambda);
// }
//
// /// linearize to a JacobianfactorQ
// virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
// const Values& values, double lambda = 0.0) const {
// return createJacobianQFactor(this->cameras(values), lambda);
// }
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_)))
return false;
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartStereoProjectionFactor: this is not ready"
<< std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
// case IMPLICIT_SCHUR:
// return createRegularImplicitSchurFactor(cameras, lambda);
case JACOBIAN_SVD:
return createJacobianSVDFactor(cameras, lambda);
// case JACOBIAN_Q:
// return createJacobianQFactor(cameras, lambda);
default:
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
}
return true;
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
Cameras cameras = this->cameras(values);
return linearizeDamped(cameras, lambda);
}
/// linearize
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return linearizeDamped(values);
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(*result_, boost::none, E);
return nonDegenerate;
}
/**
@ -437,87 +473,62 @@ public:
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
cameras.project2(point_, boost::none, E);
return nonDegenerate;
Cameras cameras = this->cameras(values);
return triangulateAndComputeE(E, cameras);
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, cameras, 0.0);
return nonDegenerate;
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!");
// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
// std::cout << "point " << point_ << std::endl;
// std::cout
// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
// << std::endl;
// if (D > 6) {
// std::cout
// << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
// << std::endl;
// }
void computeJacobiansWithTriangulatedPoint(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint");
// // Handle degeneracy
// // TODO check flag whether we should do this
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// this->measured_.at(0)); */
//
// int numKeys = this->keys_.size();
// E = zeros(2 * numKeys, 2);
// b = zero(2 * numKeys);
// double f = 0;
// for (size_t i = 0; i < this->measured_.size(); i++) {
// if (i == 0) { // first pose
// this->point_ = cameras[i].backprojectPointAtInfinity(
// this->measured_.at(i));
// // 3D parametrization of point at infinity: [px py 1]
// }
// Matrix Fi, Ei;
// Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
// - this->measured_.at(i)).vector();
//
// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
// f += bi.squaredNorm();
// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
// E.block < 2, 2 > (2 * i, 0) = Ei;
// subInsert(b, bi, 2 * i);
// }
// return f;
// Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
} else {
// nondegenerate: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else
// valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, *result_);
}
}
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::unwhitenedError(cameras, point_);
return Base::unwhitenedError(cameras, *result_);
else
return zero(cameras.size() * 3);
return zero(cameras.size() * Base::ZDim);
}
/**
@ -529,87 +540,61 @@ public:
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras;
if (externalPoint) {
nrCameras = this->keys_.size();
point_ = *externalPoint;
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
// // Otherwise, manage the exceptions with rotation-only factors
// const StereoPoint2& z0 = this->measured_.at(0);
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
//
// return Base::totalReprojectionError(cameras, backprojected);
} else {
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
this->degenerate_ = true;
}
if (this->degenerate_) {
return 0.0; // TODO: this maybe should be zero?
// std::cout
// << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
// << std::endl;
// size_t i = 0;
// double overallError = 0;
// BOOST_FOREACH(const Camera& camera, cameras) {
// const StereoPoint2& zi = this->measured_.at(i);
// if (i == 0) // first pose
// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
// StereoPoint2 reprojectionError(
// camera.projectPointAtInfinity(this->point_) - zi);
// overallError += 0.5
// * this->noise_.at(i)->distance(reprojectionError.vector());
// i += 1;
// }
// return overallError;
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/// Calculate total reprojection error
virtual double error(const Values& values) const {
if (this->active(values)) {
return totalReprojectionError(Base::cameras(values));
} else { // else of active flag
return 0.0;
}
}
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
}
TriangulationResult point() const {
return result_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
}
/** COMPUTE the landmark */
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
}
/// Is result valid?
bool isValid() const {
return result_;
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return chirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return the degenerate state */
bool isDegenerate() const {
return result_.degenerate();
}
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const {
return throwCheirality_;
}
/** return the cheirality status flag */
bool isPointBehindCamera() const {
return result_.behindCamera();
}
private:
@ -618,15 +603,15 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
}
};
/// traits
template<class CALIBRATION>
struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionFactor<CALIBRATION> > {
template<>
struct traits<SmartStereoProjectionFactor > : public Testable<
SmartStereoProjectionFactor> {
};
} // \ namespace gtsam

135
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -11,7 +11,7 @@
/**
* @file SmartStereoProjectionPoseFactor.h
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
@ -35,53 +35,40 @@ namespace gtsam {
*/
/**
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* This factor assumes that camera calibration is fixed, but each camera
* has its own calibration.
* The factor only constrains poses (variable dimension is 6).
* This factor requires that values contains the involved poses (Pose3).
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
public:
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<CALIBRATION> Base;
typedef SmartStereoProjectionFactor Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
typedef SmartStereoProjectionPoseFactor This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
linearizeTo) {
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel, params) {
}
/** Virtual destructor */
@ -91,27 +78,23 @@ public:
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
* @param K is the (fixed) camera calibration
*/
void add(const StereoPoint2 measured_i, const Key poseKey_i,
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
K_all_.push_back(K_i);
void add(const StereoPoint2 measured, const Key poseKey,
const boost::shared_ptr<Cal3_S2Stereo> K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
Base::add(measurements, poseKeys, noises);
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
}
@ -121,13 +104,12 @@ public:
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
Base::add(measurements.at(i), poseKeys.at(i));
K_all_.push_back(K);
}
}
@ -140,57 +122,19 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
BOOST_FOREACH(const boost::shared_ptr<Cal3_S2Stereo>& K, K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
const SmartStereoProjectionPoseFactor *e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
typename Base::Camera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
// depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){
case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0);
break;
case JACOBIAN_Q :
throw("JacobianQ not working yet!");
// return this->createJacobianQFactor(cameras(values), 0.0);
break;
default:
return this->createHessianFactor(cameras(values));
break;
}
}
/**
* error calculates the error of the factor.
*/
@ -203,10 +147,27 @@ public:
}
/** return the calibration object */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
return K_all_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
Base::Cameras cameras(const Values& values) const {
Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
const Pose3& pose = values.at<Pose3>(k);
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
private:
/// Serialization function
@ -220,9 +181,9 @@ private:
}; // end of class declaration
/// traits
template<class CALIBRATION>
struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionPoseFactor<CALIBRATION> > {
template<>
struct traits<SmartStereoProjectionPoseFactor> : public Testable<
SmartStereoProjectionPoseFactor> {
};
} // \ namespace gtsam

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

@ -40,11 +40,10 @@ 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 double rankTol = 1.0;
static double linThreshold = -1.0;
static SmartStereoProjectionParams params;
// static bool manageDegeneracy = true;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
@ -63,8 +62,6 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -80,37 +77,37 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
return measurements_cam;
}
LevenbergMarquardtParams params;
LevenbergMarquardtParams lm_params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor2) {
SmartFactor factor1(rankTol, linThreshold);
SmartStereoProjectionPoseFactor factor1(model, params);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor3) {
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor4) {
SmartFactor factor1(rankTol, linThreshold);
factor1.add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor factor1(model, params);
factor1.add(measurement1, poseKey1, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Equals ) {
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, K);
SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
factor2->add(measurement1, poseKey1, K);
CHECK(assert_equal(*factor1, *factor2));
}
@ -137,15 +134,15 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
values.insert(x1, level_pose);
values.insert(x2, level_pose_right);
SmartFactor factor1;
factor1.add(level_uv, x1, model, K2);
factor1.add(level_uv_right, x2, model, K2);
SmartStereoProjectionPoseFactor factor1(model);
factor1.add(level_uv, x1, K2);
factor1.add(level_uv_right, x2, K2);
double actualError = factor1.error(values);
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
SmartFactor::Cameras cameras = factor1.cameras(values);
SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
double actualError2 = factor1.totalReprojectionError(cameras);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
@ -179,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
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);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(level_uv, x1, K);
factor1->add(level_uv_right, x2, K);
double actualError1 = factor1->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
vector<StereoPoint2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(model);
noises.push_back(model);
vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
Ks.push_back(K);
Ks.push_back(K);
@ -202,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
views.push_back(x1);
views.push_back(x2);
factor2->add(measurements, views, noises, Ks);
factor2->add(measurements, views, Ks);
double actualError2 = factor2->error(values);
@ -211,6 +204,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
@ -229,11 +223,11 @@ 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,
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
@ -241,17 +235,21 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, model, K2);
SmartStereoProjectionParams smart_params;
smart_params.triangulation.enableEPI = true;
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor1->add(measurements_l1, views, K2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor2->add(measurements_l2, views, K2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor3->add(measurements_l3, views, K2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
@ -274,16 +272,24 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
// get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point();
Point3 landmark2_smart = *smartFactor2->point();
Point3 landmark3_smart = *smartFactor3->point();
Values result;
gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor);
tictoc_finishedIteration_();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
VectorValues delta = GFG->optimize();
@ -292,6 +298,51 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
/* ***************************************************************
* Same problem with regular Stereo factors, expect same error!
* ****************************************************************/
// landmark1_smart.print("landmark1_smart");
// landmark2_smart.print("landmark2_smart");
// landmark3_smart.print("landmark3_smart");
// add landmarks to values
values.insert(L(1), landmark1_smart);
values.insert(L(2), landmark2_smart);
values.insert(L(3), landmark3_smart);
// add factors
NonlinearFactorGraph graph2;
graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
bool verboseCheirality = true;
graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7);
LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize();
EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
}
/* *************************************************************************/
@ -325,17 +376,17 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -355,7 +406,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
values.insert(x3, pose3 * noise_pose);
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
@ -363,7 +414,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2;
// double excludeLandmarksFutherThanDist = 2;
vector<Key> views;
views.push_back(x1);
@ -393,20 +444,18 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
params.setLandmarkDistanceThreshold(2);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -427,7 +476,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
// All factors are disabled and pose should remain where it is
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
}
@ -435,9 +484,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -471,25 +517,26 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
params.setDynamicOutlierRejectionThreshold(1);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
smartFactor4->add(measurements_cam4, views, K);
// same as factor 4, but dynamic outlier rejection is off
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
smartFactor4b->add(measurements_cam4, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -508,9 +555,25 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
values.insert(x2, pose2);
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
// zero error due to dynamic outlier rejection
EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off
EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
// Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point());
EXPECT(smartFactor2->point());
EXPECT(smartFactor3->point());
EXPECT(smartFactor4->point().degenerate());
EXPECT(smartFactor4b->point());
// Factor 4 is disabled, pose 3 stays put
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
@ -545,13 +608,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -571,7 +634,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(x3, pose3*noise_pose);
//
//// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//}
@ -630,7 +693,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(L(2), landmark2);
// values.insert(L(3), landmark3);
//
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// Values result = optimizer.optimize();
//
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@ -669,17 +732,17 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
// Create smartfactors
double rankTol = 10;
SmartStereoProjectionParams params;
params.setRankTolerance(10);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
// Create graph to optimize
NonlinearFactorGraph graph;
@ -755,10 +818,10 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
//
// double rankTol = 50;
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K2);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K2);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -781,7 +844,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_();
@ -824,13 +887,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// double rankTol = 10;
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -855,7 +918,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_();
@ -889,7 +952,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// measurements_cam1.push_back(cam1_uv1);
// measurements_cam1.push_back(cam2_uv1);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor());
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
// smartFactor1->add(measurements_cam1,views, model, K2);
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
@ -930,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
smartFactorInstance->add(measurements_cam1, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
smartFactorInstance->add(measurements_cam1, views, K);
Values values;
values.insert(x1, pose1);
@ -977,6 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -997,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactor(new SmartFactor());
smartFactor->add(measurements_cam1, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
smartFactor->add(measurements_cam1, views, K2);
Values values;
values.insert(x1, pose1);
@ -1021,7 +1085,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-8));
hessianFactorRot->information(), 1e-6));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
@ -1037,7 +1101,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// Hessian is invariant to rotations and translations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRotTran->information(), 1e-8));
hessianFactorRotTran->information(), 1e-6));
}
/* ************************************************************************* */