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