diff --git a/.cproject b/.cproject
index acb0ea910..8783ebc31 100644
--- a/.cproject
+++ b/.cproject
@@ -1554,6 +1554,22 @@
true
true
+
+ make
+ -j4
+ testSmartProjectionCameraFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartFactorBase.run
+ true
+ true
+ true
+
make
-j3
diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h
new file mode 100644
index 000000000..d5bdc2e84
--- /dev/null
+++ b/gtsam/slam/SmartProjectionCameraFactor.h
@@ -0,0 +1,164 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SmartProjectionCameraFactor.h
+ * @brief Produces an Hessian factors on CAMERAS (Pose3+CALIBRATION) from monocular measurements of a landmark
+ * @author Luca Carlone
+ * @author Chris Beall
+ * @author Zsolt Kira
+ */
+
+#pragma once
+#include
+
+namespace gtsam {
+
+/**
+ * @addtogroup SLAM
+ */
+template
+class SmartProjectionCameraFactor: public SmartProjectionFactor {
+protected:
+
+ bool isImplicit_;
+
+public:
+
+ /// shorthand for base class type
+ typedef SmartProjectionFactor Base;
+
+ /// shorthand for this class
+ typedef SmartProjectionCameraFactor This;
+
+ /// shorthand for a smart pointer to a factor
+ typedef boost::shared_ptr 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 isImplicit if set to true linearize the factor to an implicit Schur factor
+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
+ */
+ SmartProjectionCameraFactor(const double rankTol = 1,
+ const double linThreshold = -1, const bool manageDegeneracy = false,
+ const bool enableEPI = false, const bool isImplicit = false,
+ boost::optional body_P_sensor = boost::none) :
+ Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), isImplicit_(
+ isImplicit) {
+ if (linThreshold != -1) {
+ std::cout << "SmartProjectionCameraFactor: linThreshold "
+ << linThreshold << std::endl;
+ }
+ }
+
+ /** Virtual destructor */
+ virtual ~SmartProjectionCameraFactor() {
+ }
+
+ /**
+ * print
+ * @param s optional string naming the factor
+ * @param keyFormatter optional formatter useful for printing Symbols
+ */
+ void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+ DefaultKeyFormatter) const {
+ std::cout << s << "SmartProjectionCameraFactor, z = \n ";
+ Base::print("", keyFormatter);
+ }
+
+ /// equals
+ virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+ const This *e = dynamic_cast(&p);
+
+ return e && Base::equals(p, tol);
+ }
+
+ /// get the dimension of the factor (number of rows on linearization)
+ virtual size_t dim() const {
+ return D * this->keys_.size(); // 6 for the pose and 3 for the calibration
+ }
+
+ /// Collect all cameras: important that in key order
+ typename Base::Cameras cameras(const Values& values) const {
+ typename Base::Cameras cameras;
+ BOOST_FOREACH(const Key& k, this->keys_)
+ cameras.push_back(values.at(k));
+ return cameras;
+ }
+
+ /// linearize and adds damping on the points
+ boost::shared_ptr linearizeDamped(const Values& values,
+ const double lambda=0.0) const {
+ if (!isImplicit_)
+ return Base::createHessianFactor(cameras(values), lambda);
+ else
+ return Base::createRegularImplicitSchurFactor(cameras(values));
+ }
+
+ /// linearize returns a Hessianfactor that is an approximation of error(p)
+ virtual boost::shared_ptr > linearizeToHessian(
+ const Values& values, double lambda=0.0) const {
+ return Base::createHessianFactor(cameras(values),lambda);
+ }
+
+ /// linearize returns a Hessianfactor that is an approximation of error(p)
+ virtual boost::shared_ptr > linearizeToImplicit(
+ const Values& values, double lambda=0.0) const {
+ return Base::createRegularImplicitSchurFactor(cameras(values),lambda);
+ }
+
+ /// linearize returns a Jacobianfactor that is an approximation of error(p)
+ virtual boost::shared_ptr > linearizeToJacobian(
+ const Values& values, double lambda=0.0) const {
+ return Base::createJacobianQFactor(cameras(values),lambda);
+ }
+
+ /// linearize returns a Hessianfactor that is an approximation of error(p)
+ virtual boost::shared_ptr linearizeWithLambda(
+ const Values& values, double lambda) const {
+ if (isImplicit_)
+ return linearizeToImplicit(values,lambda);
+ else
+ return linearizeToHessian(values,lambda);
+ }
+
+ /// linearize returns a Hessianfactor that is an approximation of error(p)
+ virtual boost::shared_ptr linearize(
+ const Values& values) const {
+ return linearizeWithLambda(values,0.0);
+ }
+
+ /// Calculare total reprojection error
+ virtual double error(const Values& values) const {
+ if (this->active(values)) {
+ return Base::totalReprojectionError(cameras(values));
+ } else { // else of active flag
+ return 0.0;
+ }
+ }
+
+private:
+
+ /// Serialization function
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+ }
+
+};
+
+} // \ namespace gtsam
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
new file mode 100644
index 000000000..88a0c0521
--- /dev/null
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -0,0 +1,1009 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testSmartProjectionCameraFactor.cpp
+ * @brief Unit tests for SmartProjectionCameraFactor Class
+ * @author Chris Beall
+ * @author Luca Carlone
+ * @author Zsolt Kira
+ * @date Sept 2013
+ */
+
+//#include "BundlerDefinitions.h"
+#include
+//#include "../SmartFactorsTestProblems.h"
+//#include "../LevenbergMarquardtOptimizerCERES.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+//#include "../SmartNonlinearFactorGraph.h"
+#undef CHECK
+#include
+#include
+
+using namespace std;
+using namespace boost::assign;
+using namespace gtsam;
+
+typedef PinholeCamera Camera;
+
+static bool isDebugTest = false;
+
+// make a realistic calibration matrix
+static double fov = 60; // degrees
+static size_t w=640,h=480;
+
+static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
+static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
+static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 0, 0));
+
+static double rankTol = 1.0;
+static double linThreshold = -1.0;
+
+// Create a noise model for the pixel error
+static SharedNoiseModel model(noiseModel::Unit::Create(2));
+
+// Convenience for named keys
+using symbol_shorthand::X;
+using symbol_shorthand::L;
+
+// tests data
+Key x1 = 1;
+
+Symbol l1('l', 1), l2('l', 2), l3('l', 3);
+Key c1 = 1, c2 = 2, c3 = 3;
+
+static Key poseKey1(x1);
+static Point2 measurement1(323.0, 240.0);
+static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+
+typedef PinholeCamera CameraCal3_S2;
+typedef SmartProjectionCameraFactor SmartFactor;
+typedef SmartProjectionCameraFactor SmartFactorBundler;
+typedef GeneralSFMFactor SFMFactor;
+
+template
+PinholeCamera perturbCameraPose(
+ const PinholeCamera& camera) {
+ Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+ Point3(0.5, 0.1, 0.3));
+ Pose3 cameraPose = camera.pose();
+ Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
+ return PinholeCamera(perturbedCameraPose, camera.calibration());
+}
+
+template
+PinholeCamera perturbCameraPoseAndCalibration(
+ const PinholeCamera& camera) {
+ GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
+ Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+ Point3(0.5, 0.1, 0.3));
+ Pose3 cameraPose = camera.pose();
+ Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
+ typename gtsam::traits::TangentVector d;
+ d.setRandom(); d*=0.1;
+ CALIBRATION perturbedCalibration = camera.calibration().retract(d);
+ return PinholeCamera(perturbedCameraPose, perturbedCalibration);
+}
+
+template
+void projectToMultipleCameras(const PinholeCamera& cam1,
+ const PinholeCamera& cam2,
+ const PinholeCamera& cam3, Point3 landmark,
+ vector& measurements_cam) {
+ Point2 cam1_uv1 = cam1.project(landmark);
+ Point2 cam2_uv1 = cam2.project(landmark);
+ Point2 cam3_uv1 = cam3.project(landmark);
+ measurements_cam.push_back(cam1_uv1);
+ measurements_cam.push_back(cam2_uv1);
+ measurements_cam.push_back(cam3_uv1);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, perturbCameraPose) {
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ CameraCal3_S2 level_camera(level_pose, *K2);
+ Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
+ Pose3 perturbed_level_pose = level_pose.compose(noise_pose);
+ CameraCal3_S2 actualCamera(perturbed_level_pose, *K2);
+
+ CameraCal3_S2 expectedCamera = perturbCameraPose(level_camera);
+ CHECK(assert_equal(expectedCamera, actualCamera));
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor) {
+ SmartFactor::shared_ptr factor1(new SmartFactor());
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor2) {
+ SmartFactor factor1(rankTol, linThreshold);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor3) {
+ SmartFactor::shared_ptr factor1(new SmartFactor());
+ factor1->add(measurement1, poseKey1, model);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Constructor4) {
+ SmartFactor factor1(rankTol, linThreshold);
+ factor1.add(measurement1, poseKey1, model);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, ConstructorWithTransform) {
+ bool manageDegeneracy = true;
+ bool isImplicit = false;
+ bool enableEPI = false;
+ SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit, enableEPI, body_P_sensor1);
+ factor1.add(measurement1, poseKey1, model);
+}
+
+/* ************************************************************************* */
+TEST( SmartProjectionCameraFactor, Equals ) {
+ SmartFactor::shared_ptr factor1(new SmartFactor());
+ factor1->add(measurement1, poseKey1, model);
+
+ SmartFactor::shared_ptr factor2(new SmartFactor());
+ factor2->add(measurement1, poseKey1, model);
+ //CHECK(assert_equal(*factor1, *factor2));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noiseless ){
+ // cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl;
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ CameraCal3_S2 level_camera(level_pose, *K2);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ CameraCal3_S2 level_camera_right(level_pose_right, *K2);
+
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+
+ SmartFactor::shared_ptr factor1(new SmartFactor());
+ factor1->add(level_uv, c1, model);
+ factor1->add(level_uv_right, c2, model);
+
+ double actualError = factor1->error(values);
+
+ double expectedError = 0.0;
+ DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noiselessBundler ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ Camera level_camera(level_pose, *Kbundler);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ Camera level_camera_right(level_pose_right, *Kbundler);
+
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+
+ SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
+ factor1->add(level_uv, c1, model);
+ factor1->add(level_uv_right, c2, model);
+
+ double actualError = factor1->error(values);
+
+ double expectedError = 0.0;
+ DOUBLES_EQUAL(expectedError, actualError, 1e-3);
+
+ Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this)
+ if(factor1->point())
+ oldPoint = *(factor1->point());
+
+ Point3 expectedPoint;
+ if(factor1->point(values))
+ expectedPoint = *(factor1->point(values));
+
+ EXPECT(assert_equal(expectedPoint,landmark, 1e-3));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, noisy ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ CameraCal3_S2 level_camera(level_pose, *K2);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ CameraCal3_S2 level_camera_right(level_pose_right, *K2);
+
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 pixelError(0.2,0.2);
+ Point2 level_uv = level_camera.project(landmark) + pixelError;
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ CameraCal3_S2 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, model);
+ factor1->add(level_uv_right, c2, model);
+
+ double actualError1= factor1->error(values);
+
+ SmartFactor::shared_ptr factor2(new SmartFactor());
+ vector measurements;
+ measurements.push_back(level_uv);
+ measurements.push_back(level_uv_right);
+
+ vector< SharedNoiseModel > noises;
+ noises.push_back(model);
+ noises.push_back(model);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+
+ factor2->add(measurements, views, noises);
+
+ double actualError2= factor2->error(values);
+
+ DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
+}
+
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ){
+
+ // 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));
+ CameraCal3_S2 cam1(pose1, *K2);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+ CameraCal3_S2 cam2(pose2, *K2);
+
+ // create third camera 1 meter above the first camera
+ Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+ CameraCal3_S2 cam3(pose3, *K2);
+
+ // three landmarks ~5 meters infront of camera
+ Point3 landmark1(5, 0.5, 1.2);
+ Point3 landmark2(5, -0.5, 1.2);
+ Point3 landmark3(3, 0, 3.0);
+
+ vector measurements_cam1, measurements_cam2, measurements_cam3;
+
+ // 1. Project three landmarks into three cameras and triangulate
+ projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+ views.push_back(c3);
+
+ SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+ smartFactor1->add(measurements_cam1, views, model);
+
+ SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+ smartFactor2->add(measurements_cam2, views, model);
+
+ SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+ smartFactor3->add(measurements_cam3, views, model);
+
+ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(smartFactor1);
+ graph.push_back(smartFactor2);
+ graph.push_back(smartFactor3);
+ graph.push_back(PriorFactor(c1, cam1, noisePrior));
+ graph.push_back(PriorFactor(c2, cam2, noisePrior));
+
+ Values values;
+ values.insert(c1, cam1);
+ values.insert(c2, cam2);
+ values.insert(c3, perturbCameraPose(cam3));
+ if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: ");
+
+ LevenbergMarquardtParams params;
+ if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+ if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+
+ Values result;
+ gttic_(SmartProjectionCameraFactor);
+ LevenbergMarquardtOptimizer optimizer(graph, values, params);
+ result = optimizer.optimize();
+ gttoc_(SmartProjectionCameraFactor);
+ tictoc_finishedIteration_();
+
+ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+ // VectorValues delta = GFG->optimize();
+
+ if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: ");
+ EXPECT(assert_equal(cam1,result.at(c1)));
+ EXPECT(assert_equal(cam2,result.at(c2)));
+ EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4));
+ EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2));
+ if(isDebugTest) tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){
+
+ // 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));
+ CameraCal3_S2 cam1(pose1, *K2);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+ CameraCal3_S2 cam2(pose2, *K2);
+
+ // create third camera 1 meter above the first camera
+ Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+ CameraCal3_S2 cam3(pose3, *K2);
+
+ // three landmarks ~5 meters infront of camera
+ Point3 landmark1(5, 0.5, 1.2);
+ Point3 landmark2(5, -0.5, 1.2);
+ Point3 landmark3(3, 0, 3.0);
+
+ vector measurements_cam1, measurements_cam2, measurements_cam3;
+ // 1. Project three landmarks into three cameras and triangulate
+ projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+ views.push_back(c3);
+
+ SfM_Track track1;
+ for(size_t i=0;i<3;++i){
+ SfM_Measurement measures;
+ measures.first = i+1;// cameras are from 1 to 3
+ measures.second = measurements_cam1.at(i);
+ track1.measurements.push_back(measures);
+ }
+ SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+ smartFactor1->add(track1, model);
+
+ SfM_Track track2;
+ for(size_t i=0;i<3;++i){
+ SfM_Measurement measures;
+ measures.first = i+1;// cameras are from 1 to 3
+ measures.second = measurements_cam2.at(i);
+ track2.measurements.push_back(measures);
+ }
+ SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+ smartFactor2->add(track2, model);
+
+ SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+ smartFactor3->add(measurements_cam3, views, model);
+
+ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(smartFactor1);
+ graph.push_back(smartFactor2);
+ graph.push_back(smartFactor3);
+ graph.push_back(PriorFactor(c1, cam1, noisePrior));
+ graph.push_back(PriorFactor(c2, cam2, noisePrior));
+
+ Values values;
+ values.insert(c1, cam1);
+ values.insert(c2, cam2);
+ values.insert(c3, perturbCameraPose(cam3));
+ if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: ");
+
+ LevenbergMarquardtParams params;
+ if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+ if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+
+ Values result;
+ gttic_(SmartProjectionCameraFactor);
+ LevenbergMarquardtOptimizer optimizer(graph, values, params);
+ result = optimizer.optimize();
+ gttoc_(SmartProjectionCameraFactor);
+ tictoc_finishedIteration_();
+
+ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+ // VectorValues delta = GFG->optimize();
+
+ if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: ");
+ EXPECT(assert_equal(cam1,result.at(c1)));
+ EXPECT(assert_equal(cam2,result.at(c2)));
+ EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4));
+ EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2));
+ if(isDebugTest) tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){
+
+ // 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));
+ CameraCal3_S2 cam1(pose1, *K2);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+ CameraCal3_S2 cam2(pose2, *K2);
+
+ // create third camera 1 meter above the first camera
+ Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+ CameraCal3_S2 cam3(pose3, *K2);
+
+ // three landmarks ~5 meters infront of camera
+ Point3 landmark1(5, 0.5, 1.2);
+ Point3 landmark2(5, -0.5, 1.2);
+ Point3 landmark3(3, 0, 3.0);
+ Point3 landmark4(10, 0.5, 1.2);
+ Point3 landmark5(10, -0.5, 1.2);
+
+ vector measurements_cam1, measurements_cam2, measurements_cam3,
+ measurements_cam4, measurements_cam5;
+
+ // 1. Project three landmarks into three cameras and triangulate
+ projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+ views.push_back(c3);
+
+ SmartFactor::shared_ptr smartFactor1(new SmartFactor());
+ smartFactor1->add(measurements_cam1, views, model);
+
+ SmartFactor::shared_ptr smartFactor2(new SmartFactor());
+ smartFactor2->add(measurements_cam2, views, model);
+
+ SmartFactor::shared_ptr smartFactor3(new SmartFactor());
+ smartFactor3->add(measurements_cam3, views, model);
+
+ SmartFactor::shared_ptr smartFactor4(new SmartFactor());
+ smartFactor4->add(measurements_cam4, views, model);
+
+ SmartFactor::shared_ptr smartFactor5(new SmartFactor());
+ smartFactor5->add(measurements_cam5, views, model);
+
+ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(smartFactor1);
+ graph.push_back(smartFactor2);
+ graph.push_back(smartFactor3);
+ graph.push_back(smartFactor4);
+ graph.push_back(smartFactor5);
+ graph.push_back(PriorFactor(c1, cam1, noisePrior));
+ graph.push_back(PriorFactor(c2, cam2, noisePrior));
+
+ Values values;
+ values.insert(c1, cam1);
+ values.insert(c2, cam2);
+ values.insert(c3, perturbCameraPoseAndCalibration(cam3));
+ if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: ");
+
+ LevenbergMarquardtParams params;
+ params.relativeErrorTol = 1e-8;
+ params.absoluteErrorTol = 0;
+ params.maxIterations = 20;
+ if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+ if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+
+ Values result;
+ gttic_(SmartProjectionCameraFactor);
+ LevenbergMarquardtOptimizer optimizer(graph, values, params);
+ result = optimizer.optimize();
+ gttoc_(SmartProjectionCameraFactor);
+ tictoc_finishedIteration_();
+
+ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
+ // VectorValues delta = GFG->optimize();
+
+ if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: ");
+ EXPECT(assert_equal(cam1,result.at(c1)));
+ EXPECT(assert_equal(cam2,result.at(c2)));
+ EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1));
+ EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 20));
+ if(isDebugTest) tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, Cal3Bundler ){
+
+ // 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));
+ Camera cam1(pose1, *Kbundler);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+ Camera cam2(pose2, *Kbundler);
+
+ // create third camera 1 meter above the first camera
+ Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+ Camera cam3(pose3, *Kbundler);
+
+ // three landmarks ~5 meters infront of camera
+ Point3 landmark1(5, 0.5, 1.2);
+ Point3 landmark2(5, -0.5, 1.2);
+ Point3 landmark3(3, 0, 3.0);
+ Point3 landmark4(10, 0.5, 1.2);
+ Point3 landmark5(10, -0.5, 1.2);
+
+ vector measurements_cam1, measurements_cam2, measurements_cam3,
+ measurements_cam4, measurements_cam5;
+
+ // 1. Project three landmarks into three cameras and triangulate
+ projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+ views.push_back(c3);
+
+ SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
+ smartFactor1->add(measurements_cam1, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
+ smartFactor2->add(measurements_cam2, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
+ smartFactor3->add(measurements_cam3, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
+ smartFactor4->add(measurements_cam4, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
+ smartFactor5->add(measurements_cam5, views, model);
+
+ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(smartFactor1);
+ graph.push_back(smartFactor2);
+ graph.push_back(smartFactor3);
+ graph.push_back(PriorFactor(c1, cam1, noisePrior));
+ graph.push_back(PriorFactor(c2, cam2, noisePrior));
+
+ Values values;
+ values.insert(c1, cam1);
+ values.insert(c2, cam2);
+ // initialize third pose with some noise, we expect it to move back to original pose3
+ values.insert(c3, perturbCameraPose(cam3));
+ if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: ");
+
+ LevenbergMarquardtParams params;
+ params.relativeErrorTol = 1e-8;
+ params.absoluteErrorTol = 0;
+ params.maxIterations = 20;
+ if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+ if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+
+ Values result;
+ gttic_(SmartProjectionCameraFactor);
+ LevenbergMarquardtOptimizer optimizer(graph, values, params);
+ result = optimizer.optimize();
+ gttoc_(SmartProjectionCameraFactor);
+ tictoc_finishedIteration_();
+
+ if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: ");
+ EXPECT(assert_equal(cam1,result.at(c1), 1e-4));
+ EXPECT(assert_equal(cam2,result.at(c2), 1e-4));
+ EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1));
+ EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 1));
+ if(isDebugTest) tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, Cal3Bundler2 ){
+
+ // 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));
+ Camera cam1(pose1, *Kbundler);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+ Camera cam2(pose2, *Kbundler);
+
+ // create third camera 1 meter above the first camera
+ Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+ Camera cam3(pose3, *Kbundler);
+
+ // three landmarks ~5 meters infront of camera
+ Point3 landmark1(5, 0.5, 1.2);
+ Point3 landmark2(5, -0.5, 1.2);
+ Point3 landmark3(3, 0, 3.0);
+ Point3 landmark4(10, 0.5, 1.2);
+ Point3 landmark5(10, -0.5, 1.2);
+
+ vector measurements_cam1, measurements_cam2, measurements_cam3,
+ measurements_cam4, measurements_cam5;
+
+ // 1. Project three landmarks into three cameras and triangulate
+ projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
+ projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
+
+ vector views;
+ views.push_back(c1);
+ views.push_back(c2);
+ views.push_back(c3);
+
+ SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
+ smartFactor1->add(measurements_cam1, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
+ smartFactor2->add(measurements_cam2, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
+ smartFactor3->add(measurements_cam3, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
+ smartFactor4->add(measurements_cam4, views, model);
+
+ SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
+ smartFactor5->add(measurements_cam5, views, model);
+
+ const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(smartFactor1);
+ graph.push_back(smartFactor2);
+ graph.push_back(smartFactor3);
+ graph.push_back(PriorFactor(c1, cam1, noisePrior));
+ graph.push_back(PriorFactor(c2, cam2, noisePrior));
+
+ Values values;
+ values.insert(c1, cam1);
+ values.insert(c2, cam2);
+ // initialize third pose with some noise, we expect it to move back to original pose3
+ values.insert(c3, perturbCameraPoseAndCalibration(cam3));
+ if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: ");
+
+ LevenbergMarquardtParams params;
+ params.relativeErrorTol = 1e-8;
+ params.absoluteErrorTol = 0;
+ params.maxIterations = 20;
+ if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+ if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+
+ Values result;
+ gttic_(SmartProjectionCameraFactor);
+ LevenbergMarquardtOptimizer optimizer(graph, values, params);
+ result = optimizer.optimize();
+ gttoc_(SmartProjectionCameraFactor);
+ tictoc_finishedIteration_();
+
+ if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: ");
+ EXPECT(assert_equal(cam1,result.at(c1), 1e-4));
+ EXPECT(assert_equal(cam2,result.at(c2), 1e-4));
+ EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1));
+ EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2));
+ if(isDebugTest) tictoc_print_();
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ Camera level_camera(level_pose, *Kbundler);
+
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ Camera level_camera_right(level_pose_right, *Kbundler);
+
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+
+ NonlinearFactorGraph smartGraph;
+ SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
+ factor1->add(level_uv, c1, model);
+ factor1->add(level_uv_right, c2, model);
+ smartGraph.push_back(factor1);
+ double expectedError = factor1->error(values);
+ double expectedErrorGraph = smartGraph.error(values);
+ Point3 expectedPoint;
+ if(factor1->point())
+ expectedPoint = *(factor1->point());
+ // cout << "expectedPoint " << expectedPoint.vector() << endl;
+
+ // COMMENTS:
+ // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+ // value in the generalGrap
+ NonlinearFactorGraph generalGraph;
+ SFMFactor sfm1(level_uv, model, c1, l1);
+ SFMFactor sfm2(level_uv_right, model, c2, l1);
+ generalGraph.push_back(sfm1);
+ generalGraph.push_back(sfm2);
+ values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+ Vector e1 = sfm1.evaluateError(values.at< Camera >(c1), values.at< Point3 >(l1));
+ Vector e2 = sfm2.evaluateError(values.at< Camera >(c2), values.at< Point3 >(l1));
+ double actualError = 0.5 * (norm_2(e1)*norm_2(e1) + norm_2(e2)*norm_2(e2));
+ double actualErrorGraph = generalGraph.error(values);
+
+ DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);
+ DOUBLES_EQUAL(expectedErrorGraph, expectedError, 1e-7);
+ DOUBLES_EQUAL(actualErrorGraph, actualError, 1e-7);
+ DOUBLES_EQUAL(expectedError, actualError, 1e-7);
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ Camera level_camera(level_pose, *Kbundler);
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ Camera level_camera_right(level_pose_right, *Kbundler);
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+
+ NonlinearFactorGraph smartGraph;
+ SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
+ factor1->add(level_uv, c1, model);
+ factor1->add(level_uv_right, c2, model);
+ smartGraph.push_back(factor1);
+ Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
+ Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
+ Point3 expectedPoint;
+ if(factor1->point())
+ expectedPoint = *(factor1->point());
+
+ // COMMENTS:
+ // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+ // value in the generalGrap
+ NonlinearFactorGraph generalGraph;
+ SFMFactor sfm1(level_uv, model, c1, l1);
+ SFMFactor sfm2(level_uv_right, model, c2, l1);
+ generalGraph.push_back(sfm1);
+ generalGraph.push_back(sfm2);
+ values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+ Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
+ Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
+ Matrix actualHessian = actualFullHessian.block(0,0,18,18) -
+ actualFullHessian.block(0,18,18,3) * (actualFullHessian.block(18,18,3,3)).inverse() * actualFullHessian.block(18,0,3,18);
+ Vector actualInfoVector = actualFullInfoVector.block(0,0,18,1) -
+ actualFullHessian.block(0,18,18,3) * (actualFullHessian.block(18,18,3,3)).inverse() * actualFullInfoVector.block(18,0,3,1);
+
+ EXPECT(assert_equal(expectedHessian,actualHessian, 1e-7));
+ EXPECT(assert_equal(expectedInfoVector,actualInfoVector, 1e-7));
+}
+
+/* *************************************************************************/
+// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
+//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
+//
+// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+// Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+// cameraObjectBundler level_camera(level_pose, *Kbundler);
+// // create second camera 1 meter to the right of first camera
+// Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+// cameraObjectBundler level_camera_right(level_pose_right, *Kbundler);
+// // landmark ~5 meters infront of camera
+// Point3 landmark(5, 0.5, 1.2);
+// // 1. Project two landmarks into two cameras
+// Point2 level_uv = level_camera.project(landmark);
+// Point2 level_uv_right = level_camera_right.project(landmark);
+//
+// Values values;
+// values.insert(c1, level_camera);
+// values.insert(c2, level_camera_right);
+//
+// NonlinearFactorGraph smartGraph;
+// SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
+// factor1->add(level_uv, c1, model);
+// factor1->add(level_uv_right, c2, model);
+// smartGraph.push_back(factor1);
+// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
+//
+// Point3 expectedPoint;
+// if(factor1->point())
+// expectedPoint = *(factor1->point());
+//
+// // COMMENTS:
+// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
+// // value in the generalGrap
+// NonlinearFactorGraph generalGraph;
+// SFMFactor sfm1(level_uv, model, c1, l1);
+// SFMFactor sfm2(level_uv_right, model, c2, l1);
+// generalGraph.push_back(sfm1);
+// generalGraph.push_back(sfm2);
+// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+// GaussianFactorGraph::shared_ptr gfg = generalGraph.linearize(values);
+//
+// double alpha = 1.0;
+//
+// VectorValues yExpected, yActual, ytmp;
+// VectorValues xtmp = map_list_of
+// (c1, (Vec(9) << 0,0,0,0,0,0,0,0,0))
+// (c2, (Vec(9) << 0,0,0,0,0,0,0,0,0))
+// (l1, (Vec(3) << 5.5, 0.5, 1.2));
+// gfg ->multiplyHessianAdd(alpha, xtmp, ytmp);
+//
+// VectorValues x = map_list_of
+// (c1, (Vec(9) << 1,2,3,4,5,6,7,8,9))
+// (c2, (Vec(9) << 11,12,13,14,15,16,17,18,19))
+// (l1, (Vec(3) << 5.5, 0.5, 1.2));
+//
+// gfgSmart->multiplyHessianAdd(alpha, ytmp + x, yActual);
+// gfg ->multiplyHessianAdd(alpha, x, yExpected);
+//
+// EXPECT(assert_equal(yActual,yExpected, 1e-7));
+//}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, computeImplicitJacobian ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ Camera level_camera(level_pose, *Kbundler);
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ Camera level_camera_right(level_pose_right, *Kbundler);
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+
+ SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
+ factor1->add(level_uv, c1, model);
+ factor1->add(level_uv_right, c2, model);
+ Matrix expectedF, expectedE;
+ Vector expectedb;
+
+ CameraSet< Camera > cameras;
+ cameras.push_back(level_camera);
+ cameras.push_back(level_camera_right);
+
+ factor1->error(values); // this is important to have a triangulation of the point
+ Point3 expectedPoint;
+ if(factor1->point())
+ expectedPoint = *(factor1->point());
+ factor1->computeJacobians(expectedF, expectedE, expectedb, cameras);
+
+ NonlinearFactorGraph generalGraph;
+ SFMFactor sfm1(level_uv, model, c1, l1);
+ SFMFactor sfm2(level_uv_right, model, c2, l1);
+ generalGraph.push_back(sfm1);
+ generalGraph.push_back(sfm2);
+ values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
+ Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
+ Matrix actualVinv = (actualFullHessian.block(18,18,3,3)).inverse();
+
+ Matrix3 expectedVinv = factor1->PointCov(expectedE);
+ EXPECT(assert_equal(expectedVinv,actualVinv, 1e-7));
+}
+
+/* *************************************************************************/
+TEST( SmartProjectionCameraFactor, implicitJacobianFactor ){
+
+ // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+ Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+ Camera level_camera(level_pose, *Kbundler);
+ // create second camera 1 meter to the right of first camera
+ Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+ Camera level_camera_right(level_pose_right, *Kbundler);
+ // landmark ~5 meters infront of camera
+ Point3 landmark(5, 0.5, 1.2);
+ // 1. Project two landmarks into two cameras and triangulate
+ Point2 level_uv = level_camera.project(landmark);
+ Point2 level_uv_right = level_camera_right.project(landmark);
+
+ Values values;
+ values.insert(c1, level_camera);
+ values.insert(c2, level_camera_right);
+ double rankTol = 1;
+ double linThreshold = -1;
+ bool manageDegeneracy = false;
+ bool useEPI = false;
+ bool isImplicit = false;
+
+ // Hessian version
+ SmartFactorBundler::shared_ptr explicitFactor(new SmartFactorBundler(rankTol,linThreshold, manageDegeneracy, useEPI, isImplicit));
+ explicitFactor->add(level_uv, c1, model);
+ explicitFactor->add(level_uv_right, c2, model);
+
+ GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(values);
+ HessianFactor& hessianFactor = dynamic_cast(*gaussianHessianFactor);
+
+ // Implicit Schur version
+ isImplicit = true;
+ SmartFactorBundler::shared_ptr implicitFactor(new SmartFactorBundler(rankTol,linThreshold, manageDegeneracy, useEPI, isImplicit));
+ implicitFactor->add(level_uv, c1, model);
+ implicitFactor->add(level_uv_right, c2, model);
+ GaussianFactor::shared_ptr gaussianImplicitSchurFactor = implicitFactor->linearize(values);
+ typedef RegularImplicitSchurFactor<9> Implicit9;
+ Implicit9& implicitSchurFactor = dynamic_cast(*gaussianImplicitSchurFactor);
+
+ VectorValues x = map_list_of
+ (c1, (Vector(9) << 1,2,3,4,5,6,7,8,9).finished())
+ (c2, (Vector(9) << 11,12,13,14,15,16,17,18,19).finished());
+
+ VectorValues yExpected, yActual;
+ double alpha = 1.0;
+ hessianFactor.multiplyHessianAdd(alpha, x, yActual);
+ implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected);
+ EXPECT(assert_equal(yActual,yExpected, 1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+