Adding a spline optimisation example in the test/ceres folder

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.24)
+cmake_minimum_required(VERSION 3.22)
 
 file(READ "SOPHUS_VERSION" SOPHUS_VERSION)
 project(Sophus VERSION ${SOPHUS_VERSION})
@@ -26,20 +26,21 @@ if(SOPHUS_MASTER_PROJECT)
 
   set(CMAKE_CXX_STANDARD 17)
 
-  set(CMAKE_COMPILE_WARNING_AS_ERROR Off)
-
-  # Set compiler specific settings (FixMe: Should not cmake do this for us
-  # automatically?)
-  if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
-    add_compile_options(-Wall -Wextra -Wno-deprecated-register
-                        -Qunused-arguments -fcolor-diagnostics)
-  elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
-    add_compile_options(-Wall -Wextra -Wno-deprecated-declarations
-                        -ftemplate-backtrace-limit=0 -Wno-array-bounds)
-  elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
-    add_compile_options(/bigobj /wd4305 /wd4244 /MP)
-    add_compile_definitions(_USE_MATH_DEFINES)
-  endif()
+  # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?)
+  IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+    SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+    SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra  -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics")
+  ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+    SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+    SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -g -ggdb")
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++17 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0")
+    SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline")
+    SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage")
+    SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage")
+  ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$")
+    ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP")
+  ENDIF()
 
   # Add local path for finding packages, set the local version first
   list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")

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,9 +4,11 @@ list(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR})
 # git clone https://ceres-solver.googlesource.com/ceres-solver
 find_package(Ceres 2)
 
+
+
 function(add_test_ceres source postfix)
   add_executable(${source}_${postfix} ${source}.cpp)
-  target_link_libraries(${source}_${postfix} sophus Ceres::ceres)
+  target_link_libraries(${source}_${postfix} sophus Ceres::ceres gflags glog)
   target_compile_definitions(${source}_${postfix} PRIVATE ${ARGN})
   add_test(${source}_${postfix} ${source}_${postfix})
 endfunction()
@@ -16,14 +18,15 @@ 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} "manifold")

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