/*
 * camera_lidar_calibration.cpp
 *
 *  Created on: Sep 4, 2017
 *      Author: amc-jp
 */

#include <string>
#include <vector>
#include <ctime>
#include <sstream>
#include <unistd.h>
#include <sys/types.h>
#include <pwd.h>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <boost/filesystem.hpp>

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#if (CV_MAJOR_VERSION == 3)
  #include <opencv2/imgcodecs.hpp>
#else
  #include <opencv2/contrib/contrib.hpp>
#endif

#define __APP_NAME__ "autoware_camera_lidar_calibration_node"

class ROSCameraLidarApp

{
  ros::NodeHandle   node_handle_;

  ros::Subscriber   subscriber_image_raw_;
  ros::Subscriber   subscriber_points_raw_;
  ros::Subscriber     subscriber_intrinsics_;
  ros::Subscriber     subscriber_clicked_point_;
  ros::Subscriber     subscriber_image_point_;
  ros::Subscriber     subscriber_image_point_stamped_;

  cv::Size            image_size_;
  cv::Mat             camera_instrinsics_;
  cv::Mat             distortion_coefficients_;

  cv::Mat             lidar_camera_rotation_matrix_;
  cv::Mat             lidar_camera_translation_vector_;

  std::vector<cv::Point2f> clicked_image_points_;
  std::vector<cv::Point3f> clicked_velodyne_points_;

  bool is_rotation_matrix(const cv::Mat& in_matrix)
  {
    cv::Mat rotation_matrix;
    transpose(in_matrix, rotation_matrix);

    cv::Mat test_matrix = rotation_matrix * in_matrix;
    cv::Mat test_result = cv::Mat::eye(3,3, test_matrix.type());

    return cv::norm(test_result, test_matrix) < 1e-6;

  }

  cv::Point3f get_rpy_from_matrix(const cv::Mat& in_rotation)
  {

    assert(is_rotation_matrix(in_rotation));

    float sy = sqrt(in_rotation.at<double>(0,0) * in_rotation.at<double>(0,0) +
                        in_rotation.at<double>(1,0) * in_rotation.at<double>(1,0) );

    bool is_singular = sy < 1e-6;

    float x, y, z;
    if (!is_singular)
    {
      x = atan2(in_rotation.at<double>(2,1), in_rotation.at<double>(2,2));
      y = atan2(-in_rotation.at<double>(2,0), sy);
      z = atan2(in_rotation.at<double>(1,0), in_rotation.at<double>(0,0));
    }
    else
    {
      x = atan2(-in_rotation.at<double>(1,2), in_rotation.at<double>(1,1));
      y = atan2(-in_rotation.at<double>(2,0), sy);
      z = 0;
    }
    return cv::Point3f(RAD2DEG(x), RAD2DEG(y), RAD2DEG(z));
  }

  void SaveCalibrationFile(cv::Mat in_extrinsic, cv::Mat in_intrinsic, cv::Mat in_dist_coeff, cv::Size in_size)
  {
    std::string path_filename_str;
    const char *homedir;

    time_t rawtime;
    struct tm * timeinfo;
    char buffer[80];

    time (&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer,sizeof(buffer),"%Y%m%d_%H%M%S",timeinfo);
    std::string datetime_str(buffer);

    if ((homedir = getenv("HOME")) == NULL) {
      homedir = getpwuid(getuid())->pw_dir;
    }

    path_filename_str = std::string(homedir) + "/"+datetime_str+"_autoware_lidar_camera_calibration.yaml";

    cv::FileStorage fs(path_filename_str.c_str(), cv::FileStorage::WRITE);
    if(!fs.isOpened()){
      fprintf(stderr, "%s : cannot open file\n", path_filename_str.c_str());
      exit(EXIT_FAILURE);
    }

    fs << "CameraExtrinsicMat" << in_extrinsic;
    fs << "CameraMat" << in_intrinsic;
    fs << "DistCoeff" << in_dist_coeff;
    fs << "ImageSize" << in_size;
    fs << "ReprojectionError" << 0;
    fs << "DistModel" << "plumb_bob";

    ROS_INFO("Wrote Autoware Calibration file in: %s", path_filename_str.c_str());
  }

  void CalibrateSensors()
  {
    std::cout << "Number of points: " << clicked_velodyne_points_.size() << ":"  << clicked_image_points_.size() << std::endl;

    if (clicked_velodyne_points_.size() == clicked_image_points_.size()
        && clicked_image_points_.size() > 8)
    {
      cv::Mat rotation_vector, translation_vector;

      cv::solvePnP(clicked_velodyne_points_,
                   clicked_image_points_,
                   camera_instrinsics_,
                   distortion_coefficients_,
                   rotation_vector,
                   translation_vector,
                   true,
                   CV_EPNP
      );

      cv::Mat rotation_matrix;
      cv::Rodrigues(rotation_vector, rotation_matrix);

      cv::Mat camera_velodyne_rotation = rotation_matrix.t();
      cv::Point3f camera_velodyne_point(translation_vector);
      cv::Point3f camera_velodyne_translation;

      camera_velodyne_translation.x = -camera_velodyne_point.z;
      camera_velodyne_translation.y = camera_velodyne_point.x;
      camera_velodyne_translation.z = camera_velodyne_point.y;

      std::cout << "Rotation:" << camera_velodyne_rotation << std::endl << std::endl;
      std::cout << "Translation:" << camera_velodyne_translation << std::endl;
      std::cout << "RPY: " << get_rpy_from_matrix(camera_velodyne_rotation) << std::endl << std::endl;

      cv::Mat extrinsics = cv::Mat::eye(4,4, CV_64F);
      camera_velodyne_rotation.copyTo(extrinsics(cv::Rect_<float>(0,0,3,3)));

      std::cout << extrinsics << std::endl;

      extrinsics.at<double>(0,3) = camera_velodyne_translation.x;
      extrinsics.at<double>(1,3) = camera_velodyne_translation.y;
      extrinsics.at<double>(2,3) = camera_velodyne_translation.z;

      std::cout << extrinsics << std::endl;

      SaveCalibrationFile(extrinsics ,camera_instrinsics_, distortion_coefficients_, image_size_);
    }
  }

  void RvizClickedPointCallback(const geometry_msgs::PointStamped& in_clicked_point)
  {
    clicked_velodyne_points_.push_back(cv::Point3f(in_clicked_point.point.x,
                                                   in_clicked_point.point.y,
                                                   in_clicked_point.point.z));
    std::cout << cv::Point3f(in_clicked_point.point.x,
                             in_clicked_point.point.y,
                             in_clicked_point.point.z) << std::endl << std::endl;
    CalibrateSensors();
  }

  void ImageClickedPointStampedCallback(const geometry_msgs::PointStamped& in_clicked_point)
  {
    ImageClickedPointCallback(in_clicked_point.point);
  }

  void ImageClickedPointCallback(const geometry_msgs::Point& in_clicked_point)
  {
    clicked_image_points_.push_back(cv::Point2f(in_clicked_point.x,
                                                   in_clicked_point.y));

    std::cout << cv::Point2f(in_clicked_point.x,
                             in_clicked_point.y) << std::endl << std::endl;

    CalibrateSensors();
  }

  void ImageCallback(const sensor_msgs::Image& in_image_sensor)
  {

    if (camera_instrinsics_.empty() || distortion_coefficients_.empty())
    {
      ROS_INFO("[%s] No Camera intrinsics loaded. Make sure to publish and set correctly camera_info source topic.", __APP_NAME__);
      return;
    }
  }

  void IntrinsicsCallback(const sensor_msgs::CameraInfo& in_message)
  {
    if (camera_instrinsics_.empty() || distortion_coefficients_.empty())
    {
      image_size_.height = in_message.height;
      image_size_.width = in_message.width;

      camera_instrinsics_ = cv::Mat(3, 3, CV_64F);
      for (int row = 0; row < 3; row++)
      {
        for (int col = 0; col < 3; col++)
        {
          camera_instrinsics_.at<double>(row, col) = in_message.K[row * 3 + col];
        }
      }

      distortion_coefficients_ = cv::Mat(1, 5, CV_64F);
      for (int col = 0; col < 5; col++)
      {
        distortion_coefficients_.at<double>(col) = in_message.D[col];
      }
      ROS_INFO("[%s] Camera Intrinsics Loaded", __APP_NAME__);
    }
  }

public:
  void Run()
  {
    ros::NodeHandle private_node_handle("~");//to receive args

    std::string image_raw_topic_str, points_raw_topic_str, clusters_topic_str, camera_info_topic_str;
    bool use_rqt_image_view;
    std::string name_space_str = ros::this_node::getNamespace();

    private_node_handle.param<std::string>("image_src", image_raw_topic_str, "/image_rectified");
    private_node_handle.param<std::string>("camera_info_src", camera_info_topic_str, "camera_info");
    private_node_handle.param<bool>("use_rqt_image_view", use_rqt_image_view, false);

    if (name_space_str != "/")
    {
      if (name_space_str.substr(0, 2) == "//")
      {
        name_space_str.erase(name_space_str.begin());
      }
      image_raw_topic_str = name_space_str + image_raw_topic_str;
      camera_info_topic_str = name_space_str + camera_info_topic_str;
    }

    ROS_INFO("[%s] image_src: %s",__APP_NAME__, image_raw_topic_str.c_str());
    ROS_INFO("[%s] camera_info_src: %s",__APP_NAME__, camera_info_topic_str.c_str());

    ROS_INFO("[%s] Subscribing to... %s",__APP_NAME__, image_raw_topic_str.c_str());
    subscriber_image_raw_ = node_handle_.subscribe(image_raw_topic_str, 1, &ROSCameraLidarApp::ImageCallback, this);

    ROS_INFO("[%s] Subscribing to... %s",__APP_NAME__, camera_info_topic_str.c_str());
    subscriber_intrinsics_ = node_handle_.subscribe(camera_info_topic_str, 1, &ROSCameraLidarApp::IntrinsicsCallback, this);

    ROS_INFO("[%s] Subscribing to PointCloud ClickedPoint from RVIZ... /clicked_point",__APP_NAME__);
    subscriber_clicked_point_ = node_handle_.subscribe("/clicked_point", 1, &ROSCameraLidarApp::RvizClickedPointCallback, this);

    if (!use_rqt_image_view)
    {
      ROS_INFO("[%s] Subscribing to Image ClickedPoint from JSK ImageView2... %s/screenpoint",__APP_NAME__, image_raw_topic_str.c_str());
      subscriber_image_point_stamped_ = node_handle_.subscribe(image_raw_topic_str+"/screenpoint", 1, &ROSCameraLidarApp::ImageClickedPointStampedCallback, this);
      ROS_INFO("[%s] ClickedPoint: %s",__APP_NAME__, (image_raw_topic_str+"/screenpoint").c_str());
    }
    else
    {
      ROS_INFO("[%s] Subscribing to Image ClickedPoint from RQT Image View... %s_mouse_left",__APP_NAME__, image_raw_topic_str.c_str());
      subscriber_image_point_ = node_handle_.subscribe(image_raw_topic_str+"_mouse_left", 1, &ROSCameraLidarApp::ImageClickedPointCallback, this);
      ROS_INFO("[%s] ClickedPoint: %s",__APP_NAME__, (image_raw_topic_str+"_mouse_left").c_str());
    }

    ROS_INFO("[%s] Ready. Waiting for data...",__APP_NAME__);
    ros::spin();
    ROS_INFO("[%s] END",__APP_NAME__);
  }

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, __APP_NAME__);

  ROSCameraLidarApp app;

  app.Run();

  return 0;
}


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

 * 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  testSmartProjectionRigFactor.cpp
 *  @brief Unit tests for SmartProjectionRigFactor Class
 *  @author Chris Beall
 *  @author Luca Carlone
 *  @author Zsolt Kira
 *  @author Frank Dellaert
 *  @date   August 2021
 */

#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 <boost/assign/std/map.hpp>
#include <iostream>

#include "smartFactorScenarios.h"
#define DISABLE_TIMING

using namespace boost::assign;
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;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor2) {
  using namespace vanillaRig;
  boost::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;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
  SmartRigFactor::shared_ptr factor1(
      new SmartRigFactor(model, cameraRig, params));
  factor1->add(measurement1, x1, cameraId1);
}

/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor4) {
  using namespace vanillaRig;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), 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;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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
  boost::optional<Point3> 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;

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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));
  boost::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::identity();

  boost::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;

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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};

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

  SmartRigFactor::shared_ptr smartFactor1 = boost::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());
  boost::optional<Point3> 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());

    boost::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
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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)));

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

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

  boost::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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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());

  boost::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;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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
  boost::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};
  boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
  cameraRig->push_back(Camera(Pose3::identity(), 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::identity();

  boost::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);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), 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::identity();
  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++) {
    boost::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++) {
    boost::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);

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), 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);

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
            new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), 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);

    boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
        new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), 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);

  boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
          new CameraSet<SphericalCamera>());  // single camera in the rig
  cameraRig->push_back(SphericalCamera(Pose3::identity(), 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);
}
/* ************************************************************************* */


/**
   \file pyfunc_obj.hpp
   \brief function object wrappers for python functions
   \author Junhua Gu
 */

#ifndef PYFUNC_OBJ_HPP
#define PYFUNC_OBJ_HPP
#include <boost/python.hpp>
#include <core/optimizer.hpp>
#include <core/opt_traits.hpp>

namespace opt_utilities
{

  template <typename Ty,typename Tx>
  class pyfunc_obj
    :public func_obj<Ty,Tx>
  {
  private:
    boost::python::object pyfunc;
  public:
    pyfunc_obj()
    {
      if(!Py_IsInitialized())
	{
	  Py_Initialize();
	}
    }

    pyfunc_obj(const pyfunc_obj& rhs)
      :pyfunc(rhs.pyfunc)
    {}

    pyfunc_obj& operator=(const pyfunc_obj& rhs)
    {
      pyfunc=rhs.pyfunc;
    }

    ~pyfunc_obj()
    {}

  public:
    void attach(const std::string module_name,
		const std::string func_name)
    {
      boost::python::object mod(boost::python::import(module_name.c_str()));
      pyfunc=mod.attr(func_name.c_str());

    }
  private:
    func_obj<Ty,Tx>* do_clone()const
    {
      return new pyfunc_obj(*this);
    }

    void do_destroy()
    {
      delete this;
    }

    Ty do_eval(const Tx& x)
    {
      boost::python::list args;
      for(size_t i=0;i<get_size(x);++i)
	{
	  args.append(get_element(x,i));
	}
      return boost::python::extract<Ty>(pyfunc(args));
    }
  };
}

#endif
//EOF


#ifndef WAVE_INTEGRALS_HPP
#define WAVE_INTEGRALS_HPP

#include <Eigen/Core>
#include "wave/utils/utils.hpp"

namespace wave {

/**
 * This class implements the integrand for the covariance of constant acceleration priors
 */
class ConstantAccelerationCovariance {
 private:
    /// Power spectral density matrix on acceleration noise
    const Mat6 Qc;
    const double tmax;
    /// Rotational velocity
    const Vec6 w;

    /**
     * This is to calculate what I've been calling gamma in my head.
     * It's similar to the Rodrigez formula, and pops out of the NL-SDE that
     * models constant acceleration.
     * @param deltaT required
     * @return
     */
    void evaluateGamma(const double &deltaT, Mat6 &gamma);

 public:
    ConstantAccelerationCovariance(Mat6 QC, double Tmax, Vec6 omega) : Qc(QC), tmax(Tmax), w(omega) {}

    /**
     * Numerically integrate using trapezoidal rule to get Qtotal
     * @param Qtotal Covariance
     * @param slices How many discrete chunks to use when integrating
     */
    void calculateCovariance(Mat6 &Qtotal, const int& slices = 50);
};
}

#endif  // WAVE_INTEGRALS_HPP


#include <boost/test/unit_test.hpp>

#include "test_tsearch.hpp"

struct record {
    unsigned key;
};

static int record_comp(const void *o1, const void *o2) {
    return static_cast<const record *>(o2)->key - static_cast<const record *>(o1)->key;
}

BOOST_AUTO_TEST_CASE(tdelete_simple) {
    void *root = NULL;
    // This example was taken from:
    // https://en.wikipedia.org/wiki/Binary_search_tree#/media/File:Binary_search_tree.svg
    tsearch(new record{ 8}, &root, record_comp);

    tsearch(new record{ 3}, &root, record_comp);
    tsearch(new record{10}, &root, record_comp);

    tsearch(new record{ 1}, &root, record_comp);
    tsearch(new record{ 6}, &root, record_comp);
    tsearch(new record{14}, &root, record_comp);

    tsearch(new record{ 4}, &root, record_comp);
    tsearch(new record{ 7}, &root, record_comp);
    tsearch(new record{13}, &root, record_comp);

    // delete entire tree
    int counter = 0;
    while (root) {
        counter++;
        tdelete(*(void**)root, &root, record_comp);
    }

    BOOST_REQUIRE_EQUAL(root, nullptr);
    BOOST_REQUIRE_EQUAL(counter, 9);
}


#include <fstream>
#include <boost/algorithm/string.hpp>
#include <harp.hpp>
#include <specex_fits.h>
#include <specex_trace.h>
#include <specex_gauss_hermite_psf.h>

using namespace std ;

static void AddRow2(specex::FitsTable& table,const string& PARAM, harp::vector_double& coeff, int legdegx, int legdegw) {
  std::vector<specex::FitsTableEntry> row;
  {specex::FitsTableEntry entry; entry.string_val = PARAM; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.double_vals = coeff; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.int_vals.resize(1); entry.int_vals[0] = legdegx; row.push_back(entry);}
  {specex::FitsTableEntry entry; entry.int_vals.resize(1); entry.int_vals[0] = legdegw; row.push_back(entry);}
  table.data.push_back(row);
}

void write_gauss_hermite_psf_fits_version_3(const specex::GaussHermitePSF& psf, fitsfile* fp) {

  SPECEX_INFO("write PSF");
  SPECEX_DEBUG("write_gauss_hermite_psf_fits_version_3");

  ////////////////////////////
  string PSFVER = "3";
  ////////////////////////////

  int NPIX_X = psf.ccd_image_n_cols;
  int NPIX_Y = psf.ccd_image_n_rows;
  int GHDEGX = psf.Degree();
  int GHDEGY = psf.Degree();

  // FIBERMIN FIBERMAX WAVEMIN WAVEMAX are always defined by the trace set
  // to ensure consistency
  double WAVEMIN=1000000;
  double WAVEMAX=0;
  int    FIBERMIN=100000;
  int    FIBERMAX=0;
  for(std::map<int,specex::Trace>::const_iterator it=psf.FiberTraces.begin(); it!=psf.FiberTraces.end(); it++) {
    FIBERMIN=min(FIBERMIN,it->second.fiber);
    FIBERMAX=max(FIBERMAX,it->second.fiber);
    if(it->second.Y_vs_W.xmin>0)
      WAVEMIN=min(WAVEMIN,it->second.Y_vs_W.xmin);
    WAVEMAX=max(WAVEMAX,it->second.Y_vs_W.xmax);
  }
  WAVEMIN = floor(WAVEMIN);
  WAVEMAX = floor(WAVEMAX)+1.;
  int NFIBERS=(FIBERMAX-FIBERMIN+1);

  int status = 0;

  // get largest legendre degree of all params in all bundles and check param numbers match !!

  int nparams=psf.LocalNAllPar();
  int nparams_all = nparams;
  nparams_all += 1; // GH trivial order zero
#ifdef CONTINUUM
  nparams_all += 1; // continuum
#endif

  int ncoeff=0;
  for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
      bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
    if( int(bundle_it->second.AllParPolXW.size()) != nparams ) SPECEX_ERROR("Fatal inconsistency in number of parameters in psf between bundles: AllParPolXW.size=" << bundle_it->second.AllParPolXW.size() << " psf.LocalNAllPar()=" << nparams);
    for(size_t p=0;p<bundle_it->second.AllParPolXW.size();p++) {
      ncoeff=max(ncoeff,bundle_it->second.AllParPolXW[p]->ydeg+1);
    }
  }
  SPECEX_DEBUG("ncoeff = " << ncoeff);

  specex::FitsTable table;

  { // column description
    table.AddColumnDescription("PARAM","8A","","");
    char coeff_tform[100];
    sprintf(coeff_tform,"%dD",ncoeff*NFIBERS);
    vector <int> dim; dim.resize(2);
    dim[0]=ncoeff;
    dim[1]=NFIBERS;

    string sdim = table.encode_dimension(dim);
    table.AddColumnDescription("COEFF",coeff_tform,sdim,"");
    dim.resize(1); dim[0]=1;
    sdim = table.encode_dimension(dim);
    table.AddColumnDescription("LEGDEGX","J",sdim,"");
    table.AddColumnDescription("LEGDEGW","J",sdim,"");
  }

  harp::vector_double wave(ncoeff);
  {
    double wavestep = (WAVEMAX-WAVEMIN)/(ncoeff-1);
    for(int w=0;w<ncoeff;w++) {
      wave[w]   = WAVEMIN + wavestep*w;
    }
  }

  vector<string> keys;

  harp::vector_double coeff(ncoeff*NFIBERS);
  harp::vector_double values(ncoeff);

  bool need_to_add_first_gh = true;
  for(int p=0;p<nparams;p++) {  // loop on all PSF parameters

    string pname = psf.ParamName(p);
    if(need_to_add_first_gh && pname.find("GH-")<pname.npos) {
      // insert now GH param 0 which is not a PSF parameter in C++ but needed but specter

      for(int fiber=0;fiber<NFIBERS;fiber++)
	coeff(fiber*ncoeff)=1;
      AddRow2(table,"GH-0-0",coeff,0,0);
      need_to_add_first_gh = false;
    }

    int legdegx=0;
    int legdegw=0;
    // loop on bundles
    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
      const specex::PSF_Params & params_of_bundle = bundle_it->second;
      const specex::Pol_p pol2d = params_of_bundle.AllParPolXW[p]; // this is the 2D polynomiald of x_ccd and wave for this param and bundle
      legdegx=max(legdegx,pol2d->xdeg);
      legdegw=max(legdegw,pol2d->ydeg);

      for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++) {

	const specex::Trace& trace = psf.FiberTraces.find(fiber)->second; // X_vs_W.Value();

	// build a Legendre1DPol out of the Legendre2DPol
	specex::Legendre1DPol pol1d(ncoeff-1,WAVEMIN,WAVEMAX);
	for(int w=0;w<ncoeff;w++) {
	  values[w] = pol2d->Value(trace.X_vs_W.Value(wave[w]),wave[w]);
	}
	pol1d.Fit(wave,values,0,false);

	// now copy parameters;
	for(int w = 0; w < ncoeff ; w++) {
	  coeff((fiber-FIBERMIN)*ncoeff+w) = pol1d.coeff(w); // this is the definition of the ordering, (wave,fiber)
	}
      } // end of loop on fibers of bundle
    } // end of loop on bundles

    AddRow2(table,pname,coeff,legdegx,legdegw);
  } // end of loop on params

  { // add a parameter to link fibers and bundles in fit
    coeff.clear();
    for(int fiber=FIBERMIN;fiber<=FIBERMAX;fiber++)
      coeff((fiber-FIBERMIN)*ncoeff) = -1; // no bundle
    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
      const specex::PSF_Params & params_of_bundle = bundle_it->second;
      for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++) {
	coeff((fiber-FIBERMIN)*ncoeff) = bundle_it->first;
      }
    }
    AddRow2(table,"BUNDLE",coeff,0,0);
  }
  { // add a parameter with the fit status
    coeff.clear();
    for(int fiber=FIBERMIN;fiber<=FIBERMAX;fiber++)
      coeff((fiber-FIBERMIN)*ncoeff) = -1; // no bundle
    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
      const specex::PSF_Params & params_of_bundle = bundle_it->second;
      for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++) {
	coeff((fiber-FIBERMIN)*ncoeff) = params_of_bundle.fit_status;
      }
    }
    AddRow2(table,"STATUS",coeff,0,0);
  }

#ifdef CONTINUUM
  { // add a parameter for continuum
    coeff.clear();
    int fiber_index=0;
    int legdegx=0;
    int legdegw=0;
    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {
      const specex::PSF_Params & params_of_bundle = bundle_it->second;
      legdegw=params_of_bundle.ContinuumPol.deg;
      specex::Legendre1DPol pol1d(ncoeff-1,WAVEMIN,WAVEMAX);
      for(int w=0;w<ncoeff;w++) {
	values[w]   = params_of_bundle.ContinuumPol.Value(wave[w]);
      }
      pol1d.Fit(wave,values,0,false);
      for(int fiber=params_of_bundle.fiber_min; fiber<=params_of_bundle.fiber_max; fiber++,fiber_index++) {
	for(int w = 0; w < ncoeff ; w++) {
	  coeff((fiber-FIBERMIN)*ncoeff+w)   =  pol1d.coeff(w);
	}
      }
    }
    AddRow2(table,"CONT",coeff,legdegx,legdegw);
  }
#endif

  // write table
  table.Write(fp);

  // write keywords
  {

    fits_write_comment(fp,"------------------------------------------------------------------------",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF generated by specex, https://github.com/julienguy/specex",&status); harp::fits::check ( status );

    {
      char date_comment[80];
      time_t t = time(0);   // get time now
      struct tm * now = localtime( & t );
      sprintf(date_comment,"PSF fit date %04d-%02d-%02d",(now->tm_year + 1900),(now->tm_mon + 1),now->tm_mday);
      fits_write_comment(fp,date_comment,&status); harp::fits::check ( status );
    }
    ///////////////////////xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx////////
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Each row of the table contains the data vector of one PSF parameter",&status); harp::fits::check ( status );
    fits_write_comment(fp,"The size of the vector is ((FIBERMAX-FIBERMIN+1)*(LEGDEG+1))",&status); harp::fits::check ( status );
    fits_write_comment(fp,"Description of  the NPARAMS parameters : ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GHSIGX   : Sigma of first Gaussian along CCD columns for PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GHSIGY   : Sigma of first Gaussian along CCD rows for PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"GH-i-j   : Hermite pol. coefficents, i along columns, j along rows,",&status); harp::fits::check ( status );
    fits_write_comment(fp,"         i is integer from 0 to GHDEGX, j is integer from 0 to GHDEGY,",&status); harp::fits::check ( status );
    fits_write_comment(fp,"         there are (GHDEGX+1)*(GHDEGY+1) such coefficents.",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILAMP  : Amplitude of PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILCORE : Size in pixels of PSF tail saturation in PSF core",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILXSCA : Scaling apply to CCD coordinate along columns for PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILYSCA : Scaling apply to CCD coordinate along rows for PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"TAILINDE : Asymptotic power law index of PSF tail",&status); harp::fits::check ( status );
    fits_write_comment(fp,"CONT     : Continuum flux in arc image (not part of PSF)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_core(X,Y) = [ SUM_ij (GH-i-j)*HERM(i,X/GHSIGX)*HERM(j,Y/GHSIGX) ]",&status); harp::fits::check ( status );
    fits_write_comment(fp,"                                       *GAUS(X,GHSIGX)*GAUS(Y,GHSIGY)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_tail(X,Y) = TAILAMP*R^2/(TAILCORE^2+R^2)^(1+TAILINDE/2)",&status); harp::fits::check ( status );
    fits_write_comment(fp,"                with R^2=(X/TAILXSCA)^2+(Y/TAILYSCA)^2",&status); harp::fits::check ( status );
    fits_write_comment(fp,"-  ",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_core is integrated in pixel",&status); harp::fits::check ( status );
    fits_write_comment(fp,"PSF_tail is not, it is evaluated at center of pixel",&status); harp::fits::check ( status );
    fits_write_comment(fp,"------------------------------------------------------------------------",&status); harp::fits::check ( status );
    harp::fits::key_write(fp,"EXTNAME","PSF","");
    harp::fits::key_write(fp,"PSFTYPE","GAUSS-HERMITE","");
    harp::fits::key_write(fp,"PSFVER",PSFVER,"");

    harp::fits::key_write(fp,"MJD",(long long int)psf.mjd,"MJD of arc lamp exposure");
    harp::fits::key_write(fp,"PLATEID",psf.plate_id,"plate ID of arc lamp exposure");
    harp::fits::key_write(fp,"CAMERA",psf.camera_id,"camera ID");
    harp::fits::key_write(fp,"ARCEXP",psf.arc_exposure_id,"ID of arc lamp exposure used to fit PSF");

    harp::fits::key_write(fp,"NPIX_X",(long long int)NPIX_X,"number of columns in input CCD image");
    harp::fits::key_write(fp,"NPIX_Y",(long long int)NPIX_Y,"number of rows in input CCD image");
    harp::fits::key_write(fp,"HSIZEX",(long long int)psf.hSizeX,"Half size of PSF in fit, NX=2*HSIZEX+1");
    harp::fits::key_write(fp,"HSIZEY",(long long int)psf.hSizeY,"Half size of PSF in fit, NY=2*HSIZEY+1");
    harp::fits::key_write(fp,"FIBERMIN",(long long int)FIBERMIN,"first fiber (starting at 0)");
    harp::fits::key_write(fp,"FIBERMAX",(long long int)FIBERMAX,"last fiber (included)");
    harp::fits::key_write(fp,"NPARAMS",(long long int)nparams_all,"number of PSF parameters");
    harp::fits::key_write(fp,"LEGDEG",(long long int)(ncoeff-1),"degree of Legendre pol.(wave) for parameters");
    harp::fits::key_write(fp,"GHDEGX",(long long int)GHDEGX,"degree of Hermite polynomial along CCD columns");
    harp::fits::key_write(fp,"GHDEGY",(long long int)GHDEGY,"degree of Hermite polynomial along CCD rows");
    harp::fits::key_write(fp,"WAVEMIN",WAVEMIN,"minimum wavelength (A), used for the Legendre polynomials");
    harp::fits::key_write(fp,"WAVEMAX",WAVEMAX,"maximum wavelength (A), used for the Legendre polynomials");

    // add chi2
    harp::fits::key_write(fp,"PSFERROR",psf.psf_error,"assumed PSF fractional error in chi2");
    harp::fits::key_write(fp,"READNOIS",psf.readout_noise,"assumed read out noise in chi2");
    harp::fits::key_write(fp,"GAIN",psf.gain,"assumed gain in chi2");

    for(std::map<int,specex::PSF_Params>::const_iterator bundle_it = psf.ParamsOfBundles.begin();
	bundle_it != psf.ParamsOfBundles.end(); ++bundle_it) {

      const specex::PSF_Params & params_of_bundle = bundle_it->second;

      int ndf = params_of_bundle.ndata - params_of_bundle.nparams;
      double chi2pdf = 0;
      if(ndf>0) chi2pdf = params_of_bundle.chi2/ndf;

      char key[20];
      char comment[800];

      sprintf(key,"B%02dRCHI2",params_of_bundle.bundle_id);
      sprintf(comment,"best fit chi2/ndf for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,chi2pdf,comment);

      sprintf(key,"B%02dNDATA",params_of_bundle.bundle_id);
      sprintf(comment,"number of pixels in fit for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,(long long int)params_of_bundle.ndata,comment);

      sprintf(key,"B%02dNPAR",params_of_bundle.bundle_id);
      sprintf(comment,"number of parameters in fit for fiber bundle %d",params_of_bundle.bundle_id);
      harp::fits::key_write(fp,key,(long long int)params_of_bundle.nparams,comment);

    }

  } // end of write key words
} // end of routine

void read_gauss_hermite_psf_fits_version_3(specex::PSF_p& psf, fitsfile* fp, int hdu) {

  SPECEX_DEBUG("read_gauss_hermite_psf_fits_version_3 hdu=" << hdu);

  int status = 0;
  fits_movabs_hdu ( fp, hdu, NULL, &status ); harp::fits::check ( status );

  int GHDEGX;  harp::fits::key_read(fp,"GHDEGX",GHDEGX);
  int GHDEGY;  harp::fits::key_read(fp,"GHDEGY",GHDEGY);
  if(GHDEGX != GHDEGY) {
    SPECEX_ERROR("expect GHDEGX=GHDEGY");
  }

  // psf is now already created
  // psf = specex::PSF_p(new specex::GaussHermitePSF(GHDEGX));
  ((specex::GaussHermitePSF*)(&(*psf)))->SetDegree(GHDEGX);

  // read header keywords
  harp::fits::key_read(fp,"MJD",psf->mjd);
  harp::fits::key_read(fp,"PLATEID",psf->plate_id);
  harp::fits::key_read(fp,"CAMERA",psf->camera_id);
  harp::fits::key_read(fp,"ARCEXP",psf->arc_exposure_id);
  int nx; harp::fits::key_read(fp,"NPIX_X",nx); psf->ccd_image_n_cols = nx;
  int ny; harp::fits::key_read(fp,"NPIX_Y",ny); psf->ccd_image_n_rows = ny;
  harp::fits::key_read(fp,"HSIZEX",psf->hSizeX);
  harp::fits::key_read(fp,"HSIZEY",psf->hSizeY);
  int FIBERMIN; harp::fits::key_read(fp,"FIBERMIN",FIBERMIN);
  int FIBERMAX; harp::fits::key_read(fp,"FIBERMAX",FIBERMAX);
  double WAVEMIN; harp::fits::key_read(fp,"WAVEMIN",WAVEMIN);
  double WAVEMAX; harp::fits::key_read(fp,"WAVEMAX",WAVEMAX);
  int LEGDEG; harp::fits::key_read(fp,"LEGDEG",LEGDEG);

  // read table
  specex::FitsTable table;
  table.Read(fp);
  int param_col = table.columns["PARAM"].col;
  int coeff_col = table.columns["COEFF"].col;
  int degx_col = table.columns["LEGDEGX"].col;
  int degw_col = table.columns["LEGDEGW"].col;

  std::vector<std::string> params;
  std::map<std::string,int> param_row;
  std::map<std::string,harp::vector_double > param_coeff;
  std::map<std::string,int > param_degx;
  std::map<std::string,int > param_degw;

  for(int i=0;i<table.data.size();i++) {
    std::string pname=table.data[i][param_col].string_val;
    boost::trim(pname);
    params.push_back(pname);
    param_row[pname]=i;
    param_coeff[pname]=table.data[i][coeff_col].double_vals;
    param_degx[pname]=table.data[i][degx_col].int_vals[0];
    param_degw[pname]=table.data[i][degw_col].int_vals[0];
    SPECEX_DEBUG("read_gauss_hermite_psf " << i << " '" << pname << "' degx=" << param_degx[pname] << " degw=" << param_degw[pname]);
  }

  // find bundles
  vector<int> bundles;
  for(int fiber=FIBERMIN;fiber<=FIBERMAX;fiber++) {
    int bundle = int(param_coeff["BUNDLE"][(LEGDEG+1)*fiber]);
    if(bundle<0) continue;
    if(std::find(bundles.begin(), bundles.end(), bundle) == bundles.end()) bundles.push_back(bundle);
  }
  SPECEX_DEBUG("Number of bundles = " << bundles.size());

  // loop on bundles and fill PSF parameters
  for(int b=0;b<bundles.size();b++) {
    int bundle=bundles[b];

    // here we can test the input bundle requirement

    int bundle_fibermin=10000;
    int bundle_fibermax=0;
    vector<int> fibers_in_bundle;
    for(int fiber=FIBERMIN;fiber<=FIBERMAX;fiber++) {
      int fiber_bundle = int(param_coeff["BUNDLE"][(LEGDEG+1)*fiber]);
      if(fiber_bundle==bundle) {
	fibers_in_bundle.push_back(fiber);
	bundle_fibermin=min(fiber,bundle_fibermin);
	bundle_fibermax=max(fiber,bundle_fibermax);
      }
    }
    if(fibers_in_bundle.size()==0) {
      SPECEX_WARNING("No fiber in bundle " << bundle << " ???");
      continue;
    }

    specex::PSF_Params bundle_params;
    bundle_params.bundle_id=bundle;
    bundle_params.fiber_min=bundle_fibermin;
    bundle_params.fiber_max=bundle_fibermax;

    double xmin=100000;
    double xmax=0;

    for(int fiber=bundle_fibermin;fiber<=bundle_fibermax;fiber++) {
      const specex::Trace& trace = psf->FiberTraces[fiber];
      for(double wave=WAVEMIN;wave<=WAVEMAX;wave+=10) {
	double x = trace.X_vs_W.Value(wave);
	xmin=min(xmin,x);
	xmax=max(xmax,x);
      }
    }
    SPECEX_DEBUG("bundle=" << bundle << " xmin xmax : " << xmin << " " << xmax);

#ifdef CONTINUUM
    // dealing with continuum
    bundle_params.ContinuumPol = specex::Legendre1DPol(param_degw["CONT"],WAVEMIN,WAVEMAX);
    for(int i=0;i<param_degw["CONT"]+1;i++) {
      bundle_params.ContinuumPol.coeff[i] = param_coeff["CONT"][i];
    }
#endif

    // loop on parameters
    for(int pi=0;pi<(int)params.size();pi++) {
      const string& pname=params[pi];
      if(pname=="GH-0-0") continue; // not used in c++ version
      if(pname=="CONT") continue; // not a local param
      if(pname=="BUNDLE") continue; // not a local param
      if(pname=="STATUS") continue; // not a local param

      SPECEX_DEBUG("Fitting pol of parameter " << pname << " in bundle " << bundle << " fibers in [" << bundle_fibermin << "," << bundle_fibermax << "]" );

      // now we need to fit a 2D legendre polynomial of X and wave
      // for the subset of fibers of this bundle

      int degx = param_degx[pname];
      int degw = param_degw[pname];

      specex::Pol_p pol(new specex::Pol(degx,xmin,xmax,degw,WAVEMIN,WAVEMAX));
      pol->name = pname;
      pol->Fill(true); // sparse or not sparse ????

      int npar = pol->Npar();
      harp::matrix_double A(npar,npar); A.clear();
      harp::vector_double B(npar); B.clear();

      int npoints=0;
      for(int fiber=bundle_fibermin;fiber<=bundle_fibermax;fiber++) {
	const specex::Trace& trace = psf->FiberTraces[fiber];
	specex::Legendre1DPol fiberpol(LEGDEG,WAVEMIN,WAVEMAX);
	for(int cj=0;cj<=LEGDEG;cj++)
	  fiberpol.coeff[cj]=param_coeff[pname][cj+(fiber-FIBERMIN)*(LEGDEG+1)];

	for(double wave=WAVEMIN;wave<WAVEMAX+0.01;wave+=(WAVEMAX-WAVEMIN)/(degw+1)) {
	  double x    = trace.X_vs_W.Value(wave);
	  double pval = fiberpol.Value(wave);
	  harp::vector_double der = pol->Monomials(x,wave);
	  specex::syr(1.,der,A); // A += der*der.transposed;
	  specex::axpy(pval,der,B); // B += pval*der;
	  npoints += 1;
	}
      }
      // now need to solve
      int status = specex::cholesky_solve(A,B);
      if(status != 0)
	SPECEX_ERROR("Oups, failed to convert LegPol(fiber,wave) -> LegPol(x,wave) for bundle " << bundle << " and parameter " << pname << " " << "bundle fibermin,fibermax=" << bundle_fibermin << "," << bundle_fibermax << " degx,degw=" << degx << "," << degw << " " << "xmin,xmax=" << xmin << "," << xmax << " WAVEMIN,WAVEMAX=" << WAVEMIN << "," << WAVEMAX << " npoints=" << npoints );

      pol->coeff = B;
      bundle_params.AllParPolXW.push_back(pol);

    }
    psf->ParamsOfBundles[bundle] = bundle_params;
  }
}


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

// duration
// Period shall be a specialization of ratio, diagnostic required.

#include <boost/chrono/duration.hpp>

void test()
{
    typedef boost::chrono::duration<int,
        boost::ratio_add<
            boost::ratio<1,2>,
            boost::ratio<1,3>
        >
    > D;
    D d;
    (void)d;
}


/*!
 * @file
 * Contains unit tests for the @ref Field typeclass.
 */

#include <boost/mpl11/field.hpp>

#include <boost/mpl11/core.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Field<Archetype<0>> : instantiate<Field>::with<Archetype<0>> {
        template <typename, typename> using quot_impl  = method_tag<quot>;
        template <typename>           using recip_impl = method_tag<recip>;
    };
}}

struct tests
    : method<quot, archetype<0>, archetype<0>>
    // , method<quot, archetype<0>, archetype<1>>
    // , method<quot, archetype<1>, archetype<0>>

    , method<recip, archetype<0>>
{ };

int main() { }