testSmartProjectionRigFactor.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>

#include <iostream>

#include "smartFactorScenarios.h"
#define DISABLE_TIMING

using namespace std::placeholders;

static const double rankTol = 1.0;
// Create a noise model for the pixel error
static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));

// Convenience for named keys
using symbol_shorthand::L;
using symbol_shorthand::X;

// tests data
static Symbol x1('X', 1);
static Symbol x2('X', 2);
static Symbol x3('X', 3);

Key cameraId1 = 0;  // first camera
Key cameraId2 = 1;
Key cameraId3 = 2;

static Point2 measurement1(323.0, 240.0);

LevenbergMarquardtParams lmParams;

/* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect
namespace vanillaRig {
using namespace vanillaPose;
SmartProjectionParams params(
    gtsam::HESSIAN,
    gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
}  // namespace vanillaRig

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor) {
  using namespace vanillaRig;
  std::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor2) {
  using namespace vanillaRig;
  std::shared_ptr<Cameras> cameraRig(new Cameras());
  SmartProjectionParams params2(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
  params2.setRankTolerance(rankTol);
  SmartRigFactor factor1(model, cameraRig, params2);
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor3) {
  using namespace vanillaRig;
  std::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
  factor1->add(measurement1, x1, cameraId1);
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor4) {
  using namespace vanillaRig;
  std::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3(), sharedK));
  SmartProjectionParams params2(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
  params2.setRankTolerance(rankTol);
  SmartRigFactor factor1(model, cameraRig, params2);
  factor1.add(measurement1, x1, cameraId1);
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Equals) {
  using namespace vanillaRig;
  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
  factor1->add(measurement1, x1, cameraId1);

  SmartRigFactor::shared_ptr factor2(
      new SmartRigFactor(model, cameraRig, params));
  factor2->add(measurement1, x1, cameraId1);

  CHECK(assert_equal(*factor1, *factor2));

  SmartRigFactor::shared_ptr factor3(
      new SmartRigFactor(model, cameraRig, params));
  factor3->add(measurement1, x1);  // now use default camera ID

  CHECK(assert_equal(*factor1, *factor3));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, noiseless) {
  using namespace vanillaRig;

  // Project two landmarks into two cameras
  Point2 level_uv = level_camera.project(landmark1);
  Point2 level_uv_right = level_camera_right.project(landmark1);

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor factor(model, cameraRig, params);
  factor.add(level_uv, x1);  // both taken from the same camera
  factor.add(level_uv_right, x2);

  Values values;  // it's a pose factor, hence these are poses
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());

  double actualError = factor.error(values);
  double expectedError = 0.0;
  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);

  SmartRigFactor::Cameras cameras = factor.cameras(values);
  double actualError2 = factor.totalReprojectionError(cameras);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);

  // Calculate expected derivative for point (easiest to check)
  std::function<Vector(Point3)> f =  //
      std::bind(&SmartRigFactor::whitenedError<Point3>, factor, cameras,
                std::placeholders::_1);

  // Calculate using computeEP
  Matrix actualE;
  factor.triangulateAndComputeE(actualE, values);

  // get point
  auto point = factor.point();
  CHECK(point);

  // calculate numerical derivative with triangulated point
  Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
  EXPECT(assert_equal(expectedE, actualE, 1e-7));

  // Calculate using reprojectionError
  SmartRigFactor::Cameras::FBlocks F;
  Matrix E;
  Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
  EXPECT(assert_equal(expectedE, E, 1e-7));

  EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));

  // Calculate using computeJacobians
  Vector b;
  SmartRigFactor::FBlocks Fs;
  factor.computeJacobians(Fs, E, b, cameras, *point);
  double actualError3 = b.squaredNorm();
  EXPECT(assert_equal(expectedE, E, 1e-7));
  EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, noisy) {
  using namespace vanillaRig;

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));

  // Project two landmarks into two cameras
  Point2 pixelError(0.2, 0.2);
  Point2 level_uv = level_camera.project(landmark1) + pixelError;
  Point2 level_uv_right = level_camera_right.project(landmark1);

  Values values;
  values.insert(x1, cam1.pose());
  Pose3 noise_pose =
      Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
  values.insert(x2, pose_right.compose(noise_pose));

  SmartRigFactor::shared_ptr factor(
      new SmartRigFactor(model, cameraRig, params));
  factor->add(level_uv, x1, cameraId1);
  factor->add(level_uv_right, x2, cameraId1);

  double actualError1 = factor->error(values);

  // create other factor by passing multiple measurements
  SmartRigFactor::shared_ptr factor2(
      new SmartRigFactor(model, cameraRig, params));

  Point2Vector measurements;
  measurements.push_back(level_uv);
  measurements.push_back(level_uv_right);

  KeyVector views{x1, x2};
  FastVector<size_t> cameraIds{0, 0};

  factor2->add(measurements, views, cameraIds);
  double actualError2 = factor2->error(values);
  DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
  using namespace vanillaRig;

  // create arbitrary body_T_sensor (transforms from sensor to body)
  Pose3 body_T_sensor =
      Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(body_T_sensor, sharedK));

  // These are the poses we want to estimate, from camera measurements
  const Pose3 sensor_T_body = body_T_sensor.inverse();
  Pose3 wTb1 = cam1.pose() * sensor_T_body;
  Pose3 wTb2 = cam2.pose() * sensor_T_body;
  Pose3 wTb3 = cam3.pose() * sensor_T_body;

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // Create smart factors
  KeyVector views{x1, x2, x3};
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setDegeneracyMode(ZERO_ON_DEGENERACY);
  params.setEnableEPI(false);

  SmartRigFactor smartFactor1(model, cameraRig, params);
  smartFactor1.add(
      measurements_cam1,
      views);  // use default CameraIds since we have a single camera

  SmartRigFactor smartFactor2(model, cameraRig, params);
  smartFactor2.add(measurements_cam2, views);

  SmartRigFactor smartFactor3(model, cameraRig, params);
  smartFactor3.add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  // Put all factors in factor graph, adding priors
  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, wTb1, noisePrior);
  graph.addPrior(x2, wTb2, noisePrior);

  // Check errors at ground truth poses
  Values gtValues;
  gtValues.insert(x1, wTb1);
  gtValues.insert(x2, wTb2);
  gtValues.insert(x3, wTb3);
  double actualError = graph.error(gtValues);
  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-7)

  Pose3 noise_pose =
      Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
  Values values;
  values.insert(x1, wTb1);
  values.insert(x2, wTb2);
  // initialize third pose with some noise, we expect it to move back to
  // original pose3
  values.insert(x3, wTb3 * noise_pose);

  LevenbergMarquardtParams lmParams;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
  using namespace vanillaRig;

  // create arbitrary body_T_sensor (transforms from sensor to body)
  Pose3 body_T_sensor1 =
      Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
  Pose3 body_T_sensor2 =
      Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
  Pose3 body_T_sensor3 = Pose3();

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(body_T_sensor1, sharedK));
  cameraRig->push_back(Camera(body_T_sensor2, sharedK));
  cameraRig->push_back(Camera(body_T_sensor3, sharedK));

  // These are the poses we want to estimate, from camera measurements
  const Pose3 sensor_T_body1 = body_T_sensor1.inverse();
  const Pose3 sensor_T_body2 = body_T_sensor2.inverse();
  const Pose3 sensor_T_body3 = body_T_sensor3.inverse();
  Pose3 wTb1 = cam1.pose() * sensor_T_body1;
  Pose3 wTb2 = cam2.pose() * sensor_T_body2;
  Pose3 wTb3 = cam3.pose() * sensor_T_body3;

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // Create smart factors
  KeyVector views{x1, x2, x3};
  FastVector<size_t> cameraIds{0, 1, 2};

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setDegeneracyMode(ZERO_ON_DEGENERACY);
  params.setEnableEPI(false);

  SmartRigFactor smartFactor1(model, cameraRig, params);
  smartFactor1.add(measurements_cam1, views, cameraIds);

  SmartRigFactor smartFactor2(model, cameraRig, params);
  smartFactor2.add(measurements_cam2, views, cameraIds);

  SmartRigFactor smartFactor3(model, cameraRig, params);
  smartFactor3.add(measurements_cam3, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  // Put all factors in factor graph, adding priors
  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, wTb1, noisePrior);
  graph.addPrior(x2, wTb2, noisePrior);

  // Check errors at ground truth poses
  Values gtValues;
  gtValues.insert(x1, wTb1);
  gtValues.insert(x2, wTb2);
  gtValues.insert(x3, wTb3);
  double actualError = graph.error(gtValues);
  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-7)

  Pose3 noise_pose =
      Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
  Values values;
  values.insert(x1, wTb1);
  values.insert(x2, wTb2);
  // initialize third pose with some noise, we expect it to move back to
  // original pose3
  values.insert(x3, wTb3 * noise_pose);

  LevenbergMarquardtParams lmParams;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(wTb3, result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
  using namespace vanillaPose2;
  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK2));

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  KeyVector views{x1, x2, x3};
  FastVector<size_t> cameraIds{
      0, 0, 0};  // 3 measurements from the same camera in the rig

  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_cam3, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  Values groundTruth;
  groundTruth.insert(x1, cam1.pose());
  groundTruth.insert(x2, cam2.pose());
  groundTruth.insert(x3, cam3.pose());
  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(assert_equal(
      Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
            Point3(0.1, -0.1, 1.9)),
      values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, Factors) {
  using namespace vanillaRig;

  // Default cameras for simple derivatives
  Rot3 R;
  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK),
      cam2(Pose3(R, Point3(1, 0, 0)), sharedK);

  // one landmarks 1m in front of camera
  Point3 landmark1(0, 0, 10);

  Point2Vector measurements_cam1;

  // Project 2 landmarks into 2 cameras
  measurements_cam1.push_back(cam1.project(landmark1));
  measurements_cam1.push_back(cam2.project(landmark1));

  // Create smart factors
  KeyVector views{x1, x2};
  FastVector<size_t> cameraIds{0, 0};

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));

  SmartRigFactor::shared_ptr smartFactor1 = std::make_shared<SmartRigFactor>(
      model, cameraRig, params);
  smartFactor1->add(measurements_cam1,
                    views);  // we have a single camera so use default cameraIds

  SmartRigFactor::Cameras cameras;
  cameras.push_back(cam1);
  cameras.push_back(cam2);

  // Make sure triangulation works
  CHECK(smartFactor1->triangulateSafe(cameras));
  CHECK(!smartFactor1->isDegenerate());
  CHECK(!smartFactor1->isPointBehindCamera());
  auto p = smartFactor1->point();
  CHECK(p);
  EXPECT(assert_equal(landmark1, *p));

  VectorValues zeroDelta;
  Vector6 delta;
  delta.setZero();
  zeroDelta.insert(x1, delta);
  zeroDelta.insert(x2, delta);

  VectorValues perturbedDelta;
  delta.setOnes();
  perturbedDelta.insert(x1, delta);
  perturbedDelta.insert(x2, delta);
  double expectedError = 2500;

  // After eliminating the point, A1 and A2 contain 2-rank information on
  // cameras:
  Matrix16 A1, A2;
  A1 << -10, 0, 0, 0, 1, 0;
  A2 << 10, 0, 1, 0, -1, 0;
  A1 *= 10. / sigma;
  A2 *= 10. / sigma;
  Matrix expectedInformation;  // filled below
  {
    // createHessianFactor
    Matrix66 G11 = 0.5 * A1.transpose() * A1;
    Matrix66 G12 = 0.5 * A1.transpose() * A2;
    Matrix66 G22 = 0.5 * A2.transpose() * A2;

    Vector6 g1;
    g1.setZero();
    Vector6 g2;
    g2.setZero();

    double f = 0;

    RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f);
    expectedInformation = expected.information();

    Values values;
    values.insert(x1, cam1.pose());
    values.insert(x2, cam2.pose());

    std::shared_ptr<RegularHessianFactor<6>> actual =
        smartFactor1->createHessianFactor(values, 0.0);
    EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
    EXPECT(assert_equal(expected, *actual, 1e-6));
    EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
    EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
  }
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
  using namespace vanillaRig;

  KeyVector views{x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // create smart factor
  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};
  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_cam3, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(assert_equal(Pose3(Rot3(1.11022302e-16, -0.0314107591, 0.99950656,
                                 -0.99950656, -0.0313952598, -0.000986635786,
                                 0.0314107591, -0.999013364, -0.0313952598),
                            Point3(0.1, -0.1, 1.9)),
                      values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, landmarkDistance) {
  using namespace vanillaRig;

  double excludeLandmarksFutherThanDist = 2;

  KeyVector views{x1, x2, x3};

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setLinearizationMode(gtsam::HESSIAN);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setEnableEPI(false);

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_cam3, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, pose_above * noise_pose);

  // All factors are disabled and pose should remain where it is
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
  using namespace vanillaRig;

  double excludeLandmarksFutherThanDist = 1e10;
  double dynamicOutlierRejectionThreshold =
      1;  // max 1 pixel of average reprojection error

  KeyVector views{x1, x2, x3};

  // add fourth landmark
  Point3 landmark4(5, -0.5, 1);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
      measurements_cam4;

  // Project 4 landmarks into three cameras
  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);
  measurements_cam4.at(0) =
      measurements_cam4.at(0) + Point2(10, 10);  // add outlier

  SmartProjectionParams params;
  params.setLinearizationMode(gtsam::HESSIAN);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_cam3, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor4(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor4->add(measurements_cam4, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(smartFactor4);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  values.insert(x3, cam3.pose());

  // All factors are disabled and pose should remain where it is
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, CheckHessian) {
  KeyVector views{x1, x2, x3};

  using namespace vanillaRig;

  // Two slightly different cameras
  Pose3 pose2 =
      level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
  Camera cam2(pose2, sharedK);
  Camera cam3(pose3, sharedK);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  SmartProjectionParams params;
  params.setRankTolerance(10);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));  // HESSIAN, by default
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));  // HESSIAN, by default
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));  // HESSIAN, by default
  smartFactor3->add(measurements_cam3, views, cameraIds);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose3 * noise_pose);
  EXPECT(assert_equal(Pose3(Rot3(0.00563056869, -0.130848107, 0.991386438,
                                 -0.991390265, -0.130426831, -0.0115837907,
                                 0.130819108, -0.98278564, -0.130455917),
                            Point3(0.0897734171, -0.110201006, 0.901022872)),
                      values.at<Pose3>(x3)));

  std::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
  std::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
  std::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);

  Matrix CumulativeInformation =
      factor1->information() + factor2->information() + factor3->information();

  std::shared_ptr<GaussianFactorGraph> GaussianGraph =
      graph.linearize(values);
  Matrix GraphInformation = GaussianGraph->hessian().first;

  // Check Hessian
  EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));

  Matrix AugInformationMatrix = factor1->augmentedInformation() +
                                factor2->augmentedInformation() +
                                factor3->augmentedInformation();

  // Check Information vector
  Vector InfoVector = AugInformationMatrix.block(
      0, 18, 18, 1);  // 18x18 Hessian + information vector

  // Check Hessian
  EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, Hessian) {
  using namespace vanillaPose2;

  KeyVector views{x1, x2};

  // Project three landmarks into 2 cameras
  Point2 cam1_uv1 = cam1.project(landmark1);
  Point2 cam2_uv1 = cam2.project(landmark1);
  Point2Vector measurements_cam1;
  measurements_cam1.push_back(cam1_uv1);
  measurements_cam1.push_back(cam2_uv1);

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK2));
  FastVector<size_t> cameraIds{0, 0};

  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  Pose3 noise_pose =
      Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());

  std::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);

  // compute triangulation from linearization point
  // compute reprojection errors (sum squared)
  // compare with factor.info(): the bottom right element is the squared sum of
  // the reprojection errors (normalized by the covariance) check that it is
  // correctly scaled when using noiseProjection = [1/4  0; 0 1/4]
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
  using namespace bundlerPose;
  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));

  SmartProjectionParams params;
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  SmartRigFactor factor(model, cameraRig, params);
  factor.add(measurement1, x1, cameraId1);
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, Cal3Bundler) {
  using namespace bundlerPose;
  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors

  // three landmarks ~5 meters in front of camera
  Point3 landmark3(3, 0, 3.0);

  Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  KeyVector views{x1, x2, x3};

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
  FastVector<size_t> cameraIds{0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_cam1, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_cam2, views, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_cam3, views, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, cam1.pose(), noisePrior);
  graph.addPrior(x2, cam2.pose(), noisePrior);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, cam1.pose());
  values.insert(x2, cam2.pose());
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(assert_equal(
      Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
                 -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
            Point3(0.1, -0.1, 1.9)),
      values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
}

#include <gtsam/slam/ProjectionFactor.h>
typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
static Symbol l0('L', 0);
/* *************************************************************************/
TEST(SmartProjectionRigFactor,
     hessianComparedToProjFactors_measurementsFromSamePose) {
  // in this test we make sure the fact works even if we have multiple pixel
  // measurements of the same landmark at a single pose, a setup that occurs in
  // multi-camera systems

  using namespace vanillaRig;
  Point2Vector measurements_lmk1;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);

  // create redundant measurements:
  Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
  measurements_lmk1_redundant.push_back(
      measurements_lmk1.at(0));  // we readd the first measurement

  // create inputs
  KeyVector keys{x1, x2, x3, x1};

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0, 0};

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  // initialize third pose with some noise to get a nontrivial linearization
  // point
  values.insert(x3, pose_above * noise_pose);
  EXPECT(  // check that the pose is actually noisy
      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
                              -0.0313952598, -0.000986635786, 0.0314107591,
                              -0.999013364, -0.0313952598),
                         Point3(0.1, -0.1, 1.9)),
                   values.at<Pose3>(x3)));

  // linearization point for the poses
  Pose3 pose1 = level_pose;
  Pose3 pose2 = pose_right;
  Pose3 pose3 = pose_above * noise_pose;

  // ==== check Hessian of smartFactor1 =====
  // -- compute actual Hessian
  std::shared_ptr<GaussianFactor> linearfactor1 =
      smartFactor1->linearize(values);
  Matrix actualHessian = linearfactor1->information();

  // -- compute expected Hessian from manual Schur complement from Jacobians
  // linearization point for the 3D point
  smartFactor1->triangulateSafe(smartFactor1->cameras(values));
  TriangulationResult point = smartFactor1->point();
  EXPECT(point.valid());  // check triangulated point is valid

  // Use standard ProjectionFactor factor to calculate the Jacobians
  Matrix F = Matrix::Zero(2 * 4, 6 * 3);
  Matrix E = Matrix::Zero(2 * 4, 3);
  Vector b = Vector::Zero(2 * 4);

  // create projection factors rolling shutter
  TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
                                sharedK);
  Matrix HPoseActual, HEActual;
  // note: b is minus the reprojection error, cf the smart factor jacobian
  // computation
  b.segment<2>(0) =
      -factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
  F.block<2, 6>(0, 0) = HPoseActual;
  E.block<2, 3>(0, 0) = HEActual;

  TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
                                sharedK);
  b.segment<2>(2) =
      -factor12.evaluateError(pose2, *point, HPoseActual, HEActual);
  F.block<2, 6>(2, 6) = HPoseActual;
  E.block<2, 3>(2, 0) = HEActual;

  TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
                                sharedK);
  b.segment<2>(4) =
      -factor13.evaluateError(pose3, *point, HPoseActual, HEActual);
  F.block<2, 6>(4, 12) = HPoseActual;
  E.block<2, 3>(4, 0) = HEActual;

  TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
                                sharedK);
  b.segment<2>(6) =
      -factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
  F.block<2, 6>(6, 0) = HPoseActual;
  E.block<2, 3>(6, 0) = HEActual;

  // whiten
  F = (1 / sigma) * F;
  E = (1 / sigma) * E;
  b = (1 / sigma) * b;
  //* G = F' * F - F' * E * P * E' * F
  Matrix P = (E.transpose() * E).inverse();
  Matrix expectedHessian =
      F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));

  // ==== check Information vector of smartFactor1 =====
  GaussianFactorGraph gfg;
  gfg.add(linearfactor1);
  Matrix actualHessian_v2 = gfg.hessian().first;
  EXPECT(assert_equal(actualHessian_v2, actualHessian,
                      1e-6));  // sanity check on hessian

  // -- compute actual information vector
  Vector actualInfoVector = gfg.hessian().second;

  // -- compute expected information vector from manual Schur complement from
  // Jacobians
  //* g = F' * (b - E * P * E' * b)
  Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));

  // ==== check error of smartFactor1 (again) =====
  NonlinearFactorGraph nfg_projFactors;
  nfg_projFactors.add(factor11);
  nfg_projFactors.add(factor12);
  nfg_projFactors.add(factor13);
  nfg_projFactors.add(factor14);
  values.insert(l0, *point);

  double actualError = smartFactor1->error(values);
  double expectedError = nfg_projFactors.error(values);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}

/* *************************************************************************/
TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
  using namespace vanillaRig;
  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);

  // create inputs
  KeyVector keys{x1, x2, x3};
  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3(), sharedK));
  FastVector<size_t> cameraIds{0, 0, 0};
  FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};

  // For first factor, we create redundant measurement (taken by the same keys
  // as factor 1, to make sure the redundancy in the keys does not create
  // problems)
  Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
  measurements_lmk1_redundant.push_back(
      measurements_lmk1.at(0));  // we readd the first measurement
  KeyVector keys_redundant = keys;
  keys_redundant.push_back(keys.at(0));  // we readd the first key

  SmartRigFactor::shared_ptr smartFactor1(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1_redundant, keys_redundant,
                    cameraIdsRedundant);

  SmartRigFactor::shared_ptr smartFactor2(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, keys, cameraIds);

  SmartRigFactor::shared_ptr smartFactor3(
      new SmartRigFactor(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, keys, cameraIds);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, level_pose, noisePrior);
  graph.addPrior(x2, pose_right, noisePrior);

  Values groundTruth;
  groundTruth.insert(x1, level_pose);
  groundTruth.insert(x2, pose_right);
  groundTruth.insert(x3, pose_above);
  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

  //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
  //  Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
                           Point3(0.1, 0.1, 0.1));  // smaller noise
  Values values;
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose_above * noise_pose);
  EXPECT(  // check that the pose is actually noisy
      assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
                              -0.0313952598, -0.000986635786, 0.0314107591,
                              -0.999013364, -0.0313952598),
                         Point3(0.1, -0.1, 1.9)),
                   values.at<Pose3>(x3)));

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
}

#ifndef DISABLE_TIMING
#include <gtsam/base/timing.h>
// this factor is slightly slower (but comparable) to original
// SmartProjectionPoseFactor
//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0)
//|   -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05
// children, min: 0 max: 0) |   -SmartPoseFactor LINEARIZE: 0.05 CPU (10000
// times, 0.069647 wall, 0.05 children, min: 0 max: 0)
/* *************************************************************************/
TEST(SmartProjectionRigFactor, timing) {
  using namespace vanillaRig;

  // Default cameras for simple derivatives
  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));

  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
  Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
  Pose3 body_P_sensorId = Pose3();

  std::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));

  // one landmarks 1m in front of camera
  Point3 landmark1(0, 0, 10);

  Point2Vector measurements_lmk1;

  // Project 2 landmarks into 2 cameras
  measurements_lmk1.push_back(cam1.project(landmark1));
  measurements_lmk1.push_back(cam2.project(landmark1));

  size_t nrTests = 10000;

  for (size_t i = 0; i < nrTests; i++) {
    SmartRigFactor::shared_ptr smartRigFactor(
        new SmartRigFactor(model, cameraRig, params));
    smartRigFactor->add(measurements_lmk1[0], x1, cameraId1);
    smartRigFactor->add(measurements_lmk1[1], x1, cameraId1);

    Values values;
    values.insert(x1, pose1);
    values.insert(x2, pose2);
    gttic_(SmartRigFactor_LINEARIZE);
    smartRigFactor->linearize(values);
    gttoc_(SmartRigFactor_LINEARIZE);
  }

  for (size_t i = 0; i < nrTests; i++) {
    SmartFactor::shared_ptr smartFactor(
        new SmartFactor(model, sharedKSimple, params));
    smartFactor->add(measurements_lmk1[0], x1);
    smartFactor->add(measurements_lmk1[1], x2);

    Values values;
    values.insert(x1, pose1);
    values.insert(x2, pose2);
    gttic_(SmartPoseFactor_LINEARIZE);
    smartFactor->linearize(values);
    gttoc_(SmartPoseFactor_LINEARIZE);
  }
  tictoc_print_();
}
#endif

/* *************************************************************************/
TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
  using namespace sphericalCamera;
  Camera::MeasurementVector measurements_lmk1, measurements_lmk2,
      measurements_lmk3;

  // Project three landmarks into three cameras
  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1,
                                   measurements_lmk1);
  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2,
                                   measurements_lmk2);
  projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3,
                                   measurements_lmk3);

  // create inputs
  KeyVector keys;
  keys.push_back(x1);
  keys.push_back(x2);
  keys.push_back(x3);

  std::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3(), emptyK));

  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
  params.setRankTolerance(0.1);

  SmartFactorP::shared_ptr smartFactor1(
      new SmartFactorP(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, keys);

  SmartFactorP::shared_ptr smartFactor2(
      new SmartFactorP(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, keys);

  SmartFactorP::shared_ptr smartFactor3(
      new SmartFactorP(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, keys);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.addPrior(x1, level_pose, noisePrior);
  graph.addPrior(x2, pose_right, noisePrior);

  Values groundTruth;
  groundTruth.insert(x1, level_pose);
  groundTruth.insert(x2, pose_right);
  groundTruth.insert(x3, pose_above);
  DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 100),
                           Point3(0.2, 0.2, 0.2));  // note: larger noise!

  Values values;
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  // initialize third pose with some noise, we expect it to move back to
  // original pose_above
  values.insert(x3, pose_above * noise_pose);

  DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9);

  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();

  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
}

#ifndef DISABLE_TIMING
#include <gtsam/base/timing.h>
// using spherical camera is slightly slower (but comparable) to
// PinholePose<Cal3_S2>
//|   -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall,
// 0.01 children, min: 0 max: 0) |   -SmartFactorP pinhole LINEARIZE: 0.01 CPU
//(1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0)
/* *************************************************************************/
TEST(SmartProjectionFactorP, timing_sphericalCamera) {
  // create common data
  Rot3 R = Rot3::Identity();
  Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
  Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
  Pose3 body_P_sensorId = Pose3();
  Point3 landmark1(0, 0, 10);

  // create spherical data
  EmptyCal::shared_ptr emptyK;
  SphericalCamera cam1_sphere(pose1, emptyK), cam2_sphere(pose2, emptyK);
  // Project 2 landmarks into 2 cameras
  std::vector<Unit3> measurements_lmk1_sphere;
  measurements_lmk1_sphere.push_back(cam1_sphere.project(landmark1));
  measurements_lmk1_sphere.push_back(cam2_sphere.project(landmark1));

  // create Cal3_S2 data
  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
  PinholePose<Cal3_S2> cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
  // Project 2 landmarks into 2 cameras
  std::vector<Point2> measurements_lmk1;
  measurements_lmk1.push_back(cam1.project(landmark1));
  measurements_lmk1.push_back(cam2.project(landmark1));

  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors

  size_t nrTests = 1000;

  for (size_t i = 0; i < nrTests; i++) {
    std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
        new CameraSet<SphericalCamera>());  // single camera in the rig
    cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK));

    SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP(
        new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig,
                                                      params));
    smartFactorP->add(measurements_lmk1_sphere[0], x1);
    smartFactorP->add(measurements_lmk1_sphere[1], x1);

    Values values;
    values.insert(x1, pose1);
    values.insert(x2, pose2);
    gttic_(SmartFactorP_spherical_LINEARIZE);
    smartFactorP->linearize(values);
    gttoc_(SmartFactorP_spherical_LINEARIZE);
  }

  for (size_t i = 0; i < nrTests; i++) {
    std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple));

    SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2(
        new SmartProjectionRigFactor<PinholePose<Cal3_S2>>(model, cameraRig,
                                                           params));
    smartFactorP2->add(measurements_lmk1[0], x1);
    smartFactorP2->add(measurements_lmk1[1], x1);

    Values values;
    values.insert(x1, pose1);
    values.insert(x2, pose2);
    gttic_(SmartFactorP_pinhole_LINEARIZE);
    smartFactorP2->linearize(values);
    gttoc_(SmartFactorP_pinhole_LINEARIZE);
  }
  tictoc_print_();
}
#endif

/* *************************************************************************/
TEST(SmartProjectionFactorP, 2poses_rankTol) {
  Pose3 poseA = Pose3(
      Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
      Point3(0.0, 0.0, 0.0));  // with z pointing along x axis of global frame
  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
                      Point3(0.0, -0.1, 0.0));  // 10cm to the right of poseA
  Point3 landmarkL = Point3(5.0, 0.0, 0.0);     // 5m in front of poseA

  // triangulate from a stereo with 10cm baseline, assuming standard calibration
  {  // default rankTol = 1 gives a valid point (compare with calibrated and
     // spherical cameras below)
    using namespace vanillaPose;  // pinhole with Cal3_S2 calibration

    Camera cam1(poseA, sharedK);
    Camera cam2(poseB, sharedK);

    SmartProjectionParams params(
        gtsam::HESSIAN,
        gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
    params.setRankTolerance(1);

    std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
    smartFactor1->add(cam1.project(landmarkL), x1);
    smartFactor1->add(cam2.project(landmarkL), x2);

    NonlinearFactorGraph graph;
    graph.push_back(smartFactor1);

    Values groundTruth;
    groundTruth.insert(x1, poseA);
    groundTruth.insert(x2, poseB);
    DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

    // get point
    TriangulationResult point = smartFactor1->point();
    EXPECT(point.valid());  // valid triangulation
    EXPECT(assert_equal(landmarkL, *point, 1e-7));
  }
  // triangulate from a stereo with 10cm baseline, assuming canonical
  // calibration
  {  // default rankTol = 1 or 0.1 gives a degenerate point, which is
     // undesirable for a point 5m away and 10cm baseline
    using namespace vanillaPose;  // pinhole with Cal3_S2 calibration
    static Cal3_S2::shared_ptr canonicalK(
        new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0));  // canonical camera

    Camera cam1(poseA, canonicalK);
    Camera cam2(poseB, canonicalK);

    SmartProjectionParams params(
        gtsam::HESSIAN,
        gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
    params.setRankTolerance(0.1);

    std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
            new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
    smartFactor1->add(cam1.project(landmarkL), x1);
    smartFactor1->add(cam2.project(landmarkL), x2);

    NonlinearFactorGraph graph;
    graph.push_back(smartFactor1);

    Values groundTruth;
    groundTruth.insert(x1, poseA);
    groundTruth.insert(x2, poseB);
    DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

    // get point
    TriangulationResult point = smartFactor1->point();
    EXPECT(point.degenerate());  // valid triangulation
  }
  // triangulate from a stereo with 10cm baseline, assuming canonical
  // calibration
  {  // smaller rankTol = 0.01 gives a valid point (compare with calibrated and
     // spherical cameras below)
    using namespace vanillaPose;  // pinhole with Cal3_S2 calibration
    static Cal3_S2::shared_ptr canonicalK(
        new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0));  // canonical camera

    Camera cam1(poseA, canonicalK);
    Camera cam2(poseB, canonicalK);

    SmartProjectionParams params(
        gtsam::HESSIAN,
        gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
    params.setRankTolerance(0.01);

    std::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
    smartFactor1->add(cam1.project(landmarkL), x1);
    smartFactor1->add(cam2.project(landmarkL), x2);

    NonlinearFactorGraph graph;
    graph.push_back(smartFactor1);

    Values groundTruth;
    groundTruth.insert(x1, poseA);
    groundTruth.insert(x2, poseB);
    DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

    // get point
    TriangulationResult point = smartFactor1->point();
    EXPECT(point.valid());  // valid triangulation
    EXPECT(assert_equal(landmarkL, *point, 1e-7));
  }
}

/* *************************************************************************/
TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
  typedef SphericalCamera Camera;
  typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
  EmptyCal::shared_ptr emptyK(new EmptyCal());
  Pose3 poseA = Pose3(
      Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
      Point3(0.0, 0.0, 0.0));  // with z pointing along x axis of global frame
  Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
                      Point3(0.0, -0.1, 0.0));  // 10cm to the right of poseA
  Point3 landmarkL = Point3(5.0, 0.0, 0.0);     // 5m in front of poseA

  Camera cam1(poseA);
  Camera cam2(poseB);

  std::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
          new CameraSet<SphericalCamera>());  // single camera in the rig
  cameraRig->push_back(SphericalCamera(Pose3(), emptyK));

  // TRIANGULATION TEST WITH DEFAULT RANK TOL
  {  // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
     // point 5m away and 10cm baseline
    SmartProjectionParams params(
        gtsam::HESSIAN,
        gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
    params.setRankTolerance(0.1);

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
    smartFactor1->add(cam1.project(landmarkL), x1);
    smartFactor1->add(cam2.project(landmarkL), x2);

    NonlinearFactorGraph graph;
    graph.push_back(smartFactor1);

    Values groundTruth;
    groundTruth.insert(x1, poseA);
    groundTruth.insert(x2, poseB);
    DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

    // get point
    TriangulationResult point = smartFactor1->point();
    EXPECT(point.degenerate());  // not enough parallax
  }
  // SAME TEST WITH SMALLER RANK TOL
  {  // rankTol = 0.01 gives a valid point
    // By playing with this test, we can show we can triangulate also with a
    // baseline of 5cm (even for points far away, >100m), but the test fails
    // when the baseline becomes 1cm. This suggests using rankTol = 0.01 and
    // setting a reasonable max landmark distance to obtain best results.
    SmartProjectionParams params(
        gtsam::HESSIAN,
        gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
    params.setRankTolerance(0.01);

    SmartRigFactor::shared_ptr smartFactor1(
        new SmartRigFactor(model, cameraRig, params));
    smartFactor1->add(cam1.project(landmarkL), x1);
    smartFactor1->add(cam2.project(landmarkL), x2);

    NonlinearFactorGraph graph;
    graph.push_back(smartFactor1);

    Values groundTruth;
    groundTruth.insert(x1, poseA);
    groundTruth.insert(x2, poseB);
    DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);

    // get point
    TriangulationResult point = smartFactor1->point();
    EXPECT(point.valid());  // valid triangulation
    EXPECT(assert_equal(landmarkL, *point, 1e-7));
  }
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */