Adding a spline optimisation example in the test/ceres folder

sophus/spline.hpp

@@ -280,6 +280,16 @@ struct BasisSplineSegment {
   T const* raw_params3_;
 };
 
+struct KnotsAndU {
+  SegmentCase segment_case;
+  int idx_prev;
+  int idx_0;
+  int idx_1;
+  int idx_2;
+  double u;
+};
+
+
 template <class LieGroup_>
 class BasisSplineImpl {
  public:
@@ -295,28 +305,35 @@ class BasisSplineImpl {
                   parent_Ts_control_point_.size());
   }
 
+  KnotsAndU knots_and_u(int i, double u) const {
+      KnotsAndU ku;
+      ku.u = u;
+      ku.segment_case =
+          i == 0 ? SegmentCase::first
+          : (i == this->getNumSegments() - 1 ? SegmentCase::last
+                  : SegmentCase::normal);
+
+      ku.idx_prev = std::max(0, i - 1);
+      ku.idx_0 = i;
+      ku.idx_1 = std::min(i + 1, int(this->parent_Ts_control_point_.size()) - 1);
+      ku.idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);
+      return ku;
+  }
+
   LieGroup parent_T_spline(int i, double u) const {
     SOPHUS_ENSURE(i >= 0, "i = {}", i);
     SOPHUS_ENSURE(i < this->getNumSegments(),
                   "i = {};  this->getNumSegments() = {};  "
                   "parent_Ts_control_point_.size() = {}",
                   i, this->getNumSegments(), parent_Ts_control_point_.size());
 
-    SegmentCase segment_case =
-        i == 0 ? SegmentCase::first
-               : (i == this->getNumSegments() - 1 ? SegmentCase::last
-                                                  : SegmentCase::normal);
-
-    int idx_prev = std::max(0, i - 1);
-    int idx_0 = i;
-    int idx_1 = i + 1;
-    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);
+    KnotsAndU ku = knots_and_u(i,u);
 
     return BasisSplineSegment<LieGroup>(
-               segment_case, parent_Ts_control_point_[idx_prev].data(),
-               parent_Ts_control_point_[idx_0].data(),
-               parent_Ts_control_point_[idx_1].data(),
-               parent_Ts_control_point_[idx_2].data())
+               ku.segment_case, parent_Ts_control_point_[ku.idx_prev].data(),
+               parent_Ts_control_point_[ku.idx_0].data(),
+               parent_Ts_control_point_[ku.idx_1].data(),
+               parent_Ts_control_point_[ku.idx_2].data())
         .parent_T_spline(u);
   }
 
@@ -327,21 +344,13 @@ class BasisSplineImpl {
                   "parent_Ts_control_point_.size() = {}",
                   i, this->getNumSegments(), parent_Ts_control_point_.size());
 
-    SegmentCase segment_case =
-        i == 0 ? SegmentCase::first
-               : (i == this->getNumSegments() - 1 ? SegmentCase::last
-                                                  : SegmentCase::normal);
-
-    int idx_prev = std::max(0, i - 1);
-    int idx_0 = i;
-    int idx_1 = i + 1;
-    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);
+    KnotsAndU ku = knots_and_u(i,u);
 
     return BasisSplineSegment<LieGroup>(
-               segment_case, parent_Ts_control_point_[idx_prev].data(),
-               parent_Ts_control_point_[idx_0].data(),
-               parent_Ts_control_point_[idx_1].data(),
-               parent_Ts_control_point_[idx_2].data())
+               ku.segment_case, parent_Ts_control_point_[ku.idx_prev].data(),
+               parent_Ts_control_point_[ku.idx_0].data(),
+               parent_Ts_control_point_[ku.idx_1].data(),
+               parent_Ts_control_point_[ku.idx_2].data())
         .Dt_parent_T_spline(u, delta_t_);
   }
 
@@ -352,21 +361,13 @@ class BasisSplineImpl {
                   "parent_Ts_control_point_.size() = {}",
                   i, this->getNumSegments(), parent_Ts_control_point_.size());
 
-    SegmentCase segment_case =
-        i == 0 ? SegmentCase::first
-               : (i == this->getNumSegments() - 1 ? SegmentCase::last
-                                                  : SegmentCase::normal);
-
-    int idx_prev = std::max(0, i - 1);
-    int idx_0 = i;
-    int idx_1 = i + 1;
-    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);
+    KnotsAndU ku = knots_and_u(i,u);
 
     return BasisSplineSegment<LieGroup>(
-               segment_case, parent_Ts_control_point_[idx_prev].data(),
-               parent_Ts_control_point_[idx_0].data(),
-               parent_Ts_control_point_[idx_1].data(),
-               parent_Ts_control_point_[idx_2].data())
+               ku.segment_case, parent_Ts_control_point_[ku.idx_prev].data(),
+               parent_Ts_control_point_[ku.idx_0].data(),
+               parent_Ts_control_point_[ku.idx_1].data(),
+               parent_Ts_control_point_[ku.idx_2].data())
         .Dt2_parent_T_spline(u, delta_t_);
   }
 
@@ -466,6 +467,12 @@ class BasisSpline {
     return index_and_u;
   }
 
+  KnotsAndU knots_and_u(double t) const {
+      IndexAndU iu = index_and_u(t);
+      KnotsAndU ku = impl_.knots_and_u(iu.i,iu.u);
+      return ku;
+  }
+
  private:
   BasisSplineImpl<LieGroup> impl_;
 

test/ceres/CMakeLists.txt

@@ -4,6 +4,8 @@ list(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR})
 # git clone https://ceres-solver.googlesource.com/ceres-solver
 find_package(Ceres 2.1.0 QUIET)
 
+
+
 function(add_test_ceres source postfix)
   add_executable(${source}_${postfix} ${source}.cpp)
   target_link_libraries(${source}_${postfix} sophus Ceres::ceres)
@@ -16,18 +18,19 @@ if(Ceres_FOUND)
 
   # Tests to run
   set(TEST_SOURCES
+      test_ceres_so2
       test_ceres_so3
       test_ceres_rxso3
       test_ceres_se3
       test_ceres_sim3
-      test_ceres_so2
       test_ceres_rxso2
       test_ceres_se2
-      test_ceres_sim2)
+      test_ceres_sim2
+      )
 
-  foreach(test_src ${TEST_SOURCES})
-    add_test_ceres(${test_src} "local_parameterization")
-  endforeach()
+  # foreach(test_src ${TEST_SOURCES})
+  #   add_test_ceres(${test_src} "local_parameterization")
+  # endforeach()
   foreach(test_src ${TEST_SOURCES})
     add_test_ceres(${test_src} "manifold")
   endforeach()

test/ceres/ceres_flags.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+
+#include "gflags/gflags.h"
+
+
+DEFINE_bool(robustify_trilateration, false, "Use a robust loss function for trilateration.");
+
+DEFINE_string(trust_region_strategy, "levenberg_marquardt",
+        "Options are: levenberg_marquardt, dogleg.");
+DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
+        "subspace_dogleg.");
+
+DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
+        "refine each successful trust region step.");
+
+DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
+        "automatic, cameras, points, cameras,points, points,cameras");
+
+DEFINE_string(linear_solver, "sparse_normal_cholesky", "Options are: "
+        "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
+        "dense_qr, dense_normal_cholesky and cgnr.");
+
+DEFINE_string(preconditioner, "jacobi", "Options are: "
+        "identity, jacobi, schur_jacobi, cluster_jacobi, "
+        "cluster_tridiagonal.");
+
+DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
+        "Options are: suite_sparse and cx_sparse.");
+
+DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
+
+DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
+        " nonmonotic steps.");
+

test/ceres/test_ceres_rxso2.cpp

@@ -1,5 +1,6 @@
 #include <ceres/ceres.h>
 #include <iostream>
+#include <fstream>
 #include <sophus/rxso2.hpp>
 
 #include "tests.hpp"
@@ -46,6 +47,24 @@ int main(int, char **) {
   point_vec.push_back(Point(5.8, 9.2));
 
   std::cerr << "Test Ceres RxSO2" << std::endl;
-  Sophus::LieGroupCeresTests<Sophus::RxSO2>(rxso2_vec, point_vec).testAll();
+  Sophus::LieGroupCeresTests<Sophus::RxSO2> test(rxso2_vec, point_vec);
+  test.testAll();
+
+
+#if 0
+  // Example code to output the spline curve into a plottable format
+  std::shared_ptr<Sophus::BasisSpline<RxSO2d>> so2_spline = test.testSpline(6);
+  std::ofstream control("ctrl_pts", std::ofstream::out);
+  for (size_t i=0;i<rxso2_vec.size();i++) {
+      control << i << " " << rxso2_vec[i].log().transpose() << std::endl;
+  }
+  control.close();
+  std::ofstream inter("inter_pts", std::ofstream::out);
+  for (double t=0;t<rxso2_vec.size();t+=0.1) {
+      RxSO2d g = so2_spline->parent_T_spline(t);
+      inter << t << " " << g.log().transpose() << std::endl;
+  }
+  inter.close();
+#endif
   return 0;
 }

test/ceres/test_ceres_se3.cpp

@@ -1,5 +1,6 @@
 #include <ceres/ceres.h>
 #include <iostream>
+#include <fstream>
 #include <sophus/se3.hpp>
 
 #include "tests.hpp"
@@ -48,6 +49,19 @@ int main(int, char **) {
   point_vec.push_back(Point(5.8, 9.2, 0.0));
 
   std::cerr << "Test Ceres SE3" << std::endl;
-  Sophus::LieGroupCeresTests<Sophus::SE3>(se3_vec, point_vec).testAll();
+  Sophus::LieGroupCeresTests<Sophus::SE3> test(se3_vec, point_vec);
+  test.testAll();
+  std::shared_ptr<Sophus::BasisSpline<SE3d>> se3_spline = test.testSpline(6);
+  std::ofstream control("ctrl_pts", std::ofstream::out);
+  for (size_t i=0;i<se3_vec.size();i++) {
+      control << i << " " << se3_vec[i].log() << std::endl;
+  }
+  control.close();
+  std::ofstream inter("inter_pts", std::ofstream::out);
+  for (double t=0;t<se3_vec.size();t+=0.1) {
+      SE3d g = se3_spline->parent_T_spline(t);
+      inter << t << " " << g.log() << std::endl;
+  }
+  inter.close();
   return 0;
 }

test/ceres/test_ceres_so2.cpp

@@ -1,5 +1,6 @@
 #include <ceres/ceres.h>
 #include <iostream>
+#include <fstream>
 #include <sophus/so2.hpp>
 
 #include "tests.hpp"
@@ -17,16 +18,41 @@ struct RotationalPart<Sophus::SO2d> {
 int main(int, char **) {
   using SO2d = Sophus::SO2d;
   using Point = SO2d::Point;
-  double const kPi = Sophus::Constants<double>::pi();
 
   StdVector<SO2d> so2_vec;
+#if 1
+  double const kPi = Sophus::Constants<double>::pi();
+  so2_vec.emplace_back(SO2d::exp(0.0));
+  so2_vec.emplace_back(SO2d::exp(0.0));
   so2_vec.emplace_back(SO2d::exp(0.0));
   so2_vec.emplace_back(SO2d::exp(0.2));
   so2_vec.emplace_back(SO2d::exp(10.));
   so2_vec.emplace_back(SO2d::exp(0.00001));
   so2_vec.emplace_back(SO2d::exp(kPi));
   so2_vec.emplace_back(SO2d::exp(0.2) * SO2d::exp(kPi) * SO2d::exp(-0.2));
   so2_vec.emplace_back(SO2d::exp(-0.3) * SO2d::exp(kPi) * SO2d::exp(0.3));
+  so2_vec.emplace_back(SO2d::exp(0.0));
+  so2_vec.emplace_back(SO2d::exp(0.0));
+  so2_vec.emplace_back(SO2d::exp(0.0));
+#else
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+  so2_vec.emplace_back(SO2d::exp(-1.0));
+#endif
+
 
   StdVector<Point> point_vec;
   point_vec.emplace_back(Point(1.012, 2.73));
@@ -40,6 +66,24 @@ int main(int, char **) {
   point_vec.emplace_back(Point(5.8, 9.2));
 
   std::cerr << "Test Ceres SO2" << std::endl;
-  Sophus::LieGroupCeresTests<Sophus::SO2>(so2_vec, point_vec).testAll();
+  Sophus::LieGroupCeresTests<Sophus::SO2> test(so2_vec, point_vec);
+  test.testAll();
+
+#if 0
+  // Example code to plot the interpolated spline
+  std::shared_ptr<Sophus::BasisSpline<SO2d>> so2_spline = test.testSpline(6);
+  std::ofstream control("ctrl_pts", std::ofstream::out);
+  for (size_t i=0;i<so2_vec.size();i++) {
+      control << i << " " << so2_vec[i].log() << std::endl;
+  }
+  control.close();
+  std::ofstream inter("inter_pts", std::ofstream::out);
+  for (double t=0;t<so2_vec.size();t+=0.1) {
+      SO2d g = so2_spline->parent_T_spline(t);
+      inter << t << " " << g.log() << std::endl;
+  }
+  inter.close();
+#endif
+
   return 0;
 }