From e34cc9f38f03f368e9b9f0888633a0c0b536ea02 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Sat, 23 Aug 2025 18:00:48 +0200 Subject: [PATCH 1/5] Added Knot and U struct and accessor to the spline class --- sophus/spline.hpp | 85 +++++++++++++++++++++++++---------------------- 1 file changed, 46 insertions(+), 39 deletions(-) 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_; From 8ff31eb35fee1059a3c0fca0b14b56752606be7f Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Sun, 24 Aug 2025 19:28:36 +0200 Subject: [PATCH 2/5] Prepared PR for the integration of the spline error wrapper --- sophus/spline_error.hpp | 780 +++++++++++++++++++++++++++++++++++++ test/ceres/CMakeLists.txt | 2 +- test/ceres/ceres_flags.hpp | 35 ++ test/ceres/tests.hpp | 264 +++++++++++++ 4 files changed, 1080 insertions(+), 1 deletion(-) create mode 100644 sophus/spline_error.hpp create mode 100644 test/ceres/ceres_flags.hpp diff --git a/sophus/spline_error.hpp b/sophus/spline_error.hpp new file mode 100644 index 000000000..558ec665a --- /dev/null +++ b/sophus/spline_error.hpp @@ -0,0 +1,780 @@ +#pragma once + + +#include "ceres/ceres.h" + +#include +#include + +namespace Sophus { + + template class LieGroup_> + struct SplineErrorSupport { + 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) : + 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) : + segment_case(scase), derivative_order(derivative_order), u(u), delta_t(delta_t), soph(soph) { + } + + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + switch (this->segment_case) { + case Sophus::SegmentCase::normal: + return this->template call(P0,P1,P2,P3,residuals); + case Sophus::SegmentCase::first: + case Sophus::SegmentCase::last: + assert(this->segment_case == Sophus::SegmentCase::normal); + return false; + } + return false; + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, + T* residuals) const { + switch (this->segment_case) { + case Sophus::SegmentCase::first: + return this->template call(P0,P0,P1,P2,residuals); + case Sophus::SegmentCase::last: + return this->template call(P0,P1,P2,P2,residuals); + case Sophus::SegmentCase::normal: + assert(this->segment_case != Sophus::SegmentCase::normal); + return false; + } + return false; + } + + 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; + } + + Sophus::SegmentCase segment_case; + unsigned int derivative_order; + double u, delta_t; + 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) : + 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) : + segment_case1(scase1), segment_case2(scase2), + derivative_order(derivative_order), u1(u1), u2(u2), delta_t(delta_t), soph(soph) { + } + + + template + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, + const T* Q0, const T* Q1, const T* Q2, const T* Q3, + T* residuals) const { + if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { + return this->template call(P0,P1,P2,P3, Q0,Q1,Q2,Q3, residuals); + } else { + assert(this->segment_case1==Sophus::SegmentCase::normal); + assert(this->segment_case2==Sophus::SegmentCase::normal); + return false; + } + return false; + } + + 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 { + if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { + // One segment must not be normal, but only one + assert((this->segment_case1 != Sophus::SegmentCase::normal) + ^ (this->segment_case2 != Sophus::SegmentCase::normal)); + return false; + } else if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::first)) { + return this->template call(P0,P1,P2,P3, + P4,P4,P5,P6, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::last)) { + return this->template call(P0,P1,P2,P3, + P4,P5,P6,P6, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::normal)) { + return this->template call(P0,P0,P1,P2, + P3,P4,P5,P6, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::normal)) { + return this->template call(P0,P1,P2,P2, + P3,P4,P5,P6, residuals); + } else { + // One segment must not be normal, but only one + assert((this->segment_case1 != Sophus::SegmentCase::normal) + ^ (this->segment_case2 != Sophus::SegmentCase::normal)); + return false; + } + return false; + } + + template + bool operator()(const T* P0, const T* P1, const T* P2, + const T* P3, const T* P4, const T* P5, + T* residuals) const { + if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { + // Both segments must be extremes + assert((this->segment_case1 != Sophus::SegmentCase::normal) + && (this->segment_case2 != Sophus::SegmentCase::normal)); + return false; + } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::first)) { + return this->template call(P0,P0,P1,P2, + P3,P3,P4,P5, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::last)) { + return this->template call(P0,P0,P1,P2, + P3,P4,P5,P5, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::first)) { + return this->template call(P0,P1,P2,P2, + P3,P3,P4,P5, residuals); + } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::last)) { + return this->template call(P0,P1,P2,P2, + P3,P4,P5,P5, residuals); + } else { + // Both segments must be extremes + assert((this->segment_case1 != Sophus::SegmentCase::normal) + && (this->segment_case2 != Sophus::SegmentCase::normal)); + return false; + } + return false; + } + + 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; + } + + 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) : + SplineErrorWrapper0(scase,u,soph) {} + + SplineErrorWrapper1(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, std::shared_ptr soph) : + SplineErrorWrapper0(derivative_order,scase,u,delta_t,soph) {} + + + template + bool operator()(const T* const C0, + const T* P0, const T* P1, const T* P2, const T* P3, + T* residuals) const { + switch (this->segment_case) { + case Sophus::SegmentCase::normal: + return this->call(C0,P0,P1,P2,P3,residuals); + case Sophus::SegmentCase::first: + case Sophus::SegmentCase::last: + assert(this->segment_case == Sophus::SegmentCase::normal); + return false; + } + return false; + } + + template + bool operator()(const T* const C0, + const T* P0, const T* P1, const T* P2, + T* residuals) const { + switch (this->segment_case) { + case Sophus::SegmentCase::first: + return this->call(C0,P0,P0,P1,P2,residuals); + case Sophus::SegmentCase::last: + return this->call(C0,P0,P1,P2,P2,residuals); + case Sophus::SegmentCase::normal: + assert(this->segment_case != Sophus::SegmentCase::normal); + return false; + } + return false; + } + + 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) : + SplineErrorWrapper0(scase, u, soph) {} + + SplineErrorWrapper2(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, std::shared_ptr soph) : + SplineErrorWrapper0(derivative_order, scase, u, delta_t, soph) {} + + + 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 { + switch (this->segment_case) { + case Sophus::SegmentCase::normal: + return this->call(C0,C1,P0,P1,P2,P3,residuals); + case Sophus::SegmentCase::first: + case Sophus::SegmentCase::last: + assert(this->segment_case == Sophus::SegmentCase::normal); + return false; + } + return false; + } + + template + bool operator()(const T* const C0, const T* const C1, + const T* P0, const T* P1, const T* P2, + T* residuals) const { + switch (this->segment_case) { + case Sophus::SegmentCase::first: + return this->call(C0,C1,P0,P0,P1,P2,residuals); + case Sophus::SegmentCase::last: + return this->call(C0,C1,P0,P1,P2,P2,residuals); + case Sophus::SegmentCase::normal: + assert(this->segment_case != Sophus::SegmentCase::normal); + return false; + } + return false; + } + + 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); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + ceres::CostFunction *cost_function = NULL; + switch (ku.segment_case) { + case Sophus::SegmentCase::first: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.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 Sophus::SegmentCase::normal: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.data(), + 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 Sophus::SegmentCase::last: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.data(), + 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; + } + 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); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + ceres::CostFunction *cost_function = NULL; + switch (ku.segment_case) { + case Sophus::SegmentCase::first: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.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 Sophus::SegmentCase::normal: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.data(), + 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 Sophus::SegmentCase::last: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function,par1.data(), + 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; + } + 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. + // + // + 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); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + ceres::CostFunction *cost_function = NULL; + switch (ku.segment_case) { + case Sophus::SegmentCase::first: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + 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 Sophus::SegmentCase::normal: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + 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 Sophus::SegmentCase::last: + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + 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; + } + return true; + } + + template + static bool addResidualFunction2Point0(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); + Wrapper * ew = new Wrapper(derivative_order, + ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, delta_t, soph); + ceres::CostFunction *cost_function = NULL; + if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::first)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::normal)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::last)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::first)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_prev].data(), + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::normal)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_prev].data(), + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::last)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_prev].data(), + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::first)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_prev].data(), + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::normal)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku1.idx_2].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_2].data() + ); + } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::last)) { + cost_function = new ceres::AutoDiffCostFunction (ew); + problem.AddResidualBlock(cost_function, loss_function, + spline->parent_Ts_control_point()[ku1.idx_prev].data(), + spline->parent_Ts_control_point()[ku1.idx_0].data(), + spline->parent_Ts_control_point()[ku1.idx_1].data(), + spline->parent_Ts_control_point()[ku2.idx_prev].data(), + spline->parent_Ts_control_point()[ku2.idx_0].data(), + spline->parent_Ts_control_point()[ku2.idx_1].data() + ); + } + + 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 addResidualFunction2Point0(ceres::Problem &problem, + double t1, double t2, + std::shared_ptr spline, + std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { + return addResidualFunction2Point0(problem, 0, t1, t2, 0.0, spline, functor, loss_function); + } + }; + +} + diff --git a/test/ceres/CMakeLists.txt b/test/ceres/CMakeLists.txt index b5851bbf3..e4adc18b0 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 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/tests.hpp b/test/ceres/tests.hpp index 63bd4ab3c..8e0ecdcbe 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; From 246159212bda6b356393b24cde27e573882c6683 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Sun, 24 Aug 2025 19:42:59 +0200 Subject: [PATCH 3/5] Fixed typo while linting --- sophus/spline_error.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sophus/spline_error.hpp b/sophus/spline_error.hpp index 558ec665a..462b378db 100644 --- a/sophus/spline_error.hpp +++ b/sophus/spline_error.hpp @@ -417,7 +417,7 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper2; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); ceres::CostFunction *cost_function = NULL; switch (ku.segment_case) { case Sophus::SegmentCase::first: @@ -461,7 +461,7 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper1; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); ceres::CostFunction *cost_function = NULL; switch (ku.segment_case) { case Sophus::SegmentCase::first: @@ -567,7 +567,7 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper0; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, soph); + Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); ceres::CostFunction *cost_function = NULL; switch (ku.segment_case) { case Sophus::SegmentCase::first: @@ -612,7 +612,7 @@ namespace Sophus { KnotsAndU ku1 = spline->knots_and_u(t1); KnotsAndU ku2 = spline->knots_and_u(t2); Wrapper * ew = new Wrapper(derivative_order, - ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, delta_t, soph); + ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, delta_t, functor); ceres::CostFunction *cost_function = NULL; if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::first)) { cost_function = new ceres::AutoDiffCostFunction Date: Sat, 30 Aug 2025 09:50:48 +0200 Subject: [PATCH 4/5] Fixed a conceptual mistake in spline functor wrapper. These were completely wrong for functor applied on two points on the spline because the previous code was not guaranteeing that unique parameter blocks were passed to the autodiff function. --- CMakeLists.txt | 2 +- sophus/spline_error.hpp | 631 ++++++++++++++++++---------------------- 2 files changed, 282 insertions(+), 351 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4f318527..dea74dedb 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_error.hpp b/sophus/spline_error.hpp index 462b378db..ed575197d 100644 --- a/sophus/spline_error.hpp +++ b/sophus/spline_error.hpp @@ -10,6 +10,17 @@ 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; @@ -21,45 +32,48 @@ namespace Sophus { static int constexpr num_residuals = num_residuals_; // using SplineErrorWrapper::call; SplineErrorWrapper0(Sophus::SegmentCase scase, double u, - std::shared_ptr soph) : - segment_case(scase), derivative_order(0), u(u), delta_t(0.0), soph(soph) { + 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) : - segment_case(scase), derivative_order(derivative_order), u(u), delta_t(delta_t), soph(soph) { + 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, const T* P3, + bool operator()(const T* P0, const T* P1, const T* P2, T* residuals) const { - switch (this->segment_case) { - case Sophus::SegmentCase::normal: - return this->template call(P0,P1,P2,P3,residuals); - case Sophus::SegmentCase::first: - case Sophus::SegmentCase::last: - assert(this->segment_case == Sophus::SegmentCase::normal); - return false; - } - return false; + 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, + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, T* residuals) const { - switch (this->segment_case) { - case Sophus::SegmentCase::first: - return this->template call(P0,P0,P1,P2,residuals); - case Sophus::SegmentCase::last: - return this->template call(P0,P1,P2,P2,residuals); - case Sophus::SegmentCase::normal: - assert(this->segment_case != Sophus::SegmentCase::normal); - return false; - } - return false; + 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 { @@ -92,6 +106,7 @@ namespace Sophus { return false; } + std::vector pmap; Sophus::SegmentCase segment_case; unsigned int derivative_order; double u, delta_t; @@ -104,8 +119,9 @@ namespace Sophus { 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) : - segment_case1(scase1),segment_case2(scase2), + 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) { } @@ -113,86 +129,85 @@ namespace Sophus { Sophus::SegmentCase scase1, double u1, Sophus::SegmentCase scase2, double u2, double delta_t, - std::shared_ptr soph) : - segment_case1(scase1), segment_case2(scase2), + 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, - const T* Q0, const T* Q1, const T* Q2, const T* Q3, T* residuals) const { - if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { - return this->template call(P0,P1,P2,P3, Q0,Q1,Q2,Q3, residuals); - } else { - assert(this->segment_case1==Sophus::SegmentCase::normal); - assert(this->segment_case2==Sophus::SegmentCase::normal); - return false; - } - return false; + 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, const T* P5, const T* P6, + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, T* residuals) const { - if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { - // One segment must not be normal, but only one - assert((this->segment_case1 != Sophus::SegmentCase::normal) - ^ (this->segment_case2 != Sophus::SegmentCase::normal)); - return false; - } else if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::first)) { - return this->template call(P0,P1,P2,P3, - P4,P4,P5,P6, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::last)) { - return this->template call(P0,P1,P2,P3, - P4,P5,P6,P6, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::normal)) { - return this->template call(P0,P0,P1,P2, - P3,P4,P5,P6, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::normal)) { - return this->template call(P0,P1,P2,P2, - P3,P4,P5,P6, residuals); - } else { - // One segment must not be normal, but only one - assert((this->segment_case1 != Sophus::SegmentCase::normal) - ^ (this->segment_case2 != Sophus::SegmentCase::normal)); - return false; - } - return false; + 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, + bool operator()(const T* P0, const T* P1, const T* P2, const T* P3, const T* P4, const T* P5, T* residuals) const { - if ((this->segment_case1==Sophus::SegmentCase::normal) && (this->segment_case2==Sophus::SegmentCase::normal)) { - // Both segments must be extremes - assert((this->segment_case1 != Sophus::SegmentCase::normal) - && (this->segment_case2 != Sophus::SegmentCase::normal)); - return false; - } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::first)) { - return this->template call(P0,P0,P1,P2, - P3,P3,P4,P5, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::first) && (this->segment_case2==Sophus::SegmentCase::last)) { - return this->template call(P0,P0,P1,P2, - P3,P4,P5,P5, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::first)) { - return this->template call(P0,P1,P2,P2, - P3,P3,P4,P5, residuals); - } else if ((this->segment_case1==Sophus::SegmentCase::last) && (this->segment_case2==Sophus::SegmentCase::last)) { - return this->template call(P0,P1,P2,P2, - P3,P4,P5,P5, residuals); - } else { - // Both segments must be extremes - assert((this->segment_case1 != Sophus::SegmentCase::normal) - && (this->segment_case2 != Sophus::SegmentCase::normal)); - return false; - } - return false; + 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, @@ -239,6 +254,7 @@ namespace Sophus { return false; } + std::vector pmap; Sophus::SegmentCase segment_case1, segment_case2; unsigned int derivative_order; double u1, u2, delta_t; @@ -248,44 +264,36 @@ namespace Sophus { template struct SplineErrorWrapper1 : public SplineErrorWrapper0 { - SplineErrorWrapper1(Sophus::SegmentCase scase, double u, std::shared_ptr soph) : - SplineErrorWrapper0(scase,u,soph) {} + 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) : - SplineErrorWrapper0(derivative_order,scase,u,delta_t,soph) {} + 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, const T* P3, + const T* P0, const T* P1, const T* P2, T* residuals) const { - switch (this->segment_case) { - case Sophus::SegmentCase::normal: - return this->call(C0,P0,P1,P2,P3,residuals); - case Sophus::SegmentCase::first: - case Sophus::SegmentCase::last: - assert(this->segment_case == Sophus::SegmentCase::normal); - return false; - } - return false; + 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* P0, const T* P1, const T* P2, const T* P3, T* residuals) const { - switch (this->segment_case) { - case Sophus::SegmentCase::first: - return this->call(C0,P0,P0,P1,P2,residuals); - case Sophus::SegmentCase::last: - return this->call(C0,P0,P1,P2,P2,residuals); - case Sophus::SegmentCase::normal: - assert(this->segment_case != Sophus::SegmentCase::normal); - return false; - } - return false; + 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, @@ -325,42 +333,34 @@ namespace Sophus { template struct SplineErrorWrapper2 : public SplineErrorWrapper0 { - SplineErrorWrapper2(Sophus::SegmentCase scase, double u, std::shared_ptr soph) : - SplineErrorWrapper0(scase, u, soph) {} + 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) {} - SplineErrorWrapper2(unsigned int derivative_order, Sophus::SegmentCase scase, double u, double delta_t, std::shared_ptr soph) : - SplineErrorWrapper0(derivative_order, scase, u, delta_t, soph) {} template - bool operator()(const T* const C0, const T* const C1, - const T* P0, const T* P1, const T* P2, const T* P3, + bool operator()(const T* const C0,const T* const C1, + const T* P0, const T* P1, const T* P2, T* residuals) const { - switch (this->segment_case) { - case Sophus::SegmentCase::normal: - return this->call(C0,C1,P0,P1,P2,P3,residuals); - case Sophus::SegmentCase::first: - case Sophus::SegmentCase::last: - assert(this->segment_case == Sophus::SegmentCase::normal); - return false; - } - return false; + 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, + 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 { - switch (this->segment_case) { - case Sophus::SegmentCase::first: - return this->call(C0,C1,P0,P0,P1,P2,residuals); - case Sophus::SegmentCase::last: - return this->call(C0,C1,P0,P1,P2,P2,residuals); - case Sophus::SegmentCase::normal: - assert(this->segment_case != Sophus::SegmentCase::normal); - return false; - } - return false; + 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 @@ -417,40 +417,41 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper2; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); + 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 (ku.segment_case) { - case Sophus::SegmentCase::first: - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.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 Sophus::SegmentCase::normal: - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.data(), - 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()); + switch (parameter_blocks.size()) { + case 5: + cost_function = new ceres::AutoDiffCostFunction (ew); break; - case Sophus::SegmentCase::last: - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.data(),par2.data(), - 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()); + 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; } @@ -461,39 +462,38 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper1; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); + 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 (ku.segment_case) { - case Sophus::SegmentCase::first: - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.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 Sophus::SegmentCase::normal: + switch (parameter_blocks.size()) { + case 4: cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.data(), - 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()); + num_residuals, ParamClass1::num_parameters, + LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters> (ew); break; - case Sophus::SegmentCase::last: + case 5: cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function,par1.data(), - 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()); + num_residuals, ParamClass1::num_parameters, + LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters> (ew); break; + default: + assert((parameter_blocks.size()>=4) && (parameter_blocks.size()<=5)); } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); return true; } @@ -567,39 +567,37 @@ namespace Sophus { std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { using Wrapper = SplineErrorWrapper0; KnotsAndU ku = spline->knots_and_u(t); - Wrapper * ew = new Wrapper(derivative_order, ku.segment_case,ku.u, delta_t, functor); + 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 (ku.segment_case) { - case Sophus::SegmentCase::first: - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - 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 Sophus::SegmentCase::normal: + switch (parameter_blocks.size()) { + case 3: cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - 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()); + num_residuals, + LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters> (ew); break; - case Sophus::SegmentCase::last: + case 4: cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - 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()); + num_residuals, + LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters, LieGroupd::num_parameters> (ew); break; + default: + assert((parameter_blocks.size()>=3) && (parameter_blocks.size()<=4)); } + + problem.AddResidualBlock(cost_function, loss_function, parameter_blocks); return true; } @@ -611,134 +609,67 @@ namespace Sophus { 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); + ku1.segment_case,ku1.u, ku2.segment_case,ku2.u, delta_t, functor, pmap); ceres::CostFunction *cost_function = NULL; - if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::first)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::normal)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::first) && (ku2.segment_case==Sophus::SegmentCase::last)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::first)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_prev].data(), - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::normal)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_prev].data(), - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::normal) && (ku2.segment_case==Sophus::SegmentCase::last)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_prev].data(), - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::first)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_prev].data(), - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::normal)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku1.idx_2].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_2].data() - ); - } else if ((ku1.segment_case==Sophus::SegmentCase::last) && (ku2.segment_case==Sophus::SegmentCase::last)) { - cost_function = new ceres::AutoDiffCostFunction (ew); - problem.AddResidualBlock(cost_function, loss_function, - spline->parent_Ts_control_point()[ku1.idx_prev].data(), - spline->parent_Ts_control_point()[ku1.idx_0].data(), - spline->parent_Ts_control_point()[ku1.idx_1].data(), - spline->parent_Ts_control_point()[ku2.idx_prev].data(), - spline->parent_Ts_control_point()[ku2.idx_0].data(), - spline->parent_Ts_control_point()[ku2.idx_1].data() - ); + 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; } From 7a835e6c034ff5fb54a5fbe6f9da91ae4bf19263 Mon Sep 17 00:00:00 2001 From: Cedric Pradalier Date: Tue, 16 Sep 2025 22:45:15 +0200 Subject: [PATCH 5/5] Added an error residual for situation where points from two splines need to be used in a functor. --- sophus/spline_error.hpp | 198 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 195 insertions(+), 3 deletions(-) diff --git a/sophus/spline_error.hpp b/sophus/spline_error.hpp index ed575197d..20414b864 100644 --- a/sophus/spline_error.hpp +++ b/sophus/spline_error.hpp @@ -114,6 +114,120 @@ namespace Sophus { }; + 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_; @@ -559,6 +673,17 @@ namespace Sophus { // - 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, @@ -602,7 +727,7 @@ namespace Sophus { } template - static bool addResidualFunction2Point0(ceres::Problem &problem, + 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) { @@ -673,6 +798,64 @@ namespace Sophus { 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, @@ -699,11 +882,20 @@ namespace Sophus { } template - static bool addResidualFunction2Point0(ceres::Problem &problem, + static bool addResidualFunction2Points0(ceres::Problem &problem, double t1, double t2, std::shared_ptr spline, std::shared_ptr functor, ceres::LossFunction * loss_function = nullptr) { - return addResidualFunction2Point0(problem, 0, t1, t2, 0.0, spline, functor, loss_function); + 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); } };