diff --git a/CMakeLists.txt b/CMakeLists.txt index c4f31852..dea74ded 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/sophus/spline.hpp b/sophus/spline.hpp index 6d5fd101..c5c5ea38 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/sophus/spline_error.hpp b/sophus/spline_error.hpp new file mode 100644 index 00000000..20414b86 --- /dev/null +++ b/sophus/spline_error.hpp @@ -0,0 +1,903 @@ +#pragma once + + +#include "ceres/ceres.h" + +#include +#include + +namespace Sophus { + + template class LieGroup_> + struct SplineErrorSupport { + typedef enum { + SPLINE_P0=0, + SPLINE_P1=1, + SPLINE_P2=2, + SPLINE_P3=3, + SPLINE_Q0=4, + SPLINE_Q1=5, + SPLINE_Q2=6, + SPLINE_Q3=7, + } SplineParameterId; + + template + using LieGroup = LieGroup_; + using LieGroupd = LieGroup; + using Splined = Sophus::BasisSpline; + static int constexpr num_parameters = LieGroupd::num_parameters; + + template + struct SplineErrorWrapper0 { + static int constexpr num_residuals = num_residuals_; + // using SplineErrorWrapper::call; + SplineErrorWrapper0(Sophus::SegmentCase scase, double u, + std::shared_ptr soph, const std::vector & pmap) : + pmap(pmap), segment_case(scase), derivative_order(0), u(u), delta_t(0.0), soph(soph) { + } + + SplineErrorWrapper0(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, + std::shared_ptr soph, const std::vector & pmap) : + pmap(pmap), segment_case(scase), derivative_order(derivative_order), u(u), delta_t(delta_t), soph(soph) { + } + + void check_map(unsigned char maxval) const { + for (auto x : this->pmap) { + SOPHUS_ENSURE(x <= maxval, "but %", x); + } + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, + T* residuals) const { + const T * P[] = {P0,P1,P2}; + //alias + const std::vector & pmap = this->pmap; + this->check_map(3); + return this->template call(P[pmap[SPLINE_P0]], + P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]], + P[pmap[SPLINE_P3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3}; + //alias + const std::vector & pmap = this->pmap; + this->check_map(4); + return this->template call(P[pmap[SPLINE_P0]], + P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]], + P[pmap[SPLINE_P3]],residuals); + } + + + template + bool call(const T2* const P0, const T2* const P1, const T2* const P2, const T2* const P3, + T* residuals) const { + + using LGT2 = LieGroup_; + using dLGT2 = typename LieGroup_::Transformation; + // Mapper class is only used to facciliate difference between + // SO2 (which uses Scalar as tangent vector type) and other groups + // (which use Vector<...> as tangent vector type). + using Mapper = Mapper::Tangent>; + Sophus::BasisSplineSegment s(this->segment_case,P0,P1,P2,P3); + if (derivative_order==0) { + LGT2 Tt = s.parent_T_spline(this->u); + return this->soph->operator()(Tt.data(),residuals); + } else if (derivative_order==1) { + dLGT2 Tt = s.Dt_parent_T_spline(this->u,this->delta_t); + T2 tangent_data[LGT2::DoF]; + typename Mapper::Map v = Mapper::map(tangent_data); + v = LGT2::vee(Tt); + return this->soph->operator()(tangent_data,residuals); + } else if (derivative_order==2) { + dLGT2 Tt = s.Dt2_parent_T_spline(this->u,this->delta_t); + T2 tangent_data[LGT2::DoF]; + typename Mapper::Map v = Mapper::map(tangent_data); + v = LGT2::vee(Tt); + return this->soph->operator()(tangent_data,residuals); + } else { + assert(derivative_order < 3); + } + return false; + } + + std::vector pmap; + Sophus::SegmentCase segment_case; + unsigned int derivative_order; + double u, delta_t; + std::shared_ptr soph; + + }; + + template + struct SplineError2FunctionsWrapper0 { + static int constexpr num_residuals = num_residuals_; + SplineError2FunctionsWrapper0(Sophus::SegmentCase scase1, double u1, Sophus::SegmentCase scase2, double u2, + std::shared_ptr soph, + const std::vector & pmap) : + pmap(pmap),segment_case1(scase1),segment_case2(scase2), + derivative_order(0), u1(u1), u2(u2), delta_t1(0.0), delta_t2(0.0), soph(soph) { + } + + SplineError2FunctionsWrapper0(unsigned int derivative_order, + Sophus::SegmentCase scase1, double u1, double delta_t1, + Sophus::SegmentCase scase2, double u2, double delta_t2, + double delta_t, + std::shared_ptr soph, + const std::vector & pmap) : + pmap(pmap), segment_case1(scase1), segment_case2(scase2), + derivative_order(derivative_order), u1(u1), u2(u2), delta_t1(delta_t1), delta_t2(delta_t2), soph(soph) { + } + + void check_map(unsigned char maxval) const { + for (auto x : pmap) { + SOPHUS_ENSURE(x <= maxval, "but %", x); + } + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5}; + this->check_map(6); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, const T* P6, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5,P6}; + this->check_map(7); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, const T* P6, const T* P7, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5,P6,P7}; + this->check_map(8); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + + template + bool call(const T1* const P0, const T1* const P1, const T1* const P2, const T1* const P3, + const T2* const Q0, const T2* const Q1, const T2* const Q2, const T2* const Q3, + T* residuals) const { + + using LGT1 = LieGroup_; + using dLGT1 = typename LieGroup_::Transformation; + // Mapper class is only used to facciliate difference between + // SO2 (which uses Scalar as tangent vector type) and other groups + // (which use Vector<...> as tangent vector type). + using Mapper1 = Mapper::Tangent>; + using LGT2 = LieGroup_; + using dLGT2 = typename LieGroup_::Transformation; + using Mapper2 = Mapper::Tangent>; + Sophus::BasisSplineSegment s1(this->segment_case1,P0,P1,P2,P3); + Sophus::BasisSplineSegment s2(this->segment_case2,Q0,Q1,Q2,Q3); + if (derivative_order==0) { + LGT1 t1 = s1.parent_T_spline(this->u1); + LGT2 t2 = s2.parent_T_spline(this->u2); + return this->soph->operator()(t1.data(),t2.data(),residuals); + } else if (derivative_order==1) { + dLGT1 t1 = s1.Dt_parent_T_spline(this->u1,this->delta_t1); + dLGT2 t2 = s2.Dt_parent_T_spline(this->u2,this->delta_t2); + T1 tangent_data1[LGT1::DoF]; + typename Mapper1::Map v1 = Mapper1::map(tangent_data1); + v1 = LGT2::vee(t1); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + return this->soph->operator()(tangent_data1,tangent_data2,residuals); + v2 = LGT2::vee(t2); + } else if (derivative_order==2) { + dLGT1 t1 = s1.Dt2_parent_T_spline(this->u1,this->delta_t1); + dLGT2 t2 = s2.Dt2_parent_T_spline(this->u2,this->delta_t2); + T1 tangent_data1[LGT1::DoF]; + typename Mapper1::Map v1 = Mapper1::map(tangent_data1); + v1 = LGT2::vee(t1); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(t2); + return this->soph->operator()(tangent_data1,tangent_data2,residuals); + } else { + assert(derivative_order < 3); + } + return false; + } + + std::vector pmap; + Sophus::SegmentCase segment_case1, segment_case2; + unsigned int derivative_order; + double u1, u2, delta_t1, delta_t2; + std::shared_ptr soph; + + }; + + template + struct SplineError2PointsWrapper0 { + static int constexpr num_residuals = num_residuals_; + // using SplineErrorWrapper::call; + SplineError2PointsWrapper0(Sophus::SegmentCase scase1, double u1, Sophus::SegmentCase scase2, double u2, + std::shared_ptr soph, + const std::vector & pmap) : + pmap(pmap),segment_case1(scase1),segment_case2(scase2), + derivative_order(0), u1(u1), u2(u2), delta_t(0.0), soph(soph) { + } + + SplineError2PointsWrapper0(unsigned int derivative_order, + Sophus::SegmentCase scase1, double u1, + Sophus::SegmentCase scase2, double u2, + double delta_t, + std::shared_ptr soph, + const std::vector & pmap) : + pmap(pmap), segment_case1(scase1), segment_case2(scase2), + derivative_order(derivative_order), u1(u1), u2(u2), delta_t(delta_t), soph(soph) { + } + + void check_map(unsigned char maxval) const { + for (auto x : pmap) { + SOPHUS_ENSURE(x <= maxval, "but %", x); + } + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, + T* residuals) const { + const T * P[] = {P0,P1,P2}; + this->check_map(3); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3}; + this->check_map(4); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4}; + this->check_map(5); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5}; + this->check_map(6); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, const T* P6, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5,P6}; + this->check_map(7); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, const T* P6, const T* P7, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3,P4,P5,P6,P7}; + this->check_map(8); + //alias + const std::vector & pmap = this->pmap; + return this->template call(P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]],P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]], + P[pmap[SPLINE_Q0]],P[pmap[SPLINE_Q1]],P[pmap[SPLINE_Q2]],P[pmap[SPLINE_Q3]],residuals); + } + + + template + bool call(const T1* const P0, const T1* const P1, const T1* const P2, const T1* const P3, + const T2* const Q0, const T2* const Q1, const T2* const Q2, const T2* const Q3, + T* residuals) const { + + using LGT1 = LieGroup_; + using dLGT1 = typename LieGroup_::Transformation; + // Mapper class is only used to facciliate difference between + // SO2 (which uses Scalar as tangent vector type) and other groups + // (which use Vector<...> as tangent vector type). + using Mapper1 = Mapper::Tangent>; + using LGT2 = LieGroup_; + using dLGT2 = typename LieGroup_::Transformation; + using Mapper2 = Mapper::Tangent>; + Sophus::BasisSplineSegment s1(this->segment_case1,P0,P1,P2,P3); + Sophus::BasisSplineSegment s2(this->segment_case2,Q0,Q1,Q2,Q3); + if (derivative_order==0) { + LGT1 t1 = s1.parent_T_spline(this->u1); + LGT2 t2 = s2.parent_T_spline(this->u2); + return this->soph->operator()(t1.data(),t2.data(),residuals); + } else if (derivative_order==1) { + dLGT1 t1 = s1.Dt_parent_T_spline(this->u1,this->delta_t); + dLGT2 t2 = s2.Dt_parent_T_spline(this->u2,this->delta_t); + T1 tangent_data1[LGT1::DoF]; + typename Mapper1::Map v1 = Mapper1::map(tangent_data1); + v1 = LGT2::vee(t1); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + return this->soph->operator()(tangent_data1,tangent_data2,residuals); + v2 = LGT2::vee(t2); + } else if (derivative_order==2) { + dLGT1 t1 = s1.Dt2_parent_T_spline(this->u1,this->delta_t); + dLGT2 t2 = s2.Dt2_parent_T_spline(this->u2,this->delta_t); + T1 tangent_data1[LGT1::DoF]; + typename Mapper1::Map v1 = Mapper1::map(tangent_data1); + v1 = LGT2::vee(t1); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(t2); + return this->soph->operator()(tangent_data1,tangent_data2,residuals); + } else { + assert(derivative_order < 3); + } + return false; + } + + std::vector pmap; + Sophus::SegmentCase segment_case1, segment_case2; + unsigned int derivative_order; + double u1, u2, delta_t; + std::shared_ptr soph; + + }; + + template + struct SplineErrorWrapper1 : public SplineErrorWrapper0 { + SplineErrorWrapper1(Sophus::SegmentCase scase, double u, std::shared_ptr soph, const std::vector & pmap) : + SplineErrorWrapper0(scase,u,soph,pmap) {} + + SplineErrorWrapper1(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, std::shared_ptr soph, const std::vector & pmap) : + SplineErrorWrapper0(derivative_order,scase,u,delta_t,soph,pmap) {} + + + template + bool operator()(const T* const C0, + const T* P0, const T* P1, const T* P2, + T* residuals) const { + const T * P[] = {P0,P1,P2}; + const std::vector & pmap = this->pmap; + this->check_map(3); + return this->template call(C0, P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]],residuals); + } + + template + bool operator()(const T* const C0, + const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3}; + const std::vector & pmap = this->pmap; + this->check_map(4); + return this->template call(C0, P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]],residuals); + } + + + template + bool call(const T1* const C0, + const T2* const P0, const T2* const P1, const T2* const P2, const T2* const P3, + T* residuals) const { + + using LGT2 = LieGroup_; + using dLGT2 = typename LieGroup_::Transformation; + // Mapper class is only used to facciliate difference between + // SO2 (which uses Scalar as tangent vector type) and other groups + // (which use Vector<...> as tangent vector type). + using Mapper2 = Mapper::Tangent>; + Sophus::BasisSplineSegment s(this->segment_case,P0,P1,P2,P3); + if (this->derivative_order==0) { + LGT2 Tt = s.parent_T_spline(this->u); + return this->soph->operator()(C0,Tt.data(),residuals); + } else if (this->derivative_order==1) { + dLGT2 Tt = s.Dt_parent_T_spline(this->u,this->delta_t); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(Tt); + return this->soph->operator()(C0,tangent_data2,residuals); + } else if (this->derivative_order==2) { + dLGT2 Tt = s.Dt2_parent_T_spline(this->u,this->delta_t); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(Tt); + return this->soph->operator()(C0,tangent_data2,residuals); + } else { + assert(this->derivative_order < 3); + } + return false; + } + + }; + + + template + struct SplineErrorWrapper2 : public SplineErrorWrapper0 { + + SplineErrorWrapper2(Sophus::SegmentCase scase, double u, std::shared_ptr soph, const std::vector & pmap) : + SplineErrorWrapper0(scase, u, soph,pmap) {} + + SplineErrorWrapper2(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, std::shared_ptr soph, const std::vector & pmap) : + SplineErrorWrapper0(derivative_order, scase, u, delta_t, soph, pmap) {} + + + + template + bool operator()(const T* const C0,const T* const C1, + const T* P0, const T* P1, const T* P2, + T* residuals) const { + const T * P[] = {P0,P1,P2}; + const std::vector & pmap = this->pmap; + this->check_map(3); + return this->template call(C0, C1, P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]],residuals); + } + + template + bool operator()(const T* const C0,const T* const C1, + const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + const T * P[] = {P0,P1,P2,P3}; + const std::vector & pmap = this->pmap; + this->check_map(4); + return this->template call(C0, C1, P[pmap[SPLINE_P0]],P[pmap[SPLINE_P1]], + P[pmap[SPLINE_P2]],P[pmap[SPLINE_P3]],residuals); + } + + template + bool call(const T1* const C0, const T1* const C1, + const T2* const P0, const T2* const P1, const T2* const P2, const T2* const P3, + T* residuals) const { + + using LGT2 = LieGroup_; + using dLGT2 = typename LieGroup_::Transformation; + // Mapper class is only used to facciliate difference between + // SO2 (which uses Scalar as tangent vector type) and other groups + // (which use Vector<...> as tangent vector type). + using Mapper2 = Mapper::Tangent>; + Sophus::BasisSplineSegment s(this->segment_case,P0,P1,P2,P3); + if (this->derivative_order==0) { + LGT2 Tt = s.parent_T_spline(this->u); + return this->soph->operator()(C0,C1,Tt.data(),residuals); + } else if (this->derivative_order==1) { + dLGT2 Tt = s.Dt_parent_T_spline(this->u,this->delta_t); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(Tt); + return this->soph->operator()(C0,C1,tangent_data2,residuals); + } else if (this->derivative_order==2) { + dLGT2 Tt = s.Dt2_parent_T_spline(this->u,this->delta_t); + T2 tangent_data2[LGT2::DoF]; + typename Mapper2::Map v2 = Mapper2::map(tangent_data2); + v2 = LGT2::vee(Tt); + return this->soph->operator()(C0,C1,tangent_data2,residuals); + } else { + assert(this->derivative_order < 3); + } + return false; + } + + }; + + + + ////////////////////////////////////////////////////////////////////////////////////////////// + // + // Helper functions to insert residual blocks using splines + + + + // Add an autodiff'ed residual function to a problem defined on a spline. The residual functor is expected to take as + // as arguments a class par1, another class par2 and to be estimated at the spline position t on an LieGroup_ class + // + template + static bool addResidualFunction2(ceres::Problem &problem, + ParamClass1 & par1, ParamClass2 & par2, + unsigned int derivative_order, double t, double delta_t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + using Wrapper = SplineErrorWrapper2; + KnotsAndU ku = spline->knots_and_u(t); + std::vector map(4,255); + std::map> pmap; + pmap[spline->parent_Ts_control_point()[ku.idx_prev].data()].push_back(SPLINE_P0); + pmap[spline->parent_Ts_control_point()[ku.idx_0].data()].push_back(SPLINE_P1); + pmap[spline->parent_Ts_control_point()[ku.idx_1].data()].push_back(SPLINE_P2); + pmap[spline->parent_Ts_control_point()[ku.idx_2].data()].push_back(SPLINE_P3); + std::vector parameter_blocks; + parameter_blocks.push_back(par1.data()); + parameter_blocks.push_back(par2.data()); + for (auto it : pmap) { + for (unsigned char x : it.second) { + map[x] = parameter_blocks.size()-2; + } + parameter_blocks.push_back(it.first); + } + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor,pmap); + ceres::CostFunction *cost_function = NULL; + switch (parameter_blocks.size()) { + case 5: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 6: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + default: + assert((parameter_blocks.size()>=5) && (parameter_blocks.size()<=6)); + } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); + return true; + } + + template + static bool addResidualFunction1(ceres::Problem &problem, ParamClass1 & par1, + unsigned int derivative_order, double t, double delta_t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + using Wrapper = SplineErrorWrapper1; + KnotsAndU ku = spline->knots_and_u(t); + std::vector map(4,255); + std::map> pmap; + pmap[spline->parent_Ts_control_point()[ku.idx_prev].data()].push_back(SPLINE_P0); + pmap[spline->parent_Ts_control_point()[ku.idx_0].data()].push_back(SPLINE_P1); + pmap[spline->parent_Ts_control_point()[ku.idx_1].data()].push_back(SPLINE_P2); + pmap[spline->parent_Ts_control_point()[ku.idx_2].data()].push_back(SPLINE_P3); + std::vector parameter_blocks; + parameter_blocks.push_back(par1.data()); + for (auto it : pmap) { + for (unsigned char x : it.second) { + map[x] = parameter_blocks.size()-1; + } + parameter_blocks.push_back(it.first); + } + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor, map); + ceres::CostFunction *cost_function = NULL; + switch (parameter_blocks.size()) { + case 4: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 5: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + default: + assert((parameter_blocks.size()>=4) && (parameter_blocks.size()<=5)); + } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); + return true; + } + + // Add an autodiff'ed residual function to a problem defined on a spline. + // derivate order is the derivate order of the spline evaluate (0, 1 or 2). + // The residual functor is assumed to be defined as + // + // class Functor { + // public: + // Functor() {} + // template + // bool operator()(T const * const P, T* residuals) const { + // ... + // return true; + // } + // }; + // + // Depending on derivative_order P will either be the representation of + // a LieGroup class (Sophus::SO3d().data()) or its tangent space for + // order larger than 0. + // + // The functor will be evaluated at t. delta_t is used as an argument + // to the spline derivative for order larger than 0. + // + // The following functions are provided: + // - addResidualFunction0(problem, derivative_order, t, delta_t, + // spline, functor, loss_function): + // functor defined as above. + // - addResidualFunction0(problem,t,spline,functor,loss_function): + // equivalent to the previous one, with derivative_order=0. + // + // - addResidualFunction1(problem, par1, derivative_order, t, delta_t, + // spline, functor, loss_function): + // functor defined with: + // template + // bool operator()(T1 const * const C1, + // T const * const P, T* residuals) const { ... } + // where C1 is the representation of par1 (par1.data()), which can + // can be a parameter or calibration class. + // - addResidualFunction1(problem,par1,t,spline,functor,loss_function): + // equivalent to the previous one, with derivative_order=0. + // + // - addResidualFunction1(problem, par1, derivative_order, t, delta_t, + // spline, functor, loss_function): + // functor defined with: + // template + // bool operator()(T1 const * const C1, T2 const * const C2, + // T const * const P, T* residuals) const { ... } + // where C1 is the representation of par1 (par1.data()), which can + // can be a parameter or calibration class, and C2 is similarly + // another calibration parameter. + // - addResidualFunction1(problem,par1,t,spline,functor,loss_function): + // equivalent to the previous one, with derivative_order=0. + // + // - addResidualFunction2Points0(problem, derivative_order, t1, t2, + // delta_t, spline, functor, loss_function): + // functor defined with: + // template + // bool operator()(T const * const P, + // T const * const Q, T* residuals) const { ... } + // where P and Q are two lie-group representations, sampled on + // the spline at t1 and t2. + // - addResidualFunction2Points0(problem,t1,t2,spline,functor,loss_function): + // equivalent to the previous one, with derivative_order=0. + // + // - addResidual2Functions0(problem, derivative_order, delta_t, + // t1, spline1, t2, spline2, functor, loss_function): + // functor defined with: + // template + // bool operator()(T const * const P, + // T const * const Q, T* residuals) const { ... } + // where P and Q are two lie-group representations, sampled on + // the spline1 at t1 and spline2 at t2. + // - addResidualFunction2Points0(problem,t1,spline1,t2,spline2,functor,loss_function): + // equivalent to the previous one, with derivative_order=0. + // + // + template + static bool addResidualFunction0(ceres::Problem &problem, + unsigned int derivative_order, double t, double delta_t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + using Wrapper = SplineErrorWrapper0; + KnotsAndU ku = spline->knots_and_u(t); + std::vector map(4,255); + std::map> pmap; + pmap[spline->parent_Ts_control_point()[ku.idx_prev].data()].push_back(SPLINE_P0); + pmap[spline->parent_Ts_control_point()[ku.idx_0].data()].push_back(SPLINE_P1); + pmap[spline->parent_Ts_control_point()[ku.idx_1].data()].push_back(SPLINE_P2); + pmap[spline->parent_Ts_control_point()[ku.idx_2].data()].push_back(SPLINE_P3); + std::vector parameter_blocks; + for (auto it : pmap) { + for (unsigned char x : it.second) { + map[x] = parameter_blocks.size(); + } + parameter_blocks.push_back(it.first); + } + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor, map); + ceres::CostFunction *cost_function = NULL; + switch (parameter_blocks.size()) { + case 3: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 4: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + default: + assert((parameter_blocks.size()>=3) && (parameter_blocks.size()<=4)); + } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); + return true; + } + + template + static bool addResidualFunction2Points0(ceres::Problem &problem, + unsigned int derivative_order, double t1, double t2, double delta_t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + using Wrapper = SplineError2PointsWrapper0; + KnotsAndU ku1 = spline->knots_and_u(t1); + KnotsAndU ku2 = spline->knots_and_u(t2); + std::vector map(8,255); + std::map> pmap; + pmap[spline->parent_Ts_control_point()[ku1.idx_prev].data()].push_back(SPLINE_P0); + pmap[spline->parent_Ts_control_point()[ku1.idx_0].data()].push_back(SPLINE_P1); + pmap[spline->parent_Ts_control_point()[ku1.idx_1].data()].push_back(SPLINE_P2); + pmap[spline->parent_Ts_control_point()[ku1.idx_2].data()].push_back(SPLINE_P3); + pmap[spline->parent_Ts_control_point()[ku2.idx_prev].data()].push_back(SPLINE_Q0); + pmap[spline->parent_Ts_control_point()[ku2.idx_0].data()].push_back(SPLINE_Q1); + pmap[spline->parent_Ts_control_point()[ku2.idx_1].data()].push_back(SPLINE_Q2); + pmap[spline->parent_Ts_control_point()[ku2.idx_2].data()].push_back(SPLINE_Q3); + std::vector parameter_blocks; + for (auto it : pmap) { + for (unsigned char x : it.second) { + map[x] = parameter_blocks.size(); + } + parameter_blocks.push_back(it.first); + } + Wrapper * ew = new Wrapper(derivative_order, + ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, delta_t, functor, pmap); + ceres::CostFunction *cost_function = NULL; + switch (parameter_blocks.size()) { + case 3: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 4: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 5: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 6: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 7: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 8: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + default: + assert((parameter_blocks.size()>=3) && (parameter_blocks.size()<=8)); + } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); + + return true; + } + + template + static bool addResidual2Functions0(ceres::Problem &problem, + unsigned int derivative_order, + double delta_t1, double t1, std::shared_ptr spline1, + double delta_t2, double t2, std::shared_ptr spline2, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + using Wrapper = SplineError2FunctionsWrapper0; + KnotsAndU ku1 = spline1->knots_and_u(t1); + KnotsAndU ku2 = spline2->knots_and_u(t2); + std::vector map(8,255); + std::map> pmap; + pmap[spline1->parent_Ts_control_point()[ku1.idx_prev].data()].push_back(SPLINE_P0); + pmap[spline1->parent_Ts_control_point()[ku1.idx_0].data()].push_back(SPLINE_P1); + pmap[spline1->parent_Ts_control_point()[ku1.idx_1].data()].push_back(SPLINE_P2); + pmap[spline1->parent_Ts_control_point()[ku1.idx_2].data()].push_back(SPLINE_P3); + pmap[spline2->parent_Ts_control_point()[ku2.idx_prev].data()].push_back(SPLINE_Q0); + pmap[spline2->parent_Ts_control_point()[ku2.idx_0].data()].push_back(SPLINE_Q1); + pmap[spline2->parent_Ts_control_point()[ku2.idx_1].data()].push_back(SPLINE_Q2); + pmap[spline2->parent_Ts_control_point()[ku2.idx_2].data()].push_back(SPLINE_Q3); + std::vector parameter_blocks; + for (auto it : pmap) { + for (unsigned char x : it.second) { + map[x] = parameter_blocks.size(); + } + parameter_blocks.push_back(it.first); + } + Wrapper * ew = new Wrapper(derivative_order, + ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, + delta_t1, delta_t2, functor, pmap); + ceres::CostFunction *cost_function = NULL; + switch (parameter_blocks.size()) { + case 6: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 7: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + case 8: + cost_function = new ceres::AutoDiffCostFunction (ew); + break; + default: + assert((parameter_blocks.size()>=6) && (parameter_blocks.size()<=8)); + } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); + + return true; + } + + template + static bool addResidualFunction2(ceres::Problem &problem, + ParamClass1 & par1, ParamClass2 & par2, + double t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidualFunction2(problem, par1, par2, 0, t, 0.0, spline, functor, loss_function); + } + + template + static bool addResidualFunction1(ceres::Problem &problem, ParamClass1 & par1, + double t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidualFunction1(problem, par1, 0, t, 0.0, spline, functor, loss_function); + } + + template + static bool addResidualFunction0(ceres::Problem &problem, + double t, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidualFunction0(problem, 0, t, 0.0, spline, functor, loss_function); + } + + template + static bool addResidualFunction2Points0(ceres::Problem &problem, + double t1, double t2, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidualFunction2Points0(problem, 0, t1, t2, 0.0, spline, functor, loss_function); + } + + template + static bool addResidual2Functions0(ceres::Problem &problem, + double t1, std::shared_ptr spline1, + double t2, std::shared_ptr spline2, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidual2Functions0(problem, 0, + 0.0, t1, spline1, 0.0, t2, spline2, functor, loss_function); + } + }; + +} + diff --git a/test/ceres/CMakeLists.txt b/test/ceres/CMakeLists.txt index b5851bbf..e4adc18b 100644 --- a/test/ceres/CMakeLists.txt +++ b/test/ceres/CMakeLists.txt @@ -6,7 +6,7 @@ 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() diff --git a/test/ceres/ceres_flags.hpp b/test/ceres/ceres_flags.hpp new file mode 100644 index 00000000..8eef5d5a --- /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/tests.hpp b/test/ceres/tests.hpp index 63bd4ab3..8e0ecdcb 100644 --- a/test/ceres/tests.hpp +++ b/test/ceres/tests.hpp @@ -3,7 +3,10 @@ #include #include +#include +#include #include +#include "ceres_flags.hpp" template struct RotationalPart; @@ -172,6 +175,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) { @@ -186,9 +251,208 @@ struct LieGroupCeresTests { passed &= testAveraging(N, .5, .1); processTestResult(passed); } + std::cerr << "Spline test: "; + passed &= testSpline() != nullptr; + processTestResult(passed); + + std::cerr << "SplineError test: "; + passed &= testSplineError() != 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>(); + } + } + + std::shared_ptr> testSplineError(int n_knots=-1) { + using SES = SplineErrorSupport; + using Splined = typename SES::Splined; + using Functor = TestLieGroupCostFunctor; + 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 Splined(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; + LieGroupd pred = spline->parent_T_spline(t); + LieGroupd err = group_vec[i].inverse() * pred; + initial_error += squaredNorm(err.log()); + + std::shared_ptr functor(new Functor(group_vec[i])); + SES::template addResidualFunction0(problem,t,spline,functor); + } + + 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;