diff --git a/sophus/spline.hpp b/sophus/spline.hpp index 6d5fd1014..c5c5ea386 100644 --- a/sophus/spline.hpp +++ b/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 BasisSplineImpl { public: @@ -295,6 +305,21 @@ 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(), @@ -302,21 +327,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( - 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( - 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( - 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 impl_; diff --git a/test/ceres/CMakeLists.txt b/test/ceres/CMakeLists.txt index 75605d4ac..bd11291ca 100644 --- a/test/ceres/CMakeLists.txt +++ b/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() diff --git a/test/ceres/ceres_flags.hpp b/test/ceres/ceres_flags.hpp new file mode 100644 index 000000000..8eef5d5a6 --- /dev/null +++ b/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."); + diff --git a/test/ceres/test_ceres_rxso2.cpp b/test/ceres/test_ceres_rxso2.cpp index 6d30c370d..aafcbe594 100644 --- a/test/ceres/test_ceres_rxso2.cpp +++ b/test/ceres/test_ceres_rxso2.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #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(rxso2_vec, point_vec).testAll(); + Sophus::LieGroupCeresTests test(rxso2_vec, point_vec); + test.testAll(); + + +#if 0 + // Example code to output the spline curve into a plottable format + std::shared_ptr> so2_spline = test.testSpline(6); + std::ofstream control("ctrl_pts", std::ofstream::out); + for (size_t i=0;iparent_T_spline(t); + inter << t << " " << g.log().transpose() << std::endl; + } + inter.close(); +#endif return 0; } diff --git a/test/ceres/test_ceres_se3.cpp b/test/ceres/test_ceres_se3.cpp index b507b0803..4fab7e96b 100644 --- a/test/ceres/test_ceres_se3.cpp +++ b/test/ceres/test_ceres_se3.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #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(se3_vec, point_vec).testAll(); + Sophus::LieGroupCeresTests test(se3_vec, point_vec); + test.testAll(); + std::shared_ptr> se3_spline = test.testSpline(6); + std::ofstream control("ctrl_pts", std::ofstream::out); + for (size_t i=0;iparent_T_spline(t); + inter << t << " " << g.log() << std::endl; + } + inter.close(); return 0; } diff --git a/test/ceres/test_ceres_so2.cpp b/test/ceres/test_ceres_so2.cpp index 97ca0465e..ef6f80483 100644 --- a/test/ceres/test_ceres_so2.cpp +++ b/test/ceres/test_ceres_so2.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include "tests.hpp" @@ -17,9 +18,12 @@ struct RotationalPart { int main(int, char **) { using SO2d = Sophus::SO2d; using Point = SO2d::Point; - double const kPi = Sophus::Constants::pi(); StdVector so2_vec; +#if 1 + double const kPi = Sophus::Constants::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.)); @@ -27,6 +31,28 @@ int main(int, char **) { 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_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(so2_vec, point_vec).testAll(); + Sophus::LieGroupCeresTests test(so2_vec, point_vec); + test.testAll(); + +#if 0 + // Example code to plot the interpolated spline + std::shared_ptr> so2_spline = test.testSpline(6); + std::ofstream control("ctrl_pts", std::ofstream::out); + for (size_t i=0;iparent_T_spline(t); + inter << t << " " << g.log() << std::endl; + } + inter.close(); +#endif + return 0; } diff --git a/test/ceres/tests.hpp b/test/ceres/tests.hpp index a3572f4cc..3b2ccdaf6 100644 --- a/test/ceres/tests.hpp +++ b/test/ceres/tests.hpp @@ -3,7 +3,9 @@ #include #include +#include #include +#include "ceres_flags.hpp" template struct RotationalPart; @@ -172,6 +174,68 @@ struct LieGroupCeresTests { const LieGroupd diff; }; + struct TestSplineFunctor { + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + using LieGroupT = LieGroup; + if (segment_case != SegmentCase::normal) { + std::cerr << "Invalid segment_case in spline functor (4)" << std::endl; + return false; + } + BasisSplineSegment s(segment_case,P0,P1,P2,P3); + LieGroupT pred = s.parent_T_spline(u); + LieGroupT diff = y.inverse() * pred; + using Mapper = Mapper; + typename Mapper::Map diff_log = Mapper::map(residuals); + + // Jet LieGroup multiplication with LieGroupd + diff_log = diff.log(); + return true; + } + + + template + bool operator()(const T* P0, const T* P1, const T* P2, + T* residuals) const { + using LieGroupT = LieGroup; + LieGroupT pred; + switch (segment_case) { + case SegmentCase::first: + { + BasisSplineSegment s(segment_case,P0,P0,P1,P2); + pred = s.parent_T_spline(u); + } + break; + case SegmentCase::last: + { + BasisSplineSegment 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 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,128 @@ struct LieGroupCeresTests { passed &= testAveraging(N, .5, .1); processTestResult(passed); } + std::cerr << "Spline test: N = " << N; + passed &= testSpline() != nullptr; + processTestResult(passed); return passed; } + std::shared_ptr> testSpline(int n_knots=-1) { + if (group_vec.empty()) + return std::shared_ptr>(); + if (n_knots<0) { + n_knots = 3 * group_vec.size() / 4; + } + // Running Lie group spline approximation + std::vector control_poses(n_knots,LieGroupd()); + std::shared_ptr> spline(new BasisSpline(control_poses, -1.0, float(group_vec.size()+2)/(n_knots-1))); + ceres::Problem problem; + + double initial_error = 0.; + auto parametrization = new Sophus::Manifold; + + 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; + KnotsAndU ku = spline->knots_and_u(t); + LieGroupd pred = spline->parent_T_spline(t); + LieGroupd err = group_vec[i].inverse() * pred; + initial_error += squaredNorm(err.log()); + ceres::CostFunction* cost; + switch (ku.segment_case) { + case SegmentCase::first: + cost = new ceres::AutoDiffCostFunction( + new TestSplineFunctor(ku.segment_case,ku.u,group_vec[i])); + // For real-world problems you should consider using robust + // loss-function + problem.AddResidualBlock(cost, nullptr, + spline->parent_Ts_control_point()[ku.idx_0].data(), + spline->parent_Ts_control_point()[ku.idx_1].data(), + spline->parent_Ts_control_point()[ku.idx_2].data()); + break; + case SegmentCase::normal: + cost = new ceres::AutoDiffCostFunction( + new TestSplineFunctor(ku.segment_case,ku.u,group_vec[i])); + // For real-world problems you should consider using robust + // loss-function + problem.AddResidualBlock(cost, nullptr, + spline->parent_Ts_control_point()[ku.idx_prev].data(), + spline->parent_Ts_control_point()[ku.idx_0].data(), + spline->parent_Ts_control_point()[ku.idx_1].data(), + spline->parent_Ts_control_point()[ku.idx_2].data()); + break; + case SegmentCase::last: + cost = new ceres::AutoDiffCostFunction( + new TestSplineFunctor(ku.segment_case,ku.u,group_vec[i])); + // For real-world problems you should consider using robust + // loss-function + problem.AddResidualBlock(cost, nullptr, + spline->parent_Ts_control_point()[ku.idx_prev].data(), + spline->parent_Ts_control_point()[ku.idx_0].data(), + spline->parent_Ts_control_point()[ku.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::epsilon(); + options.function_tolerance = 1e-2 * Sophus::Constants::epsilon(); + options.parameter_tolerance = 1e-2 * Sophus::Constants::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>(); + } + } + bool testAveraging(const size_t num_vertices, const double sigma_init, const double sigma_observation) { if (!num_vertices) return true; diff --git a/test/core/test_common.cpp b/test/core/test_common.cpp index 435bc59c6..f67176959 100644 --- a/test/core/test_common.cpp +++ b/test/core/test_common.cpp @@ -77,8 +77,8 @@ bool testSpline() { BasisSplineImpl spline(control_poses, 1.0); - SE3d T = spline.parent_T_spline(0.0, 1.0); - SE3d T2 = spline.parent_T_spline(1.0, 0.0); + SE3d T = spline.parent_T_spline(0, 1.0); + SE3d T2 = spline.parent_T_spline(1, 0.0); Eigen::Matrix3d R = T.so3().matrix(); Eigen::Matrix3d R2 = T2.so3().matrix(); @@ -89,19 +89,19 @@ bool testSpline() { SOPHUS_TEST_APPROX(passed, t, t2, kSmallEpsSqrt, "lambdsa"); - Eigen::Matrix4d Dt_parent_T_spline = spline.Dt_parent_T_spline(0.0, 0.5); + Eigen::Matrix4d Dt_parent_T_spline = spline.Dt_parent_T_spline(0, 0.5); Eigen::Matrix4d Dt_parent_T_spline2 = curveNumDiff( [&](double u_bar) -> Eigen::Matrix4d { - return spline.parent_T_spline(0.0, u_bar).matrix(); + return spline.parent_T_spline(0, u_bar).matrix(); }, 0.5); SOPHUS_TEST_APPROX(passed, Dt_parent_T_spline, Dt_parent_T_spline2, kSmallEpsSqrt, "Dt_parent_T_spline"); - Eigen::Matrix4d Dt2_parent_T_spline = spline.Dt2_parent_T_spline(0.0, 0.5); + Eigen::Matrix4d Dt2_parent_T_spline = spline.Dt2_parent_T_spline(0, 0.5); Eigen::Matrix4d Dt2_parent_T_spline2 = curveNumDiff( [&](double u_bar) -> Eigen::Matrix4d { - return spline.Dt_parent_T_spline(0.0, u_bar).matrix(); + return spline.Dt_parent_T_spline(0, u_bar).matrix(); }, 0.5); SOPHUS_TEST_APPROX(passed, Dt2_parent_T_spline, Dt2_parent_T_spline2,