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

 * 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  testSmartProjectionPoseFactorRollingShutter.cpp
 *  @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class
 *  @author Luca Carlone
 *  @date   July 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 <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>

#include <boost/assign/std/map.hpp>
#include <iostream>

#include "gtsam/slam/tests/smartFactorScenarios.h"
#define DISABLE_TIMING

using namespace gtsam;
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);
static Symbol x4('X', 4);
static Symbol l0('L', 0);
static Pose3 body_P_sensor =
    Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0));

static Point2 measurement1(323.0, 240.0);
static Point2 measurement2(200.0, 220.0);
static Point2 measurement3(320.0, 10.0);
static double interp_factor = 0.5;
static double interp_factor1 = 0.3;
static double interp_factor2 = 0.4;
static double interp_factor3 = 0.5;

static size_t cameraId1 = 0;

/* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect
namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
Camera cam1(interp_pose1, sharedK);
Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK);
SmartProjectionParams params(
    gtsam::HESSIAN,
    gtsam::ZERO_ON_DEGENERACY);  // only config that works with RS factors
}  // namespace vanillaPoseRS

LevenbergMarquardtParams lmParams;
typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
    SmartFactorRS;

/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
  SmartFactorRS::shared_ptr factor1(
      new SmartFactorRS(model, cameraRig, params));
}

/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
  params.setRankTolerance(rankTol);
  SmartFactorRS factor1(model, cameraRig, params);
}

/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) {
  using namespace vanillaPoseRS;
  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
  SmartFactorRS::shared_ptr factor1(
      new SmartFactorRS(model, cameraRig, params));
  factor1->add(measurement1, x1, x2, interp_factor);
}

/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
  using namespace vanillaPoseRS;

  // create fake measurements
  Point2Vector measurements;
  measurements.push_back(measurement1);
  measurements.push_back(measurement2);
  measurements.push_back(measurement3);

  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x4));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  FastVector<size_t> cameraIds{0, 0, 0};

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensor, sharedK));

  // create by adding a batch of measurements with a bunch of calibrations
  SmartFactorRS::shared_ptr factor2(
      new SmartFactorRS(model, cameraRig, params));
  factor2->add(measurements, key_pairs, interp_factors, cameraIds);

  // create by adding a batch of measurements with a single calibrations
  SmartFactorRS::shared_ptr factor3(
      new SmartFactorRS(model, cameraRig, params));
  factor3->add(measurements, key_pairs, interp_factors, cameraIds);

  {  // create equal factors and show equal returns true
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig, params));
    factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
    factor1->add(measurement2, x2, x3, interp_factor2, cameraId1);
    factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);

    EXPECT(factor1->equals(*factor2));
    EXPECT(factor1->equals(*factor3));
  }
  {  // create equal factors and show equal returns true (use default cameraId)
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig, params));
    factor1->add(measurement1, x1, x2, interp_factor1);
    factor1->add(measurement2, x2, x3, interp_factor2);
    factor1->add(measurement3, x3, x4, interp_factor3);

    EXPECT(factor1->equals(*factor2));
    EXPECT(factor1->equals(*factor3));
  }
  {  // create equal factors and show equal returns true (use default cameraId)
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig, params));
    factor1->add(measurements, key_pairs, interp_factors);

    EXPECT(factor1->equals(*factor2));
    EXPECT(factor1->equals(*factor3));
  }
  {  // create slightly different factors (different keys) and show equal
     // returns false (use default cameraIds)
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig, params));
    factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
    factor1->add(measurement2, x2, x2, interp_factor2,
                 cameraId1);  // different!
    factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);

    EXPECT(!factor1->equals(*factor2));
    EXPECT(!factor1->equals(*factor3));
  }
  {  // create slightly different factors (different extrinsics) and show equal
     // returns false
    boost::shared_ptr<Cameras> cameraRig2(new Cameras());
    cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig2, params));
    factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
    factor1->add(measurement2, x2, x3, interp_factor2,
                 cameraId1);  // different!
    factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);

    EXPECT(!factor1->equals(*factor2));
    EXPECT(!factor1->equals(*factor3));
  }
  {  // create slightly different factors (different interp factors) and show
     // equal returns false
    SmartFactorRS::shared_ptr factor1(
        new SmartFactorRS(model, cameraRig, params));
    factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
    factor1->add(measurement2, x2, x3, interp_factor1,
                 cameraId1);  // different!
    factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);

    EXPECT(!factor1->equals(*factor2));
    EXPECT(!factor1->equals(*factor3));
  }
}

static const int DimBlock = 12;  ///< size of the variable stacking 2 poses from
                                 ///< which the observation pose is interpolated
static const int ZDim = 2;       ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, DimBlock>
    MatrixZD;  // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
    FBlocks;  // vector of F blocks

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
  using namespace vanillaPoseRS;

  // Project two landmarks into two cameras
  Point2 level_uv = cam1.project(landmark1);
  Point2 level_uv_right = cam2.project(landmark1);
  Pose3 body_P_sensorId = Pose3::identity();

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensorId, sharedK));

  SmartFactorRS factor(model, cameraRig, params);
  factor.add(level_uv, x1, x2, interp_factor1);
  factor.add(level_uv_right, x2, x3, interp_factor2);

  Values values;  // it's a pose factor, hence these are poses
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  values.insert(x3, pose_above);

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

  // Check triangulation
  factor.triangulateSafe(factor.cameras(values));
  TriangulationResult point = factor.point();
  EXPECT(point.valid());  // check triangulated point is valid
  EXPECT(assert_equal(
      landmark1,
      *point));  // check triangulation result matches expected 3D landmark

  // Check Jacobians
  // -- actual Jacobians
  FBlocks actualFs;
  Matrix actualE;
  Vector actualb;
  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
                                               values);
  EXPECT(actualE.rows() == 4);
  EXPECT(actualE.cols() == 3);
  EXPECT(actualb.rows() == 4);
  EXPECT(actualb.cols() == 1);
  EXPECT(actualFs.size() == 2);

  // -- expected Jacobians from ProjectionFactorsRollingShutter
  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
                                         x2, l0, sharedK, body_P_sensorId);
  Matrix expectedF11, expectedF12, expectedE1;
  Vector expectedb1 = factor1.evaluateError(
      level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1);
  EXPECT(
      assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
  EXPECT(
      assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
  // by definition computeJacobiansWithTriangulatedPoint returns minus
  // reprojectionError
  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));

  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
                                         x2, x3, l0, sharedK, body_P_sensorId);
  Matrix expectedF21, expectedF22, expectedE2;
  Vector expectedb2 = factor2.evaluateError(
      pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2);
  EXPECT(
      assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
  EXPECT(
      assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
  // by definition computeJacobiansWithTriangulatedPoint returns minus
  // reprojectionError
  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
  // also includes non-identical extrinsic calibration
  using namespace vanillaPoseRS;

  // Project two landmarks into two cameras
  Point2 pixelError(0.5, 1.0);
  Point2 level_uv = cam1.project(landmark1) + pixelError;
  Point2 level_uv_right = cam2.project(landmark1);
  Pose3 body_P_sensorNonId = body_P_sensor;

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensorNonId, sharedK));

  SmartFactorRS factor(model, cameraRig, params);
  factor.add(level_uv, x1, x2, interp_factor1);
  factor.add(level_uv_right, x2, x3, interp_factor2);

  Values values;  // it's a pose factor, hence these are poses
  values.insert(x1, level_pose);
  values.insert(x2, pose_right);
  values.insert(x3, pose_above);

  // Perform triangulation
  factor.triangulateSafe(factor.cameras(values));
  TriangulationResult point = factor.point();
  EXPECT(point.valid());  // check triangulated point is valid
  Point3 landmarkNoisy = *point;

  // Check Jacobians
  // -- actual Jacobians
  FBlocks actualFs;
  Matrix actualE;
  Vector actualb;
  factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb,
                                               values);
  EXPECT(actualE.rows() == 4);
  EXPECT(actualE.cols() == 3);
  EXPECT(actualb.rows() == 4);
  EXPECT(actualb.cols() == 1);
  EXPECT(actualFs.size() == 2);

  // -- expected Jacobians from ProjectionFactorsRollingShutter
  ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1,
                                         x2, l0, sharedK, body_P_sensorNonId);
  Matrix expectedF11, expectedF12, expectedE1;
  Vector expectedb1 =
      factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11,
                            expectedF12, expectedE1);
  EXPECT(
      assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5));
  EXPECT(
      assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5));
  EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5));
  // by definition computeJacobiansWithTriangulatedPoint returns minus
  // reprojectionError
  EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5));

  ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model,
                                         x2, x3, l0, sharedK,
                                         body_P_sensorNonId);
  Matrix expectedF21, expectedF22, expectedE2;
  Vector expectedb2 =
      factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21,
                            expectedF22, expectedE2);
  EXPECT(
      assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5));
  EXPECT(
      assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5));
  EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5));
  // by definition computeJacobiansWithTriangulatedPoint returns minus
  // reprojectionError
  EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5));

  // Check errors
  double actualError = factor.error(values);  // from smart factor
  NonlinearFactorGraph nfg;
  nfg.add(factor1);
  nfg.add(factor2);
  values.insert(l0, landmarkNoisy);
  double expectedError = nfg.error(values);
  EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
  using namespace vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor2(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor3(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);

  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-6));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
  using namespace vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensor, sharedK));
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1});

  SmartFactorRS::shared_ptr smartFactor2(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1});

  SmartFactorRS::shared_ptr smartFactor3(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1});

  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);  // pose above is the pose of the camera
  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-6));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
  using namespace vanillaPoseRS;

  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;

  // create arbitrary body_T_sensor (transforms from sensor to body)
  Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1));
  Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1));
  Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1));

  Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
  Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
  Camera camera3(interp_pose3 * body_T_sensor3, sharedK);

  // Project three landmarks into three cameras
  projectToMultipleCameras(camera1, camera2, camera3, landmark1,
                           measurements_lmk1);
  projectToMultipleCameras(camera1, camera2, camera3, landmark2,
                           measurements_lmk2);
  projectToMultipleCameras(camera1, camera2, camera3, landmark3,
                           measurements_lmk3);

  // create inputs
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_T_sensor1, sharedK));
  cameraRig->push_back(Camera(body_T_sensor2, sharedK));
  cameraRig->push_back(Camera(body_T_sensor3, sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2});

  SmartFactorRS::shared_ptr smartFactor2(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2});

  SmartFactorRS::shared_ptr smartFactor3(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2});

  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);  // pose above is the pose of the camera
  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-4));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
  // here we replicate a test in SmartProjectionPoseFactor by setting
  // interpolation factors to 0 and 1 (such that the rollingShutter measurements
  // falls back to standard pixel measurements) Note: this is a quite extreme
  // test since in typical camera you would not have more than 1 measurement per
  // landmark at each interpolated pose
  using namespace vanillaPoseRS;

  // 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();

  // 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));

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  double interp_factor = 0;  // equivalent to measurement taken at pose 1
  smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
  interp_factor = 1;  // equivalent to measurement taken at pose 2
  smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor);

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

  // Make sure triangulation works
  EXPECT(smartFactor1->triangulateSafe(cameras));
  EXPECT(!smartFactor1->isDegenerate());
  EXPECT(!smartFactor1->isPointBehindCamera());
  boost::optional<Point3> p = smartFactor1->point();
  EXPECT(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, pose1);
  values.insert(x2, pose2);
  boost::shared_ptr<RegularHessianFactor<6>> actual =
      smartFactor1->createHessianFactor(values);
  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(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
  using namespace vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  double excludeLandmarksFutherThanDist = 1e10;  // very large
  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setLinearizationMode(gtsam::HESSIAN);
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setEnableEPI(true);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS smartFactor1(model, cameraRig, params);
  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);

  SmartFactorRS smartFactor2(model, cameraRig, params);
  smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);

  SmartFactorRS smartFactor3(model, cameraRig, params);
  smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);

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

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

  // Optimization should correct 3rd pose
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter,
     optimization_3poses_landmarkDistance) {
  using namespace vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  double excludeLandmarksFutherThanDist = 2;
  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setLinearizationMode(gtsam::HESSIAN);
  // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an
  // exception as expected
  params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
  params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
  params.setEnableEPI(false);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS smartFactor1(model, cameraRig, params);
  smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);

  SmartFactorRS smartFactor2(model, cameraRig, params);
  smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);

  SmartFactorRS smartFactor3(model, cameraRig, params);
  smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);

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

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

  // All factors are disabled (due to the distance threshold) 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(SmartProjectionPoseFactorRollingShutter,
     optimization_3poses_dynamicOutlierRejection) {
  using namespace vanillaPoseRS;
  // add fourth landmark
  Point3 landmark4(5, -0.5, 1);

  Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3,
      measurements_lmk4;
  // Project 4 landmarks into cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
  projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4);
  measurements_lmk4.at(0) =
      measurements_lmk4.at(0) + Point2(10, 10);  // add outlier

  // create inputs
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

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

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

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor2(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor3(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor4(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);

  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, level_pose, noisePrior);
  graph.addPrior(x2, pose_right, noisePrior);

  Pose3 noise_pose = Pose3(
      Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
      Point3(0.01, 0.01,
             0.01));  // smaller noise, otherwise outlier rejection will kick in
  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);

  // Optimization should correct 3rd pose
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter,
     hessianComparedToProjFactorsRollingShutter) {
  using namespace vanillaPoseRS;
  Point2Vector measurements_lmk1;

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

  // create inputs
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);

  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 the factor to calculate the Jacobians
  Matrix F = Matrix::Zero(2 * 3, 6 * 3);
  Matrix E = Matrix::Zero(2 * 3, 3);
  Vector b = Vector::Zero(6);

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

  ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2,
                                          model, x2, x3, l0, sharedK);
  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
                                            H2Actual, H3Actual);
  F.block<2, 6>(2, 6) = H1Actual;
  F.block<2, 6>(2, 12) = H2Actual;
  E.block<2, 3>(2, 0) = H3Actual;

  ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3,
                                          model, x3, x1, l0, sharedK);
  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
                                            H2Actual, H3Actual);
  F.block<2, 6>(4, 12) = H1Actual;
  F.block<2, 6>(4, 0) = H2Actual;
  E.block<2, 3>(4, 0) = H3Actual;

  // 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_projFactorsRS;
  nfg_projFactorsRS.add(factor11);
  nfg_projFactorsRS.add(factor12);
  nfg_projFactorsRS.add(factor13);
  values.insert(l0, *point);

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

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter,
     hessianComparedToProjFactorsRollingShutter_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 vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));
  key_pairs.push_back(std::make_pair(x1, x2));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);
  interp_factors.push_back(interp_factor1);

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);

  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 ProjectionFactorRollingShutter factor to calculate the
  // Jacobians
  Matrix F = Matrix::Zero(2 * 4, 6 * 3);
  Matrix E = Matrix::Zero(2 * 4, 3);
  Vector b = Vector::Zero(8);

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

  ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1],
                                          interp_factor2, model, x2, x3, l0,
                                          sharedK);
  b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual,
                                            H2Actual, H3Actual);
  F.block<2, 6>(2, 6) = H1Actual;
  F.block<2, 6>(2, 12) = H2Actual;
  E.block<2, 3>(2, 0) = H3Actual;

  ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2],
                                          interp_factor3, model, x3, x1, l0,
                                          sharedK);
  b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual,
                                            H2Actual, H3Actual);
  F.block<2, 6>(4, 12) = H1Actual;
  F.block<2, 6>(4, 0) = H2Actual;
  E.block<2, 3>(4, 0) = H3Actual;

  ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3],
                                          interp_factor1, model, x1, x2, l0,
                                          sharedK);
  b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual,
                                            H2Actual, H3Actual);
  F.block<2, 6>(6, 0) = H1Actual;
  F.block<2, 6>(6, 6) = H2Actual;
  E.block<2, 3>(6, 0) = H3Actual;

  // 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_projFactorsRS;
  nfg_projFactorsRS.add(factor11);
  nfg_projFactorsRS.add(factor12);
  nfg_projFactorsRS.add(factor13);
  nfg_projFactorsRS.add(factor14);
  values.insert(l0, *point);

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

/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter,
     optimization_3poses_measurementsFromSamePose) {
  using namespace vanillaPoseRS;
  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
  std::vector<std::pair<Key, Key>> key_pairs;
  key_pairs.push_back(std::make_pair(x1, x2));
  key_pairs.push_back(std::make_pair(x2, x3));
  key_pairs.push_back(std::make_pair(x3, x1));

  std::vector<double> interp_factors;
  interp_factors.push_back(interp_factor1);
  interp_factors.push_back(interp_factor2);
  interp_factors.push_back(interp_factor3);

  // 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
  std::vector<std::pair<Key, Key>> key_pairs_redundant = key_pairs;
  key_pairs_redundant.push_back(
      key_pairs.at(0));  // we readd the first pair of keys
  std::vector<double> interp_factors_redundant = interp_factors;
  interp_factors_redundant.push_back(
      interp_factors.at(0));  // we readd the first interp factor

  boost::shared_ptr<Cameras> cameraRig(new Cameras());
  cameraRig->push_back(Camera(Pose3::identity(), sharedK));

  SmartFactorRS::shared_ptr smartFactor1(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
                    interp_factors_redundant);

  SmartFactorRS::shared_ptr smartFactor2(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);

  SmartFactorRS::shared_ptr smartFactor3(
      new SmartFactorRS(model, cameraRig, params));
  smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);

  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>
//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
//|   -SF RS LINEARIZE: 0.09 CPU
// (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0)
//|   -RS LINEARIZE: 0.09 CPU
// (10000 times, 0.068719 wall, 0.09 children, min: 0 max: 0)
/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, timing) {
  using namespace vanillaPose;

  // Default cameras for simple derivatives
  static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
  SmartProjectionParams params(
      gtsam::HESSIAN,
      gtsam::ZERO_ON_DEGENERACY);  // only config that works with RS factors

  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();

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

    SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
        model, cameraRig, params));
    double interp_factor = 0;  // equivalent to measurement taken at pose 1
    smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
    interp_factor = 1;  // equivalent to measurement taken at pose 2
    smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);

    Values values;
    values.insert(x1, pose1);
    values.insert(x2, pose2);
    gttic_(SF_RS_LINEARIZE);
    smartFactorRS->linearize(values);
    gttoc_(SF_RS_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_(RS_LINEARIZE);
    smartFactor->linearize(values);
    gttoc_(RS_LINEARIZE);
  }
  tictoc_print_();
}
#endif

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


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#pragma once

#include "ResolveType.hpp"
#include "DataLayoutIndexed.hpp"
#include "EndToEndTestImpl.hpp"

#include "armnn/INetwork.hpp"

#include "backendsCommon/test/CommonTestUtils.hpp"

#include <Permute.hpp>
#include <boost/test/unit_test.hpp>

#include <vector>

namespace
{
template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
void PermuteDataToNCHW(const std::vector<armnn::BackendId>& backends,
                       const armnn::DataLayout& dataLayout,
                       TensorInfo& tensorInfo,
                       std::vector<T>& data)
{
    const armnn::PermutationVector NHWCToNCHW = {0, 2, 3, 1};

    tensorInfo = armnnUtils::Permuted(tensorInfo, NHWCToNCHW);

    std::vector<T> tmp(data.size());
    armnnUtils::Permute(tensorInfo.GetShape(), NHWCToNCHW, data.data(), tmp.data(), sizeof(T));

    data = tmp;
}

template<typename armnn::DataType DataType>
armnn::INetworkPtr CreateSpaceToDepthNetwork(const armnn::TensorShape& inputShape,
                                             const armnn::TensorShape& outputShape,
                                             const armnn::DataLayout dataLayout,
                                             unsigned int blockSize,
                                             const float qScale = 1.0f,
                                             const int32_t qOffset = 0)
{
    using namespace armnn;
    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    TensorInfo inputTensorInfo(inputShape, DataType, qScale, qOffset);

    armnnUtils::DataLayoutIndexed dimensionIndices(dataLayout);
    if (inputShape[dimensionIndices.GetHeightIndex()] % blockSize!=0
        || inputShape[dimensionIndices.GetWidthIndex()] % blockSize!=0)
    {
        throw InvalidArgumentException("Input shape must be divisible by block size in all spatial dimensions");
    }

    SpaceToDepthDescriptor spaceToDepthDesc;
    spaceToDepthDesc.m_BlockSize = blockSize;
    spaceToDepthDesc.m_DataLayout = dataLayout;

    IConnectableLayer* SpaceToDepth = net->AddSpaceToDepthLayer(spaceToDepthDesc, "SpaceToDepth");
    IConnectableLayer* input        = net->AddInputLayer(0, "input");
    Connect(input, SpaceToDepth, inputTensorInfo, 0, 0);

    TensorInfo outputTensorInfo(outputShape, DataType, qScale, qOffset);
    IConnectableLayer* output = net->AddOutputLayer(0, "output");
    Connect(SpaceToDepth, output, outputTensorInfo, 0, 0);

    return net;
}

void SpaceToDepthEndToEnd(const std::vector<armnn::BackendId>& backends,
                          const armnn::DataLayout& dataLayout,
                          TensorInfo& inputTensorInfo,
                          TensorInfo& outputTensorInfo,
                          std::vector<float>& inputData,
                          std::vector<float>& expectedOutputData,
                          const unsigned int blockSize)
{
    using namespace armnn;

    if (dataLayout == armnn::DataLayout::NCHW)
    {
        PermuteDataToNCHW<armnn::DataType::Float32>(backends, dataLayout, inputTensorInfo, inputData);
        PermuteDataToNCHW<armnn::DataType::Float32>(backends, dataLayout, outputTensorInfo, expectedOutputData);
    }

    // Builds up the structure of the network
    INetworkPtr net = CreateSpaceToDepthNetwork<armnn::DataType::Float32>(
            inputTensorInfo.GetShape(),
            outputTensorInfo.GetShape(),
            dataLayout,
            blockSize);

    BOOST_TEST_CHECKPOINT("Create a network");

    std::map<int, std::vector<float>> inputTensorData = { { 0, inputData } };
    std::map<int, std::vector<float>> expectedOutputTensorData = { { 0, expectedOutputData } };

    EndToEndLayerTestImpl<armnn::DataType::Float32, armnn::DataType::Float32>(
            move(net),
            inputTensorData,
            expectedOutputTensorData,
            backends);
}

void SpaceToDepthNHWCEndToEndTest1(const std::vector<armnn::BackendId>& defaultBackends)
{
    const unsigned int blockSize = 2;

    armnn::TensorShape inputShape{1, 2, 2, 1};
    armnn::TensorInfo inputTensorInfo(inputShape, armnn::DataType::Float32);

    armnn::TensorShape outputShape{1, 1, 1, 4};
    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);

    std::vector<float> inputData = std::vector<float>(
    {
        1.0f, 2.0f, 3.0f, 4.0f
    });

    std::vector<float> expectedOutputData = std::vector<float>(
    {
        1.0f, 2.0f, 3.0f, 4.0f
    });

    SpaceToDepthEndToEnd(defaultBackends,
                         armnn::DataLayout::NHWC,
                         inputTensorInfo,
                         outputTensorInfo,
                         inputData,
                         expectedOutputData,
                         blockSize);
}

void SpaceToDepthNCHWEndToEndTest1(const std::vector<armnn::BackendId>& defaultBackends)
{
    const unsigned int blockSize = 2;

    armnn::TensorShape inputShape{1, 2, 2, 1};
    armnn::TensorInfo inputTensorInfo(inputShape, armnn::DataType::Float32);

    armnn::TensorShape outputShape{1, 1, 1, 4};
    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);

    std::vector<float> inputData = std::vector<float>(
    {
        1.0f, 2.0f, 3.0f, 4.0f
    });

    std::vector<float> expectedOutputData = std::vector<float>(
    {
        1.0f, 2.0f, 3.0f, 4.0f
    });

    SpaceToDepthEndToEnd(defaultBackends,
                         armnn::DataLayout::NCHW,
                         inputTensorInfo,
                         outputTensorInfo,
                         inputData,
                         expectedOutputData,
                         blockSize);
}

void SpaceToDepthNHWCEndToEndTest2(const std::vector<armnn::BackendId>& defaultBackends)
{
    const unsigned int blockSize = 2;

    armnn::TensorShape inputShape{1, 2, 2, 2};
    armnn::TensorShape outputShape{1, 1, 1, 8};

    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);
    armnn::TensorInfo inputTensorInfo(inputShape, armnn::DataType::Float32);

    std::vector<float> inputData = std::vector<float>(
    {
        1.4f, 2.3f, 3.2f, 4.1f, 5.4f, 6.3f, 7.2f, 8.1f
    });

    std::vector<float> expectedOutputData = std::vector<float>(
    {
        1.4f, 2.3f, 3.2f, 4.1f, 5.4f, 6.3f, 7.2f, 8.1f
    });

    SpaceToDepthEndToEnd(defaultBackends,
                         armnn::DataLayout::NHWC,
                         inputTensorInfo,
                         outputTensorInfo,
                         inputData,
                         expectedOutputData,
                         blockSize);
}

void SpaceToDepthNCHWEndToEndTest2(const std::vector<armnn::BackendId>& defaultBackends)
{
    const unsigned int blockSize = 2;

    armnn::TensorShape inputShape{1, 2, 2, 2};
    armnn::TensorShape outputShape{1, 1, 1, 8};

    armnn::TensorInfo inputTensorInfo(inputShape, armnn::DataType::Float32);
    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);

    std::vector<float> inputData = std::vector<float>(
    {
        1.4f, 2.3f, 3.2f, 4.1f, 5.4f, 6.3f, 7.2f, 8.1f
    });

    std::vector<float> expectedOutputData = std::vector<float>(
    {
        1.4f, 2.3f, 3.2f, 4.1f, 5.4f, 6.3f, 7.2f, 8.1f
    });

    SpaceToDepthEndToEnd(defaultBackends,
                         armnn::DataLayout::NCHW,
                         inputTensorInfo,
                         outputTensorInfo,
                         inputData,
                         expectedOutputData,
                         blockSize);
}

} // anonymous namespace


/*!
 * *****************************************************************************
 *   \file main.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-02-19
 *   *****************************************************************************/

#include "flutils/imgFileLst.h"
#include "flutils/cv_utils.h"

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include <boost/filesystem.hpp>

#include <array>
#include <iostream>

using namespace std;
using namespace cv;
using namespace glm;
using namespace boost;
using namespace Eigen;

namespace
{
//
bool filterEq( const Mat& img, const vec3& val )
{
   const ivec2 sz = {img.cols, img.rows};

   // store parallel results for every rows
   vector<bool> res( sz.y, false );
#pragma omp parallel for
   for ( unsigned y = 0; y < sz.y; y++ )
   {
      const vec3* ptr = img.ptr<vec3>( y );
      for ( unsigned x = 0; x < sz.x; x++ )
      {
         if ( all( equal( val, ptr[x] ) ) )
         {
            res[y] = true;
            break;
         }
      }
   }

   // check the results
   for ( const auto r : res )
      if ( r ) return true;
   return false;
}

}  // namespace

//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------

const string keys =
    "{help h usage ? |         | print this message   }"
    "{@imgFileLst   |         | images list   }"
    "{@imgRootDir   |         | images root dir   }"
    "{@value        | 0.0     | value to find }"
    "{lstSz         | 1       |    }"
    "{lstId         | 0       |    }";

int main( int argc, char* argv[] )
{
   CommandLineParser parser( argc, argv, keys );
   if ( parser.has( "help" ) )
   {
      parser.printMessage();
      return ( 0 );
   }

   const int lstSz = parser.get<int>( "lstSz" );
   const int lstId = parser.get<int>( "lstId" );

   const vec3 value( parser.get<float>( "@value" ) );

   // Create the list
   ImgNFileLst imgLst(
       lstSz,
       parser.get<string>( "@imgFileLst" ).c_str(),
       parser.get<string>( "@imgRootDir" ).c_str() );
   if ( imgLst.size() == 0 )
   {
      cerr << "Invalid dataset : " << parser.get<string>( "@imgFileLst" ) << endl;
      return -1;
   }

   // stats
   const size_t nbSamples = imgLst.size();
   size_t nbFilteredSamples = 0;
   for ( size_t i = 0; i < nbSamples; ++i )
   {
      Mat img = cv_utils::imread32FC3( imgLst.filePath( i, lstId ), false, true );
      if ( img.empty() )
      {
         cout << imgLst.filePath( i, lstId ) << 0 << endl;
         cerr << "Cannot load image : " << imgLst.filePath( i, lstId ) << endl;
         continue;
      }

      const bool filtered = filterEq( img, val );
      if ( filtered ) nbFilteredSamples++;
      cout << imgLst.filePath( i, lstId ) << 0 << endl;
   }

   cerr << "Filtered : " << nbFilteredSamples << " / " << nbSamples << " ("
        << ( 100.0f * nbFilteredSamples / nbSamples ) << "%)" << endl;

   return ( 0 );
}


// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators/count.hpp>
#include <boost/histogram/algorithm/sum.hpp>
#include <boost/histogram/axis/integer.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/ostream.hpp>
#include <boost/histogram/storage_adaptor.hpp>
#include <iostream>
#include <random>
#include <thread>
#include "throw_exception.hpp"
#include "utility_histogram.hpp"

using namespace boost::histogram;

constexpr auto n_fill = 80000;
static_assert(n_fill % 4 == 0, "must be multiple of 4");

template <class Tag, class A1, class A2, class X, class Y>
void fill_test(const A1& a1, const A2& a2, const X& x, const Y& y) {
  auto h1 = make_s(Tag{}, dense_storage<int>(), a1, a2);
  auto xy = {x, y};
  h1.fill(xy);

  auto h2 = make_s(Tag{}, dense_storage<accumulators::count<int, true>>(), a1, a2);
  auto run = [&h2, &x, &y](int k) {
    constexpr auto shift = n_fill / 4;
    auto xit = x.cbegin() + k * shift;
    auto yit = y.cbegin() + k * shift;
    for (unsigned i = 0; i < shift; ++i) h2(*xit++, *yit++);
  };

  std::thread t1([&] { run(0); });
  std::thread t2([&] { run(1); });
  std::thread t3([&] { run(2); });
  std::thread t4([&] { run(3); });
  t1.join();
  t2.join();
  t3.join();
  t4.join();

  BOOST_TEST_EQ(algorithm::sum(h1), n_fill);
  BOOST_TEST_EQ(algorithm::sum(h2), n_fill);
  BOOST_TEST_EQ(h1, h2);
}

template <class T>
void tests() {
  std::mt19937 gen(1);
  std::uniform_int_distribution<> id(-5, 5);
  std::vector<int> vi(n_fill), vj(n_fill);
  std::generate(vi.begin(), vi.end(), [&] { return id(gen); });
  std::generate(vj.begin(), vj.end(), [&] { return id(gen); });

  using i = axis::integer<>;
  using ig = axis::integer<int, use_default, axis::option::growth_t>;
  fill_test<T>(i{0, 1}, i{0, 1}, vi, vj);
  fill_test<T>(ig{0, 1}, i{0, 1}, vi, vj);
  fill_test<T>(i{0, 1}, ig{0, 1}, vi, vj);
  fill_test<T>(ig{0, 1}, ig{0, 1}, vi, vj);
}

int main() {
  tests<static_tag>();
  tests<dynamic_tag>();

  return boost::report_errors();
}


// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/StdVector>
#include <random>
#include <iostream>
#include <stdint.h>

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/types/icp/types_icp.h"

using namespace Eigen;
using namespace std;
using namespace g2o;

// sampling distributions
  class Sample
  {

    static default_random_engine gen_real;
    static default_random_engine gen_int;
  public:
    static int uniform(int from, int to);

    static double uniform();

    static double gaussian(double sigma);
  };

  default_random_engine Sample::gen_real;
  default_random_engine Sample::gen_int;

  int Sample::uniform(int from, int to)
  {
    uniform_int_distribution<int> unif(from, to);
    int sam = unif(gen_int);
    return  sam;
  }

  double Sample::uniform()
  {
    std::uniform_real_distribution<double> unif(0.0, 1.0);
    double sam = unif(gen_real);
    return  sam;
  }

  double Sample::gaussian(double sigma)
  {
    std::normal_distribution<double> gauss(0.0, sigma);
    double sam = gauss(gen_real);
    return  sam;
  }

//
// set up simulated system with noise, optimize it
//

int main(int argc, char **argv)
{
  int num_points = 0;

  // check for arg, # of points to use in projection SBA
  if (argc > 1)
    num_points = atoi(argv[1]);

  double euc_noise = 0.1;      // noise in position, m
  double pix_noise = 1.0;       // pixel noise
  //  double outlier_ratio = 0.1;

  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // variable-size block solver
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<BlockSolverX>(g2o::make_unique<LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>>()));

  optimizer.setAlgorithm(solver);

  vector<Vector3d> true_points;
  for (size_t i=0;i<1000; ++i)
  {
    true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
                                   Sample::uniform()-0.5,
                                   Sample::uniform()+10));
  }

  // set up camera params
  Vector2d focal_length(500,500); // pixels
  Vector2d principal_point(320,240); // 640x480 image
  double baseline = 0.075;      // 7.5 cm baseline

  // set up camera params and projection matrices on vertices
  g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
                           principal_point[0],principal_point[1],
                           baseline);

  // set up two poses
  int vertex_id = 0;
  for (size_t i=0; i<2; ++i)
  {
    // set up rotation and translation for this node
    Vector3d t(0,0,i);
    Quaterniond q;
    q.setIdentity();

    Eigen::Isometry3d cam;           // camera pose
    cam = q;
    cam.translation() = t;

    // set up node
    VertexSCam *vc = new VertexSCam();
    vc->setEstimate(cam);
    vc->setId(vertex_id);      // vertex id

    cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;

    // set first cam pose fixed
    if (i==0)
      vc->setFixed(true);

    // make sure projection matrices are set
    vc->setAll();

    // add to optimizer
    optimizer.addVertex(vc);

    vertex_id++;
  }

  // set up point matches for GICP
  for (size_t i=0; i<true_points.size(); ++i)
  {
    // get two poses
    VertexSE3* vp0 =
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
    VertexSE3* vp1 =
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

    // calculate the relative 3D position of the point
    Vector3d pt0,pt1;
    pt0 = vp0->estimate().inverse() * true_points[i];
    pt1 = vp1->estimate().inverse() * true_points[i];

    // add in noise
    pt0 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    pt1 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    // form edge, with normals in varioius positions
    Vector3d nm0, nm1;
    nm0 << 0, i, 1;
    nm1 << 0, i, 1;
    nm0.normalize();
    nm1.normalize();

    Edge_V_V_GICP * e           // new edge with correct cohort for caching
        = new Edge_V_V_GICP();

    e->vertices()[0]            // first viewpoint
      = dynamic_cast<OptimizableGraph::Vertex*>(vp0);

    e->vertices()[1]            // second viewpoint
      = dynamic_cast<OptimizableGraph::Vertex*>(vp1);

    EdgeGICP meas;

    meas.pos0 = pt0;
    meas.pos1 = pt1;
    meas.normal0 = nm0;
    meas.normal1 = nm1;
    e->setMeasurement(meas);
    meas = e->measurement();
    //        e->inverseMeasurement().pos() = -kp;

    // use this for point-plane
    e->information() = meas.prec0(0.01);

    // use this for point-point
    //    e->information().setIdentity();

    //    e->setRobustKernel(true);
    //e->setHuberWidth(0.01);

    optimizer.addEdge(e);
  }

  // set up SBA projections with some number of points

  true_points.clear();
  for (int i=0;i<num_points; ++i)
  {
    true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
                                   Sample::uniform()-0.5,
                                   Sample::uniform()+10));
  }

  // add point projections to this vertex
  for (size_t i=0; i<true_points.size(); ++i)
  {
    g2o::VertexSBAPointXYZ * v_p
        = new g2o::VertexSBAPointXYZ();

    v_p->setId(vertex_id++);
    v_p->setMarginalized(true);
    v_p->setEstimate(true_points.at(i)
        + Vector3d(Sample::gaussian(1),
                   Sample::gaussian(1),
                   Sample::gaussian(1)));

    optimizer.addVertex(v_p);

    for (size_t j=0; j<2; ++j)
      {
        Vector3d z;
        dynamic_cast<g2o::VertexSCam*>
          (optimizer.vertices().find(j)->second)
          ->mapPoint(z,true_points.at(i));

        if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
        {
          z += Vector3d(Sample::gaussian(pix_noise),
                        Sample::gaussian(pix_noise),
                        Sample::gaussian(pix_noise/16.0));

          g2o::Edge_XYZ_VSC * e
              = new g2o::Edge_XYZ_VSC();

          e->vertices()[0]
              = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);

          e->vertices()[1]
              = dynamic_cast<g2o::OptimizableGraph::Vertex*>
              (optimizer.vertices().find(j)->second);

          e->setMeasurement(z);
          //e->inverseMeasurement() = -z;
          e->information() = Matrix3d::Identity();

          //e->setRobustKernel(false);
          //e->setHuberWidth(1);

          optimizer.addEdge(e);
        }

      }
  } // done with adding projection points

  // move second cam off of its true position
  VertexSE3* vc =
    dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
  Eigen::Isometry3d cam = vc->estimate();
  cam.translation() = Vector3d(-0.1,0.1,0.2);
  vc->setEstimate(cam);
  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;

  optimizer.setVerbose(true);

  optimizer.optimize(20);

  cout << endl << "Second vertex should be near 0,0,1" << endl;
  cout <<  dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
    ->estimate().translation().transpose() << endl;
  cout <<  dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
    ->estimate().translation().transpose() << endl;
}


#pragma once

#include <cmath>
#include <chrono>
#include <iomanip>
#include <random>
#include <stdexcept>

#include <dtl/dtl.hpp>
#include <dtl/batchwise.hpp>
#include <dtl/math.hpp>
#include <dtl/mem.hpp>
#include <dtl/simd.hpp>

#include "block_addressing_logic.hpp"
#include "blocked_bloomfilter_logic.hpp"
#include "blocked_bloomfilter_tune.hpp"
#include "hash_family.hpp"

#include <boost/math/common_factor.hpp>

namespace dtl {

//===----------------------------------------------------------------------===//
template<typename Tw = $u32>
struct blocked_bloomfilter {

  using key_t = $u32;
  using hash_value_t = $u32;
  using word_t = Tw;

  static constexpr u1 early_out = false; // TODO make configurable and also adaptive

  // The operations used for dynamic dispatching.
  enum class op_t {
    CONSTRUCT,  // constructs the Bloom filter logic
    BIND,       // assigns the function pointers of the Bloom filter API
    DESTRUCT,   // destructs the Bloom filter logic
  };

  // The first hash function to use inside the block. Note: 0 is used for block addressing
  static constexpr u32 block_hash_fn_idx = 1;

  // The block type.
  template<u32 word_cnt, u32 sector_cnt, u32 k, u1 early_out = false>
  using bbf_block_t = typename blocked_bloomfilter_block_logic<key_t, word_t, word_cnt, sector_cnt, k,
                                                               hasher, hash_value_t, early_out, block_hash_fn_idx>::type;

  template<u32 word_cnt, u32 sector_cnt, u32 k, dtl::block_addressing addr = dtl::block_addressing::POWER_OF_TWO, u1 early_out = false>
  using bbf = dtl::blocked_bloomfilter_logic<key_t, hasher, bbf_block_t<word_cnt, sector_cnt, k, early_out>, addr, early_out>;

  //===----------------------------------------------------------------------===//
  // Members
  //===----------------------------------------------------------------------===//
  /// The (desired) bit length of the Bloom filter.
  $u64 m;
  /// The (actual) bit length of the Bloom filter.
  $u64 m_actual;
  /// The number of bits set per entry.
  $u32 k;
  /// The number of words per block.
  $u32 word_cnt_per_block;
  /// The number of sectors per block.
  $u32 sector_cnt;
  /// Pointer to the Bloom filter logic instance.
  void* instance = nullptr;
  /// A container for the (hardware dependent) tuning parameters.
  const blocked_bloomfilter_tune& tune;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The API functions. (Function pointers to the actual implementation.)
  //===----------------------------------------------------------------------===//
  std::function<void(__restrict word_t* /*filter data*/, const key_t /*key*/)>
  insert;

  std::function<void(__restrict word_t* /*filter data*/, const key_t* /*keys*/, u32 /*key_cnt*/)>
  batch_insert;

  std::function<$u1(const __restrict word_t* /*filter data*/, const key_t /*key*/)>
  contains;

  std::function<$u64(const __restrict word_t* /*filter data*/,
                     const key_t* /*keys*/, u32 /*key_cnt*/,
                     $u32* /*match_positions*/, u32 /*match_offset*/)>
  batch_contains;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_bloomfilter(const std::size_t m, u32 k, u32 word_cnt_per_block, u32 sector_cnt,
                      const blocked_bloomfilter_tune& tune)
      : m(m), k(k), word_cnt_per_block(word_cnt_per_block), sector_cnt(sector_cnt), tune(tune) {

    // Construct the Bloom filter logic instance.
    dispatch(*this, op_t::CONSTRUCT);

    // Bind the API functions.
    dispatch(*this, op_t::BIND);

    // Check whether the constructed filter matches the given arguments.
    if (this->k != k
        || this->word_cnt_per_block != word_cnt_per_block
        || this->sector_cnt != sector_cnt) {
      dispatch(*this, op_t::DESTRUCT);
      throw std::invalid_argument("Invalid configuration: k=" + std::to_string(k)
                                  + ", w=" + std::to_string(word_cnt_per_block)
                                  + ", s=" + std::to_string(sector_cnt));
    }

  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_bloomfilter(blocked_bloomfilter&& src)
      : m(src.m), m_actual(src.m_actual), k(src.k),
        word_cnt_per_block(src.word_cnt_per_block), sector_cnt(src.sector_cnt),
        instance(src.instance),
        insert(std::move(src.insert)),
        batch_insert(std::move(src.batch_insert)),
        contains(std::move(src.contains)),
        batch_contains(std::move(src.batch_contains)),
        tune(std::move(src.tune)) {
    // Invalidate pointer in src
    src.instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  ~blocked_bloomfilter() {
    // Destruct logic instance (if any).
    if (instance != nullptr) dispatch(*this, op_t::DESTRUCT);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  blocked_bloomfilter&
  operator=(blocked_bloomfilter&& src) {
    m = src.m;
    m_actual = src.m_actual;
    k = src.k;
    word_cnt_per_block = src.word_cnt_per_block;
    sector_cnt = src.sector_cnt;
    instance = src.instance;
    insert = std::move(src.insert);
    batch_insert = std::move(src.batch_insert);
    contains = std::move(src.contains);
    batch_contains = std::move(src.batch_contains);
    // invalidate pointers
    src.instance = nullptr;
    return *this;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Dynamic Dispatching
  //===----------------------------------------------------------------------===//
  //TODO make private
  static void
  dispatch(blocked_bloomfilter& instance, op_t op) {
      switch (instance.word_cnt_per_block) {
        case  1: _s< 1>(instance, op); break;
        case  2: _s< 2>(instance, op); break;
#ifndef FAST_BUILD
        case  4: _s< 4>(instance, op); break;
        case  8: _s< 8>(instance, op); break;
        case 16: _s<16>(instance, op); break;
#endif
        default:
          throw std::invalid_argument("The given 'word_cnt_per_block' is not supported.");
      }
  };

  static constexpr u32 max_sector_cnt_per_word = sizeof(word_t); // a sector is at least one byte in size

  template<u32 w>
  static void
  _s(blocked_bloomfilter& instance, op_t op) {
    switch (instance.sector_cnt) {
      case  1: _k<w,  1>(instance, op); break;
#ifndef FAST_BUILD
      case  2: _k<w, boost::static_unsigned_min<(w * max_sector_cnt_per_word),  2>::value>(instance, op); break;
      case  4: _k<w, boost::static_unsigned_min<(w * max_sector_cnt_per_word),  4>::value>(instance, op); break;
      case  8: _k<w, boost::static_unsigned_min<(w * max_sector_cnt_per_word),  8>::value>(instance, op); break;
      case 16: _k<w, boost::static_unsigned_min<(w * max_sector_cnt_per_word), 16>::value>(instance, op); break;
#endif
      default:
        throw std::invalid_argument("The given 'sector_cnt' is not supported.");
    }
  }

  template<u32 w, u32 s>
  static void
  _k(blocked_bloomfilter& instance, op_t op) {
    switch (instance.k) {
      case  1: _a<w, s, boost::static_unsigned_max<1, s>::value>(instance, op); break;
      case  2: _a<w, s, boost::static_unsigned_max<( 2 % s == 0 ?  2 : 1 /*invalid*/), s>::value>(instance, op); break;
      case  4: _a<w, s, boost::static_unsigned_max<( 4 % s == 0 ?  4 : 1 /*invalid*/), s>::value>(instance, op); break;
      case  6: _a<w, s, boost::static_unsigned_max<( 6 % s == 0 ?  6 : 1 /*invalid*/), s>::value>(instance, op); break;
      case  8: _a<w, s, boost::static_unsigned_max<( 8 % s == 0 ?  8 : 1 /*invalid*/), s>::value>(instance, op); break;
#ifndef FAST_BUILD
      case  3: _a<w, s, boost::static_unsigned_max<( 3 % s == 0 ?  3 : 1 /*invalid*/), s>::value>(instance, op); break;
      case  5: _a<w, s, boost::static_unsigned_max<( 5 % s == 0 ?  5 : 1 /*invalid*/), s>::value>(instance, op); break;
      case  7: _a<w, s, boost::static_unsigned_max<( 7 % s == 0 ?  7 : 1 /*invalid*/), s>::value>(instance, op); break;

      case  9: _a<w, s, boost::static_unsigned_max<( 9 % s == 0 ?  9 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 10: _a<w, s, boost::static_unsigned_max<(10 % s == 0 ? 10 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 11: _a<w, s, boost::static_unsigned_max<(11 % s == 0 ? 11 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 12: _a<w, s, boost::static_unsigned_max<(12 % s == 0 ? 12 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 13: _a<w, s, boost::static_unsigned_max<(13 % s == 0 ? 13 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 14: _a<w, s, boost::static_unsigned_max<(14 % s == 0 ? 14 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 15: _a<w, s, boost::static_unsigned_max<(15 % s == 0 ? 15 : 1 /*invalid*/), s>::value>(instance, op); break;
      case 16: _a<w, s, boost::static_unsigned_max<16 , s>::value>(instance, op); break;
#endif
      default:
        throw std::invalid_argument("The given 'k' is not supported.");
    }
  }

  template<u32 w, u32 s, u32 k>
  static void
  _a(blocked_bloomfilter& instance, op_t op) {
    dtl::block_addressing addr = dtl::is_power_of_two(instance.m)
                                 ? dtl::block_addressing::POWER_OF_TWO
                                 : dtl::block_addressing::MAGIC;
    switch (addr) {
      case dtl::block_addressing::POWER_OF_TWO: _u<w, s, k, dtl::block_addressing::POWER_OF_TWO>(instance, op); break;
      case dtl::block_addressing::MAGIC:        _u<w, s, k, dtl::block_addressing::MAGIC>(instance, op);        break;
      case dtl::block_addressing::DYNAMIC:      /* must not happen */                                           break;
    }
  }

  template<u32 w, u32 s, u32 k, dtl::block_addressing a>
  static void
  _u(blocked_bloomfilter& instance, op_t op) {
    blocked_bloomfilter_config c;
    c.k = k;
    c.word_size = sizeof(word_t);
    c.word_cnt_per_block = w;
    c.sector_cnt = s;
    c.addr_mode = a;
    switch (instance.tune.get_unroll_factor(c)) {
      case  0: _o<w, s, k, a,  0>(instance, op); break;
      case  1: _o<w, s, k, a,  1>(instance, op); break;
      case  2: _o<w, s, k, a,  2>(instance, op); break;
      case  4: _o<w, s, k, a,  4>(instance, op); break;
#ifndef FAST_BUILD
      case  8: _o<w, s, k, a,  8>(instance, op); break;
#endif
      default:
        throw std::invalid_argument("The given 'unroll_factor' is not supported.");
    }
  }

  template<u32 w, u32 s, u32 k, dtl::block_addressing a, u32 unroll_factor>
  static void
  _o(blocked_bloomfilter& instance, op_t op) {
//    using _t = typename internal::bbf_switch<key_t, word_t, w, s, k, a, early_out>::type;
    using _t = bbf<w, s, k, a, early_out>;
    switch (op) {
      case op_t::CONSTRUCT: instance._construct_logic<_t>();           break;
      case op_t::BIND:      instance._bind_logic<_t, unroll_factor>(); break;
      case op_t::DESTRUCT:  instance._destruct_logic<_t>();            break;
    }
  };
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Constructs a blocked Bloom filter logic instance.
  template<
      typename bf_t
  >
  void
  _construct_logic() {
    // Instantiate a Bloom filter logic.
    bf_t* bf = new bf_t(m);
    instance = bf;
    k = bf_t::k;
    word_cnt_per_block = bf_t::word_cnt_per_block;
    sector_cnt = bf_t::sector_cnt;

    // Get the actual size of the filter.
    m_actual = bf->get_length();
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Bind API functions to the (previously constructed) Bloom filter logic.
  template<
      typename bf_t,
      u32 unroll_factor = 1
  >
  void
  _bind_logic() {
    using namespace std::placeholders;
    auto* bf = static_cast<bf_t*>(instance);

    // Bind the API functions.
    insert = std::bind(&bf_t::insert, bf, _1, _2);
    batch_insert = std::bind(&bf_t::batch_insert, bf, _1, _2, _3);
    contains = std::bind(&bf_t::contains, bf, _1, _2);

    // SIMD vector length (0 = run scalar code)
    // TODO fix unrolling for word_t = u64
    static constexpr u64 vector_len = dtl::simd::lane_count<key_t> * unroll_factor;
    batch_contains = std::bind(&bf_t::template batch_contains<vector_len>, bf, _1, _2, _3, _4, _5);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Destructs the Bloom filter logic.
  template<typename bf_t>
  void
  _destruct_logic() {
    bf_t* bf = static_cast<bf_t*>(instance);
    delete bf;
    instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the block addressing mode.
  block_addressing get_addressing_mode() const {
    return dtl::is_power_of_two(m)
           ? block_addressing::POWER_OF_TWO
           : block_addressing::MAGIC;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (actual) size in bytes.
  std::size_t
  size_in_bytes() const noexcept {
    return (m_actual + 7) / 8;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (total) number of words.
  std::size_t
  size() const noexcept {
    constexpr u32 word_bitlength = sizeof(word_t) * 8;
    return (m_actual + (word_bitlength - 1)) / word_bitlength;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the name of the Bloom filter instance including the most
  /// important parameters (in JSON).
  std::string
  name() {
    blocked_bloomfilter_config c;
    c.k = k;
    c.word_size = sizeof(word_t);
    c.word_cnt_per_block = word_cnt_per_block;
    c.sector_cnt = sector_cnt;
    c.zone_cnt = 1;
    c.addr_mode = get_addressing_mode();

    return "{\"name\":\"blocked_bloom_multiword\",\"size\":" + std::to_string(size_in_bytes())
         + ",\"word_size\":" + std::to_string(sizeof(word_t))
         + ",\"k\":" + std::to_string(k)
         + ",\"w\":" + std::to_string(word_cnt_per_block)
         + ",\"s\":" + std::to_string(sector_cnt)
         + ",\"u\":" + std::to_string(tune.get_unroll_factor(c))
         + ",\"e\":" + std::to_string(early_out ? 1 : 0)
         + ",\"addr\":" + (get_addressing_mode() == dtl::block_addressing::POWER_OF_TWO ? "\"pow2\"" : "\"magic\"")
         + "}";
  }
  //===----------------------------------------------------------------------===//

};

} // namespace dtl


#include "streaming_clouds.h"

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

namespace Streaming
{

//----( distance logger )-----------------------------------------------------

DistanceLogger::DistanceLogger (size_t size, bool verbose)
  : m_prev(size),
    m_continuing(false),
    m_total_squared_distance(0),
    m_num_frames(0),
    m_verbose(verbose)
{}

DistanceLogger::~DistanceLogger ()
{
  size_t num_pairs = m_num_frames - 1;
  if (num_pairs) {
    double rms_point_distance = sqrt(m_total_squared_distance / num_pairs);
    PRINT2(rms_point_distance, num_pairs);
  }
}

double DistanceLogger::get_rms_distance ()
{
  size_t num_pairs = m_num_frames - 1;
  if (num_pairs) {
    return sqrt(m_total_squared_distance / num_pairs);
  } else {
    return 0;
  }
}

void DistanceLogger::add (const Cloud::Point & point)
{
  if (m_continuing) {
    float squared_distance = Cloud::squared_distance(point, m_prev);
    m_total_squared_distance += squared_distance;

    if (m_verbose) {
      float point_distance = sqrtf(squared_distance);
      PRINT(point_distance);
    }
  }

  m_prev = point;
  m_continuing = true;
}

void DistanceLogger::add (
    const Cloud::Point & point,
    const Cloud::Point & prev)
{
  ++m_num_frames;

  m_total_squared_distance += Cloud::squared_distance(point, prev);

  // this is unnecessary in the pairwise mode of use
  //m_prev = point;
}

//----( denoiser )------------------------------------------------------------

CloudDenoiser::CloudDenoiser (const Cloud::PointSet & points, size_t iters)
  : m_points(points),
    m_iters(iters),
    m_likes(points.size),
    m_point(points.dim),
    out("CloudDenoiser.out", points.dim)
{
  ASSERT_LT(0, iters);
}

void CloudDenoiser::push (Seconds time, const Cloud::Point & point)
{
  m_points.quantize(point, m_likes);
  m_points.construct(m_likes, m_point);

  for (size_t iters = 1; iters < m_iters; ++iters) {
    m_points.quantize(m_point, m_likes);
    m_points.construct(m_likes, m_point);
  }

  out.push(time, m_point);
}

//----( simulator )-----------------------------------------------------------

CloudSimulator::CloudSimulator (
    const Cloud::JointPrior & flow,
    float framerate)

  : TimedThread(framerate),

    m_points(flow.dom),
    m_flow(flow),

    m_state(0),
    m_next(* new VectorXf(m_points.size)),
    m_point(m_points.dim),

    out("CloudSimulator.out", m_points.dim)
{
  reset();
}

CloudSimulator::~CloudSimulator ()
{
  delete & m_next;
}

void CloudSimulator::reset ()
{
  m_mutex.lock();

  m_points.get_prior(m_next);
  m_state = Cloud::random_index(m_next);

  m_mutex.unlock();
}

void CloudSimulator::step ()
{
  m_mutex.lock();

  VectorSf next_like = m_flow.joint.col(m_state);
  m_state = Cloud::random_index(next_like);

  m_points.get_point(m_state, m_point);

  m_mutex.unlock();

  out.push(Seconds::now(), m_point);

  PROGRESS_TICKER('s');
}

void CloudSimulator::keyboard (const SDL_KeyboardEvent & event)
{
  switch (event.keysym.sym) {
    case SDLK_SPACE:
      if (event.type == SDL_KEYDOWN) reset();
      break;

    default:
      break;
  }
}

//----( diffuser )------------------------------------------------------------

CloudDiffuser::CloudDiffuser (
    const Cloud::JointPrior & flow,
    float framerate)

  : TimedThread(framerate),

    m_points(flow.dom),
    m_flow(flow),

    m_state(* new VectorXf(m_points.size)),
    m_next(* new VectorXf(m_points.size)),
    m_point(m_points.dim),

    out("CloudDiffuser.out", m_points.dim)
{
  reset();
}

CloudDiffuser::~CloudDiffuser ()
{
  delete & m_state;
  delete & m_next;
}

void CloudDiffuser::reset ()
{
  m_mutex.lock();

  m_points.get_prior(m_next);
  int p = Cloud::random_index(m_next);

  m_next.setZero();
  m_next(p) = m_next.size();

  m_points.construct(m_next, m_point);
  m_points.quantize(m_point, m_state);

  m_mutex.unlock();
}

void CloudDiffuser::step ()
{
  m_mutex.lock();

  m_flow.push_forward(m_state, m_next);
  Cloud::normalize_l1(m_next, m_next.size());
  PRINT(Cloud::likelihood_entropy(m_next));

  m_points.construct(m_next, m_point);
  m_points.quantize(m_point, m_state);

  m_mutex.unlock();

  out.push(Seconds::now(), m_point);

  PROGRESS_TICKER('s');
}

void CloudDiffuser::keyboard (const SDL_KeyboardEvent & event)
{
  switch (event.keysym.sym) {
    case SDLK_SPACE:
      if (event.type == SDL_KEYDOWN) reset();
      break;

    default:
      break;
  }
}

//----( gloves <- gloves )----------------------------------------------------

GlovesToGloves::GlovesToGloves (Cloud::Controller & controller)
  : Rectangle(controller.cod().shape),
    m_controller(controller),
    m_image(controller.dom().shape.size()),
    out("GlovesToGloves.out", controller.dom().shape)
{
}

void GlovesToGloves::push (Seconds time, const Gloves8Image & image)
{
  ASSERT_SIZE(image, size() * Gloves8Image::num_channels);

  m_controller.update_track(image, m_image);

  out.push(time, m_image);
}

//----( gloves <- voice )-----------------------------------------------------

GlovesToVoice::GlovesToVoice (
      Cloud::Controller & controller,
      const char * voice_config)
  : Rectangle(controller.cod().shape),

    m_controller(controller),
    m_feature_processor(voice_config),
    m_voice_features(controller.dom().dim),

    out("GlovesToVoice.out", controller.dom().dim)
{
}

void GlovesToVoice::push (Seconds time, const Gloves8Image & image)
{
  ASSERT_SIZE(image, size() * Gloves8Image::num_channels);

  m_controller.update_track(image, m_voice_features);

  if (m_controller.is_tracking() and m_controller.is_coalescing()) {
    m_feature_processor.update_history(m_voice_features);
    m_controller.set_track(m_voice_features);
  }

  out.push(time, m_voice_features);
}

} // namespace Streaming