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

Merged in feature/smartFactorNoiseCleanup (pull request #183) 

SmartFactor noise model in constructor
release/4.3a0
Chris Beall 2015-08-27 12:44:09 -04:00
parent 0503df31fa 92e210b893
commit c73b835848
14 changed files with 312 additions and 333 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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) { 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. // 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) { 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 // 2. the corresponding camera's key
// 3. camera noise model // 3. camera noise model
// 4. camera calibration // 4. camera calibration
smartfactor->add(measurement, i, measurementNoise); smartfactor->add(measurement, i);
} }
// insert the smart factor in the graph // 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) { 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. // 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) { 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]); Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add: // 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 // insert the smart factor in the graph

12
gtsam.h
View File

@ -2364,15 +2364,17 @@ class SmartProjectionParams {
template<CALIBRATION> template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const CALIBRATION* K); SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
SmartProjectionPoseFactor(const CALIBRATION* K, const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor); 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::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params); const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i, void add(const gtsam::Point2& measured_i, size_t poseKey_i);
const gtsam::noiseModel::Base* noise_i);
// enabling serialization functionality // enabling serialization functionality
//void serialize() const; //void serialize() const;

59
gtsam/slam/SmartFactorBase.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone * @author Luca Carlone
* @author Zsolt Kira * @author Zsolt Kira
* @author Frank Dellaert * @author Frank Dellaert
* @author Chris Beall
*/ */
#pragma once #pragma once
@ -79,22 +80,6 @@ protected:
typedef Eigen::Matrix<double, Dim, 1> VectorD; typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2; 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: public:
// Definitions for blocks of F, externally visible // Definitions for blocks of F, externally visible
@ -109,8 +94,21 @@ public:
typedef CameraSet<CAMERA> Cameras; typedef CameraSet<CAMERA> Cameras;
/// Constructor /// Constructor
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) : SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
body_P_sensor_(body_P_sensor){} 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 destructor, subclasses from NonlinearFactor
virtual ~SmartFactorBase() { virtual ~SmartFactorBase() {
@ -122,34 +120,18 @@ public:
* @param poseKey is the index corresponding to the camera observing the landmark * @param poseKey is the index corresponding to the camera observing the landmark
* @param sharedNoiseModel is the measurement noise * @param sharedNoiseModel is the measurement noise
*/ */
void add(const Z& measured_i, const Key& cameraKey_i, void add(const Z& measured_i, const Key& cameraKey_i) {
const SharedNoiseModel& sharedNoiseModel) {
this->measured_.push_back(measured_i); this->measured_.push_back(measured_i);
this->keys_.push_back(cameraKey_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, void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
std::vector<SharedNoiseModel>& noises) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i)); this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.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 * The noise is assumed to be the same for all measurements
*/ */
template<class SFM_TRACK> 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++) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second); this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first); this->keys_.push_back(trackToAdd.measurements[k].first);
maybeSetNoiseModel(noise);
} }
} }

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -159,10 +159,10 @@ public:
* @param body_P_sensor pose of the camera in the body frame * @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartProjectionFactor( SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor = boost::none, const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) : const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor), params_(params), // Base(sharedNoiseModel, body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) { result_(TriangulationResult::Degenerate()) {
} }

6
gtsam/slam/SmartProjectionPoseFactor.h
View File

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

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

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

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

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

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 //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 size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) { if(current_l != l) {
graph.push_back(factor); graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(K)); factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l; current_l = l;
} }
factor->add(Point2(uL,v), i, model); factor->add(Point2(uL,v), i);
} }
Pose3 firstPose = initial_estimate.at<Pose3>(1); Pose3 firstPose = initial_estimate.at<Pose3>(1);

6
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -109,17 +109,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors //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 size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) { if(current_l != l) {
graph.push_back(factor); graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor()); factor = SmartFactor::shared_ptr(new SmartFactor(model));
current_l = l; 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)); Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));

4
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -159,8 +159,10 @@ public:
* Constructor * Constructor
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartStereoProjectionFactor(const SmartStereoProjectionParams& params = SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) : SmartStereoProjectionParams()) :
Base(sharedNoiseModel), //
params_(params), // params_(params), //
result_(TriangulationResult::Degenerate()) { result_(TriangulationResult::Degenerate()) {
} }

33
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -62,15 +62,13 @@ public:
/** /**
* Constructor * Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate * @param Isotropic measurement noise
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) * @param params internal parameters of the smart factors
* @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
*/ */
SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params = SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) : SmartStereoProjectionParams()) :
Base(params) { Base(sharedNoiseModel, params) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -80,27 +78,23 @@ public:
* add a new measurement and pose key * 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 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 poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise * @param K is the (fixed) camera calibration
* @param K_i is the (known) camera calibration
*/ */
void add(const StereoPoint2 measured_i, const Key poseKey_i, void add(const StereoPoint2 measured, const Key poseKey,
const SharedNoiseModel noise_i, const boost::shared_ptr<Cal3_S2Stereo> K) {
const boost::shared_ptr<Cal3_S2Stereo> K_i) { Base::add(measured, poseKey);
Base::add(measured_i, poseKey_i, noise_i); K_all_.push_back(K);
K_all_.push_back(K_i);
} }
/** /**
* Variant of the previous one in which we include a set of measurements * 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 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 poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects * @param Ks vector of calibration objects
*/ */
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys, void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) { std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys, noises); Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i)); K_all_.push_back(Ks.at(i));
} }
@ -110,13 +104,12 @@ public:
* Variant of the previous one in which we include a set of measurements with the same noise and calibration * 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 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 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) * @param K the (known) camera calibration (same for all measurements)
*/ */
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys, void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<Cal3_S2Stereo> K) { const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) { 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); K_all_.push_back(K);
} }
} }

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

@ -81,33 +81,33 @@ LevenbergMarquardtParams lm_params;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) { TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor2) { TEST( SmartStereoProjectionPoseFactor, Constructor2) {
SmartStereoProjectionPoseFactor factor1(params); SmartStereoProjectionPoseFactor factor1(model, params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor3) { TEST( SmartStereoProjectionPoseFactor, Constructor3) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, model, K); factor1->add(measurement1, poseKey1, K);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor4) { TEST( SmartStereoProjectionPoseFactor, Constructor4) {
SmartStereoProjectionPoseFactor factor1(params); SmartStereoProjectionPoseFactor factor1(model, params);
factor1.add(measurement1, poseKey1, model, K); factor1.add(measurement1, poseKey1, K);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Equals ) { TEST( SmartStereoProjectionPoseFactor, Equals ) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, model, K); factor1->add(measurement1, poseKey1, K);
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
factor2->add(measurement1, poseKey1, model, K); factor2->add(measurement1, poseKey1, K);
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }
@ -134,9 +134,9 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, level_pose_right); values.insert(x2, level_pose_right);
SmartStereoProjectionPoseFactor factor1; SmartStereoProjectionPoseFactor factor1(model);
factor1.add(level_uv, x1, model, K2); factor1.add(level_uv, x1, K2);
factor1.add(level_uv_right, x2, model, K2); factor1.add(level_uv_right, x2, K2);
double actualError = factor1.error(values); double actualError = factor1.error(values);
double expectedError = 0.0; double expectedError = 0.0;
@ -176,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
Point3(0.5, 0.1, 0.3)); Point3(0.5, 0.1, 0.3));
values.insert(x2, level_pose_right.compose(noise_pose)); values.insert(x2, level_pose_right.compose(noise_pose));
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(level_uv, x1, model, K); factor1->add(level_uv, x1, K);
factor1->add(level_uv_right, x2, model, K); factor1->add(level_uv_right, x2, K);
double actualError1 = factor1->error(values); double actualError1 = factor1->error(values);
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
vector<StereoPoint2> measurements; vector<StereoPoint2> measurements;
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); 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) vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
Ks.push_back(K); Ks.push_back(K);
Ks.push_back(K); Ks.push_back(K);
@ -199,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
factor2->add(measurements, views, noises, Ks); factor2->add(measurements, views, Ks);
double actualError2 = factor2->error(values); double actualError2 = factor2->error(values);
@ -241,14 +237,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
SmartStereoProjectionParams smart_params; SmartStereoProjectionParams smart_params;
smart_params.triangulation.enableEPI = true; smart_params.triangulation.enableEPI = true;
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor1->add(measurements_l1, views, model, K2); smartFactor1->add(measurements_l1, views, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor2->add(measurements_l2, views, model, K2); smartFactor2->add(measurements_l2, views, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor3->add(measurements_l3, views, model, K2); smartFactor3->add(measurements_l3, views, K2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -383,14 +379,14 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
SmartStereoProjectionParams params; SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD); params.setLinearizationMode(JACOBIAN_SVD);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -452,14 +448,14 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
params.setLinearizationMode(JACOBIAN_SVD); params.setLinearizationMode(JACOBIAN_SVD);
params.setLandmarkDistanceThreshold(2); params.setLandmarkDistanceThreshold(2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -526,21 +522,21 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
params.setDynamicOutlierRejectionThreshold(1); params.setDynamicOutlierRejectionThreshold(1);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
smartFactor4->add(measurements_cam4, views, model, K); smartFactor4->add(measurements_cam4, views, K);
// same as factor 4, but dynamic outlier rejection is off // same as factor 4, but dynamic outlier rejection is off
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
smartFactor4b->add(measurements_cam4, views, model, K); smartFactor4b->add(measurements_cam4, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -739,14 +735,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
SmartStereoProjectionParams params; SmartStereoProjectionParams params;
params.setRankTolerance(10); params.setRankTolerance(10);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, K);
// Create graph to optimize // Create graph to optimize
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -997,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1); cam2, cam3, landmark1);
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
smartFactorInstance->add(measurements_cam1, views, model, K); smartFactorInstance->add(measurements_cam1, views, K);
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
@ -1065,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1); cam2, cam3, landmark1);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor()); SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
smartFactor->add(measurements_cam1, views, model, K2); smartFactor->add(measurements_cam1, views, K2);
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);