#include <boost/test/unit_test.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

BOOST_FIXTURE_TEST_SUITE(Operators_t, Context_Fixture)

BOOST_AUTO_TEST_CASE(logical_not_t1) {
  randv<unsigned int> a(0);
  Generator gen;
  gen(!(a() != 0));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(a, 0);
}

BOOST_AUTO_TEST_CASE(logical_not_t2) {
  randv<unsigned char> a(0);
  randv<unsigned int> b(0);
  Generator gen;

  gen(if_then_else(!(a() % 2 == 0), b() > 0 && b() <= 50, b() > 50 && b() <= 100));

  BOOST_REQUIRE(gen.next());
  std::cout << "a =" << a << ", b = " << b << std::endl;
  if (a % 2 != 0) {
    BOOST_REQUIRE_GT(b, 0u);
    BOOST_REQUIRE_LE(b, 50u);
  } else {
    BOOST_REQUIRE_GT(b, 50u);
    BOOST_REQUIRE_LE(b, 100u);
  }
}

BOOST_AUTO_TEST_CASE(logical_and_t1) {
  randv<bool> a(0);
  randv<bool> b(0);
  randv<bool> c(0);
  Generator gen(a() == true);
  gen(b() == true);

  BOOST_REQUIRE(gen.next());
  gen(c() == (a() && b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c, true);

  Generator gen2(a() == true);
  gen2(b() == false);

  BOOST_REQUIRE(gen2.next());
  gen2(c() == (a() && b()));

  BOOST_REQUIRE(gen2.next());
  BOOST_CHECK_EQUAL(c, false);
}

BOOST_AUTO_TEST_CASE(logical_or_t1) {
  randv<bool> a(0);
  randv<bool> b(0);
  randv<bool> c(0);
  Generator gen(a() == false);
  gen(b() == false);

  BOOST_REQUIRE(gen.next());
  gen(c() == (a() || b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c, false);

  Generator gen2(a() == true);
  gen2(b() == false);

  BOOST_REQUIRE(gen2.next());
  gen2(c() == (a() || b()));

  BOOST_REQUIRE(gen2.next());
  BOOST_CHECK_EQUAL(c, true);
}

BOOST_AUTO_TEST_CASE(equal_t1) {
  randv<unsigned int> a(0);
  randv<unsigned int> b(0);
  Generator gen(a() == 65535);
  gen(b() == a());

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(not_equal_t1) {
  randv<unsigned int> a(0);
  randv<unsigned int> b(0);
  Generator gen(a() < 65535);
  gen(b() != a());

  for (int i = 0; i < 300; ++i) {
    BOOST_REQUIRE(gen.next());
    BOOST_CHECK_NE(a, b);
  }
}

BOOST_AUTO_TEST_CASE(less) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;

  Generator gen;
  gen(a < 256u);

  std::set<unsigned> generated;
  for (unsigned iterations = 0; gen.next(); ++iterations) {
    unsigned av = gen[a];
    generated.insert(av);
    gen(a != av);
    BOOST_REQUIRE_LT(iterations, 300);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 256);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(less_equal) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;

  Generator gen;
  gen(a <= 256u);

  std::set<unsigned> generated;
  for (unsigned iterations = 0; gen.next(); ++iterations) {
    unsigned av = gen[a];
    generated.insert(av);
    gen(a != av);
    BOOST_REQUIRE_LT(iterations, 300);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 257);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(greater) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;

  Generator gen;
  gen(a > (std::numeric_limits<unsigned>::max() - 256));

  std::set<unsigned> generated;
  for (unsigned iterations = 0; gen.next(); ++iterations) {
    unsigned av = gen[a];
    generated.insert(av);
    gen(a != av);
    BOOST_REQUIRE_LT(iterations, 300);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 256);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(greater_equal) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;

  Generator gen;
  gen(a >= (std::numeric_limits<unsigned>::max() - 256));

  std::set<unsigned> generated;
  for (unsigned iterations = 0; gen.next(); ++iterations) {
    unsigned av = gen[a];
    generated.insert(av);
    gen(a != av);
    BOOST_REQUIRE_LT(iterations, 300);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 257);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(neg_t1) {
  randv<int> a(0);
  randv<int> b(0);
  Generator gen(a() == -1337);

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(a, -1337);
  gen(b() == -a());

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(b, 1337);
}

BOOST_AUTO_TEST_CASE(neg_t2) {
  randv<bool> a(0);
  randv<int> b(0);
  randv<int> c(0);
  randv<int> d(0);
  Generator gen(dist(a(), distribution<bool>::create(0.5)));
  gen(b() == 1337 && c() == 42);
  gen(if_then_else(a(), d() == -b(), d() == -c()));

  for (int i = 0; i < 50; ++i) {
    BOOST_CHECK(gen.next());
    if (a)
      BOOST_CHECK_EQUAL(d, -1337);
    else
      BOOST_CHECK_EQUAL(d, -42);
  }
}

BOOST_AUTO_TEST_CASE(complement_t1) {
  randv<int> a(0);
  randv<int> b(0);
  Generator gen(a() == 0);

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(a, 0);
  gen(b() == ~a());

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(b, -1);
}

BOOST_AUTO_TEST_CASE(bitwise_and_t1) {
  randv<unsigned int> a(0);
  randv<unsigned int> b(0);
  randv<unsigned int> c(0);
  Generator gen(a() == 42);
  gen(b() == 1337);

  BOOST_REQUIRE(gen.next());
  gen(c() == (a() & b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c, 40);
}

BOOST_AUTO_TEST_CASE(bitwise_or_t1) {
  randv<unsigned int> a(0);
  randv<unsigned int> b(0);
  randv<unsigned int> c(0);
  Generator gen(a() == 42);
  gen(b() == 1337);

  BOOST_REQUIRE(gen.next());
  gen(c() == (a() | b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c, 1339);
}

BOOST_AUTO_TEST_CASE(xor_t1) {
  randv<bool> a(0);
  randv<bool> b(0);
  randv<unsigned int> c(0);
  Generator gen(a() == false);
  gen(b() == false);
  gen(c() == (a() ^ b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c != 0, false);

  Generator gen2(a() == false);
  gen2(b() == true);
  gen2(c() == (a() ^ b()));

  BOOST_REQUIRE(gen2.next());
  BOOST_CHECK_EQUAL(c != 0, true);
}

BOOST_AUTO_TEST_CASE(xor_t2) {
  randv<unsigned int> a(0);
  randv<unsigned int> b(0);
  randv<unsigned int> c(0);
  Generator gen(a() == 65535);
  gen(b() == 4080);

  BOOST_REQUIRE(gen.next());
  gen(c() == (a() ^ b()));

  BOOST_REQUIRE(gen.next());
  BOOST_CHECK_EQUAL(c, 61455);
}

BOOST_AUTO_TEST_CASE(shiftleft) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;
  Variable<unsigned> b;
  Variable<unsigned> c;

  Generator gen;
  gen(a < 256u)
  (b < (unsigned)(sizeof(unsigned) * 8u))(c == (a << b));

  int count = 0;
  while (gen.next() && ++count < 500) {
    unsigned av = gen[a];
    unsigned bv = gen[b];
    unsigned r = av << bv;

    BOOST_REQUIRE_EQUAL(r, gen[c]);

    gen(a != gen[a] || b != gen[b]);
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(shiftright) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned> a;
  Variable<unsigned> b;
  Variable<unsigned> c;

  Generator gen;
  gen(a > 256u)
  (b < 8u)(c == (a >> b));

  int count = 0;
  while (gen.next() && ++count < 500) {
    unsigned av = gen[a];
    unsigned bv = gen[b];
    unsigned r = av >> bv;

    BOOST_REQUIRE_EQUAL(r, gen[c]);

    gen(a != gen[a] || b != gen[b]);
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(plus_minus) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Variable<unsigned int> q;
  Variable<unsigned int> r;

  Generator gen;
  gen(b != 0u)
  (b < a)(q == a + b)(r == a - b);

  unsigned int cnt = 0u;
  while (gen.next() && cnt < 300) {
    BOOST_REQUIRE_EQUAL(gen[a] + gen[b], gen[q]);
    BOOST_REQUIRE_EQUAL(gen[a] - gen[b], gen[r]);

    gen(a != gen[a] || b != gen[b]);
    std::cout << "#" << cnt++ << ": result: a=" << gen[a] << ", b=" << gen[b] << ", q=" << gen[q] << ", r=" << gen[r]
              << "\n" << std::endl;
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(mult_mod) {
  randv<int> a(0);
  randv<int> b(0);

  Generator gen;
  gen(-3 <= a() && a() <= 3)
  (-3 <= b() && b() <= 3)(a() * b() % 6 == 0);

  int cnt = 0;
  for (int i = -3; i <= 3; i++)
    for (int j = -3; j <= 3; j++)
      if (i * j % 6 == 0) cnt++;

  int cnt1 = 0;
  while (gen.next()) {
    cnt1++;
    BOOST_REQUIRE_EQUAL(a * b % 6, 0);
    gen(a() != a || b() != b);
    std::cout << "result: a1=" << a << ", b1=" << b << "\n" << std::endl;
    BOOST_REQUIRE_LE(cnt1, cnt);
  }

  BOOST_REQUIRE_EQUAL(cnt, cnt1);
}

BOOST_AUTO_TEST_CASE(divide) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Variable<unsigned char> a;
  Variable<unsigned char> b;
  Variable<unsigned char> q;
  Variable<unsigned char> r;

  Generator gen;
  gen(b != (unsigned char)0u)
  (a < (unsigned char)16u)(b < (unsigned char)16u)(q == a / b)(r == a % b);

  while (gen.next()) {
    BOOST_REQUIRE_EQUAL(gen[a] / gen[b], gen[q]);
    BOOST_REQUIRE_EQUAL(gen[a] % gen[b], gen[r]);

    gen(a != gen[a] || b != gen[b]);
    std::cout << "result: a=" << gen[a] << ", b=" << gen[b] << ", q=" << gen[q] << ", r=" << gen[r] << "\n"
              << std::endl;
  }

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

BOOST_AUTO_TEST_CASE(element_inside_set) {
  std::set<unsigned> s;
  s.insert(1);
  s.insert(7);
  s.insert(9);

  randv<unsigned> x(0);

  Generator gen;
  gen(inside(x(), s));

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(s.find(x) != s.end());

  unsigned first = x;
  gen(x() != first);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(s.find(x) != s.end());

  unsigned second = x;
  BOOST_REQUIRE_NE(first, second);
  gen(x() != second);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(s.find(x) != s.end());

  unsigned third = x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  gen(x() != third);

  BOOST_REQUIRE(!gen.next());
}

BOOST_AUTO_TEST_CASE(element_inside_vec) {
  std::vector<unsigned> v;
  v.push_back(1);
  v.push_back(7);
  v.push_back(9);

  randv<unsigned> x(0);

  Generator gen;
  gen(inside(x(), v));

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(v.begin(), v.end(), x) != v.end());

  unsigned first = x;
  gen(x() != first);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(v.begin(), v.end(), x) != v.end());

  unsigned second = x;
  BOOST_REQUIRE_NE(first, second);
  gen(x() != second);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(v.begin(), v.end(), x) != v.end());

  unsigned third = x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  gen(x() != third);

  BOOST_REQUIRE(!gen.next());
}

BOOST_AUTO_TEST_CASE(element_inside_array) {
  unsigned a[3];
  a[0] = 1;
  a[1] = 7;
  a[2] = 9;

  randv<unsigned> x(0);
  // unsigned y = a.begin();
  Generator gen;
  gen(inside(x(), a));

  BOOST_REQUIRE(gen.next());
  // BOOST_REQUIRE( find(a.begin(), a.end(), x) != a.end());

  unsigned first = x;
  gen(x() != first);

  BOOST_REQUIRE(gen.next());
  // BOOST_REQUIRE( find(a.begin(), a.end(), x) != a.end());

  unsigned second = x;
  BOOST_REQUIRE_NE(first, second);
  gen(x() != second);

  BOOST_REQUIRE(gen.next());
  // BOOST_REQUIRE( find(a.begin(), a.end(), x) != a.end());
  unsigned third = x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  gen(x() != third);

  BOOST_REQUIRE(!gen.next());
}

BOOST_AUTO_TEST_CASE(element_inside_list) {
  std::list<unsigned> l;
  l.push_back(1);
  l.push_back(7);
  l.push_back(9);

  randv<unsigned> x(0);

  Generator gen;
  gen(inside(x(), l));

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(l.begin(), l.end(), x) != l.end());

  unsigned first = x;
  gen(x() != first);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(l.begin(), l.end(), x) != l.end());

  unsigned second = x;
  BOOST_REQUIRE_NE(first, second);
  gen(x() != second);

  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE(find(l.begin(), l.end(), x) != l.end());

  unsigned third = x;
  BOOST_REQUIRE_NE(third, second);
  BOOST_REQUIRE_NE(first, third);
  gen(x() != third);

  BOOST_REQUIRE(!gen.next());
}

BOOST_AUTO_TEST_CASE(element_not_inside) {
  {
    std::set<unsigned> s;

    randv<unsigned> x(0);

    Generator gen;
    gen(inside(x(), s));

    BOOST_REQUIRE(!gen.next());
  }
  {
    std::vector<unsigned> v;

    randv<unsigned> x(0);

    Generator gen;
    gen(inside(x(), v));

    BOOST_REQUIRE(!gen.next());
  }
  {
    std::vector<unsigned> l;

    randv<unsigned> x(0);

    Generator gen;
    gen(inside(x(), l));

    BOOST_REQUIRE(!gen.next());
  }
}

BOOST_AUTO_TEST_CASE(if_then_else_t1) {
  unsigned int a;
  randv<unsigned int> b(0);
  Generator gen;

  gen(if_then_else(reference(a)<5, b()> 0 && b() <= 50, b() > 50 && b() <= 100));

  for (a = 0; a < 10; ++a) {
    BOOST_REQUIRE(gen.next());
    std::cout << "a =" << a << ", b = " << b << std::endl;
    if (a < 5) {
      BOOST_REQUIRE_GT(b, 0);
      BOOST_REQUIRE_LE(b, 50);
    } else {
      BOOST_REQUIRE_GT(b, 50);
      BOOST_REQUIRE_LE(b, 100);
    }
  }
}

BOOST_AUTO_TEST_CASE(if_then_t1) {
  unsigned int a;
  randv<unsigned int> b(0);
  Generator gen;

  gen(if_then(reference(a)<5, b()> 0 && b() <= 100));
  gen(if_then(reference(a) >= 5, b() > 100 && b() <= 1000));

  for (a = 0; a < 10; ++a) {
    BOOST_REQUIRE(gen.next());
    std::cout << "a =" << a << ", b = " << b << std::endl;
    if (a < 5) {
      BOOST_REQUIRE_GT(b, 0);
      BOOST_REQUIRE_LE(b, 100);
    } else {
      BOOST_REQUIRE_GT(b, 100);
      BOOST_REQUIRE_LE(b, 1000);
    }
  }
}

BOOST_AUTO_TEST_CASE(bitslice_t) {
  randv<short> x(0);

  Generator gen;
  gen(bitslice(10, 3, x()) == 0xFF);
  BOOST_REQUIRE(gen.next());
  BOOST_REQUIRE_EQUAL((x >> 3) & 0xFF, 0xFF);

  BOOST_CHECK_THROW(gen(bitslice(3, 10, x()) == 0xFF), std::runtime_error);
  BOOST_CHECK_THROW(gen(bitslice(16, 3, x()) == 0xFF), std::runtime_error);
}

BOOST_AUTO_TEST_SUITE_END()  // Context


// Copyright 2019 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <planning_msgs/Path.h>
#include <planning_msgs/WaypointList.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <trajectory_math/curve_algorithms.h>
#include <trajectory_math/point_algorithms.h>

#include <Eigen/Core>

constexpr char kInputTopic[] = "/waypoints";
constexpr char kOutputTopic[] = "/desired_path";
constexpr double kDesiredSpacing = 0.5;

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

  ros::NodeHandle node_handle;
  ros::Publisher publisher =
      node_handle.advertise<planning_msgs::Path>(kOutputTopic, 10);

  boost::function<void(const planning_msgs::WaypointList&)>
      subscriber_callback =
          [&publisher](
              const planning_msgs::WaypointList& waypoint_msg) -> void {
    planning_msgs::Path plan_msg;
    plan_msg.header.stamp = waypoint_msg.header.stamp;
    plan_msg.header.frame_id = waypoint_msg.header.frame_id;
    for (int i = 0; i < waypoint_msg.waypoints.size(); ++i) {
      planning_msgs::PathPoint point_msg;
      const auto& waypoint = waypoint_msg.waypoints[i];
      point_msg.x = waypoint.x;
      point_msg.y = waypoint.y;

      // Get s value based on distance from previous point
      point_msg.distance_along_path = 0.;
      if (!plan_msg.points.empty()) {
        const planning_msgs::PathPoint& prev_point_msg = plan_msg.points.back();
        const double dx = point_msg.x - prev_point_msg.x;
        const double dy = point_msg.y - prev_point_msg.y;
        const double distance = std::sqrt(dx * dx + dy * dy);

        // Linearly interpolate points at desired spacing around corners
        if (distance >= 2 * kDesiredSpacing) {
          planning_msgs::PathPoint interpolation_point_after_msg;
          const double interpolation_fraction = kDesiredSpacing / distance;
          interpolation_point_after_msg.distance_along_path =
              prev_point_msg.distance_along_path + kDesiredSpacing;
          interpolation_point_after_msg.x = bookbot::LinearInterpolation(
              prev_point_msg.x, point_msg.x, interpolation_fraction);
          interpolation_point_after_msg.y = bookbot::LinearInterpolation(
              prev_point_msg.y, point_msg.y, interpolation_fraction);
          plan_msg.points.push_back(interpolation_point_after_msg);

          planning_msgs::PathPoint interpolation_point_before_msg;
          interpolation_point_before_msg.distance_along_path =
              prev_point_msg.distance_along_path + distance - kDesiredSpacing;
          interpolation_point_before_msg.x = bookbot::LinearInterpolation(
              prev_point_msg.x, point_msg.x, 1 - interpolation_fraction);
          interpolation_point_before_msg.y = bookbot::LinearInterpolation(
              prev_point_msg.y, point_msg.y, 1 - interpolation_fraction);
          plan_msg.points.push_back(interpolation_point_before_msg);
        }

        point_msg.distance_along_path =
            prev_point_msg.distance_along_path + distance;
      }
      plan_msg.points.push_back(point_msg);
    }

    for (int i = 0; i < plan_msg.points.size(); ++i) {
      auto& current_point = plan_msg.points[i];

      // get yaw value based on sequential points
      current_point.yaw = 0.;
      if (i + 1 < plan_msg.points.size()) {
        const auto& next_point = plan_msg.points[i + 1];
        current_point.yaw = std::atan2(next_point.y - current_point.y,
                                       next_point.x - current_point.x);
      } else if (i > 0) {
        const auto& previous_point = plan_msg.points[i - 1];
        current_point.yaw = std::atan2(current_point.y - previous_point.y,
                                       current_point.x - previous_point.x);
      }

      // get kappa value based on three point curvature approximation
      current_point.curvature = 0.;
      if (i > 0 && i + 1 < plan_msg.points.size()) {
        const auto& previous_point = plan_msg.points[i - 1];
        const auto& next_point = plan_msg.points[i + 1];
        current_point.curvature = bookbot::ApproximateCurvatureMagnitude(
            {previous_point.x, previous_point.y},
            {current_point.x, current_point.y}, {next_point.x, next_point.y});
      }
    }
    publisher.publish(plan_msg);
  };

  ros::Subscriber subscriber =
      node_handle.subscribe<planning_msgs::WaypointList>(kInputTopic, 10,
                                                         subscriber_callback);

  ros::spin();
}


#include "dumpGraph.h"

#include <ros/time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include <boost/filesystem.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/BetweenFactor.h>

/**
 * Function to save the graph, point clouds and gps data
 **/
void dump(const std::string& dump_directory,
  const gtsam::ISAM2& isam,
  const gtsam::Values& isam_current_estimate,
  const std::vector<double>& keyframe_stamps,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& corner_cloud_keyframes,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& surf_cloud_keyframes,
  const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& outlier_cloud_keyframes,
  const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloudKeyPoses3D,
  const pcl::PointCloud<PointTypePose>::Ptr& cloudKeyPoses6D,
  const std::vector<sensor_msgs::NavSatFix>& gps_data
) {
  boost::filesystem::create_directories(dump_directory);

  // Save the graph
  std::vector<gtsam::Pose3> keyframe_poses(isam_current_estimate.size());

  std::ofstream graph_ofs(dump_directory + "/graph.g2o");
  for(const auto& vertex : isam_current_estimate) {
    Eigen::Matrix4d pose = vertex.value.cast<gtsam::Pose3>().matrix();
    keyframe_poses[vertex.key] = vertex.value.cast<gtsam::Pose3>();
    Eigen::Vector3d t = pose.block<3, 1>(0, 3) + Eigen::Vector3d::Random();
    Eigen::Quaterniond q(pose.block<3, 3>(0, 0));
    graph_ofs << "VERTEX_SE3:QUAT " << vertex.key << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n";
  }
  graph_ofs << "FIX 0" << "\n";

  std::cout << "factors:" << isam.getFactorsUnsafe().size() << std::endl;
  for(const auto& factor_: isam.getFactorsUnsafe()) {
    auto between_factor = boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor_);
    if(between_factor) {
      Eigen::Matrix4d relative = between_factor->measured().matrix();
      Eigen::Vector3d t = relative.block<3, 1>(0, 3);
      Eigen::Quaterniond q(relative.block<3, 3>(0, 0));

      graph_ofs << "EDGE_SE3:QUAT " << between_factor->key1() << " " << between_factor->key2();
      graph_ofs << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();

      Eigen::VectorXd vars = 1.0 / between_factor->noiseModel()->sigmas().array();
      Eigen::MatrixXd inf = vars.asDiagonal();

      for(int i = 0; i < inf.rows(); i++) {
        for(int j=i; j<inf.cols(); j++) {
          graph_ofs << " " << inf(i, j);
        }
      }
      graph_ofs << "\n";
    }
  }

  // Save the Point Clouds
  for(int i = 0; i < corner_cloud_keyframes.size(); i++) {
    std::string keyframe_directory = (boost::format("%s/%06d") % dump_directory % i).str();
    boost::filesystem::create_directories(keyframe_directory);

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_corner(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_surf(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_outlier(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    *cloud_corner += *corner_cloud_keyframes[i];
    *cloud_surf += *surf_cloud_keyframes[i];
    *cloud_outlier += *outlier_cloud_keyframes[i];
    *cloud += *corner_cloud_keyframes[i];
    *cloud += *surf_cloud_keyframes[i];
    *cloud += *outlier_cloud_keyframes[i];

    Eigen::Isometry3f camera2lidar = Eigen::AngleAxisf(M_PI / 2.0f, Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(M_PI / 2.0, Eigen::Vector3f::UnitY()) * Eigen::Isometry3f::Identity();

    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed(new pcl::PointCloud<pcl::PointXYZI>());

    pcl::transformPointCloud(*cloud, *transformed, camera2lidar);
    pcl::io::savePCDFileBinary(keyframe_directory + "/cloud.pcd", *transformed);

    // pcl::transformPointCloud(*cloud_corner, *transformed, camera2lidar);
    // cloud = transformPointCloud(cloud, keyframe_poses[i]);
    pcl::io::savePCDFileBinary(keyframe_directory + "/cloud_corner.pcd", *cloud_corner);

    // pcl::transformPointCloud(*cloud_surf, *transformed, camera2lidar);
    pcl::io::savePCDFileBinary(keyframe_directory + "/cloud_surf.pcd", *cloud_surf);

    // pcl::transformPointCloud(*cloud_outlier, *transformed, camera2lidar);
    pcl::io::savePCDFileBinary(keyframe_directory + "/cloud_outlier.pcd", *cloud_outlier);

    ros::Time stamp(keyframe_stamps[i]);

    std::ofstream data_ofs(keyframe_directory + "/data");
    data_ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n";
    data_ofs << "estimate\n" << keyframe_poses[i].matrix() << "\n";
    data_ofs << "odom\n" << keyframe_poses[i].matrix() << "\n";
    data_ofs << "accum_distance -1" << "\n";
    data_ofs << "id " << i << "\n";
  }

  pcl::io::savePCDFileBinary(dump_directory + "/cloudKeyPoses3D.pcd", *cloudKeyPoses3D);
  pcl::io::savePCDFileBinary(dump_directory + "/cloudKeyPoses6D.pcd", *cloudKeyPoses6D);

  // Save GPS Data
  std::ofstream gps_data_ofs(dump_directory + "/gps_data.txt");

  for(int i=0; i<gps_data.size(); i++){
    gps_data_ofs << gps_data[i].latitude << " " << gps_data[i].longitude << " " << gps_data[i].altitude << " ";
    gps_data_ofs << gps_data[i].position_covariance[0] << " " << gps_data[i].position_covariance[1] << " " << gps_data[i].position_covariance[2] << " " << gps_data[i].position_covariance[3] << " " << gps_data[i].position_covariance[4] << " " << gps_data[i].position_covariance[5] << " " << gps_data[i].position_covariance[6] << " " << gps_data[i].position_covariance[7] << " " << gps_data[i].position_covariance[8] << " ";
    gps_data_ofs << "\n";
  }
}


#include "util/coordinate_calculation.hpp"
#include "util/coordinate.hpp"
#include "util/trigonometry_table.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <mapbox/cheap_ruler.hpp>

#include <algorithm>
#include <iterator>
#include <limits>
#include <utility>

namespace osrm
{
namespace util
{

namespace coordinate_calculation
{

namespace
{

// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr double EARTH_RADIUS = 6372797.560856;

class CheapRulerContainer
{
  public:
    CheapRulerContainer(const int number_of_rulers)
        : cheap_ruler_cache(number_of_rulers, mapbox::cheap_ruler::CheapRuler(0)),
          step(90.0 * COORDINATE_PRECISION / number_of_rulers)
    {
        for (int n = 0; n < number_of_rulers; n++)
        {
            cheap_ruler_cache[n] = mapbox::cheap_ruler::CheapRuler(
                step * (n + 0.5) / COORDINATE_PRECISION, mapbox::cheap_ruler::CheapRuler::Meters);
        }
    };

    mapbox::cheap_ruler::CheapRuler &getRuler(const FixedLatitude lat_1, const FixedLatitude lat_2)
    {
        auto lat = (lat_1 + lat_2) / util::FixedLatitude{2};
        return getRuler(lat);
    }

    mapbox::cheap_ruler::CheapRuler &getRuler(const FixedLatitude lat)
    {
        BOOST_ASSERT(step > 2);
        // the |lat| > 0  ->  |lat|-1 > -1  ->  (|lat|-1)/step > -1/step > -1/2 >= -1  ->  bin >= 0
        std::size_t bin = (std::abs(static_cast<int>(lat)) - 1) / step;
        BOOST_ASSERT(bin < cheap_ruler_cache.size());
        return cheap_ruler_cache[bin];
    };

  private:
    std::vector<mapbox::cheap_ruler::CheapRuler> cheap_ruler_cache;
    const int step;
};
static CheapRulerContainer cheap_ruler_container(1800);
} // namespace

// Does not project the coordinates!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs)
{
    std::int64_t d_lon = static_cast<std::int32_t>(lhs.lon - rhs.lon);
    std::int64_t d_lat = static_cast<std::int32_t>(lhs.lat - rhs.lat);

    std::int64_t sq_lon = d_lon * d_lon;
    std::int64_t sq_lat = d_lat * d_lat;

    std::uint64_t result = static_cast<std::uint64_t>(sq_lon + sq_lat);

    return result;
}

// Uses method described here:
// https://www.gpo.gov/fdsys/pkg/CFR-2005-title47-vol4/pdf/CFR-2005-title47-vol4-sec73-208.pdf
// should be within 0.1% or so of Vincenty method (assuming 19 buckets are enough)
// Should be more faster and more precise than Haversine
double fccApproximateDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    const auto lon1 = static_cast<double>(util::toFloating(coordinate_1.lon));
    const auto lat1 = static_cast<double>(util::toFloating(coordinate_1.lat));
    const auto lon2 = static_cast<double>(util::toFloating(coordinate_2.lon));
    const auto lat2 = static_cast<double>(util::toFloating(coordinate_2.lat));
    return cheap_ruler_container.getRuler(coordinate_1.lat, coordinate_2.lat)
        .distance({lon1, lat1}, {lon2, lat2});
}

double haversineDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;

    const double dlat1 = lt1 * detail::DEGREE_TO_RAD;
    const double dlong1 = ln1 * detail::DEGREE_TO_RAD;
    const double dlat2 = lt2 * detail::DEGREE_TO_RAD;
    const double dlong2 = ln2 * detail::DEGREE_TO_RAD;

    const double dlong = dlong1 - dlong2;
    const double dlat = dlat1 - dlat2;

    const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
    return EARTH_RADIUS * charv;
}

double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const double float_lat1 = (lat1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon1 = (lon1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lat2 = (lat2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon2 = (lon2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;

    const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
    const double y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * EARTH_RADIUS;
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    FloatCoordinate projected_nearest;
    std::tie(ratio, projected_nearest) =
        projectPointOnSegment(web_mercator::fromWGS84(segment_source),
                              web_mercator::fromWGS84(segment_target),
                              web_mercator::fromWGS84(query_location));
    nearest_location = web_mercator::toWGS84(projected_nearest);

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double perpendicularDistance(const Coordinate source_coordinate,
                             const Coordinate target_coordinate,
                             const Coordinate query_location)
{
    double ratio;
    Coordinate nearest_location;

    return perpendicularDistance(
        source_coordinate, target_coordinate, query_location, nearest_location, ratio);
}

Coordinate centroid(const Coordinate lhs, const Coordinate rhs)
{
    Coordinate centroid;
    // The coordinates of the midpoints are given by:
    // x = (x1 + x2) /2 and y = (y1 + y2) /2.
    centroid.lon = (lhs.lon + rhs.lon) / FixedLongitude{2};
    centroid.lat = (lhs.lat + rhs.lat) / FixedLatitude{2};
    return centroid;
}

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate)
{
    const double lon_diff =
        static_cast<double>(toFloating(second_coordinate.lon - first_coordinate.lon));
    const double lon_delta = detail::degToRad(lon_diff);
    const double lat1 = detail::degToRad(static_cast<double>(toFloating(first_coordinate.lat)));
    const double lat2 = detail::degToRad(static_cast<double>(toFloating(second_coordinate.lat)));
    const double y = std::sin(lon_delta) * std::cos(lat2);
    const double x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    double result = detail::radToDeg(std::atan2(y, x));
    while (result < 0.0)
    {
        result += 360.0;
    }

    while (result >= 360.0)
    {
        result -= 360.0;
    }
    // If someone gives us two identical coordinates, then the concept of a bearing
    // makes no sense.  However, because it sometimes happens, we'll at least
    // return a consistent value of 0 so that the behaviour isn't random.
    BOOST_ASSERT(first_coordinate != second_coordinate || result == 0.);

    return result;
}

double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third)
{
    using namespace boost::math::constants;
    using namespace coordinate_calculation;

    if (first == second || second == third)
        return 180;

    BOOST_ASSERT(first.IsValid());
    BOOST_ASSERT(second.IsValid());
    BOOST_ASSERT(third.IsValid());

    const double v1x = static_cast<double>(toFloating(first.lon - second.lon));
    const double v1y =
        web_mercator::latToY(toFloating(first.lat)) - web_mercator::latToY(toFloating(second.lat));
    const double v2x = static_cast<double>(toFloating(third.lon - second.lon));
    const double v2y =
        web_mercator::latToY(toFloating(third.lat)) - web_mercator::latToY(toFloating(second.lat));

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. / pi<double>();

    while (angle < 0.)
    {
        angle += 360.;
    }

    BOOST_ASSERT(angle >= 0);
    return angle;
}

boost::optional<Coordinate>
circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // free after http://paulbourke.net/geometry/circlesphere/
    // require three distinct points
    if (C1 == C2 || C2 == C3 || C1 == C3)
    {
        return boost::none;
    }

    // define line through c1, c2 and c2,c3
    const double C2C1_lat = static_cast<double>(toFloating(C2.lat - C1.lat)); // yDelta_a
    const double C2C1_lon = static_cast<double>(toFloating(C2.lon - C1.lon)); // xDelta_a
    const double C3C2_lat = static_cast<double>(toFloating(C3.lat - C2.lat)); // yDelta_b
    const double C3C2_lon = static_cast<double>(toFloating(C3.lon - C2.lon)); // xDelta_b

    // check for collinear points in X-Direction / Y-Direction
    if ((std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon()) ||
        (std::abs(C2C1_lat) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lat) < std::numeric_limits<double>::epsilon()))
    {
        return boost::none;
    }
    else if (std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C2C1
        // due to c1.lon == c2.lon && c1.lon != c3.lon we can rearrange this way
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C2.lon - C3.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C1, C3, C2);
    }
    else if (std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C3C2
        // due to c2.lon == c3.lon && c1.lon != c3.lon we can rearrange this way
        // after rearrangement both deltas will be zero
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C1.lon - C2.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C2, C1, C3);
    }
    else
    {
        const double C2C1_slope = C2C1_lat / C2C1_lon;
        const double C3C2_slope = C3C2_lat / C3C2_lon;

        if (std::abs(C2C1_slope) < std::numeric_limits<double>::epsilon())
        {
            // Three non-collinear points with C2,C1 on same latitude.
            // Due to the x-values correct, we can swap C3 and C1 to obtain the correct slope value
            return circleCenter(C3, C2, C1);
        }
        // valid slope values for both lines, calculate the center as intersection of the lines

        // can this ever happen?
        if (std::abs(C2C1_slope - C3C2_slope) < std::numeric_limits<double>::epsilon())
            return boost::none;

        const double C1_y = static_cast<double>(toFloating(C1.lat));
        const double C1_x = static_cast<double>(toFloating(C1.lon));
        const double C2_y = static_cast<double>(toFloating(C2.lat));
        const double C2_x = static_cast<double>(toFloating(C2.lon));
        const double C3_y = static_cast<double>(toFloating(C3.lat));
        const double C3_x = static_cast<double>(toFloating(C3.lon));

        const double lon = (C2C1_slope * C3C2_slope * (C1_y - C3_y) + C3C2_slope * (C1_x + C2_x) -
                            C2C1_slope * (C2_x + C3_x)) /
                           (2 * (C3C2_slope - C2C1_slope));
        const double lat = (0.5 * (C1_x + C2_x) - lon) / C2C1_slope + 0.5 * (C1_y + C2_y);
        if (lon < -180.0 || lon > 180.0 || lat < -90.0 || lat > 90.0)
            return boost::none;
        else
            return Coordinate(FloatLongitude{lon}, FloatLatitude{lat});
    }
}

double circleRadius(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // a circle by three points requires thee distinct points
    auto center = circleCenter(C1, C2, C3);
    if (center)
        return haversineDistance(C1, *center);
    else
        return std::numeric_limits<double>::infinity();
}

Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to)
{
    BOOST_ASSERT(0 <= factor && factor <= 1.0);

    const auto from_lon = static_cast<std::int32_t>(from.lon);
    const auto from_lat = static_cast<std::int32_t>(from.lat);
    const auto to_lon = static_cast<std::int32_t>(to.lon);
    const auto to_lat = static_cast<std::int32_t>(to.lat);

    FixedLongitude interpolated_lon{
        static_cast<std::int32_t>(from_lon + factor * (to_lon - from_lon))};
    FixedLatitude interpolated_lat{
        static_cast<std::int32_t>(from_lat + factor * (to_lat - from_lat))};

    return {std::move(interpolated_lon), std::move(interpolated_lat)};
}

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate)
{
    const auto lat_1 = static_cast<double>(toFloating(first_coordinate.lat));
    const auto lon_1 = static_cast<double>(toFloating(first_coordinate.lon));
    const auto lat_2 = static_cast<double>(toFloating(second_coordinate.lat));
    const auto lon_2 = static_cast<double>(toFloating(second_coordinate.lon));
    const auto lat_3 = static_cast<double>(toFloating(third_coordinate.lat));
    const auto lon_3 = static_cast<double>(toFloating(third_coordinate.lon));
    return 0.5 * (-lon_2 * lat_1 + lon_3 * lat_1 + lon_1 * lat_2 - lon_3 * lat_2 - lon_1 * lat_3 +
                  lon_2 * lat_3);
}

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate)
{
    return signedArea(first_coordinate, second_coordinate, third_coordinate) > 0;
}

// find the closest distance between a coordinate and a segment
double findClosestDistance(const Coordinate coordinate,
                           const Coordinate segment_begin,
                           const Coordinate segment_end)
{
    return haversineDistance(coordinate,
                             projectPointOnSegment(segment_begin, segment_end, coordinate).second);
}

// find the closes distance between two sets of coordinates
double findClosestDistance(const std::vector<Coordinate> &lhs, const std::vector<Coordinate> &rhs)
{
    double current_min = std::numeric_limits<double>::max();

    const auto compute_minimum_distance_in_rhs = [&current_min, &rhs](const Coordinate coordinate) {
        current_min =
            std::min(current_min, findClosestDistance(coordinate, rhs.begin(), rhs.end()));
        return false;
    };

    std::find_if(std::begin(lhs), std::end(lhs), compute_minimum_distance_in_rhs);
    return current_min;
}

std::vector<double> getDeviations(const std::vector<Coordinate> &from,
                                  const std::vector<Coordinate> &to)
{
    auto find_deviation = [&to](const Coordinate coordinate) {
        return findClosestDistance(coordinate, to.begin(), to.end());
    };

    std::vector<double> deviations_from;
    deviations_from.reserve(from.size());
    std::transform(
        std::begin(from), std::end(from), std::back_inserter(deviations_from), find_deviation);

    return deviations_from;
}

Coordinate rotateCCWAroundZero(Coordinate coordinate, double angle_in_radians)
{
    /*
     * a rotation  around 0,0 in vector space is defined as
     *
     * | cos a   -sin a | . | lon |
     * | sin a    cos a |   | lat |
     *
     * resulting in cos a lon - sin a lon for the new longitude and sin a lon + cos a lat for the
     * new latitude
     */

    const auto cos_alpha = cos(angle_in_radians);
    const auto sin_alpha = sin(angle_in_radians);

    const auto lon = static_cast<double>(toFloating(coordinate.lon));
    const auto lat = static_cast<double>(toFloating(coordinate.lat));

    return {util::FloatLongitude{cos_alpha * lon - sin_alpha * lat},
            util::FloatLatitude{sin_alpha * lon + cos_alpha * lat}};
}

Coordinate difference(const Coordinate lhs, const Coordinate rhs)
{
    const auto lon_diff_int = static_cast<int>(lhs.lon) - static_cast<int>(rhs.lon);
    const auto lat_diff_int = static_cast<int>(lhs.lat) - static_cast<int>(rhs.lat);
    return {util::FixedLongitude{lon_diff_int}, util::FixedLatitude{lat_diff_int}};
}

double computeArea(const std::vector<Coordinate> &polygon)
{
    using util::coordinate_calculation::haversineDistance;

    if (polygon.empty())
        return 0.;

    BOOST_ASSERT(polygon.front() == polygon.back());

    // Take the reference point with the smallest latitude.
    // ⚠ ref_latitude is the standard parallel for the equirectangular projection
    // that is not an area-preserving projection
    const auto ref_point =
        std::min_element(polygon.begin(), polygon.end(), [](const auto &lhs, const auto &rhs) {
            return lhs.lat < rhs.lat;
        });
    const auto ref_latitude = ref_point->lat;

    // Compute area of under a curve and a line that is parallel the equator with ref_latitude
    // For closed curves it corresponds to the shoelace algorithm for polygon areas
    double area = 0.;
    auto first = polygon.begin();
    auto previous_base = util::Coordinate{first->lon, ref_latitude};
    auto previous_y = haversineDistance(previous_base, *first);
    for (++first; first != polygon.end(); ++first)
    {
        BOOST_ASSERT(first->lat >= ref_latitude);

        const auto current_base = util::Coordinate{first->lon, ref_latitude};
        const auto current_y = haversineDistance(current_base, *first);
        const auto chunk_area =
            haversineDistance(previous_base, current_base) * (previous_y + current_y);

        area += (current_base.lon >= previous_base.lon) ? chunk_area : -chunk_area;

        previous_base = current_base;
        previous_y = current_y;
    }

    return area / 2.;
}

} // namespace coordinate_calculation
} // namespace util
} // namespace osrm


#include <iostream>
#include <armadillo>
#include <mkl.h>

#include "class_data.hpp"
#include "class_approximation.hpp"
#include "class_partition.hpp"
#include "evaluate_covariance.hpp"
#include "constants.hpp"
#include <string.h>

void Approximation::loop_regions_at_finest_level_in_prior(double ***KCholTimesw)
{
    int currentLevel = NUM_LEVELS_M - 1;
    double loglikelihoodThisLevel = 0;

    unsigned long maxNumKnotsAtFinestLevel = 0;
    //Find the maximum number of knots at the finest level
    for(unsigned long iRegion = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < partition->nRegionsInTotal; iRegion++)
        maxNumKnotsAtFinestLevel =  std::max(maxNumKnotsAtFinestLevel, partition->nKnotsAtFinestLevel[iRegion-partition->nRegionsInTotal+partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]]);

    /////// Define and allocate variables

    //Allocate memory for ATilde
    if(!SAVE_TO_DISK_FLAG)
        for(unsigned long iRegion = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < partition->nRegionsInTotal; iRegion++)
            ATilde[iRegion].set_size(partition->nKnots, partition->nKnots, (NUM_LEVELS_M-1)*NUM_LEVELS_M/2);

    //Allocate memory for wTilde
    for(unsigned long iRegion = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < partition->nRegionsInTotal; iRegion++)
        wTilde[iRegion].set_size(partition->nKnots,NUM_LEVELS_M-1);

    //Define indices of all the ancestors for the current region
    unsigned long *ancestorArray = new unsigned long [currentLevel];

    //Auxilliary variables for matrix computations
    int one=1;
    char L='L', N='N';
    int nKnots = partition->nKnots;
    int status;

    //Loop the regions
    for(unsigned long iRegion = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < partition->nRegionsInTotal; iRegion++)
    {
        mat **SicB;
        vec *Sicy;
        double *RCholThisLevel;

        unsigned long indexRegionAtThisLevel = iRegion-partition->nRegionsInTotal+partition->nRegionsAtEachLevel[NUM_LEVELS_M-1];

        //Assign the number of knots in the current region accordingly
        int nKnotsInCurrentRegion = partition->nKnotsAtFinestLevel[indexRegionAtThisLevel];

        if(nKnotsInCurrentRegion == 0)
        {
            ATilde[iRegion].zeros();
            wTilde[iRegion].zeros();
            if(CALCULATION_MODE=="prediction") get_all_ancestors(ancestorArray, iRegion, currentLevel);
        }else
        {
            //Find the indices of all the ancestors
            get_all_ancestors(ancestorArray, iRegion, currentLevel);

            //Allocate memory for variables in the current region
            mat **covarianceMatrix = new mat* [currentLevel+1];

            double **covarianceMatrixMemory = new double* [currentLevel+1];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                covarianceMatrixMemory[jLevel] = new double [nKnotsInCurrentRegion*partition->nKnots];
            covarianceMatrixMemory[currentLevel] =  new double [nKnotsInCurrentRegion*nKnotsInCurrentRegion];

            SicB = new mat* [currentLevel];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                SicB[jLevel] = new mat(partition->nKnots,nKnotsInCurrentRegion);

            if(CALCULATION_MODE=="prediction") KCholTimesw[iRegion] = new double* [currentLevel];

            //Define and Allocate RCholThisLevel for the current region
            RCholThisLevel = new double [nKnotsInCurrentRegion*nKnotsInCurrentRegion];

            //Loop for all ancestors to get the cross-covariance matrix
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                //Calculate the covariances between current region and the ancestors
                covarianceMatrix[jLevel] =  new mat(covarianceMatrixMemory[jLevel],nKnotsInCurrentRegion,nKnots,false,true);
                evaluate_cross_covariance(covarianceMatrixMemory[jLevel], partition->knotsX[iRegion], partition->knotsY[iRegion], partition->knotsX[ancestorArray[jLevel]], partition->knotsY[ancestorArray[jLevel]], nKnotsInCurrentRegion, nKnots, sill, range, nugget);
            }

            //Get the current variance-covariance matrix
            covarianceMatrix[currentLevel] =  new mat(covarianceMatrixMemory[currentLevel],nKnotsInCurrentRegion,nKnotsInCurrentRegion,false,true);
            evaluate_variance_covariance(covarianceMatrixMemory[currentLevel], partition->knotsX[iRegion], partition->knotsY[iRegion], nKnotsInCurrentRegion, sill, range, nugget);

            //Loop for all ancestors to get the conditional cross-covariance matrix
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                get_conditional_covariance_matrix(jLevel, covarianceMatrixMemory, KCholTimesw[ancestorArray[jLevel]], covarianceMatrixMemory[jLevel], nKnotsInCurrentRegion,nKnots,nKnots);

                //Copy covarianceMatrix[jLevel] to SicB[jLevel] for later use
                memcpy(SicB[jLevel]->memptr(),covarianceMatrixMemory[jLevel],nKnotsInCurrentRegion*nKnots*sizeof(double));

                //Reassign covarianceMatrix[jLevel] = RChol[ancestorArray[jLevel]]^(-1)*covarianceMatrix[jLevel]
                dtrtrs_(&L,&N,&N,&nKnots,&nKnotsInCurrentRegion,RChol[ancestorArray[jLevel]].memptr(),&nKnots,covarianceMatrixMemory[jLevel],&nKnots,&status);

                if(CALCULATION_MODE=="prediction") KCholTimesw[iRegion][jLevel]=covarianceMatrixMemory[jLevel];
            }
            //Get the conditional variance-covariance matrix of this region
            get_conditional_covariance_matrix(currentLevel, covarianceMatrixMemory, covarianceMatrixMemory, covarianceMatrixMemory[currentLevel], nKnotsInCurrentRegion,nKnotsInCurrentRegion,nKnots);

            //Cholesky factorization, covarianceMatrix[currentLevel] = RCholThisLevel*RCholThisLevel^T
            memcpy(RCholThisLevel,covarianceMatrixMemory[currentLevel],nKnotsInCurrentRegion*nKnotsInCurrentRegion*sizeof(double));
            dpotrf_(&L,&nKnotsInCurrentRegion,RCholThisLevel,&nKnotsInCurrentRegion,&status);

            //Compute Sicy = RCholThisLevel^(-1)*partition->knotsResidual[indexRegionAtThisLevel]
            Sicy = new vec(partition->knotsResidual[indexRegionAtThisLevel],nKnotsInCurrentRegion,true,true);
            dtrtrs_(&L,&N,&N,&nKnotsInCurrentRegion,&one,RCholThisLevel,&nKnotsInCurrentRegion,Sicy->memptr(),&nKnotsInCurrentRegion,&status);

            //Compute SicB[jLevel] = RCholThisLevel^(-1)*covarianceMatrix[jLevel]
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                inplace_trans(*SicB[jLevel]);
                dtrtrs_(&L,&N,&N,&nKnotsInCurrentRegion,&nKnots,RCholThisLevel,&nKnotsInCurrentRegion,SicB[jLevel]->memptr(),&nKnotsInCurrentRegion,&status);
            }

            get_wTilde_and_ATilde_in_prior(nKnots, currentLevel, iRegion, Sicy, SicB);

            if(CALCULATION_MODE!="prediction")
            {
                double loglikelihoodToAdd = 0; int pos = 0;
                for(int i = 0; i < nKnotsInCurrentRegion; i++)
                {
                    loglikelihoodToAdd += (*Sicy)[i]*(*Sicy)[i] + 2*log(RCholThisLevel[pos]);
                    pos += nKnotsInCurrentRegion + 1;
                }

                loglikelihoodThisLevel += loglikelihoodToAdd;

                for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                {
                    delete SicB[jLevel];
                    delete[] covarianceMatrixMemory[jLevel];
                    delete covarianceMatrix[jLevel];
                }

                delete[] RCholThisLevel;
            }

            delete[] covarianceMatrixMemory[currentLevel];
            delete covarianceMatrix[currentLevel];
        }

        if(CALCULATION_MODE=="prediction")
        {
            int nPredictionsInCurrentRegion = partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel];
            if(nPredictionsInCurrentRegion == 0) continue;

            //Allocate memory for variables in the current region
            mat **covarianceMatrix = new mat* [currentLevel+1];

            double **covarianceMatrixMemory = new double* [currentLevel];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                covarianceMatrixMemory[jLevel] = new double [nPredictionsInCurrentRegion*nKnots];

            double **posteriorPredictionKCholTimesw = new double* [currentLevel+1];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                posteriorPredictionKCholTimesw[jLevel]= new double [nPredictionsInCurrentRegion*nKnots];

            if(nKnotsInCurrentRegion > 0)
                posteriorPredictionKCholTimesw[currentLevel]= new double [nPredictionsInCurrentRegion*nKnotsInCurrentRegion];

            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                //Calculate the covariances between prediction locations in the current region and knots in the ancestor regions
                covarianceMatrix[jLevel] =  new mat(covarianceMatrixMemory[jLevel],nKnots,nPredictionsInCurrentRegion,false,true);

                evaluate_cross_covariance(covarianceMatrixMemory[jLevel],  partition->predictionX[indexRegionAtThisLevel], partition->predictionY[indexRegionAtThisLevel], partition->knotsX[ancestorArray[jLevel]], partition->knotsY[ancestorArray[jLevel]], nPredictionsInCurrentRegion, nKnots, sill, range, nugget);

                get_conditional_covariance_matrix(jLevel, posteriorPredictionKCholTimesw, KCholTimesw[ancestorArray[jLevel]], covarianceMatrixMemory[jLevel],nPredictionsInCurrentRegion,nKnots,nKnots);

                //posteriorPredictionKCholTimesw[jLevel] = RChol[ancestorArray[jLevel]]^(-1)*covarianceMatrix[jLevel]
                memcpy(posteriorPredictionKCholTimesw[jLevel],covarianceMatrixMemory[jLevel],nKnots*nPredictionsInCurrentRegion*sizeof(double));
                dtrtrs_(&L,&N,&N,&nKnots,&nPredictionsInCurrentRegion,RChol[ancestorArray[jLevel]].memptr(),&nKnots,posteriorPredictionKCholTimesw[jLevel],&nKnots,&status);

            }

            if(nKnotsInCurrentRegion > 0)
            {
                //Calculate the covariances between prediction locations and the knots in the current region
                evaluate_cross_covariance(posteriorPredictionKCholTimesw[currentLevel],  partition->predictionX[indexRegionAtThisLevel], partition->predictionY[indexRegionAtThisLevel], partition->knotsX[iRegion], partition->knotsY[iRegion], nPredictionsInCurrentRegion, nKnotsInCurrentRegion, sill, range, nugget);

                get_conditional_covariance_matrix(currentLevel, posteriorPredictionKCholTimesw, KCholTimesw[iRegion], posteriorPredictionKCholTimesw[currentLevel],nPredictionsInCurrentRegion,nKnotsInCurrentRegion,nKnots);

                //posteriorPredictionKCholTimesw[currentLevel] := RCholThisLevel^(-1)*posteriorPredictionKCholTimesw[currentLevel]
                dtrtrs_(&L,&N,&N,&nKnotsInCurrentRegion,&nPredictionsInCurrentRegion,RCholThisLevel,&nKnotsInCurrentRegion,posteriorPredictionKCholTimesw[currentLevel],&nKnotsInCurrentRegion,&status);
            }

            //Get the variance-covariance matrix of the prediction locations in the current region
            mat *varianceCovarianceMatrix = new mat(nPredictionsInCurrentRegion,nPredictionsInCurrentRegion);

            evaluate_variance_covariance((*varianceCovarianceMatrix).memptr(),  partition->predictionX[indexRegionAtThisLevel], partition->predictionY[indexRegionAtThisLevel], nPredictionsInCurrentRegion, sill, range, nugget);

            get_conditional_covariance_matrix(currentLevel, posteriorPredictionKCholTimesw, posteriorPredictionKCholTimesw, (*varianceCovarianceMatrix).memptr(), nPredictionsInCurrentRegion, nPredictionsInCurrentRegion, nKnots);

            posteriorPredictionMean[indexRegionAtThisLevel].set_size(nPredictionsInCurrentRegion);

            if(nKnotsInCurrentRegion > 0)
            {
                cblas_dgemm(CblasColMajor,CblasTrans,CblasNoTrans,nPredictionsInCurrentRegion,nPredictionsInCurrentRegion,nKnotsInCurrentRegion,-1.0,posteriorPredictionKCholTimesw[currentLevel],nKnotsInCurrentRegion,posteriorPredictionKCholTimesw[currentLevel],nKnotsInCurrentRegion,1.0,(*varianceCovarianceMatrix).memptr(),nPredictionsInCurrentRegion);

                cblas_dgemv(CblasColMajor, CblasTrans, nKnotsInCurrentRegion, nPredictionsInCurrentRegion, 1.0, posteriorPredictionKCholTimesw[currentLevel], nKnotsInCurrentRegion, Sicy->memptr(), 1, 0.0, posteriorPredictionMean[indexRegionAtThisLevel].memptr(), 1);
            }
            else
                memset(posteriorPredictionMean[indexRegionAtThisLevel].memptr(),0,sizeof(double)*nPredictionsInCurrentRegion);

            posteriorPredictionVariance[indexRegionAtThisLevel] = (*varianceCovarianceMatrix).diag();

            //BTilde[indexRegionAtThisLevel][jLevel] := covarianceMatrix[jLevel]-posteriorPredictionKCholTimesw[currentLevel].t()*SicB[jLevel]
            BTilde[indexRegionAtThisLevel] = covarianceMatrixMemory;

            if(nKnotsInCurrentRegion > 0)
                for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                    cblas_dgemm(CblasColMajor,CblasTrans,CblasNoTrans,nKnots,nPredictionsInCurrentRegion,nKnotsInCurrentRegion,-1.0,SicB[jLevel]->memptr(),nKnotsInCurrentRegion,posteriorPredictionKCholTimesw[currentLevel],nKnotsInCurrentRegion,1.0,BTilde[indexRegionAtThisLevel][jLevel],nKnots);

            delete varianceCovarianceMatrix;
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                delete[] posteriorPredictionKCholTimesw[jLevel];

            if(nKnotsInCurrentRegion > 0)
            {
                delete[] RCholThisLevel;
                delete[] posteriorPredictionKCholTimesw[currentLevel];
            }
        }

        if(nKnotsInCurrentRegion > 0) delete Sicy;
    }

    if(CALCULATION_MODE!="prediction") loglikelihood += loglikelihoodThisLevel;

    //Free memory for ancestorArray
    delete[] ancestorArray;
}


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

 * GTSAM Copyright 2010-2019, 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   ShonanAveraging.cpp
 * @date   March 2019 - August 2020
 * @author Frank Dellaert, David Rosen, and Jing Wu
 * @brief  Shonan Averaging algorithm
 */

#include <SymEigsSolver.h>
#include <cmath>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/sfm/ShonanFactor.h>
#include <gtsam/sfm/ShonanGaugeFactor.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>

#include <Eigen/Eigenvalues>
#include <algorithm>
#include <complex>
#include <iostream>
#include <map>
#include <random>
#include <set>
#include <vector>

namespace gtsam {

// In Wrappers we have no access to this so have a default ready
static std::mt19937 kRandomNumberGenerator(42);

using Sparse = Eigen::SparseMatrix<double>;

/* ************************************************************************* */
template <size_t d>
ShonanAveragingParameters<d>::ShonanAveragingParameters(
    const LevenbergMarquardtParams &_lm, const std::string &method,
    double optimalityThreshold, double alpha, double beta, double gamma)
    : lm(_lm),
      optimalityThreshold(optimalityThreshold),
      alpha(alpha),
      beta(beta),
      gamma(gamma),
      useHuber(false),
      certifyOptimality(true) {
  // By default, we will do conjugate gradient
  lm.linearSolverType = LevenbergMarquardtParams::Iterative;

  // Create subgraph builder parameters
  SubgraphBuilderParameters builderParameters;
  builderParameters.skeletonType = SubgraphBuilderParameters::KRUSKAL;
  builderParameters.skeletonWeight = SubgraphBuilderParameters::EQUAL;
  builderParameters.augmentationWeight = SubgraphBuilderParameters::SKELETON;
  builderParameters.augmentationFactor = 0.0;

  auto pcg = boost::make_shared<PCGSolverParameters>();

  // Choose optimization method
  if (method == "SUBGRAPH") {
    lm.iterativeParams =
        boost::make_shared<SubgraphSolverParameters>(builderParameters);
  } else if (method == "SGPC") {
    pcg->preconditioner_ =
        boost::make_shared<SubgraphPreconditionerParameters>(builderParameters);
    lm.iterativeParams = pcg;
  } else if (method == "JACOBI") {
    pcg->preconditioner_ =
        boost::make_shared<BlockJacobiPreconditionerParameters>();
    lm.iterativeParams = pcg;
  } else if (method == "QR") {
    lm.setLinearSolverType("MULTIFRONTAL_QR");
  } else if (method == "CHOLESKY") {
    lm.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
  } else {
    throw std::invalid_argument("ShonanAveragingParameters: unknown method");
  }
}

/* ************************************************************************* */
// Explicit instantiation for d=2 and d=3
template struct ShonanAveragingParameters<2>;
template struct ShonanAveragingParameters<3>;

/* ************************************************************************* */
// Calculate number of unknown rotations referenced by measurements
// Throws exception of keys are not numbered as expected (may change in future).
template <size_t d>
static size_t NrUnknowns(
    const typename ShonanAveraging<d>::Measurements &measurements) {
  Key maxKey = 0;
  std::set<Key> keys;
  for (const auto &measurement : measurements) {
    for (const Key &key : measurement.keys()) {
      maxKey = std::max(key, maxKey);
      keys.insert(key);
    }
  }
  size_t N = keys.size();
  if (maxKey != N - 1) {
    throw std::invalid_argument(
        "As of now, ShonanAveraging expects keys numbered 0..N-1");
  }
  return N;
}

/* ************************************************************************* */
template <size_t d>
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
                                    const Parameters &parameters)
    : parameters_(parameters),
      measurements_(measurements),
      nrUnknowns_(NrUnknowns<d>(measurements)) {
  for (const auto &measurement : measurements_) {
    const auto &model = measurement.noiseModel();
    if (model && model->dim() != SO<d>::dimension) {
      measurement.print("Factor with incorrect noise model:\n");
      throw std::invalid_argument(
          "ShonanAveraging: measurements passed to "
          "constructor have incorrect dimension.");
    }
  }
  Q_ = buildQ();
  D_ = buildD();
  L_ = D_ - Q_;
}

/* ************************************************************************* */
template <size_t d>
NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
  NonlinearFactorGraph graph;
  auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));

  for (const auto &measurement : measurements_) {
    const auto &keys = measurement.keys();
    const auto &Rij = measurement.measured();
    const auto &model = measurement.noiseModel();
    graph.emplace_shared<ShonanFactor<d>>(keys[0], keys[1], Rij, p, model, G);
  }
  // Possibly add Karcher prior
  if (parameters_.beta > 0) {
    const size_t dim = SOn::Dimension(p);
    graph.emplace_shared<KarcherMeanFactor<SOn>>(graph.keys(), dim);
  }
  // Possibly add gauge factors - they are probably useless as gradient is zero
  if (parameters_.gamma > 0 && p > d + 1) {
    for (auto key : graph.keys())
      graph.emplace_shared<ShonanGaugeFactor>(key, p, d, parameters_.gamma);
  }
  return graph;
}

/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::costAt(size_t p, const Values &values) const {
  const NonlinearFactorGraph graph = buildGraphAt(p);
  return graph.error(values);
}

/* ************************************************************************* */
template <size_t d>
boost::shared_ptr<LevenbergMarquardtOptimizer>
ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
  // Build graph
  NonlinearFactorGraph graph = buildGraphAt(p);

  // Anchor prior is added here as depends on initial value (and cost is zero)
  if (parameters_.alpha > 0) {
    size_t i;
    Rot value;
    const size_t dim = SOn::Dimension(p);
    std::tie(i, value) = parameters_.anchor;
    auto model = noiseModel::Isotropic::Precision(dim, parameters_.alpha);
    graph.emplace_shared<PriorFactor<SOn>>(i, SOn::Lift(p, value.matrix()),
                                           model);
  }
  // Optimize
  return boost::make_shared<LevenbergMarquardtOptimizer>(graph, initial,
                                                         parameters_.lm);
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
                                           const Values &initial) const {
  auto lm = createOptimizerAt(p, initial);
  return lm->optimize();
}

/* ************************************************************************* */
// Project to pxdN Stiefel manifold
template <size_t d>
Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
  const size_t N = values.size();
  const size_t p = values.at<SOn>(0).rows();
  Matrix S(p, N * d);
  for (const auto it : values.filter<SOn>()) {
    S.middleCols<d>(it.key * d) =
        it.value.matrix().leftCols<d>();  // project Qj to Stiefel
  }
  return S;
}

/* ************************************************************************* */
template <>
Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const {
  Values result;
  for (const auto it : values.filter<SOn>()) {
    assert(it.value.rows() == p);
    const auto &M = it.value.matrix();
    const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0));
    result.insert(it.key, R);
  }
  return result;
}

template <>
Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
  Values result;
  for (const auto it : values.filter<SOn>()) {
    assert(it.value.rows() == p);
    const auto &M = it.value.matrix();
    const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>());
    result.insert(it.key, R);
  }
  return result;
}

/* ************************************************************************* */
template <size_t d>
static Matrix RoundSolutionS(const Matrix &S) {
  const size_t N = S.cols() / d;
  // First, compute a thin SVD of S
  Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
  const Vector sigmas = svd.singularValues();

  // Construct a diagonal matrix comprised of the first d singular values
  using DiagonalMatrix = Eigen::DiagonalMatrix<double, d>;
  DiagonalMatrix Sigma_d;
  Sigma_d.diagonal() = sigmas.head<d>();

  // Now, construct a rank-d truncated singular value decomposition for S
  Matrix R = Sigma_d * svd.matrixV().leftCols<d>().transpose();

  // Count the number of blocks whose determinants have positive sign
  size_t numPositiveBlocks = 0;
  for (size_t i = 0; i < N; ++i) {
    // Compute the determinant of the ith dxd block of R
    double determinant = R.middleCols<d>(d * i).determinant();
    if (determinant > 0) ++numPositiveBlocks;
  }

  if (numPositiveBlocks < N / 2) {
    // Less than half of the total number of blocks have the correct sign.
    // To reverse their orientations, multiply with a reflection matrix.
    DiagonalMatrix reflector;
    reflector.setIdentity();
    reflector.diagonal()(d - 1) = -1;
    R = reflector * R;
  }

  return R;
}

/* ************************************************************************* */
template <>
Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
  // Round to a 2*2N matrix
  Matrix R = RoundSolutionS<2>(S);

  // Finally, project each dxd rotation block to SO(2)
  Values values;
  for (size_t j = 0; j < nrUnknowns(); ++j) {
    const Rot2 Ri = Rot2::atan2(R(1, 2 * j), R(0, 2 * j));
    values.insert(j, Ri);
  }
  return values;
}

template <>
Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
  // Round to a 3*3N matrix
  Matrix R = RoundSolutionS<3>(S);

  // Finally, project each dxd rotation block to SO(3)
  Values values;
  for (size_t j = 0; j < nrUnknowns(); ++j) {
    const Rot3 Ri = Rot3::ClosestTo(R.middleCols<3>(3 * j));
    values.insert(j, Ri);
  }
  return values;
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::roundSolution(const Values &values) const {
  // Project to pxdN Stiefel manifold...
  Matrix S = StiefelElementMatrix(values);
  // ...and call version above.
  return roundSolutionS(S);
}

/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::cost(const Values &values) const {
  NonlinearFactorGraph graph;
  for (const auto &measurement : measurements_) {
    const auto &keys = measurement.keys();
    const auto &Rij = measurement.measured();
    const auto &model = measurement.noiseModel();
    graph.emplace_shared<FrobeniusBetweenFactor<SO<d>>>(
        keys[0], keys[1], SO<d>(Rij.matrix()), model);
  }
  // Finally, project each dxd rotation block to SO(d)
  Values result;
  for (const auto it : values.filter<Rot>()) {
    result.insert(it.key, SO<d>(it.value.matrix()));
  }
  return graph.error(result);
}

/* ************************************************************************* */
// Get kappa from noise model
template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement,
                    const ShonanAveragingParameters<d> &parameters) {
  const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
      measurement.noiseModel());
  double sigma;
  if (isotropic) {
    sigma = isotropic->sigma();
  } else {
    const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
        measurement.noiseModel());
    // Check if noise model is robust
    if (robust) {
      // If robust, check if optimality certificate is expected
      if (parameters.getCertifyOptimality()) {
        throw std::invalid_argument(
            "Certification of optimality does not work with robust cost.");
      } else {
        // Optimality certificate not required, so setting default sigma
        sigma = 1;
      }
    } else {
      throw std::invalid_argument(
          "Shonan averaging noise models must be isotropic (but robust losses "
          "are allowed).");
    }
  }
  return 1.0 / (sigma * sigma);
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::buildD() const {
  // Each measurement contributes 2*d elements along the diagonal of the
  // degree matrix.
  static constexpr size_t stride = 2 * d;

  // Reserve space for triplets
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(stride * measurements_.size());

  for (const auto &measurement : measurements_) {
    // Get pose keys
    const auto &keys = measurement.keys();

    // Get kappa from noise model
    double kappa = Kappa<Rot, d>(measurement, parameters_);

    const size_t di = d * keys[0], dj = d * keys[1];
    for (size_t k = 0; k < d; k++) {
      // Elements of ith block-diagonal
      triplets.emplace_back(di + k, di + k, kappa);
      // Elements of jth block-diagonal
      triplets.emplace_back(dj + k, dj + k, kappa);
    }
  }

  // Construct and return a sparse matrix from these triplets
  const size_t dN = d * nrUnknowns();
  Sparse D(dN, dN);
  D.setFromTriplets(triplets.begin(), triplets.end());

  return D;
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::buildQ() const {
  // Each measurement contributes 2*d^2 elements on a pair of symmetric
  // off-diagonal blocks
  static constexpr size_t stride = 2 * d * d;

  // Reserve space for triplets
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(stride * measurements_.size());

  for (const auto &measurement : measurements_) {
    // Get pose keys
    const auto &keys = measurement.keys();

    // Extract rotation measurement
    const auto Rij = measurement.measured().matrix();

    // Get kappa from noise model
    double kappa = Kappa<Rot, d>(measurement, parameters_);

    const size_t di = d * keys[0], dj = d * keys[1];
    for (size_t r = 0; r < d; r++) {
      for (size_t c = 0; c < d; c++) {
        // Elements of ij block
        triplets.emplace_back(di + r, dj + c, kappa * Rij(r, c));
        // Elements of ji block
        triplets.emplace_back(dj + r, di + c, kappa * Rij(c, r));
      }
    }
  }

  // Construct and return a sparse matrix from these triplets
  const size_t dN = d * nrUnknowns();
  Sparse Q(dN, dN);
  Q.setFromTriplets(triplets.begin(), triplets.end());

  return Q;
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Matrix &S) const {
  // Each pose contributes 2*d elements along the diagonal of Lambda
  static constexpr size_t stride = d * d;

  // Reserve space for triplets
  const size_t N = nrUnknowns();
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(stride * N);

  // Do sparse-dense multiply to get Q*S'
  auto QSt = Q_ * S.transpose();

  for (size_t j = 0; j < N; j++) {
    // Compute B, the building block for the j^th diagonal block of Lambda
    const size_t dj = d * j;
    Matrix B = QSt.middleRows(dj, d) * S.middleCols<d>(dj);

    // Elements of jth block-diagonal
    for (size_t r = 0; r < d; r++)
      for (size_t c = 0; c < d; c++)
        triplets.emplace_back(dj + r, dj + c, 0.5 * (B(r, c) + B(c, r)));
  }

  // Construct and return a sparse matrix from these triplets
  Sparse Lambda(d * N, d * N);
  Lambda.setFromTriplets(triplets.begin(), triplets.end());
  return Lambda;
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeLambda(const Values &values) const {
  // Project to pxdN Stiefel manifold...
  Matrix S = StiefelElementMatrix(values);
  // ...and call version above.
  return computeLambda(S);
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Values &values) const {
  assert(values.size() == nrUnknowns());
  const Matrix S = StiefelElementMatrix(values);
  auto Lambda = computeLambda(S);
  return Lambda - Q_;
}

/* ************************************************************************* */
template <size_t d>
Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
  auto Lambda = computeLambda(S);
  return Lambda - Q_;
}

/* ************************************************************************* */
// Perturb the initial initialVector by adding a spherically-uniformly
// distributed random vector with 0.03*||initialVector||_2 magnitude to
// initialVector
// ref : Part III. C, Rosen, D. and Carlone, L., 2017, September. Computational
// enhancements for certifiably correct SLAM. In Proceedings of the
// International Conference on Intelligent Robots and Systems.
static Vector perturb(const Vector &initialVector) {
  // generate a 0.03*||x_0||_2 as stated in David's paper
  int n = initialVector.rows();
  Vector disturb = Vector::Random(n);
  disturb.normalize();

  double magnitude = initialVector.norm();
  Vector perturbedVector = initialVector + 0.03 * magnitude * disturb;
  perturbedVector.normalize();
  return perturbedVector;
}

/* ************************************************************************* */
/// MINIMUM EIGENVALUE COMPUTATIONS
// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization,
// it takes in the certificate matrix A as input, the maxIterations and the
// minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default,
// there are two parts
// in this algorithm:
// (1) compute the maximum eigenpair (\lamda_dom, \vect{v}_dom) of A by power
// method. if \lamda_dom is less than zero, then return the eigenpair. (2)
// compute the maximum eigenpair (\theta, \vect{v}) of C = \lamda_dom * I - A by
// accelerated power method. Then return (\lamda_dom - \theta, \vect{v}).
static bool PowerMinimumEigenValue(
    const Sparse &A, const Matrix &S, double &minEigenValue,
    Vector *minEigenVector = 0, size_t *numIterations = 0,
    size_t maxIterations = 1000,
    double minEigenvalueNonnegativityTolerance = 10e-4) {

  // a. Compute dominant eigenpair of S using power method
  PowerMethod<Sparse> pmOperator(A);

  const bool pmConverged = pmOperator.compute(
      maxIterations, 1e-5);

  // Check convergence and bail out if necessary
  if (!pmConverged) return false;

  const double pmEigenValue = pmOperator.eigenvalue();

  if (pmEigenValue < 0) {
    // The largest-magnitude eigenvalue is negative, and therefore also the
    // minimum eigenvalue, so just return this solution
    minEigenValue = pmEigenValue;
    if (minEigenVector) {
      *minEigenVector = pmOperator.eigenvector();
      minEigenVector->normalize();  // Ensure that this is a unit vector
    }
    return true;
  }

  const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()).sparseView() - A;
  const boost::optional<Vector> initial = perturb(S.row(0));
  AcceleratedPowerMethod<Sparse> apmShiftedOperator(C, initial);

  const bool minConverged = apmShiftedOperator.compute(
      maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue);

  if (!minConverged) return false;

  minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue();
  if (minEigenVector) {
    *minEigenVector = apmShiftedOperator.eigenvector();
    minEigenVector->normalize();  // Ensure that this is a unit vector
  }
  if (numIterations) *numIterations = apmShiftedOperator.nrIterations();
  return true;
}

/** This is a lightweight struct used in conjunction with Spectra to compute
 * the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
 * nontrivial function, perform_op(x,y), that computes and returns the product
 * y = (A + sigma*I) x */
struct MatrixProdFunctor {
  // Const reference to an externally-held matrix whose minimum-eigenvalue we
  // want to compute
  const Sparse &A_;

  // Spectral shift
  double sigma_;

  // Constructor
  explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
      : A_(A), sigma_(sigma) {}

  int rows() const { return A_.rows(); }
  int cols() const { return A_.cols(); }

  // Matrix-vector multiplication operation
  void perform_op(const double *x, double *y) const {
    // Wrap the raw arrays as Eigen Vector types
    Eigen::Map<const Vector> X(x, rows());
    Eigen::Map<Vector> Y(y, rows());

    // Do the multiplication using wrapped Eigen vectors
    Y = A_ * X + sigma_ * X;
  }
};

/// Function to compute the minimum eigenvalue of A using Lanczos in Spectra.
/// This does 2 things:
///
/// (1)  Quick (coarse) eigenvalue computation to estimate the largest-magnitude
/// eigenvalue (2)  A second eigenvalue computation applied to A-sigma*I, where
/// sigma is chosen to make the minimum eigenvalue of A the extremal eigenvalue
/// of A-sigma*I
///
/// Upon completion, this returns a boolean value indicating whether the minimum
/// eigenvalue was computed to the required precision -- if so, its sets the
/// values of minEigenValue and minEigenVector appropriately

/// Note that in the following function signature, S is supposed to be the
/// block-row-matrix that is a critical point for the optimization algorithm;
/// either S (Stiefel manifold) or R (block rotations).  We use this to
/// construct a starting vector v for the Lanczos process that will be close to
/// the minimum eigenvector we're looking for whenever the relaxation is exact
/// -- this is a key feature that helps to make this method fast.  Note that
/// instead of passing in all of S, it would be enough to pass in one of S's
/// *rows*, if that's more convenient.

// For the defaults, David Rosen says:
//   - maxIterations refers to the max number of Lanczos iterations to run;
//   ~1000 should be sufficiently large
//   - We've been using 10^-4 for the nonnegativity tolerance
//   - for numLanczosVectors, 20 is a good default value

static bool SparseMinimumEigenValue(
    const Sparse &A, const Matrix &S, double *minEigenValue,
    Vector *minEigenVector = 0, size_t *numIterations = 0,
    size_t maxIterations = 1000,
    double minEigenvalueNonnegativityTolerance = 10e-4,
    Eigen::Index numLanczosVectors = 20) {
  // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
  MatrixProdFunctor lmOperator(A);
  Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
                         MatrixProdFunctor>
      lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows()));
  lmEigenValueSolver.init();

  const int lmConverged = lmEigenValueSolver.compute(
      maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);

  // Check convergence and bail out if necessary
  if (lmConverged != 1) return false;

  const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);

  if (lmEigenValue < 0) {
    // The largest-magnitude eigenvalue is negative, and therefore also the
    // minimum eigenvalue, so just return this solution
    *minEigenValue = lmEigenValue;
    if (minEigenVector) {
      *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
      minEigenVector->normalize();  // Ensure that this is a unit vector
    }
    return true;
  }

  // The largest-magnitude eigenvalue is positive, and is therefore the
  // maximum  eigenvalue.  Therefore, after shifting the spectrum of A
  // by -2*lmEigenValue (by forming A - 2*lambda_max*I), the  shifted
  // spectrum will lie in the interval [minEigenValue(A) - 2* lambda_max(A),
  // -lambda_max*A]; in particular, the largest-magnitude eigenvalue of
  //  A - 2*lambda_max*I is minEigenValue - 2*lambda_max, with corresponding
  // eigenvector v_min

  MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue);

  Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
                         MatrixProdFunctor>
      minEigenValueSolver(&minShiftedOperator, 1,
                          std::min(numLanczosVectors, A.rows()));

  // If S is a critical point of F, then S^T is also in the null space of S -
  // Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are
  // eigenvectors corresponding to the eigenvalue 0.  In the case  that the
  // relaxation is exact, this is the *minimum* eigenvalue, and therefore the
  // rows of S are exactly the eigenvectors that we're looking for.  On the
  // other hand, if the relaxation is *not* exact, then S - Lambda(S) has at
  // least one strictly negative eigenvalue, and the rows of S are *unstable
  // fixed points* for the Lanczos iterations.  Thus, we will take a slightly
  // "fuzzed" version of the first row of S as an initialization for the
  // Lanczos iterations; this allows for rapid convergence in the case that
  // the relaxation is exact (since are starting close to a solution), while
  // simultaneously allowing the iterations to escape from this fixed point in
  // the case that the relaxation is not exact.
  Vector v0 = S.row(0).transpose();
  Vector perturbation(v0.size());
  perturbation.setRandom();
  perturbation.normalize();
  Vector xinit = v0 + (.03 * v0.norm()) * perturbation;  // Perturb v0 by ~3%

  // Use this to initialize the eigensolver
  minEigenValueSolver.init(xinit.data());

  // Now determine the relative precision required in the Lanczos method in
  // order to be able to estimate the smallest eigenvalue within an *absolute*
  // tolerance of 'minEigenvalueNonnegativityTolerance'
  const int minConverged = minEigenValueSolver.compute(
      maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
      Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);

  if (minConverged != 1) return false;

  *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
  if (minEigenVector) {
    *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
    minEigenVector->normalize();  // Ensure that this is a unit vector
  }
  if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
  return true;
}

/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
                                                Vector *minEigenVector) const {
  assert(values.size() == nrUnknowns());
  const Matrix S = StiefelElementMatrix(values);
  auto A = computeA(S);

  double minEigenValue;
  bool success = SparseMinimumEigenValue(A, S, &minEigenValue, minEigenVector);
  if (!success) {
    throw std::runtime_error(
        "SparseMinimumEigenValue failed to compute minimum eigenvalue.");
  }
  return minEigenValue;
}

/* ************************************************************************* */
template <size_t d>
double ShonanAveraging<d>::computeMinEigenValueAP(const Values &values,
                                                Vector *minEigenVector) const {
  assert(values.size() == nrUnknowns());
  const Matrix S = StiefelElementMatrix(values);
  auto A = computeA(S);

  double minEigenValue = 0;
  bool success = PowerMinimumEigenValue(A, S, minEigenValue, minEigenVector);
  if (!success) {
    throw std::runtime_error(
        "PowerMinimumEigenValue failed to compute minimum eigenvalue.");
  }
  return minEigenValue;
}

/* ************************************************************************* */
template <size_t d>
std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
    const Values &values) const {
  Vector minEigenVector;
  double minEigenValue = computeMinEigenValue(values, &minEigenVector);
  return std::make_pair(minEigenValue, minEigenVector);
}

/* ************************************************************************* */
template <size_t d>
bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
  double minEigenValue = computeMinEigenValue(values);
  return minEigenValue > parameters_.optimalityThreshold;
}

/* ************************************************************************* */
template <size_t d>
VectorValues ShonanAveraging<d>::TangentVectorValues(size_t p,
                                                     const Vector &v) {
  VectorValues delta;
  // Create a tangent direction xi with eigenvector segment v_i
  const size_t dimension = SOn::Dimension(p);
  double sign0 = pow(-1.0, round((p + 1) / 2) + 1);
  for (size_t i = 0; i < v.size() / d; i++) {
    // Assumes key is 0-based integer
    const auto v_i = v.segment<d>(d * i);
    Vector xi = Vector::Zero(dimension);
    double sign = sign0;
    for (size_t j = 0; j < d; j++) {
      xi(j + p - d - 1) = sign * v_i(d - j - 1);
      sign = -sign;
    }
    delta.insert(i, xi);
  }
  return delta;
}

/* ************************************************************************* */
template <size_t d>
Matrix ShonanAveraging<d>::riemannianGradient(size_t p,
                                              const Values &values) const {
  Matrix S_dot = StiefelElementMatrix(values);
  // calculate the gradient of F(Q_dot) at Q_dot
  Matrix euclideanGradient = 2 * (L_ * (S_dot.transpose())).transpose();
  // cout << "euclidean gradient rows and cols" << euclideanGradient.rows() <<
  // "\t" << euclideanGradient.cols() << endl;

  // project the gradient onto the entire euclidean space
  Matrix symBlockDiagProduct(p, d * nrUnknowns());
  for (size_t i = 0; i < nrUnknowns(); i++) {
    // Compute block product
    const size_t di = d * i;
    const Matrix P = S_dot.middleCols<d>(di).transpose() *
                     euclideanGradient.middleCols<d>(di);
    // Symmetrize this block
    Matrix S = .5 * (P + P.transpose());
    // Compute S_dot * S and set corresponding block
    symBlockDiagProduct.middleCols<d>(di) = S_dot.middleCols<d>(di) * S;
  }
  Matrix riemannianGradient = euclideanGradient - symBlockDiagProduct;
  return riemannianGradient;
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
                                           const Vector &minEigenVector) {
  Values lifted = LiftTo<SOn>(p, values);
  VectorValues delta = TangentVectorValues(p, minEigenVector);
  return lifted.retract(delta);
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeWithDescent(
    size_t p, const Values &values, const Vector &minEigenVector,
    double minEigenValue, double gradienTolerance,
    double preconditionedGradNormTolerance) const {
  double funcVal = costAt(p - 1, values);
  double alphaMin = 1e-2;
  double alpha =
      std::max(1024 * alphaMin, 10 * gradienTolerance / fabs(minEigenValue));
  vector<double> alphas;
  vector<double> fvals;
  // line search
  while ((alpha >= alphaMin)) {
    Values Qplus = LiftwithDescent(p, values, alpha * minEigenVector);
    double funcValTest = costAt(p, Qplus);
    Matrix gradTest = riemannianGradient(p, Qplus);
    double gradTestNorm = gradTest.norm();
    // Record alpha and funcVal
    alphas.push_back(alpha);
    fvals.push_back(funcValTest);
    if ((funcVal > funcValTest) && (gradTestNorm > gradienTolerance)) {
      return Qplus;
    }
    alpha /= 2;
  }

  auto fminIter = min_element(fvals.begin(), fvals.end());
  auto minIdx = distance(fvals.begin(), fminIter);
  double fMin = fvals[minIdx];
  double aMin = alphas[minIdx];
  if (fMin < funcVal) {
    Values Qplus = LiftwithDescent(p, values, aMin * minEigenVector);
    return Qplus;
  }

  return LiftwithDescent(p, values, alpha * minEigenVector);
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
  Values initial;
  for (size_t j = 0; j < nrUnknowns(); j++) {
    initial.insert(j, Rot::Random(rng));
  }
  return initial;
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly() const {
  return initializeRandomly(kRandomNumberGenerator);
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
                                                std::mt19937 &rng) const {
  const Values randomRotations = initializeRandomly(rng);
  return LiftTo<Rot3>(p, randomRotations);  // lift to p!
}

/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
  return initializeRandomlyAt(p, kRandomNumberGenerator);
}

/* ************************************************************************* */
template <size_t d>
std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
                                                  size_t pMin,
                                                  size_t pMax) const {
  Values Qstar;
  Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);  // lift to pMin!
  for (size_t p = pMin; p <= pMax; p++) {
    // Optimize until convergence at this level
    Qstar = tryOptimizingAt(p, initialSOp);
    if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) {
      // in this case, there is no optimality certification
      if (pMin != pMax) {
        throw std::runtime_error(
            "When using robust norm, Shonan only tests a single rank. Set pMin = pMax");
      }
      const Values SO3Values = roundSolution(Qstar);
      return std::make_pair(SO3Values, 0);
    } else {
      // Check certificate of global optimality
      Vector minEigenVector;
      double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector);
      if (minEigenValue > parameters_.optimalityThreshold) {
        // If at global optimum, round and return solution
        const Values SO3Values = roundSolution(Qstar);
        return std::make_pair(SO3Values, minEigenValue);
      }

      // Not at global optimimum yet, so check whether we will go to next level
      if (p != pMax) {
        // Calculate initial estimate for next level by following minEigenVector
        initialSOp =
            initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue);
      }
    }
  }
  throw std::runtime_error("Shonan::run did not converge for given pMax");
}

/* ************************************************************************* */
// Explicit instantiation for d=2
template class ShonanAveraging<2>;

ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
                                   const Parameters &parameters)
    : ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()),
                         parameters) {}

ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
    : ShonanAveraging<2>(maybeRobust(parseMeasurements<Rot2>(g2oFile),
                                     parameters.getUseHuber()),
                         parameters) {}

/* ************************************************************************* */
// Explicit instantiation for d=3
template class ShonanAveraging<3>;

ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
                                   const Parameters &parameters)
    : ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()),
                         parameters) {}

ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
    : ShonanAveraging<3>(maybeRobust(parseMeasurements<Rot3>(g2oFile),
                                     parameters.getUseHuber()),
                         parameters) {}

// TODO(frank): Deprecate after we land pybind wrapper

// Extract Rot3 measurement from Pose3 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot3> convert(
    const BetweenFactor<Pose3>::shared_ptr &f) {
  auto gaussian =
      boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
  if (!gaussian)
    throw std::invalid_argument(
        "parseMeasurements<Rot3> can only convert Pose3 measurements "
        "with Gaussian noise models.");
  const Matrix6 M = gaussian->covariance();
  auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3));
  return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(),
                                 model);
}

static ShonanAveraging3::Measurements extractRot3Measurements(
    const BetweenFactorPose3s &factors) {
  ShonanAveraging3::Measurements result;
  result.reserve(factors.size());
  for (auto f : factors) result.push_back(convert(f));
  return result;
}

ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
                                   const Parameters &parameters)
    : ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors),
                                     parameters.getUseHuber()),
                         parameters) {}

/* ************************************************************************* */
}  // namespace gtsam