Added a test of ceres-based spline approximation in the test/ceres

folder.

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,22 @@ 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();
+
+
+ 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();
+
  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"
@@ -40,6 +41,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;
 }

test/ceres/tests.hpp

@@ -3,7 +3,9 @@
 #include <ceres/ceres.h>
 
 #include <sophus/ceres_manifold.hpp>
+#include <sophus/spline.hpp>
 #include <sophus/test_macros.hpp>
+#include "ceres_flags.hpp"
 
 template <typename LieGroup>
 struct RotationalPart;
@@ -172,6 +174,68 @@ struct LieGroupCeresTests {
  const LieGroupd diff;
  };
 
+ struct TestSplineFunctor {
+ template <typename T>
+ bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, 
+ T* residuals) const {
+ using LieGroupT = LieGroup<T>;
+ if (segment_case != SegmentCase::normal) {
+ std::cerr << "Invalid segment_case in spline functor (4)" << std::endl;
+ return false;
+ }
+ BasisSplineSegment<LieGroupT> s(segment_case,P0,P1,P2,P3);
+ LieGroupT pred = s.parent_T_spline(u);
+ LieGroupT diff = y.inverse() * pred;
+ using Mapper = Mapper<typename LieGroupT::Tangent>;
+ typename Mapper::Map diff_log = Mapper::map(residuals);
+
+ // Jet LieGroup multiplication with LieGroupd
+ diff_log = diff.log();
+ return true;
+ }
+
+
+ template <typename T>
+ bool operator()(const T* P0, const T* P1, const T* P2, 
+ T* residuals) const {
+ using LieGroupT = LieGroup<T>;
+ LieGroupT pred;
+ switch (segment_case) {
+ case SegmentCase::first:
+ { 
+ BasisSplineSegment<LieGroupT> s(segment_case,P0,P0,P1,P2);
+ pred = s.parent_T_spline(u);
+ }
+ break;
+ case SegmentCase::last:
+ { 
+ BasisSplineSegment<LieGroupT> s(segment_case,P0,P1,P2,P2);
+ pred = s.parent_T_spline(u);
+ }
+ break;
+ default:
+ std::cerr << "Invalid segment_case in spline functor (3)" << std::endl;
+ return false;
+ }
+ LieGroupT diff = y.inverse() * pred;
+ using Mapper = Mapper<typename LieGroupT::Tangent>;
+ typename Mapper::Map diff_log = Mapper::map(residuals);
+
+ // Jet LieGroup multiplication with LieGroupd
+ diff_log = diff.log();
+ return true;
+ }
+
+
+ TestSplineFunctor(SegmentCase scase, double u, const LieGroupd & yin) : 
+ segment_case(scase), u(u), y(yin){
+ }
+ SegmentCase segment_case;
+ double u;
+ const LieGroupd y;
+
+ };
+
  bool testAll() {
  bool passed = true;
  for (size_t i = 0; i < group_vec.size(); ++i) {
@@ -201,9 +265,131 @@ struct LieGroupCeresTests {
  passed &= testAveraging(N, .5, .1);
  processTestResult(passed);
  }
+ passed &= testSpline() != nullptr;
+ processTestResult(passed);
  return passed;
  }
 
+ std::shared_ptr<BasisSpline<LieGroupd>> testSpline(int n_knots=-1) {
+ if (group_vec.empty()) 
+ return std::shared_ptr<BasisSpline<LieGroupd>>();
+ 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;
+
+ double initial_error = 0.;
+ auto parametrization = new Sophus::Manifold<LieGroup_>;
+
+ for (auto v : spline->parent_Ts_control_point()) {
+
+ problem.AddParameterBlock(v.data(), LieGroupd::num_parameters, parametrization);
+ }
+
+ for (size_t i = 0; i < group_vec.size(); ++i) {
+ double t = i;
+ IndexAndU iu = spline->index_and_u(t);
+ LieGroupd pred = spline->parent_T_spline(t);
+ LieGroupd err = group_vec[i].inverse() * pred;
+ initial_error += squaredNorm(err.log());
+ SegmentCase segment_case =
+ iu.i == 0 ? SegmentCase::first
+ : (iu.i == spline->getNumSegments() - 1 ? SegmentCase::last
+ : SegmentCase::normal);
+
+ int idx_prev = std::max(0, iu.i - 1);
+ int idx_0 = iu.i;
+ int idx_1 = iu.i + 1;
+ int idx_2 = std::min(iu.i + 2, int(spline->parent_Ts_control_point().size()) - 1);
+
+ ceres::CostFunction* cost;
+ switch (segment_case) {
+ case SegmentCase::first:
+ cost = new ceres::AutoDiffCostFunction<TestSplineFunctor, LieGroupd::DoF,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters>(
+ new TestSplineFunctor(segment_case,iu.u,group_vec[i]));
+ problem.AddResidualBlock(cost, nullptr, 
+ spline->parent_Ts_control_point()[idx_0].data(),
+ spline->parent_Ts_control_point()[idx_1].data(),
+ spline->parent_Ts_control_point()[idx_2].data());
+ break;
+ case SegmentCase::normal:
+ cost = new ceres::AutoDiffCostFunction<TestSplineFunctor, LieGroupd::DoF,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters>(
+ new TestSplineFunctor(segment_case,iu.u,group_vec[i]));
+ problem.AddResidualBlock(cost, nullptr, 
+ spline->parent_Ts_control_point()[idx_prev].data(),
+ spline->parent_Ts_control_point()[idx_0].data(),
+ spline->parent_Ts_control_point()[idx_1].data(),
+ spline->parent_Ts_control_point()[idx_2].data());
+ break;
+ case SegmentCase::last:
+ cost = new ceres::AutoDiffCostFunction<TestSplineFunctor, LieGroupd::DoF,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters,
+ LieGroupd::num_parameters>(
+ new TestSplineFunctor(segment_case,iu.u,group_vec[i]));
+ problem.AddResidualBlock(cost, nullptr, 
+ spline->parent_Ts_control_point()[idx_prev].data(),
+ spline->parent_Ts_control_point()[idx_0].data(),
+ spline->parent_Ts_control_point()[idx_1].data());
+ break;
+ }
+ }
+
+ ceres::Solver::Options options;
+ CHECK(StringToLinearSolverType(FLAGS_linear_solver,
+ &options.linear_solver_type));
+ CHECK(StringToPreconditionerType(FLAGS_preconditioner,
+ &options.preconditioner_type));
+ CHECK(StringToSparseLinearAlgebraLibraryType(
+ FLAGS_sparse_linear_algebra_library,
+ &options.sparse_linear_algebra_library_type));
+ options.use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
+ CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
+ &options.trust_region_strategy_type));
+ CHECK(StringToDoglegType(FLAGS_dogleg, &options.dogleg_type));
+ options.use_inner_iterations = FLAGS_inner_iterations;
+
+ 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 = false;
+ options.max_num_iterations = 500;
+
+
+
+ ceres::Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ // std::cout << summary.FullReport() << "\n";
+
+
+ // Computing final error in the estimates
+ double final_error = 0.;
+ for (size_t i = 0; i < group_vec.size(); ++i) {
+ double t = i;
+ LieGroupd pred = spline->parent_T_spline(t);
+ LieGroupd err = group_vec[i].inverse() * pred;
+ final_error += squaredNorm(err.log());
+ }
+
+
+ // Expecting reasonable decrease of both estimates' errors and residuals
+ if (summary.final_cost < .5 * summary.initial_cost) {
+ return spline;
+ } else {
+ return std::shared_ptr<BasisSpline<LieGroupd>>();
+ }
+ }
+
  bool testAveraging(const size_t num_vertices, const double sigma_init,
  const double sigma_observation) {
  if (!num_vertices) return true;