Merge branch 'main-1.x' of https://github.com/cedricpradalier/Sophus …

…into main-1.x Adding a knots_and_u function and the corresponding type. Easier for optimisation.
strasdat · Sep 29, 2023 · 21f082e · 21f082e
2 parents 957f5f7 + 501b105
commit 21f082e
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 16 deletions.
diff --git a/test/ceres/ceres_flags.hpp b/test/ceres/ceres_flags.hpp
@@ -1,5 +1,4 @@
-#ifndef TEST_CERES_FLAGS_H
-#define TEST_CERES_FLAGS_H
+#pragma once
 
 
 #include "gflags/gflags.h"
@@ -34,4 +33,3 @@ DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
 DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
         " nonmonotic steps.");
 
-#endif // TEST_CERES_FLAGS_H
diff --git a/test/ceres/test_ceres_rxso2.cpp b/test/ceres/test_ceres_rxso2.cpp
@@ -51,6 +51,8 @@ int main(int, char **) {
   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++) {
@@ -63,6 +65,6 @@ int main(int, char **) {
       inter << t << " " << g.log().transpose() << std::endl;
   }
   inter.close();
-
+#endif
   return 0;
 }
diff --git a/test/ceres/test_ceres_so2.cpp b/test/ceres/test_ceres_so2.cpp
@@ -69,6 +69,8 @@ int main(int, char **) {
   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++) {
@@ -81,6 +83,7 @@ int main(int, char **) {
       inter << t << " " << g.log() << std::endl;
   }
   inter.close();
+#endif
 
   return 0;
 }
diff --git a/test/ceres/tests.hpp b/test/ceres/tests.hpp
@@ -238,7 +238,6 @@ struct LieGroupCeresTests {
 
   bool testAll() {
     bool passed = true;
-#if 0
     for (size_t i = 0; i < group_vec.size(); ++i) {
       for (size_t j = 0; j < group_vec.size(); ++j) {
         if (i == j) continue;
@@ -254,23 +253,19 @@ struct LieGroupCeresTests {
         }
       }
     }
-#endif
-#if 0
     for (size_t i = 0; i < group_vec.size(); ++i) {
       for (size_t j = 0; j < group_vec.size(); ++j) {
         passed &= testManifold(group_vec[i], group_vec[j]);
         processTestResult(passed);
       }
     }
-#endif
-#if 0
     int Ns[] = {20, 40, 80, 160};
     for (auto N : Ns) {
       std::cerr << "Averaging test: N = " << N;
       passed &= testAveraging(N, .5, .1);
       processTestResult(passed);
     }
-#endif
+    std::cerr << "Spline test: N = " << N;
     passed &= testSpline() != nullptr;
     processTestResult(passed);
     return passed;
@@ -282,6 +277,7 @@ struct LieGroupCeresTests {
     if (n_knots<0) {
         n_knots = 3 * group_vec.size() / 4;
     }
+    // Running Lie group spline approximation
     std::vector<LieGroupd> control_poses(n_knots,LieGroupd());
     std::shared_ptr<BasisSpline<LieGroupd>> spline(new BasisSpline<LieGroupd>(control_poses, -1.0, float(group_vec.size()+2)/(n_knots-1)));
     ceres::Problem problem;
@@ -299,8 +295,6 @@ struct LieGroupCeresTests {
         KnotsAndU ku = spline->knots_and_u(t);
         LieGroupd pred = spline->parent_T_spline(t);
         LieGroupd err = group_vec[i].inverse() * pred;
-        // std::cout << i << " : (" << iu.i << "," << iu.u << ") l(P) " << pred.log() << " l(A) " << group_vec[i].log() << std::endl; 
-        // std::cout << "l(E) " << err.log() << " ||l(E)|| " << squaredNorm(err.log()) << std::endl;
         initial_error += squaredNorm(err.log());
         ceres::CostFunction* cost;
         switch (ku.segment_case) {
@@ -365,14 +359,14 @@ struct LieGroupCeresTests {
     options.gradient_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
     options.function_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
     options.parameter_tolerance = 1e-2 * Sophus::Constants<double>::epsilon();
-    options.minimizer_progress_to_stdout = true;
+    options.minimizer_progress_to_stdout = false;
     options.max_num_iterations = 500;
 
 
 
     ceres::Solver::Summary summary;
     Solve(options, &problem, &summary);
-    std::cout << summary.FullReport() << "\n";
+    // std::cout << summary.FullReport() << "\n";
 
 
     // Computing final error in the estimates
@@ -381,8 +375,6 @@ struct LieGroupCeresTests {
         double t = i;
         LieGroupd pred = spline->parent_T_spline(t);
         LieGroupd err = group_vec[i].inverse() * pred;
-        // std::cout << i << " l(P) " << pred.log() << " l(A) " << group_vec[i].log() << std::endl; 
-        // std::cout << "l(E) " << err.log() << " ||l(E)|| " << squaredNorm(err.log()) << std::endl;
         final_error += squaredNorm(err.log());
     }