diff --git a/gtsam/certifiable/LiftedSDPProblem.cpp b/gtsam/certifiable/LiftedSDPProblem.cpp index 3508d21b0c..b746c8eeae 100644 --- a/gtsam/certifiable/LiftedSDPProblem.cpp +++ b/gtsam/certifiable/LiftedSDPProblem.cpp @@ -219,6 +219,14 @@ SymbolicFactorGraph BuildQpCostSymbolicFactorGraph(const QcqpProblem& problem) { sfg.push_back(SymbolicFactor(*cost)); } + // A multi-key linear equality couples its keys like a cost does, so they + // must share a clique for its lifted rows to exist. + for (const auto& factor : problem.eConstraints()) { + if (factor && factor->size() > 1) { + sfg.push_back(SymbolicFactor(*factor)); + } + } + if (sfg.empty()) { throw std::runtime_error( "BuildQpCostSymbolicFactorGraph: QCQP has no objective costs."); @@ -479,30 +487,43 @@ void AddQuadraticConstraint( } } -// Lower Ax = b on one key to both rows of [-b A] [1 y'; y X] = 0. +// Lower sum_i A_i x_i = b over the keys of one linear equality to the rows of +// [-b A_1 ... A_m] [1 y'; y X] = 0, one block of rows per key. void AddLinearEqualityConstraint( const mf::Model::t& M, const LinearConstraint& equality, const KeyiToLiftedVectorxiViewInSDPVariableMap& xiMap, const KeysijToLiftedVariableXijViewInSDPVariableMap& xijMap) { const JacobianFactor& J = equality.factor(); - if (J.size() != 1) { - throw std::runtime_error( - "LiftedSDPProblem: only unary linear equality QCQP constraints are " - "supported."); - } - const Key key = *J.begin(); - const auto y = xiMap.at(key)->asExpr(); - const auto X = xijMap.at({key, key})->asExpr(); - const auto A = convertToMosekDenseMatrix(Matrix(J.getA(J.begin()))); const auto b = convertToMosekDenseMatrix(Vector(J.getb())); + auto sumOverKeys = [&](auto&& term) { + auto it = J.begin(); + mf::Expression::t sum = + term(convertToMosekDenseMatrix(Matrix(J.getA(it))), *it); + for (++it; it != J.end(); ++it) { + sum = mf::Expr::add( + sum, term(convertToMosekDenseMatrix(Matrix(J.getA(it))), *it)); + } + return sum; + }; - // Ax=b implies A*x*x'=b*x', hence A*X=b*y' after lifting. Enforcing only - // Ay=b on the lifted vector x_i (row-0 slice) would leave unconstrained PSD - // slack in X. - M->constraint(mf::Expr::mul(A, mf::Expr::transpose(y)), + // Sum_i A_i y_i = b on the lifted vectors x_i (row-0 slices). + M->constraint(sumOverKeys([&](const auto& A, Key key) { + return mf::Expr::mul( + A, mf::Expr::transpose(xiMap.at(key)->asExpr())); + }), mf::Domain::equalsTo(b)); - M->constraint(mf::Expr::sub(mf::Expr::mul(A, X), mf::Expr::mul(b, y)), - mf::Domain::equalsTo(0.0)); + + // The equality times x_j' gives sum_i A_i X_ij = b*y_j' after lifting. + // Enforcing only the row-0 equality would leave unconstrained PSD slack in X. + for (auto jt = J.begin(); jt != J.end(); ++jt) { + const Key keyJ = *jt; + const auto sumOverI = sumOverKeys([&](const auto& A, Key keyI) { + return mf::Expr::mul(A, xijMap.at({keyI, keyJ})->asExpr()); + }); + M->constraint( + mf::Expr::sub(sumOverI, mf::Expr::mul(b, xiMap.at(keyJ)->asExpr())), + mf::Domain::equalsTo(0.0)); + } } // Add all QCQP equality and inequality constraints to a Fusion model. @@ -636,6 +657,31 @@ QpCost::shared_ptr SubstituteFixedVariables(const QpCost& cost, HessianFactor(freeKeys, SymmetricBlockMatrix(freeDims, Q))); } +// A multi-key linear equality that involves a fixed variable keeps its free +// terms and moves the fixed terms A_K v to the right-hand side, so the +// coupling survives the substitution. Returns null for other constraints, +// which are dropped, and for equalities with no free variable left. +NonlinearEqualityConstraint::shared_ptr SubstituteFixedLinearEquality( + const NonlinearEqualityConstraint& factor, const Values& fixedVariables) { + const auto* linear = dynamic_cast(&factor); + if (!linear || linear->size() < 2) return nullptr; + const JacobianFactor& J = linear->linearConstraint().factor(); + std::vector> freeTerms; + Vector b = J.getb(); + for (auto it = J.begin(); it != J.end(); ++it) { + if (fixedVariables.exists(*it)) { + const Matrix& value = fixedVariables.at(*it); + b -= J.getA(it) * Eigen::Map(value.data(), value.size()); + } else { + freeTerms.emplace_back(*it, Matrix(J.getA(it))); + } + } + if (freeTerms.empty()) return nullptr; + return LinearConstraint::Equal(JacobianFactor(freeTerms, b, J.get_model()), + linear->linearConstraint().sigmas()) + .createEqualityFactor(); +} + // Substitute the fixed variables out of a QCQP before it is lifted. Costs are // rewritten on the free variables. The constraints that involve a fixed // variable are dropped: from a factor graph these are its unary manifold @@ -671,6 +717,9 @@ QcqpProblem SubstituteFixedVariables(const QcqpProblem& problem, for (const auto& factor : problem.eConstraints()) { if (!InvolvesFixedVariable(factor->keys(), *fixedVariables)) { eqConstraints.push_back(factor); + } else if (auto substituted = + SubstituteFixedLinearEquality(*factor, *fixedVariables)) { + eqConstraints.push_back(substituted); } } NonlinearInequalityConstraints ineqConstraints; diff --git a/gtsam/certifiable/LiftedSDPProblem.h b/gtsam/certifiable/LiftedSDPProblem.h index d33d76d15e..3729b89ecc 100644 --- a/gtsam/certifiable/LiftedSDPProblem.h +++ b/gtsam/certifiable/LiftedSDPProblem.h @@ -79,8 +79,14 @@ class GTSAM_EXPORT LiftedSDPProblem { * Fixed variables are substituted out before lifting: a unary linear * equality I x = v, which a hard FrobeniusPrior emits, fixes x to v. The * costs that involve it are rewritten on the remaining variables, the - * constraints that involve it are dropped, and it gets no SDP block, so no - * singular rank-one block is pinned. The QCQP itself is unchanged. + * quadratic constraints that involve it are dropped, linear equalities that + * couple it to free variables keep their free terms, and it gets no SDP + * block, so no singular rank-one block is pinned. The QCQP itself is + * unchanged. + * + * Linear equalities over several keys, A_1 x_1 + ... + A_n x_n = b, are + * relaxed with the products A x_j' - b y_j' = 0 for every key j, in addition + * to the lifted constant row, and join the chordal sparsity graph. * * @param problem QCQP to relax. */ @@ -147,8 +153,14 @@ class GTSAM_EXPORT LiftedSDPProblem { * Fixed variables are substituted out before lifting: a unary linear * equality I x = v, which a hard FrobeniusPrior emits, fixes x to v. The * costs that involve it are rewritten on the remaining variables, the - * constraints that involve it are dropped, and it gets no SDP block, so no - * singular rank-one block is pinned. The QCQP itself is unchanged. + * quadratic constraints that involve it are dropped, linear equalities that + * couple it to free variables keep their free terms, and it gets no SDP + * block, so no singular rank-one block is pinned. The QCQP itself is + * unchanged. + * + * Linear equalities over several keys, A_1 x_1 + ... + A_n x_n = b, are + * relaxed with the products A x_j' - b y_j' = 0 for every key j, in addition + * to the lifted constant row, and join the chordal sparsity graph. * * @param problem QCQP to relax. * @param orderingType Ordering used for symbolic elimination. diff --git a/gtsam/certifiable/tests/testLiftedSDPs.cpp b/gtsam/certifiable/tests/testLiftedSDPs.cpp index 5bfb9d96cd..0531941d5e 100644 --- a/gtsam/certifiable/tests/testLiftedSDPs.cpp +++ b/gtsam/certifiable/tests/testLiftedSDPs.cpp @@ -702,6 +702,104 @@ TEST(LiftedSDPs, QuadraticBoundaryEquality) { } // namespace natural_form_fixture /* ************************************************************************* */ +namespace multi_key_equality_fixture { + +const Vector2 p(1.0, 0.0), q(0.0, 1.0), c(2.0, 3.0); + +// Two unary costs 1/2||x0-p||^2 + 1/2||x1-q||^2 whose keys are coupled only by +// the linear equality x0 + x1 = c, so the equality alone connects the keys. +QcqpProblem CoupledProblem() { + NonlinearFactorGraph costs; + costs.emplace_shared(HessianFactor( + 0, Matrix(Matrix2::Identity()), Vector(p), p.squaredNorm())); + costs.emplace_shared(HessianFactor( + 1, Matrix(Matrix2::Identity()), Vector(q), q.squaredNorm())); + QcqpProblem problem(costs, NonlinearEqualityConstraints()); + problem.addConstraint(LinearConstraint::Equal( + JacobianFactor(0, Matrix2::Identity(), 1, Matrix2::Identity(), c))); + return problem; +} + +// Runs the monolithic and both chordal formulations on a problem. +template +void CheckAllFormulations(const QcqpProblem& problem, const Check& check) { + MosekMonolithicSDP monolithic(problem); + check(&monolithic); + for (const auto ordering : + {ChordalOrderingType::Colamd, ChordalOrderingType::Metis}) { + MosekChordalSDP chordal(problem, ordering); + check(&chordal); + } +} + +// A linear equality over two keys is lowered to sum_i A_i X_ij = b y_j' for +// each key j, and its keys share a chordal clique. The optimum shifts both +// points by (c - p - q) / 2, so the objective is |c - p - q|^2 / 4. +TEST(LiftedSDPs, MultiKeyLinearEquality) { + const QcqpProblem problem = CoupledProblem(); + const Vector2 shift = (c - p - q) / 2.0; + const double expected = 0.25 * (c - p - q).squaredNorm(); + CheckAllFormulations(problem, [&](auto* solver) { + EXPECT(solver->solve()); + EXPECT_DOUBLES_EQUAL(expected, solver->objectiveValue(), 1e-6); + const Values values = solver->qcqpValues(); + EXPECT(assert_equal(Vector2(p + shift), Vector(values.at(0).col(0)), + 1e-5)); + EXPECT(assert_equal(Vector2(q + shift), Vector(values.at(1).col(0)), + 1e-5)); + EXPECT_DOUBLES_EQUAL(expected, problem.costs().error(values), 1e-5); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(values), + 1e-5); + for (double ratio : solver->variableEVRs()) EXPECT(ratio > 1e4); + }); +} + +// A multi-key equality with a fixed variable keeps its free terms and moves the +// fixed term to the right-hand side: x0 = v gives x1 = c - v. +TEST(LiftedSDPs, MultiKeyLinearEqualityWithFixedVariable) { + const Vector2 v(0.5, -1.0); + QcqpProblem problem = CoupledProblem(); + problem.addConstraint( + LinearConstraint::Equal(JacobianFactor(0, Matrix2::Identity(), v))); + const double expected = + 0.5 * (v - p).squaredNorm() + 0.5 * (c - v - q).squaredNorm(); + CheckAllFormulations(problem, [&](auto* solver) { + EXPECT(solver->solve()); + EXPECT_DOUBLES_EQUAL(expected, solver->objectiveValue(), 1e-6); + const Values values = solver->qcqpValues(); + EXPECT(assert_equal(v, Vector(values.at(0).col(0)), 1e-5)); + EXPECT(assert_equal(Vector2(c - v), Vector(values.at(1).col(0)), + 1e-5)); + }); +} + +// Keys of different sizes read rectangular off-diagonal blocks: with +// [1 0] x0 - [0 1 0] x1 = 0, the first key's leading entry is tied to the +// second key's middle entry. +TEST(LiftedSDPs, MultiKeyLinearEqualityRectangularBlocks) { + const Vector2 target0(1.0, 0.0); + const Vector3 target1(0.0, 3.0, 0.0); + NonlinearFactorGraph costs; + costs.emplace_shared(HessianFactor( + 0, Matrix(Matrix2::Identity()), Vector(target0), target0.squaredNorm())); + costs.emplace_shared(HessianFactor( + 1, Matrix(Matrix3::Identity()), Vector(target1), target1.squaredNorm())); + QcqpProblem problem(costs, NonlinearEqualityConstraints()); + problem.addConstraint(LinearConstraint::Equal(JacobianFactor( + 0, Matrix(Vector2(1.0, 0.0).transpose()), 1, + Matrix(-Vector3(0.0, 1.0, 0.0).transpose()), Vector1::Zero()))); + // The tied entries meet halfway: t = (1 + 3) / 2 = 2, costing 1/2 + 1/2. + CheckAllFormulations(problem, [&](auto* solver) { + EXPECT(solver->solve()); + EXPECT_DOUBLES_EQUAL(1.0, solver->objectiveValue(), 1e-6); + const Values values = solver->qcqpValues(); + EXPECT_DOUBLES_EQUAL(2.0, values.at(0)(0), 1e-5); + EXPECT_DOUBLES_EQUAL(2.0, values.at(1)(1), 1e-5); + }); +} + +} // namespace multi_key_equality_fixture +/* ************************************************************************* */ namespace fixed_variable_fixture { constexpr size_t kCount = 4; diff --git a/gtsam/nonlinear/WnoaFactor.h b/gtsam/nonlinear/WnoaFactor.h index 1e20028a71..4e2637c941 100644 --- a/gtsam/nonlinear/WnoaFactor.h +++ b/gtsam/nonlinear/WnoaFactor.h @@ -22,18 +22,44 @@ #include #include #include +#include #include #include #include #include #include -#include +#include #include #include +#include +#include namespace gtsam { +namespace internal { + +/// Row of a type's D=1 QCQP vector where its Euclidean coordinates start. +/// Defaults to 0; types with extra lifted rows define QcqpCoordinateOffset. +template +struct QcqpCoordinateOffset : std::integral_constant {}; + +template +struct QcqpCoordinateOffset< + T, std::void_t::QcqpCoordinateOffset)>> + : std::integral_constant::QcqpCoordinateOffset> {}; + +/// True for vector-space poses whose D=1 QCQP vector holds their coordinates. +template +inline constexpr bool kWnoaQcqpSupported = + std::is_same_v::structure_category, + vector_space_tag> && + HasQcqpVariableTraits::value && + HasQcqpVectorDim::value && + HasQcqpVariableTraits::TangentVector, 1>::value; + +} // namespace internal + /** * @brief WNOA (White Noise on Acceleration) motion prior factor. * @@ -199,6 +225,67 @@ class WnoaMotionFactor } /// @} + /** + * Append this factor's exact D=1 QCQP cost. + * + * For vector-space poses the residual is linear in the state: + * [p2 - p1 - dt*v1; v2 - v1]. With lifted pose vectors X1 and X2 whose + * Euclidean coordinates start at row s, it equals B*[X1; v1; X2; v2] with + * B = [[-S, -dt*I, S, 0], [0, -I, 0, I]] and S = [0 I 0] selecting rows + * s..s+N-1. The whitened cost is 0.5*||W*B*X||^2 and needs no constraints + * beyond those of the keys' own lifts. The Hessian's final constant slot + * has zero coefficient. + * + * Only vector-space poses with QCQP traits (for example AugmentedPoint) + * are supported. + * + * @throws std::invalid_argument for columnDimension other than 1. + * @throws std::runtime_error for unsupported pose types. + */ + void qcqpFactors(NonlinearFactorGraph& costs, + NonlinearEqualityConstraints& constraints, + QcqpVariableConstraints& variableConstraints, + size_t columnDimension = 1) const override { + if constexpr (!internal::kWnoaQcqpSupported) { + throw std::runtime_error( + "WnoaMotionFactor::qcqpFactors requires a vector-space pose with " + "QCQP traits, such as AugmentedPoint."); + } else { + if (columnDimension != 1) { + throw std::invalid_argument( + "WnoaMotionFactor::qcqpFactors only supports D=1."); + } + constexpr int poseDim = traits::QcqpVectorDim; + constexpr int offset = internal::QcqpCoordinateOffset::value; + constexpr int liftedDim = 2 * (poseDim + dim); + // Columns: [X1 (poseDim) | v1 | X2 (poseDim) | v2 | constant slot]. + constexpr int pose1 = 0, vel1 = poseDim, pose2 = poseDim + dim, + vel2 = 2 * poseDim + dim; + Matrix B = Matrix::Zero(2 * dim, liftedDim + 1); + B.block(0, pose1 + offset) = -kIdentity; + B.block(0, vel1) = -deltaT_ * kIdentity; + B.block(0, pose2 + offset) = kIdentity; + B.block(dim, vel1) = -kIdentity; + B.block(dim, vel2) = kIdentity; + + const Matrix whitenedB = this->noiseModel_->Whiten(B); + const KeyVector keys{this->key1(), this->key2(), this->key3(), + this->key4()}; + costs.push_back(std::make_shared(HessianFactor( + keys, SymmetricBlockMatrix( + std::vector{poseDim, dim, poseDim, dim, 1}, + whitenedB.transpose() * whitenedB)))); + variableConstraints.add(this->key1(), + traits::template QcqpConstraints<1>); + variableConstraints.add(this->key2(), + traits::template QcqpConstraints<1>); + variableConstraints.add(this->key3(), + traits::template QcqpConstraints<1>); + variableConstraints.add(this->key4(), + traits::template QcqpConstraints<1>); + } + } + /** * @brief Evaluate the WNOA factor residual and optional Jacobians. * diff --git a/gtsam/nonlinear/WnoaInterpFactor.h b/gtsam/nonlinear/WnoaInterpFactor.h index e9fad2db63..fde6ab09cb 100644 --- a/gtsam/nonlinear/WnoaInterpFactor.h +++ b/gtsam/nonlinear/WnoaInterpFactor.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -35,6 +36,7 @@ #include #include +#include #include #include #include @@ -466,6 +468,113 @@ class WnoaInterpFactor : public NoiseModelFactor { return getInterpolatedNoiseModel(JacInner, InterpCondCovs); } + /** + * Append this factor's exact D=1 QCQP cost and interpolation constraints. + * + * The inner factor's squared residual is quartic in the border states, so + * each interpolated key of the inner factor stays a QCQP variable of its + * own, lifted like any other state. The inner factor adds its cost on that + * variable, and this factor adds the linear equalities that tie it to the + * borders. For vector-space poses, with x = [p; v] and Lambda, Psi the WNOA + * interpolation matrices, + * + * x(tau) = Lambda*x_k + Psi*x_(k+1), + * + * so, with S selecting the coordinate rows of a lifted pose, the pose key + * satisfies S*X(tau) - Lambda_pp*S*X_k - Lambda_pv*v_k - Psi_pp*S*X_(k+1) + * - Psi_pv*v_(k+1) = 0 and an interpolated velocity key satisfies the + * analogous equation with the velocity rows of Lambda and Psi. The squared + * norm of the interpolated position is lifted by the pose type's own unary + * constraints. Several factors that share an interpolated key add its + * coupling once. Exactness of the relaxation needs a consistent sign of the + * lifts, for example h = 1 on every AugmentedPoint key. + * + * @throws std::invalid_argument for columnDimension other than 1. + * @throws std::runtime_error for unsupported pose types and for + * fixed_noise_model = false, because the interpolation-augmented + * noise depends on the linearization point. + */ + void qcqpFactors(NonlinearFactorGraph& costs, + NonlinearEqualityConstraints& constraints, + QcqpVariableConstraints& variableConstraints, + size_t columnDimension = 1) const override { + if constexpr (!internal::kWnoaQcqpSupported) { + throw std::runtime_error( + "WnoaInterpFactor::qcqpFactors requires a vector-space pose with " + "QCQP traits, such as AugmentedPoint."); + } else { + if (columnDimension != 1) { + throw std::invalid_argument( + "WnoaInterpFactor::qcqpFactors only supports D=1."); + } + if (!fixed_noise_model_) { + throw std::runtime_error( + "WnoaInterpFactor::qcqpFactors requires fixed_noise_model = true: " + "interpolation-augmented noise depends on the linearization " + "point, so it has no exact QCQP."); + } + inner_factor_->qcqpFactors(costs, constraints, variableConstraints, + columnDimension); + + constexpr int offset = internal::QcqpCoordinateOffset::value; + Matrix select = Matrix::Zero(dim, traits::QcqpVectorDim); + select.block(0, offset).setIdentity(); + const Matrix identity = Matrix::Identity(dim, dim); + for (Key innerKey : inner_factor_->keys()) { + const auto found = key_to_interp_.find(innerKey); + if (found == key_to_interp_.end()) continue; + const StateData& state = found->second; + const auto& [left, right] = interp_to_borders_.at(state); + const auto& precomputed = lambda_psi_pre_comp_.at(state); + const std::pair lambdaPsi = + precomputed ? std::pair(*precomputed) + : interpolator_.getLambdaPsi(left.time, right.time, + state.time); + const Matrix& Lambda = lambdaPsi.first; + const Matrix& Psi = lambdaPsi.second; + + // Rows of Lambda and Psi producing the pose or the velocity. + const bool isPose = innerKey == state.pose; + const int row = isPose ? 0 : dim; + auto couple = [&](const Matrix& block, const Matrix& liftedSelect) { + return Matrix(-block * liftedSelect); + }; + std::vector> terms{ + {innerKey, isPose ? select : identity}, + {left.pose, couple(Lambda.block(row, 0, dim, dim), select)}, + {left.velocity, couple(Lambda.block(row, dim, dim, dim), identity)}, + {right.pose, couple(Psi.block(row, 0, dim, dim), select)}, + {right.velocity, couple(Psi.block(row, dim, dim, dim), identity)}}; + const LinearConstraint coupling = LinearConstraint::Equal( + JacobianFactor(terms, Vector::Zero(dim), + noiseModel::Unit::Create(dim))); + + variableConstraints.add(left.pose, + traits::template QcqpConstraints<1>); + variableConstraints.add( + left.velocity, traits::template QcqpConstraints<1>); + variableConstraints.add(right.pose, + traits::template QcqpConstraints<1>); + variableConstraints.add( + right.velocity, traits::template QcqpConstraints<1>); + if (!isPose) { + variableConstraints.add( + innerKey, traits::template QcqpConstraints<1>); + } + // Factors sharing an interpolated key must not repeat its coupling. + const bool duplicate = std::any_of( + constraints.begin(), constraints.end(), [&](const auto& existing) { + const auto* linear = + dynamic_cast( + existing.get()); + return linear && linear->linearConstraint().factor().equals( + coupling.factor(), 1e-12); + }); + if (!duplicate) constraints.push_back(coupling.createEqualityFactor()); + } + } + } + /** * @brief Get the mapping from outer Keys to their corresponding interpolated * StateData. diff --git a/gtsam/nonlinear/WnoaInterpolator-inl.h b/gtsam/nonlinear/WnoaInterpolator-inl.h new file mode 100644 index 0000000000..38adc7e407 --- /dev/null +++ b/gtsam/nonlinear/WnoaInterpolator-inl.h @@ -0,0 +1,733 @@ +/* ---------------------------------------------------------------------------- + * @file WnoaInterpolator-inl.h + * + * Note: References to (Barfoot 2024) refer to the following textbook: + * Barfoot, Timothy D. State estimation for robotics. Cambridge University + * Press, 2024. + * -------------------------------------------------------------------------- */ + +#pragma once +#include "WnoaInterpolator.h" + +namespace { + /** Assign a fixed-size block so Eigen uses compile-time extents. Avoids GCC -Werror= + array bounds false positives in the SSE vectorizer when a fixed-size source is assigned + to a dynamic destination + */ + template + void assignBlock(const SrcType& src, DstType* dst) { + constexpr int R = SrcType::RowsAtCompileTime; + constexpr int C = SrcType::ColsAtCompileTime; + if constexpr (R != Eigen::Dynamic && C != Eigen::Dynamic) { + dst->resize(R, C); + dst->template block(0, 0) = src; + } else { + *dst = src; + } + } +} + +namespace gtsam { + +// ---- Constructors ---- +template +Interpolator::Interpolator( + const VectorN& q_psd_diag, + std::function transitionFunction, + std::function + covarianceFunction, + std::function + + inverseCovarianceFunction, + std::function&, + const std::pair&, double)> + computeJacobianPrev, + std::function&, + const std::pair&, double)> + computeJacobianNext) + : q_psd_diag_(q_psd_diag), + transitionFunction_(transitionFunction), + covarianceFunction_(covarianceFunction), + inverseCovarianceFunction_(inverseCovarianceFunction), + computeJacobianPrev_(computeJacobianPrev), + computeJacobianNext_(computeJacobianNext) {} + +template +Interpolator::Interpolator(const VectorN& q_psd_diag) + : Interpolator(q_psd_diag, WnoaMotionFactor::TransitionFunction, + WnoaMotionFactor::BuildWnoaCovariance, + WnoaMotionFactor::BuildInverseWnoaCovariance, + WnoaMotionFactor::ComputeJacobianPrev, + WnoaMotionFactor::ComputeJacobianNext) {} + +// ---- Member Functions ---- +template +typename Interpolator::PoseVel +Interpolator::interpolatePoseAndVelocity( + const std::optional& tPoseVel_k, + const std::optional& tPoseVel_kp1, double t_tau, + OptionalMatrixVecType H, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut, + const std::shared_ptr& LambdaPsiPreComp, + const std::shared_ptr& localStateVecsPreComp, + const std::shared_ptr& stateJacobiansPreComp) const { + assert((tPoseVel_k.has_value() || tPoseVel_kp1.has_value()) && + "At least one TimestampedPoseVel must be defined"); + // second point not defined, extrap from first + if (!tPoseVel_kp1.has_value()) { + const auto& [poseVel, t] = tPoseVel_k.value(); + double t_diff = t_tau - t; + return extrapolatePoseAndVelocity(poseVel, t_diff, H, + mainSolveMarginalMatrix, covarianceOut); + } + // first point not defined, extrapolated from second + if (!tPoseVel_k.has_value()) { + const auto& [poseVel, t] = tPoseVel_kp1.value(); + double t_diff = t_tau - t; + return extrapolatePoseAndVelocity(poseVel, t_diff, H, + mainSolveMarginalMatrix, covarianceOut); + } + + // extract both values + const auto& [poseVel_k, t_k] = tPoseVel_k.value(); + const auto& [poseVel_kp1, t_kp1] = tPoseVel_kp1.value(); + // interpolate + if (t_tau == t_k) { + // t_tau equal to left boundary time + return interpolateBoundaryLeft(poseVel_k, H, mainSolveMarginalMatrix, + covarianceOut); + + } else if (t_tau == t_kp1) { + // t_tau equal to right boundary time + return interpolateBoundaryRight(poseVel_kp1, H, mainSolveMarginalMatrix, + covarianceOut); + + } else if (t_tau < t_k) { + // Extrapolation from left boundary + double t_diff = t_tau - t_k; + return extrapolatePoseAndVelocity(poseVel_k, t_diff, H, + mainSolveMarginalMatrix, covarianceOut); + } else if (t_tau > t_kp1) { + // Extrapolation from right boundary + double t_diff = t_tau - t_kp1; + return extrapolatePoseAndVelocity(poseVel_kp1, t_diff, H, + mainSolveMarginalMatrix, covarianceOut); + } else { + // only remaining case is that t_tau is within border states + // call protected overload of interpolate function + return interpolatePoseAndVelocity_( + tPoseVel_k.value(), tPoseVel_kp1.value(), t_tau, H, + mainSolveMarginalMatrix, covarianceOut, LambdaPsiPreComp, + localStateVecsPreComp, stateJacobiansPreComp); + } +} + +template +typename Interpolator::PoseVel +Interpolator::interpolateBoundaryLeft( + const PoseVelocity& poseVel_k, OptionalMatrixVecType H, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut) const { + if (H) { + // dTtau_dTk + (*H)[0] = MatrixN::Identity(); + // dTtau_dvarpik + (*H)[1] = MatrixN::Zero(); + // dTtau_dTkp1 + (*H)[2] = MatrixN::Zero(); + // dTtau_dvarpikp1 + (*H)[3] = MatrixN::Zero(); + // dvarpitau_dTk + (*H)[4] = MatrixN::Zero(); + // dvarpitau_dvarpik + (*H)[5] = MatrixN::Identity(); + // dvarpitau_dTkp1 + (*H)[6] = MatrixN::Zero(); + // dvarpitau_dvarpikp1 + (*H)[7] = MatrixN::Zero(); + } + if (covarianceOut && mainSolveMarginalMatrix) { + // if t_tau == t_k, then the covariance is the same as that of Tvarpi_k + *covarianceOut = mainSolveMarginalMatrix->topLeftCorner(dim * 2, dim * 2); + } + return poseVel_k; +} + +template +typename Interpolator::PoseVel +Interpolator::interpolateBoundaryRight( + const PoseVelocity& poseVel_kp1, OptionalMatrixVecType H, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut) const { + if (H) { + (*H)[0] = MatrixN::Zero(); // dTtau_dTk + (*H)[1] = MatrixN::Zero(); // dTtau_dvarpik + (*H)[2] = MatrixN::Identity(); // dTtau_dTkp1 + (*H)[3] = MatrixN::Zero(); // dTtau_dvarpikp1 + (*H)[4] = MatrixN::Zero(); // dvarpitau_dTk + (*H)[5] = MatrixN::Zero(); // dvarpitau_dvarpik + (*H)[6] = MatrixN::Zero(); // dvarpitau_dTkp1 + (*H)[7] = MatrixN::Identity(); // dvarpitau_dvarpikp1 + } + if (covarianceOut && mainSolveMarginalMatrix) { + // if t_tau == t_kp1, then the covariance is the same as that of Tvarpi_kp1 + *covarianceOut = + mainSolveMarginalMatrix->bottomRightCorner(dim * 2, dim * 2); + } + return poseVel_kp1; +} + +template +typename Interpolator::PoseVel +Interpolator::extrapolatePoseAndVelocity( + const PoseVelocity& poseVel_ex, double t_diff, + OptionalMatrixVecType H, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut) const { + // follows (11.5) in (Barfoot 2024) + const auto& [T_ex, varpi_ex] = poseVel_ex; + // Define local state vector at time k, in the Lie algebra of Pose at time k + Vector2N gamma_k; + gamma_k.topRows(dim).setZero(); + gamma_k.bottomRows(dim) = varpi_ex; + // Extrapolate pose and velocity to time tau, using WNOA extrapolation + // equations + auto Psi = transitionFunction_(t_diff); + auto gamma_ex = Psi * gamma_k; + // Map back to manifold + auto T_tau = traits::Compose( + T_ex, traits::Expmap(gamma_ex.topRows(dim), nullptr)); + auto varpi_tau = gamma_ex.bottomRows(dim); + + // compute covariance of the extrapolated pose and velocity + // assume that mainSolveMarginalMatrix corresponds to the covariance of + // Tvarpi_extrapolate_point + if (mainSolveMarginalMatrix && covarianceOut) { + assert(mainSolveMarginalMatrix->rows() == 2 * dim && + mainSolveMarginalMatrix->cols() == 2 * dim); + Matrix2N Sigma = covarianceFunction_(t_diff, q_psd_diag_); + // (11.5) in (Barfoot 2024) + *covarianceOut = Sigma + Psi * *mainSolveMarginalMatrix * Psi.transpose(); + } + return PoseVel{T_tau, varpi_tau}; +} + +template +typename Interpolator::PoseVel +Interpolator::interpolatePoseAndVelocity_( + const TimestampedPoseVel& tPoseVel_k, + const TimestampedPoseVel& tPoseVel_kp1, double t_tau, + OptionalMatrixVecType H, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut, + const std::shared_ptr& LambdaPsiPreComp, + const std::shared_ptr& localStateVecsPreComp, + const std::shared_ptr& stateJacobiansPreComp) const { + // unpack poses and velocities + const auto& [poseVel_k, t_k] = tPoseVel_k; + const auto& [T_k, varpi_k] = poseVel_k; + const auto& [poseVel_kp1, t_kp1] = tPoseVel_kp1; + + // 1. Form local state vectors and Jacobians + // Form local state vectors at time k, in the Lie algebra of Pose at time k + // Follows Eq. (11.45) in (Barfoot 2024) + VectorN xi_k, xi_dot_k, xi_kp1, xi_dot_kp1; + xi_k.setZero(); + // Intermediate Jacobians for xi and xi_dot at next time step with respect to + // the bordering states and velocities + StateJacobians localJacs; + if (H) { + formLocalStateAndJacobians_(tPoseVel_k, tPoseVel_kp1, localStateVecsPreComp, + stateJacobiansPreComp, &xi_dot_k, &xi_kp1, + &xi_dot_kp1, &localJacs); + } else { + formLocalStateAndJacobians_(tPoseVel_k, tPoseVel_kp1, localStateVecsPreComp, + stateJacobiansPreComp, &xi_dot_k, &xi_kp1, + &xi_dot_kp1); + } + // 2. Interpolation + // Retrieve interpolation matrices + Matrix2N Lambda, Psi; + // Compute local state vectors at time tau, in the Lie algebra of Pose at time + // k, using WNOA interpolation equations + VectorN xi_tau, xidot_tau; + interpolateLocalState_(t_k, t_kp1, t_tau, xi_dot_k, xi_kp1, xi_dot_kp1, + LambdaPsiPreComp, &Lambda, &Psi, &xi_tau, &xidot_tau); + // 3. Map to manifold + // Additional intermediate Jacobians + MatrixN right_jac_tau, dTtau_dTk, dTtau_dxitau; + // Map the local state vector at time tau back to the manifold to get the + // interpolated pose Eq. (11.45) in (Barfoot 2024) + PoseVel poseVel_tau; + if (H) { + poseVel_tau = mapLocalStateToManifold_( + T_k, xi_tau, xidot_tau, &right_jac_tau, &dTtau_dTk, &dTtau_dxitau); + } else { + poseVel_tau = + mapLocalStateToManifold_(T_k, xi_tau, xidot_tau, &right_jac_tau); + } + + // 4. Compute complete Jacobians + if (H) { + computeCompleteJacobians_(Lambda, Psi, xidot_tau, right_jac_tau, dTtau_dTk, + dTtau_dxitau, localJacs, H); + } + + // 5. Compute covariance of the interpolated pose and velocity + // compute covariance of the interpolated pose (and velocity, if required) + // using Lambda and Psi computed from (11.41) in (Barfoot 2024). + // This should be equivalent to using (4.23) in the FnT paper. + computeInterpolationCovariance_(tPoseVel_k, tPoseVel_kp1, poseVel_tau, t_tau, + Lambda, Psi, mainSolveMarginalMatrix, + covarianceOut); + return poseVel_tau; +} + +template +void Interpolator::formLocalStateAndJacobians_( + const TimestampedPoseVel& tPoseVel_k, + const TimestampedPoseVel& tPoseVel_kp1, + const std::shared_ptr& localStateVecsPreComp, + const std::shared_ptr& stateJacobiansPreComp, + VectorN* xi_dot_k, VectorN* xi_kp1, VectorN* xi_dot_kp1, + StateJacobians* jacs) const { + const auto& [poseVel_k, _t_k] = tPoseVel_k; + const auto& [_T_k, varpi_k] = poseVel_k; + *xi_dot_k = varpi_k; + + const bool needComputedState = + !localStateVecsPreComp || (jacs && !stateJacobiansPreComp); + + LocalStateVecs local_state; + StateJacobians localJacs; + if (needComputedState) { + local_state = computeLocalStateVecs(tPoseVel_k, tPoseVel_kp1, + jacs ? &localJacs : nullptr); + } + + // State vectors come from precompute if available, otherwise from local. + if (localStateVecsPreComp) { + *xi_kp1 = localStateVecsPreComp->first; + *xi_dot_kp1 = localStateVecsPreComp->second; + } else { + *xi_kp1 = local_state.first; + *xi_dot_kp1 = local_state.second; + } + + if (jacs) { + // Jacobians come from precompute if available, otherwise from local. + *jacs = stateJacobiansPreComp ? *stateJacobiansPreComp : localJacs; + } +} + +template +void Interpolator::interpolateLocalState_( + double t_k, double t_kp1, double t_tau, const VectorN& xi_dot_k, + const VectorN& xi_kp1, const VectorN& xi_dot_kp1, + const std::shared_ptr& LambdaPsiPreComp, + Matrix2N* Lambda, Matrix2N* Psi, VectorN* xi_tau, + VectorN* xidot_tau) const { + if (!LambdaPsiPreComp) { + std::tie(*Lambda, *Psi) = getLambdaPsi(t_k, t_kp1, t_tau); + } else { + std::tie(*Lambda, *Psi) = *LambdaPsiPreComp; + } + + *xi_tau = Lambda->block(0, dim, dim, dim) * xi_dot_k + + Psi->block(0, 0, dim, dim) * xi_kp1 + + Psi->block(0, dim, dim, dim) * + xi_dot_kp1; // Dropping xi_k term here since it's zero + *xidot_tau = Lambda->block(dim, dim, dim, dim) * xi_dot_k + + Psi->block(dim, 0, dim, dim) * xi_kp1 + + Psi->block(dim, dim, dim, dim) * + xi_dot_kp1; // Dropping xi_k term here since it's zero +} + +template +typename Interpolator::PoseVel +Interpolator::mapLocalStateToManifold_( + const PoseType& T_k, const VectorN& xi_tau, const VectorN& xidot_tau, + MatrixN* right_jac_tau, MatrixN* dTtau_dTk, MatrixN* dTtau_dxitau) const { + PoseType T_tau; + if (dTtau_dTk && dTtau_dxitau) { + T_tau = traits::Compose( + T_k, traits::Expmap(xi_tau, right_jac_tau), dTtau_dTk, + dTtau_dxitau); + *dTtau_dxitau = *dTtau_dxitau * *right_jac_tau; + } else { + T_tau = traits::Compose( + T_k, traits::Expmap(xi_tau, right_jac_tau)); + } + auto varpi_tau = *right_jac_tau * xidot_tau; + return PoseVel{T_tau, varpi_tau}; +} + +template +void Interpolator::computeCompleteJacobians_( + const Matrix2N& Lambda, const Matrix2N& Psi, const VectorN& xidot_tau, + const MatrixN& right_jac_tau, const MatrixN& dTtau_dTk, + const MatrixN& dTtau_dxitau, const StateJacobians& jacs, + OptionalMatrixVecType H) const { + // Zero for vector spaces, use an approximation for Lie groups + MatrixN dvarpitau_dxitau; + if constexpr (std::is_same_v::structure_category, + vector_space_tag>) { + dvarpitau_dxitau.setZero(); + } else { + // For Lie groups + dvarpitau_dxitau = PoseType::adjointMap(xidot_tau) / 2.0; + } + + // derivatives of local state position at time tau with respect to bordering + // states and velocities + MatrixN dxitau_dTk = Psi(0, 0) * jacs.dxi_dTk + Psi(0, dim) * jacs.dxidot_dTk; + MatrixN dxitau_dvarpik = + Lambda(0, dim) * MatrixN::Identity(); // xi does not depend on varpi_k + // and xi_dot is exactly varpi_k + MatrixN dxitau_dTkp1 = + Psi(0, 0) * jacs.dxi_dTkp1 + Psi(0, dim) * jacs.dxidot_dTkp1; + MatrixN dxitau_dvarpikp1 = + Psi(0, dim) * + jacs.dxidotkp1_dvarpikp1; // dxikp1 does not depend on varpi_kp1 + // derivatives of local state velocity at time tau with respect to bordering + // states and velocities + MatrixN dxidottau_dTk = + Psi(dim, 0) * jacs.dxi_dTk + Psi(dim, dim) * jacs.dxidot_dTk; + MatrixN dxidottau_dvarpik = + Lambda(dim, dim) * MatrixN::Identity(); // xi does not depend on varpi_k + // and xi_dot is exactly varpi_k + MatrixN dxidottau_dTkp1 = + Psi(dim, 0) * jacs.dxi_dTkp1 + Psi(dim, dim) * jacs.dxidot_dTkp1; + MatrixN dxidottau_dvarpikp1 = + Psi(dim, dim) * + jacs.dxidotkp1_dvarpikp1; // dxikp1 does not depend on varpi_kp1 + // Compose final Jacobians using chain rule + // dTtau_dTk + assignBlock(dTtau_dTk + dTtau_dxitau * dxitau_dTk, &(*H)[0]); + // dTtau_dvarpik + assignBlock(dTtau_dxitau * dxitau_dvarpik, &(*H)[1]); + // dTtau_dTkp1 + assignBlock(dTtau_dxitau * dxitau_dTkp1, &(*H)[2]); + // dTtau_dvarpikp1 + assignBlock(dTtau_dxitau * dxitau_dvarpikp1, &(*H)[3]); + // dvarpitau_dTk + assignBlock(right_jac_tau * dxidottau_dTk + dvarpitau_dxitau * dxitau_dTk, + &(*H)[4]); + // dvarpitau_dvarpik + assignBlock( + right_jac_tau * dxidottau_dvarpik + dvarpitau_dxitau * dxitau_dvarpik, + &(*H)[5]); + // dvarpitau_dTkp1 + assignBlock(right_jac_tau * dxidottau_dTkp1 + dvarpitau_dxitau * dxitau_dTkp1, + &(*H)[6]); + // dvarpitau_dvarpikp1 + assignBlock( + right_jac_tau * dxidottau_dvarpikp1 + dvarpitau_dxitau * dxitau_dvarpikp1, + &(*H)[7]); +} + +template +void Interpolator::computeInterpolationCovariance_( + const TimestampedPoseVel& tPoseVel_k, + const TimestampedPoseVel& tPoseVel_kp1, const PoseVel& poseVel_tau, + double t_tau, const Matrix2N& Lambda, const Matrix2N& Psi, + const std::shared_ptr& mainSolveMarginalMatrix, + Matrix* covarianceOut) const { + if (mainSolveMarginalMatrix && covarianceOut) { + Eigen::Matrix LambdaPsi; + Matrix2N Sigma = computeConditionalCov( + tPoseVel_k, tPoseVel_kp1, TimestampedPoseVel{poseVel_tau, t_tau}); + LambdaPsi << Lambda, Psi; + *covarianceOut = + Sigma + LambdaPsi * *mainSolveMarginalMatrix * LambdaPsi.transpose(); + } +} + +template +std::map> +Interpolator::ComputeJointMarginals( + const std::map>& queryBuckets, + const std::unique_ptr& marginals) { + std::map> + intervalJointMarginals; // JointMarginal matrices for each interval + std::unordered_set allBoundaryKeys; + + // Lambda function for forming the boundary key vector + auto formBoundaryKeyVector = [](const StateDataInterval& stateDataBorders) { + KeyVector boundaryKeyVector; + if (stateDataBorders.first.has_value()) { + boundaryKeyVector.push_back(stateDataBorders.first->pose); // p1 + boundaryKeyVector.push_back(stateDataBorders.first->velocity); // v1 + } + if (stateDataBorders.second.has_value()) { + boundaryKeyVector.push_back(stateDataBorders.second->pose); // p2 + boundaryKeyVector.push_back(stateDataBorders.second->velocity); // v2 + } + return boundaryKeyVector; + }; + + for (const auto& [stateDataBorders, stateDataInterpVec] : queryBuckets) { + KeyVector boundaryKeyVector = formBoundaryKeyVector(stateDataBorders); + allBoundaryKeys.insert(boundaryKeyVector.begin(), boundaryKeyVector.end()); + // Compute JointMarginal for each interval separately + // This is fast if there are not too many intervals + JointMarginal mainSolveMarginal = + marginals->jointMarginalCovariance(boundaryKeyVector); + // avoid using JointMarginal.fullMatrix() as it returns covariance + // in alphabetical order of the keys... + auto mainSolveMarginalMatrix = + std::make_shared(ConstructMatrixFromJointMarginal( + mainSolveMarginal, boundaryKeyVector, dim)); + intervalJointMarginals[stateDataBorders] = mainSolveMarginalMatrix; + } + + return intervalJointMarginals; +} + +template +Values Interpolator::interpolatePosesAndVelocities( + const NonlinearFactorGraph& mainSolveGraph, const Values& mainSolveSolution, + const StateDataSet& mainSolveStates, const StateDataSet& interpolatedStates, + std::shared_ptr covarianceMapOut) const { + // Map from intervals [t1, t2) to query times inside that interval (bucket) + std::map> queryBuckets; + + // Find all intervals [t_k, t_kp1) that contain at least one query time + for (const auto& stateDataInterp : interpolatedStates) { + auto it2 = mainSolveStates.upper_bound(stateDataInterp); + StateDataInterval interval; + interval.first = it2 == mainSolveStates.begin() + ? std::nullopt + : std::optional(*std::prev(it2)); + interval.second = it2 == mainSolveStates.end() + ? std::nullopt + : std::optional(*it2); + queryBuckets[interval].push_back(stateDataInterp); + } + + Values interpolatedSolution; + + // Only construct a Marginals if requested + std::unique_ptr fullMarginal; + // JointMarginal matrices for each interval (every pair of boundary states + // involved) + auto intervalJointMarginals = + std::map>(); + if (covarianceMapOut) { + // Compute all required joint marginals + fullMarginal = + std::make_unique(mainSolveGraph, mainSolveSolution); + intervalJointMarginals = ComputeJointMarginals(queryBuckets, fullMarginal); + } + + // Perform interpolation for each bucket + for (const auto& [stateDataBorder, stateDataInterpVec] : queryBuckets) { + auto makeTimestampedPV = [&](const std::optional& s) + -> std::optional { + if (!s) return std::nullopt; + return TimestampedPoseVel(mainSolveSolution.at(s->pose), + mainSolveSolution.at(s->velocity), + s->time); + }; + + // Get the poses and velocities at t_k and t_kp1 + std::optional pvk = + makeTimestampedPV(stateDataBorder.first); + std::optional pvkp1 = + makeTimestampedPV(stateDataBorder.second); + Matrix covarianceOut; // (2*dim, 2*dim) + + // Interpolate for all query times within this query interval (bucket) + for (const auto& stateDataInterp : stateDataInterpVec) { + std::shared_ptr mainSolveMarginalMatrix; // (4*dim, 4*dim) + if (covarianceMapOut) { + mainSolveMarginalMatrix = intervalJointMarginals[stateDataBorder]; + } + auto pvtau = + interpolatePoseAndVelocity(pvk, pvkp1, stateDataInterp.time, nullptr, + mainSolveMarginalMatrix, &covarianceOut); + auto [T_tau, varpi_tau] = pvtau; + interpolatedSolution.insert(stateDataInterp.pose, T_tau); + interpolatedSolution.insert(stateDataInterp.velocity, varpi_tau); + if (covarianceMapOut) { + // upper left covariance block corresponds to pose, lower right block + // corresponds to velocity + (*covarianceMapOut)[stateDataInterp.pose] = + covarianceOut.topLeftCorner(dim, dim); + (*covarianceMapOut)[stateDataInterp.velocity] = + covarianceOut.bottomRightCorner(dim, dim); + } + } + } + return interpolatedSolution; +} + +// Lambda-Psi interpolator equations for WNOA-specific interpolator +template +std::pair Interpolator::getLambdaPsi( + double t_k, double t_kp1, double t_tau) const { + // See Section 11.1 in (Barfoot 2024) for the derivation of these equations + // for WNOA + double dt = t_kp1 - t_k; + double alpha = (t_tau - t_k) / dt; + double alpha2 = alpha * alpha; + double alpha3 = alpha2 * alpha; + + Matrix2N Lambda, Psi; + + double Psi_00 = 3 * alpha2 - 2 * alpha3; + double Psi_01 = dt * (alpha3 - alpha2); + double Psi_10 = (6 * alpha / dt) - (6 * alpha2 / dt); + double Psi_11 = 3 * alpha2 - 2 * alpha; + + double Lambda_00 = 1 - Psi_00; + double Lambda_01 = dt * (alpha - 2 * alpha2 + alpha3); + double Lambda_10 = -Psi_10; + double Lambda_11 = 1 - 4 * alpha + 3 * alpha2; + + Lambda << MatrixN::Identity() * Lambda_00, MatrixN::Identity() * Lambda_01, + MatrixN::Identity() * Lambda_10, MatrixN::Identity() * Lambda_11; + + Psi << MatrixN::Identity() * Psi_00, MatrixN::Identity() * Psi_01, + MatrixN::Identity() * Psi_10, MatrixN::Identity() * Psi_11; + + return std::make_pair(Lambda, Psi); +} + +template +std::pair Interpolator::getLambdaPsiGeneral( + double t_k, double t_kp1, double t_tau) const { + // Build general interpolation equations for Lambda and Psi using the + // transition function and See (11.41) in (Barfoot 2024) for the derivation of + // these equations for general linear time-invariant systems + double dt = t_kp1 - t_k; + auto Phi_12 = transitionFunction_(dt); + auto Phi_1tau = transitionFunction_(t_tau - t_k); + auto Phi_tau2 = transitionFunction_(t_kp1 - t_tau); + + // Construct Q + auto Q_12_inv = inverseCovarianceFunction_(dt, q_psd_diag_); + auto Q_1tau = covarianceFunction_(t_tau - t_k, q_psd_diag_); + + // Eq. (11.41) in (Barfoot 2024) + auto Lambda = Phi_1tau - Q_1tau * Phi_tau2.transpose() * Q_12_inv * Phi_12; + auto Psi = Q_1tau * Phi_tau2.transpose() * Q_12_inv; + + return std::make_pair(Lambda, Psi); +} + +template +typename Interpolator::LocalStateVecs +Interpolator::computeLocalStateVecs( + const TimestampedPoseVel& pvk, const TimestampedPoseVel& pvkp1, + Interpolator::StateJacobians* jacs) const { + const auto& [poseVel_k, t_k] = pvk; + const auto& [poseVel_kp1, t_kp1] = pvkp1; + const auto& [T_k, varpi_k] = poseVel_k; + const auto& [T_kp1, varpi_kp1] = poseVel_kp1; + + VectorN xi_kp1, xi_dot_kp1; + MatrixN invJr; + + if (jacs) { + MatrixN dbetween_Tk; + MatrixN dbetween_Tkp1; + xi_kp1 = traits::Logmap( + traits::Between(T_k, T_kp1, &dbetween_Tk, &dbetween_Tkp1), + &invJr); + // Compute derivatives + jacs->dxi_dTk = invJr * dbetween_Tk; + jacs->dxi_dTkp1 = invJr * dbetween_Tkp1; + } else { + xi_kp1 = + traits::Logmap(traits::Between(T_k, T_kp1), &invJr); + } + xi_dot_kp1 = invJr * varpi_kp1; + LocalStateVecs local_state; + local_state.first = xi_kp1; + local_state.second = xi_dot_kp1; + + if (jacs) { + // Zero for vector spaces, use an approximation for Lie groups + MatrixN dxidot_dxi; + if constexpr (std::is_same_v::structure_category, + vector_space_tag>) { + dxidot_dxi.setZero(); + } else { + // For Lie groups + dxidot_dxi = -PoseType::adjointMap(varpi_kp1) / 2.0; + } + jacs->dxidot_dTk = dxidot_dxi * jacs->dxi_dTk; + jacs->dxidot_dTkp1 = dxidot_dxi * jacs->dxi_dTkp1; + jacs->dxidotkp1_dvarpikp1 = invJr; + } + + return local_state; +} + +template +typename Interpolator::Matrix2N +Interpolator::computeConditionalCov( + const TimestampedPoseVel& tPoseVel_k, + const TimestampedPoseVel& tPoseVel_kp1, + const TimestampedPoseVel& tPoseVel_tau, OptionalMatrixType Lambda, + OptionalMatrixType Psi) const { + auto [poseVel_k, t_k] = tPoseVel_k; + auto [poseVel_kp1, t_kp1] = tPoseVel_kp1; + auto [poseVel_tau, t_tau] = tPoseVel_tau; + + // Todo (Daniel): handle the case when t_tau == t_k or t_tau == t_kp1 + assert(t_tau > t_k && t_tau < t_kp1 && + "t_tau must be in the interval (t_k, t_kp1)"); + + // see Figure 5.4 in the paper + Matrix2N Q_tau_prev_inv = + inverseCovarianceFunction_(t_tau - t_k, q_psd_diag_); + Matrix2N Q_tau_next_inv = + inverseCovarianceFunction_(t_kp1 - t_tau, q_psd_diag_); + Matrix2N E_tau = computeJacobianNext_(poseVel_k.asPair(), + poseVel_tau.asPair(), t_tau - t_k); + Matrix2N F_k_tau = computeJacobianPrev_(poseVel_tau.asPair(), + poseVel_kp1.asPair(), t_kp1 - t_tau); + Matrix2N Sigma_inv = E_tau.transpose() * Q_tau_prev_inv * E_tau + + F_k_tau.transpose() * Q_tau_next_inv * F_k_tau; + Matrix2N Sigma = Sigma_inv.inverse(); + + // (5.23) in the paper + if (Lambda) { + Matrix2N F_tau_km1 = computeJacobianPrev_( + poseVel_k.asPair(), poseVel_tau.asPair(), t_tau - t_k); + *Lambda = Sigma * E_tau.transpose() * Q_tau_prev_inv * F_tau_km1; + } + if (Psi) { + Matrix2N E_k = computeJacobianNext_(poseVel_k.asPair(), + poseVel_kp1.asPair(), t_kp1 - t_k); + *Psi = Sigma * F_k_tau.transpose() * Q_tau_next_inv * E_k; + } + return Sigma; +} + +template +Matrix Interpolator::ConstructMatrixFromJointMarginal( + const JointMarginal& blockMatrix, const KeyVector& keyVector, + size_t blockSize) { + size_t n = keyVector.size(); + Matrix M(n * blockSize, n * blockSize); + + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j <= i; + ++j) { // symmetric block matrix: j <= i to avoid repeats + auto block = blockMatrix(keyVector[i], keyVector[j]); // query block + // fill in both (i, j) and (j, i) blocks + M.block(i * blockSize, j * blockSize, blockSize, blockSize) = block; + if (i != j) { + M.block(j * blockSize, i * blockSize, blockSize, blockSize) = + block.transpose(); + } + } + } + return M; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/WnoaInterpolator.cpp b/gtsam/nonlinear/WnoaInterpolator.cpp index 3855eb206c..04e05b701d 100644 --- a/gtsam/nonlinear/WnoaInterpolator.cpp +++ b/gtsam/nonlinear/WnoaInterpolator.cpp @@ -6,734 +6,15 @@ * Press, 2024. * -------------------------------------------------------------------------- */ -#include "WnoaInterpolator.h" - -namespace { - /** Assign a fixed-size block so Eigen uses compile-time extents. Avoids GCC -Werror= - array bounds false positives in the SSE vectorizer when a fixed-size source is assigned - to a dynamic destination - */ - template - void assignBlock(const SrcType& src, DstType* dst) { - constexpr int R = SrcType::RowsAtCompileTime; - constexpr int C = SrcType::ColsAtCompileTime; - if constexpr (R != Eigen::Dynamic && C != Eigen::Dynamic) { - dst->resize(R, C); - dst->template block(0, 0) = src; - } else { - *dst = src; - } - } -} +#include "WnoaInterpolator-inl.h" namespace gtsam { -// ---- Constructors ---- -template -Interpolator::Interpolator( - const VectorN& q_psd_diag, - std::function transitionFunction, - std::function - covarianceFunction, - std::function - - inverseCovarianceFunction, - std::function&, - const std::pair&, double)> - computeJacobianPrev, - std::function&, - const std::pair&, double)> - computeJacobianNext) - : q_psd_diag_(q_psd_diag), - transitionFunction_(transitionFunction), - covarianceFunction_(covarianceFunction), - inverseCovarianceFunction_(inverseCovarianceFunction), - computeJacobianPrev_(computeJacobianPrev), - computeJacobianNext_(computeJacobianNext) {} - -template -Interpolator::Interpolator(const VectorN& q_psd_diag) - : Interpolator(q_psd_diag, WnoaMotionFactor::TransitionFunction, - WnoaMotionFactor::BuildWnoaCovariance, - WnoaMotionFactor::BuildInverseWnoaCovariance, - WnoaMotionFactor::ComputeJacobianPrev, - WnoaMotionFactor::ComputeJacobianNext) {} - -// ---- Member Functions ---- -template -typename Interpolator::PoseVel -Interpolator::interpolatePoseAndVelocity( - const std::optional& tPoseVel_k, - const std::optional& tPoseVel_kp1, double t_tau, - OptionalMatrixVecType H, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut, - const std::shared_ptr& LambdaPsiPreComp, - const std::shared_ptr& localStateVecsPreComp, - const std::shared_ptr& stateJacobiansPreComp) const { - assert((tPoseVel_k.has_value() || tPoseVel_kp1.has_value()) && - "At least one TimestampedPoseVel must be defined"); - // second point not defined, extrap from first - if (!tPoseVel_kp1.has_value()) { - const auto& [poseVel, t] = tPoseVel_k.value(); - double t_diff = t_tau - t; - return extrapolatePoseAndVelocity(poseVel, t_diff, H, - mainSolveMarginalMatrix, covarianceOut); - } - // first point not defined, extrapolated from second - if (!tPoseVel_k.has_value()) { - const auto& [poseVel, t] = tPoseVel_kp1.value(); - double t_diff = t_tau - t; - return extrapolatePoseAndVelocity(poseVel, t_diff, H, - mainSolveMarginalMatrix, covarianceOut); - } - - // extract both values - const auto& [poseVel_k, t_k] = tPoseVel_k.value(); - const auto& [poseVel_kp1, t_kp1] = tPoseVel_kp1.value(); - // interpolate - if (t_tau == t_k) { - // t_tau equal to left boundary time - return interpolateBoundaryLeft(poseVel_k, H, mainSolveMarginalMatrix, - covarianceOut); - - } else if (t_tau == t_kp1) { - // t_tau equal to right boundary time - return interpolateBoundaryRight(poseVel_kp1, H, mainSolveMarginalMatrix, - covarianceOut); - - } else if (t_tau < t_k) { - // Extrapolation from left boundary - double t_diff = t_tau - t_k; - return extrapolatePoseAndVelocity(poseVel_k, t_diff, H, - mainSolveMarginalMatrix, covarianceOut); - } else if (t_tau > t_kp1) { - // Extrapolation from right boundary - double t_diff = t_tau - t_kp1; - return extrapolatePoseAndVelocity(poseVel_kp1, t_diff, H, - mainSolveMarginalMatrix, covarianceOut); - } else { - // only remaining case is that t_tau is within border states - // call protected overload of interpolate function - return interpolatePoseAndVelocity_( - tPoseVel_k.value(), tPoseVel_kp1.value(), t_tau, H, - mainSolveMarginalMatrix, covarianceOut, LambdaPsiPreComp, - localStateVecsPreComp, stateJacobiansPreComp); - } -} - -template -typename Interpolator::PoseVel -Interpolator::interpolateBoundaryLeft( - const PoseVelocity& poseVel_k, OptionalMatrixVecType H, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut) const { - if (H) { - // dTtau_dTk - (*H)[0] = MatrixN::Identity(); - // dTtau_dvarpik - (*H)[1] = MatrixN::Zero(); - // dTtau_dTkp1 - (*H)[2] = MatrixN::Zero(); - // dTtau_dvarpikp1 - (*H)[3] = MatrixN::Zero(); - // dvarpitau_dTk - (*H)[4] = MatrixN::Zero(); - // dvarpitau_dvarpik - (*H)[5] = MatrixN::Identity(); - // dvarpitau_dTkp1 - (*H)[6] = MatrixN::Zero(); - // dvarpitau_dvarpikp1 - (*H)[7] = MatrixN::Zero(); - } - if (covarianceOut && mainSolveMarginalMatrix) { - // if t_tau == t_k, then the covariance is the same as that of Tvarpi_k - *covarianceOut = mainSolveMarginalMatrix->topLeftCorner(dim * 2, dim * 2); - } - return poseVel_k; -} - -template -typename Interpolator::PoseVel -Interpolator::interpolateBoundaryRight( - const PoseVelocity& poseVel_kp1, OptionalMatrixVecType H, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut) const { - if (H) { - (*H)[0] = MatrixN::Zero(); // dTtau_dTk - (*H)[1] = MatrixN::Zero(); // dTtau_dvarpik - (*H)[2] = MatrixN::Identity(); // dTtau_dTkp1 - (*H)[3] = MatrixN::Zero(); // dTtau_dvarpikp1 - (*H)[4] = MatrixN::Zero(); // dvarpitau_dTk - (*H)[5] = MatrixN::Zero(); // dvarpitau_dvarpik - (*H)[6] = MatrixN::Zero(); // dvarpitau_dTkp1 - (*H)[7] = MatrixN::Identity(); // dvarpitau_dvarpikp1 - } - if (covarianceOut && mainSolveMarginalMatrix) { - // if t_tau == t_kp1, then the covariance is the same as that of Tvarpi_kp1 - *covarianceOut = - mainSolveMarginalMatrix->bottomRightCorner(dim * 2, dim * 2); - } - return poseVel_kp1; -} - -template -typename Interpolator::PoseVel -Interpolator::extrapolatePoseAndVelocity( - const PoseVelocity& poseVel_ex, double t_diff, - OptionalMatrixVecType H, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut) const { - // follows (11.5) in (Barfoot 2024) - const auto& [T_ex, varpi_ex] = poseVel_ex; - // Define local state vector at time k, in the Lie algebra of Pose at time k - Vector2N gamma_k; - gamma_k.topRows(dim).setZero(); - gamma_k.bottomRows(dim) = varpi_ex; - // Extrapolate pose and velocity to time tau, using WNOA extrapolation - // equations - auto Psi = transitionFunction_(t_diff); - auto gamma_ex = Psi * gamma_k; - // Map back to manifold - auto T_tau = traits::Compose( - T_ex, traits::Expmap(gamma_ex.topRows(dim), nullptr)); - auto varpi_tau = gamma_ex.bottomRows(dim); - - // compute covariance of the extrapolated pose and velocity - // assume that mainSolveMarginalMatrix corresponds to the covariance of - // Tvarpi_extrapolate_point - if (mainSolveMarginalMatrix && covarianceOut) { - assert(mainSolveMarginalMatrix->rows() == 2 * dim && - mainSolveMarginalMatrix->cols() == 2 * dim); - Matrix2N Sigma = covarianceFunction_(t_diff, q_psd_diag_); - // (11.5) in (Barfoot 2024) - *covarianceOut = Sigma + Psi * *mainSolveMarginalMatrix * Psi.transpose(); - } - return PoseVel{T_tau, varpi_tau}; -} - -template -typename Interpolator::PoseVel -Interpolator::interpolatePoseAndVelocity_( - const TimestampedPoseVel& tPoseVel_k, - const TimestampedPoseVel& tPoseVel_kp1, double t_tau, - OptionalMatrixVecType H, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut, - const std::shared_ptr& LambdaPsiPreComp, - const std::shared_ptr& localStateVecsPreComp, - const std::shared_ptr& stateJacobiansPreComp) const { - // unpack poses and velocities - const auto& [poseVel_k, t_k] = tPoseVel_k; - const auto& [T_k, varpi_k] = poseVel_k; - const auto& [poseVel_kp1, t_kp1] = tPoseVel_kp1; - - // 1. Form local state vectors and Jacobians - // Form local state vectors at time k, in the Lie algebra of Pose at time k - // Follows Eq. (11.45) in (Barfoot 2024) - VectorN xi_k, xi_dot_k, xi_kp1, xi_dot_kp1; - xi_k.setZero(); - // Intermediate Jacobians for xi and xi_dot at next time step with respect to - // the bordering states and velocities - StateJacobians localJacs; - if (H) { - formLocalStateAndJacobians_(tPoseVel_k, tPoseVel_kp1, localStateVecsPreComp, - stateJacobiansPreComp, &xi_dot_k, &xi_kp1, - &xi_dot_kp1, &localJacs); - } else { - formLocalStateAndJacobians_(tPoseVel_k, tPoseVel_kp1, localStateVecsPreComp, - stateJacobiansPreComp, &xi_dot_k, &xi_kp1, - &xi_dot_kp1); - } - // 2. Interpolation - // Retrieve interpolation matrices - Matrix2N Lambda, Psi; - // Compute local state vectors at time tau, in the Lie algebra of Pose at time - // k, using WNOA interpolation equations - VectorN xi_tau, xidot_tau; - interpolateLocalState_(t_k, t_kp1, t_tau, xi_dot_k, xi_kp1, xi_dot_kp1, - LambdaPsiPreComp, &Lambda, &Psi, &xi_tau, &xidot_tau); - // 3. Map to manifold - // Additional intermediate Jacobians - MatrixN right_jac_tau, dTtau_dTk, dTtau_dxitau; - // Map the local state vector at time tau back to the manifold to get the - // interpolated pose Eq. (11.45) in (Barfoot 2024) - PoseVel poseVel_tau; - if (H) { - poseVel_tau = mapLocalStateToManifold_( - T_k, xi_tau, xidot_tau, &right_jac_tau, &dTtau_dTk, &dTtau_dxitau); - } else { - poseVel_tau = - mapLocalStateToManifold_(T_k, xi_tau, xidot_tau, &right_jac_tau); - } - - // 4. Compute complete Jacobians - if (H) { - computeCompleteJacobians_(Lambda, Psi, xidot_tau, right_jac_tau, dTtau_dTk, - dTtau_dxitau, localJacs, H); - } - - // 5. Compute covariance of the interpolated pose and velocity - // compute covariance of the interpolated pose (and velocity, if required) - // using Lambda and Psi computed from (11.41) in (Barfoot 2024). - // This should be equivalent to using (4.23) in the FnT paper. - computeInterpolationCovariance_(tPoseVel_k, tPoseVel_kp1, poseVel_tau, t_tau, - Lambda, Psi, mainSolveMarginalMatrix, - covarianceOut); - return poseVel_tau; -} - -template -void Interpolator::formLocalStateAndJacobians_( - const TimestampedPoseVel& tPoseVel_k, - const TimestampedPoseVel& tPoseVel_kp1, - const std::shared_ptr& localStateVecsPreComp, - const std::shared_ptr& stateJacobiansPreComp, - VectorN* xi_dot_k, VectorN* xi_kp1, VectorN* xi_dot_kp1, - StateJacobians* jacs) const { - const auto& [poseVel_k, _t_k] = tPoseVel_k; - const auto& [_T_k, varpi_k] = poseVel_k; - *xi_dot_k = varpi_k; - - const bool needComputedState = - !localStateVecsPreComp || (jacs && !stateJacobiansPreComp); - - LocalStateVecs local_state; - StateJacobians localJacs; - if (needComputedState) { - local_state = computeLocalStateVecs(tPoseVel_k, tPoseVel_kp1, - jacs ? &localJacs : nullptr); - } - - // State vectors come from precompute if available, otherwise from local. - if (localStateVecsPreComp) { - *xi_kp1 = localStateVecsPreComp->first; - *xi_dot_kp1 = localStateVecsPreComp->second; - } else { - *xi_kp1 = local_state.first; - *xi_dot_kp1 = local_state.second; - } - - if (jacs) { - // Jacobians come from precompute if available, otherwise from local. - *jacs = stateJacobiansPreComp ? *stateJacobiansPreComp : localJacs; - } -} - -template -void Interpolator::interpolateLocalState_( - double t_k, double t_kp1, double t_tau, const VectorN& xi_dot_k, - const VectorN& xi_kp1, const VectorN& xi_dot_kp1, - const std::shared_ptr& LambdaPsiPreComp, - Matrix2N* Lambda, Matrix2N* Psi, VectorN* xi_tau, - VectorN* xidot_tau) const { - if (!LambdaPsiPreComp) { - std::tie(*Lambda, *Psi) = getLambdaPsi(t_k, t_kp1, t_tau); - } else { - std::tie(*Lambda, *Psi) = *LambdaPsiPreComp; - } - - *xi_tau = Lambda->block(0, dim, dim, dim) * xi_dot_k + - Psi->block(0, 0, dim, dim) * xi_kp1 + - Psi->block(0, dim, dim, dim) * - xi_dot_kp1; // Dropping xi_k term here since it's zero - *xidot_tau = Lambda->block(dim, dim, dim, dim) * xi_dot_k + - Psi->block(dim, 0, dim, dim) * xi_kp1 + - Psi->block(dim, dim, dim, dim) * - xi_dot_kp1; // Dropping xi_k term here since it's zero -} - -template -typename Interpolator::PoseVel -Interpolator::mapLocalStateToManifold_( - const PoseType& T_k, const VectorN& xi_tau, const VectorN& xidot_tau, - MatrixN* right_jac_tau, MatrixN* dTtau_dTk, MatrixN* dTtau_dxitau) const { - PoseType T_tau; - if (dTtau_dTk && dTtau_dxitau) { - T_tau = traits::Compose( - T_k, traits::Expmap(xi_tau, right_jac_tau), dTtau_dTk, - dTtau_dxitau); - *dTtau_dxitau = *dTtau_dxitau * *right_jac_tau; - } else { - T_tau = traits::Compose( - T_k, traits::Expmap(xi_tau, right_jac_tau)); - } - auto varpi_tau = *right_jac_tau * xidot_tau; - return PoseVel{T_tau, varpi_tau}; -} - -template -void Interpolator::computeCompleteJacobians_( - const Matrix2N& Lambda, const Matrix2N& Psi, const VectorN& xidot_tau, - const MatrixN& right_jac_tau, const MatrixN& dTtau_dTk, - const MatrixN& dTtau_dxitau, const StateJacobians& jacs, - OptionalMatrixVecType H) const { - // Zero for vector spaces, use an approximation for Lie groups - MatrixN dvarpitau_dxitau; - if constexpr (std::is_same_v::structure_category, - vector_space_tag>) { - dvarpitau_dxitau.setZero(); - } else { - // For Lie groups - dvarpitau_dxitau = PoseType::adjointMap(xidot_tau) / 2.0; - } - - // derivatives of local state position at time tau with respect to bordering - // states and velocities - MatrixN dxitau_dTk = Psi(0, 0) * jacs.dxi_dTk + Psi(0, dim) * jacs.dxidot_dTk; - MatrixN dxitau_dvarpik = - Lambda(0, dim) * MatrixN::Identity(); // xi does not depend on varpi_k - // and xi_dot is exactly varpi_k - MatrixN dxitau_dTkp1 = - Psi(0, 0) * jacs.dxi_dTkp1 + Psi(0, dim) * jacs.dxidot_dTkp1; - MatrixN dxitau_dvarpikp1 = - Psi(0, dim) * - jacs.dxidotkp1_dvarpikp1; // dxikp1 does not depend on varpi_kp1 - // derivatives of local state velocity at time tau with respect to bordering - // states and velocities - MatrixN dxidottau_dTk = - Psi(dim, 0) * jacs.dxi_dTk + Psi(dim, dim) * jacs.dxidot_dTk; - MatrixN dxidottau_dvarpik = - Lambda(dim, dim) * MatrixN::Identity(); // xi does not depend on varpi_k - // and xi_dot is exactly varpi_k - MatrixN dxidottau_dTkp1 = - Psi(dim, 0) * jacs.dxi_dTkp1 + Psi(dim, dim) * jacs.dxidot_dTkp1; - MatrixN dxidottau_dvarpikp1 = - Psi(dim, dim) * - jacs.dxidotkp1_dvarpikp1; // dxikp1 does not depend on varpi_kp1 - // Compose final Jacobians using chain rule - // dTtau_dTk - assignBlock(dTtau_dTk + dTtau_dxitau * dxitau_dTk, &(*H)[0]); - // dTtau_dvarpik - assignBlock(dTtau_dxitau * dxitau_dvarpik, &(*H)[1]); - // dTtau_dTkp1 - assignBlock(dTtau_dxitau * dxitau_dTkp1, &(*H)[2]); - // dTtau_dvarpikp1 - assignBlock(dTtau_dxitau * dxitau_dvarpikp1, &(*H)[3]); - // dvarpitau_dTk - assignBlock(right_jac_tau * dxidottau_dTk + dvarpitau_dxitau * dxitau_dTk, - &(*H)[4]); - // dvarpitau_dvarpik - assignBlock( - right_jac_tau * dxidottau_dvarpik + dvarpitau_dxitau * dxitau_dvarpik, - &(*H)[5]); - // dvarpitau_dTkp1 - assignBlock(right_jac_tau * dxidottau_dTkp1 + dvarpitau_dxitau * dxitau_dTkp1, - &(*H)[6]); - // dvarpitau_dvarpikp1 - assignBlock( - right_jac_tau * dxidottau_dvarpikp1 + dvarpitau_dxitau * dxitau_dvarpikp1, - &(*H)[7]); -} - -template -void Interpolator::computeInterpolationCovariance_( - const TimestampedPoseVel& tPoseVel_k, - const TimestampedPoseVel& tPoseVel_kp1, const PoseVel& poseVel_tau, - double t_tau, const Matrix2N& Lambda, const Matrix2N& Psi, - const std::shared_ptr& mainSolveMarginalMatrix, - Matrix* covarianceOut) const { - if (mainSolveMarginalMatrix && covarianceOut) { - Eigen::Matrix LambdaPsi; - Matrix2N Sigma = computeConditionalCov( - tPoseVel_k, tPoseVel_kp1, TimestampedPoseVel{poseVel_tau, t_tau}); - LambdaPsi << Lambda, Psi; - *covarianceOut = - Sigma + LambdaPsi * *mainSolveMarginalMatrix * LambdaPsi.transpose(); - } -} - -template -std::map> -Interpolator::ComputeJointMarginals( - const std::map>& queryBuckets, - const std::unique_ptr& marginals) { - std::map> - intervalJointMarginals; // JointMarginal matrices for each interval - std::unordered_set allBoundaryKeys; - - // Lambda function for forming the boundary key vector - auto formBoundaryKeyVector = [](const StateDataInterval& stateDataBorders) { - KeyVector boundaryKeyVector; - if (stateDataBorders.first.has_value()) { - boundaryKeyVector.push_back(stateDataBorders.first->pose); // p1 - boundaryKeyVector.push_back(stateDataBorders.first->velocity); // v1 - } - if (stateDataBorders.second.has_value()) { - boundaryKeyVector.push_back(stateDataBorders.second->pose); // p2 - boundaryKeyVector.push_back(stateDataBorders.second->velocity); // v2 - } - return boundaryKeyVector; - }; - - for (const auto& [stateDataBorders, stateDataInterpVec] : queryBuckets) { - KeyVector boundaryKeyVector = formBoundaryKeyVector(stateDataBorders); - allBoundaryKeys.insert(boundaryKeyVector.begin(), boundaryKeyVector.end()); - // Compute JointMarginal for each interval separately - // This is fast if there are not too many intervals - JointMarginal mainSolveMarginal = - marginals->jointMarginalCovariance(boundaryKeyVector); - // avoid using JointMarginal.fullMatrix() as it returns covariance - // in alphabetical order of the keys... - auto mainSolveMarginalMatrix = - std::make_shared(ConstructMatrixFromJointMarginal( - mainSolveMarginal, boundaryKeyVector, dim)); - intervalJointMarginals[stateDataBorders] = mainSolveMarginalMatrix; - } - - return intervalJointMarginals; -} - -template -Values Interpolator::interpolatePosesAndVelocities( - const NonlinearFactorGraph& mainSolveGraph, const Values& mainSolveSolution, - const StateDataSet& mainSolveStates, const StateDataSet& interpolatedStates, - std::shared_ptr covarianceMapOut) const { - // Map from intervals [t1, t2) to query times inside that interval (bucket) - std::map> queryBuckets; - - // Find all intervals [t_k, t_kp1) that contain at least one query time - for (const auto& stateDataInterp : interpolatedStates) { - auto it2 = mainSolveStates.upper_bound(stateDataInterp); - StateDataInterval interval; - interval.first = it2 == mainSolveStates.begin() - ? std::nullopt - : std::optional(*std::prev(it2)); - interval.second = it2 == mainSolveStates.end() - ? std::nullopt - : std::optional(*it2); - queryBuckets[interval].push_back(stateDataInterp); - } - - Values interpolatedSolution; - - // Only construct a Marginals if requested - std::unique_ptr fullMarginal; - // JointMarginal matrices for each interval (every pair of boundary states - // involved) - auto intervalJointMarginals = - std::map>(); - if (covarianceMapOut) { - // Compute all required joint marginals - fullMarginal = - std::make_unique(mainSolveGraph, mainSolveSolution); - intervalJointMarginals = ComputeJointMarginals(queryBuckets, fullMarginal); - } - - // Perform interpolation for each bucket - for (const auto& [stateDataBorder, stateDataInterpVec] : queryBuckets) { - auto makeTimestampedPV = [&](const std::optional& s) - -> std::optional { - if (!s) return std::nullopt; - return TimestampedPoseVel(mainSolveSolution.at(s->pose), - mainSolveSolution.at(s->velocity), - s->time); - }; - - // Get the poses and velocities at t_k and t_kp1 - std::optional pvk = - makeTimestampedPV(stateDataBorder.first); - std::optional pvkp1 = - makeTimestampedPV(stateDataBorder.second); - Matrix covarianceOut; // (2*dim, 2*dim) - - // Interpolate for all query times within this query interval (bucket) - for (const auto& stateDataInterp : stateDataInterpVec) { - std::shared_ptr mainSolveMarginalMatrix; // (4*dim, 4*dim) - if (covarianceMapOut) { - mainSolveMarginalMatrix = intervalJointMarginals[stateDataBorder]; - } - auto pvtau = - interpolatePoseAndVelocity(pvk, pvkp1, stateDataInterp.time, nullptr, - mainSolveMarginalMatrix, &covarianceOut); - auto [T_tau, varpi_tau] = pvtau; - interpolatedSolution.insert(stateDataInterp.pose, T_tau); - interpolatedSolution.insert(stateDataInterp.velocity, varpi_tau); - if (covarianceMapOut) { - // upper left covariance block corresponds to pose, lower right block - // corresponds to velocity - (*covarianceMapOut)[stateDataInterp.pose] = - covarianceOut.topLeftCorner(dim, dim); - (*covarianceMapOut)[stateDataInterp.velocity] = - covarianceOut.bottomRightCorner(dim, dim); - } - } - } - return interpolatedSolution; -} - -// Lambda-Psi interpolator equations for WNOA-specific interpolator -template -std::pair Interpolator::getLambdaPsi( - double t_k, double t_kp1, double t_tau) const { - // See Section 11.1 in (Barfoot 2024) for the derivation of these equations - // for WNOA - double dt = t_kp1 - t_k; - double alpha = (t_tau - t_k) / dt; - double alpha2 = alpha * alpha; - double alpha3 = alpha2 * alpha; - - Matrix2N Lambda, Psi; - - double Psi_00 = 3 * alpha2 - 2 * alpha3; - double Psi_01 = dt * (alpha3 - alpha2); - double Psi_10 = (6 * alpha / dt) - (6 * alpha2 / dt); - double Psi_11 = 3 * alpha2 - 2 * alpha; - - double Lambda_00 = 1 - Psi_00; - double Lambda_01 = dt * (alpha - 2 * alpha2 + alpha3); - double Lambda_10 = -Psi_10; - double Lambda_11 = 1 - 4 * alpha + 3 * alpha2; - - Lambda << MatrixN::Identity() * Lambda_00, MatrixN::Identity() * Lambda_01, - MatrixN::Identity() * Lambda_10, MatrixN::Identity() * Lambda_11; - - Psi << MatrixN::Identity() * Psi_00, MatrixN::Identity() * Psi_01, - MatrixN::Identity() * Psi_10, MatrixN::Identity() * Psi_11; - - return std::make_pair(Lambda, Psi); -} - -template -std::pair Interpolator::getLambdaPsiGeneral( - double t_k, double t_kp1, double t_tau) const { - // Build general interpolation equations for Lambda and Psi using the - // transition function and See (11.41) in (Barfoot 2024) for the derivation of - // these equations for general linear time-invariant systems - double dt = t_kp1 - t_k; - auto Phi_12 = transitionFunction_(dt); - auto Phi_1tau = transitionFunction_(t_tau - t_k); - auto Phi_tau2 = transitionFunction_(t_kp1 - t_tau); - - // Construct Q - auto Q_12_inv = inverseCovarianceFunction_(dt, q_psd_diag_); - auto Q_1tau = covarianceFunction_(t_tau - t_k, q_psd_diag_); - - // Eq. (11.41) in (Barfoot 2024) - auto Lambda = Phi_1tau - Q_1tau * Phi_tau2.transpose() * Q_12_inv * Phi_12; - auto Psi = Q_1tau * Phi_tau2.transpose() * Q_12_inv; - - return std::make_pair(Lambda, Psi); -} - -template -typename Interpolator::LocalStateVecs -Interpolator::computeLocalStateVecs( - const TimestampedPoseVel& pvk, const TimestampedPoseVel& pvkp1, - Interpolator::StateJacobians* jacs) const { - const auto& [poseVel_k, t_k] = pvk; - const auto& [poseVel_kp1, t_kp1] = pvkp1; - const auto& [T_k, varpi_k] = poseVel_k; - const auto& [T_kp1, varpi_kp1] = poseVel_kp1; - - VectorN xi_kp1, xi_dot_kp1; - MatrixN invJr; - - if (jacs) { - MatrixN dbetween_Tk; - MatrixN dbetween_Tkp1; - xi_kp1 = traits::Logmap( - traits::Between(T_k, T_kp1, &dbetween_Tk, &dbetween_Tkp1), - &invJr); - // Compute derivatives - jacs->dxi_dTk = invJr * dbetween_Tk; - jacs->dxi_dTkp1 = invJr * dbetween_Tkp1; - } else { - xi_kp1 = - traits::Logmap(traits::Between(T_k, T_kp1), &invJr); - } - xi_dot_kp1 = invJr * varpi_kp1; - LocalStateVecs local_state; - local_state.first = xi_kp1; - local_state.second = xi_dot_kp1; - - if (jacs) { - // Zero for vector spaces, use an approximation for Lie groups - MatrixN dxidot_dxi; - if constexpr (std::is_same_v::structure_category, - vector_space_tag>) { - dxidot_dxi.setZero(); - } else { - // For Lie groups - dxidot_dxi = -PoseType::adjointMap(varpi_kp1) / 2.0; - } - jacs->dxidot_dTk = dxidot_dxi * jacs->dxi_dTk; - jacs->dxidot_dTkp1 = dxidot_dxi * jacs->dxi_dTkp1; - jacs->dxidotkp1_dvarpikp1 = invJr; - } - - return local_state; -} - -template -typename Interpolator::Matrix2N -Interpolator::computeConditionalCov( - const TimestampedPoseVel& tPoseVel_k, - const TimestampedPoseVel& tPoseVel_kp1, - const TimestampedPoseVel& tPoseVel_tau, OptionalMatrixType Lambda, - OptionalMatrixType Psi) const { - auto [poseVel_k, t_k] = tPoseVel_k; - auto [poseVel_kp1, t_kp1] = tPoseVel_kp1; - auto [poseVel_tau, t_tau] = tPoseVel_tau; - - // Todo (Daniel): handle the case when t_tau == t_k or t_tau == t_kp1 - assert(t_tau > t_k && t_tau < t_kp1 && - "t_tau must be in the interval (t_k, t_kp1)"); - - // see Figure 5.4 in the paper - Matrix2N Q_tau_prev_inv = - inverseCovarianceFunction_(t_tau - t_k, q_psd_diag_); - Matrix2N Q_tau_next_inv = - inverseCovarianceFunction_(t_kp1 - t_tau, q_psd_diag_); - Matrix2N E_tau = computeJacobianNext_(poseVel_k.asPair(), - poseVel_tau.asPair(), t_tau - t_k); - Matrix2N F_k_tau = computeJacobianPrev_(poseVel_tau.asPair(), - poseVel_kp1.asPair(), t_kp1 - t_tau); - Matrix2N Sigma_inv = E_tau.transpose() * Q_tau_prev_inv * E_tau + - F_k_tau.transpose() * Q_tau_next_inv * F_k_tau; - Matrix2N Sigma = Sigma_inv.inverse(); - - // (5.23) in the paper - if (Lambda) { - Matrix2N F_tau_km1 = computeJacobianPrev_( - poseVel_k.asPair(), poseVel_tau.asPair(), t_tau - t_k); - *Lambda = Sigma * E_tau.transpose() * Q_tau_prev_inv * F_tau_km1; - } - if (Psi) { - Matrix2N E_k = computeJacobianNext_(poseVel_k.asPair(), - poseVel_kp1.asPair(), t_kp1 - t_k); - *Psi = Sigma * F_k_tau.transpose() * Q_tau_next_inv * E_k; - } - return Sigma; -} - -template -Matrix Interpolator::ConstructMatrixFromJointMarginal( - const JointMarginal& blockMatrix, const KeyVector& keyVector, - size_t blockSize) { - size_t n = keyVector.size(); - Matrix M(n * blockSize, n * blockSize); - - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j <= i; - ++j) { // symmetric block matrix: j <= i to avoid repeats - auto block = blockMatrix(keyVector[i], keyVector[j]); // query block - // fill in both (i, j) and (j, i) blocks - M.block(i * blockSize, j * blockSize, blockSize, blockSize) = block; - if (i != j) { - M.block(j * blockSize, i * blockSize, blockSize, blockSize) = - block.transpose(); - } - } - } - return M; -} - // ---- Explicit Instantiations ---- -template class Interpolator; -template class Interpolator; -template class Interpolator; -template class Interpolator; -template class Interpolator; +template class GTSAM_EXPORT Interpolator; +template class GTSAM_EXPORT Interpolator; +template class GTSAM_EXPORT Interpolator; +template class GTSAM_EXPORT Interpolator; +template class GTSAM_EXPORT Interpolator; } // namespace gtsam diff --git a/gtsam/nonlinear/WnoaInterpolator.h b/gtsam/nonlinear/WnoaInterpolator.h index 67aa3120f7..2cab793277 100644 --- a/gtsam/nonlinear/WnoaInterpolator.h +++ b/gtsam/nonlinear/WnoaInterpolator.h @@ -103,7 +103,7 @@ typedef std::map InterpCovarianceMap; * @tparam PoseType Pose group/type (e.g., Pose2, Pose3, or a vector-space pose) */ template -class GTSAM_EXPORT Interpolator { +class Interpolator { protected: static constexpr int dim = traits::dimension; using VelocityType = typename traits::TangentVector; diff --git a/gtsam_unstable/geometry/AugmentedPoint.h b/gtsam_unstable/geometry/AugmentedPoint.h index 676549d92a..a5c536309a 100644 --- a/gtsam_unstable/geometry/AugmentedPoint.h +++ b/gtsam_unstable/geometry/AugmentedPoint.h @@ -156,6 +156,9 @@ struct traits> /// Number of rows in the D=1 lifted QCQP variable. static constexpr int QcqpVectorDim = N + 2; + /// Row of the lifted variable [h, x, u] where the coordinates x start. + static constexpr int QcqpCoordinateOffset = 1; + /** * Lift a point to [1, x.transpose(), x.dot(x)].transpose(). * diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 54869e4e2e..73a8d3865d 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -36,6 +36,7 @@ class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; class gtsam::Values; +class gtsam::StateData; class gtsam::Key; class gtsam::VectorValues; class gtsam::KeyList; @@ -466,6 +467,38 @@ virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { gtsam::Values calculateEstimate() const; }; + +#include +template +virtual class WnoaMotionFactor : gtsam::NoiseModelFactor { + WnoaMotionFactor(const gtsam::StateData& state_k, + const gtsam::StateData& state_kp1, + const gtsam::Vector& q_psd_diag); +}; + +typedef gtsam::WnoaMotionFactor> + WnoaMotionFactorAugmentedPoint2; +typedef gtsam::WnoaMotionFactor> + WnoaMotionFactorAugmentedPoint3; + +#include +template +virtual class WnoaInterpFactor : gtsam::NoiseModelFactor { + WnoaInterpFactor( + const gtsam::NoiseModelFactor::shared_ptr inner_factor, + const std::set estimated_states, + const std::set interp_states, + const gtsam::Vector q_psd_diag, + const bool fixed_noise_model = false, + const bool precomp_interp_mats = true); +}; + +typedef gtsam::WnoaInterpFactor> + WnoaInterpFactorAugmentedPoint2; +typedef gtsam::WnoaInterpFactor> + WnoaInterpFactorAugmentedPoint3; + + //************************************************************************* // slam //************************************************************************* diff --git a/gtsam_unstable/nonlinear/WnoaAugmentedPointInterpolator.cpp b/gtsam_unstable/nonlinear/WnoaAugmentedPointInterpolator.cpp new file mode 100644 index 0000000000..8667c8699d --- /dev/null +++ b/gtsam_unstable/nonlinear/WnoaAugmentedPointInterpolator.cpp @@ -0,0 +1,11 @@ +#include +#include +#include + +namespace gtsam { + +template class GTSAM_UNSTABLE_EXPORT Interpolator>; +template class GTSAM_UNSTABLE_EXPORT Interpolator>; +template class GTSAM_UNSTABLE_EXPORT Interpolator>; + +} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testWnoaAugmentedPoint.cpp b/gtsam_unstable/nonlinear/tests/testWnoaAugmentedPoint.cpp new file mode 100644 index 0000000000..36fb5ae0f9 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testWnoaAugmentedPoint.cpp @@ -0,0 +1,625 @@ +/* ---------------------------------------------------------------------------- + * + * GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * + * See LICENSE for the license information + * + * -------------------------------------------------------------------------- */ +/** + * @file testWnoaAugmentedPoint.cpp + * @brief WNOA motion factor tests for AugmentedPoint. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtsam; +using symbol_shorthand::P; +using symbol_shorthand::V; + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, ConstantVelocity) { + using Point = AugmentedPoint<3>; + const double dt = 0.2; + const Point p0(Vector3(1.0, -2.0, 3.0)); + const Vector3 velocity(0.5, -0.25, 0.75); + const Point p1 = p0 + dt * velocity; + + WnoaMotionFactor factor(P(0), V(0), P(1), V(1), dt, + Vector3::Ones()); + const Vector actual = factor.evaluateError(p0, velocity, p1, velocity); + const Vector6 expected = Vector6::Zero(); + CHECK(assert_equal(expected, actual, 1e-9)); +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, Jacobians) { + using Point = AugmentedPoint<3>; + const Point p0(Vector3(1.0, -2.0, 3.0)); + const Point p1(Vector3(1.3, -1.7, 2.8)); + const Vector3 v0(0.5, -0.25, 0.75); + const Vector3 v1(0.25, 0.1, 0.5); + + WnoaMotionFactor factor(P(0), V(0), P(1), V(1), 0.2, + Vector3::Ones()); + Values values; + values.insert(P(0), p0); + values.insert(V(0), v0); + values.insert(P(1), p1); + values.insert(V(1), v1); + + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, InterpolationConstantVelocity) { + using Point = AugmentedPoint<3>; + const double dt = 0.2; + const Point p0(Vector3(1.0, -2.0, 3.0)); + const Vector3 velocity(0.5, -0.25, 0.75); + const Point p1 = p0 + dt * velocity; + Interpolator interpolator(Vector3::Ones()); + + for (double ratio : {0.0, 0.25, 0.5, 0.75, 1.0}) { + const auto result = interpolator.interpolatePoseAndVelocity( + TimestampedPoseVelocity(p0, velocity, 0.0), + TimestampedPoseVelocity(p1, velocity, dt), ratio * dt); + const Point expected = p0 + ratio * dt * velocity; + CHECK(assert_equal(expected, result.pose, 1e-9)); + CHECK(assert_equal(velocity, result.vel, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, InterpolationJacobians) { + using Point = AugmentedPoint<3>; + const Point p0(Vector3(1.0, -2.0, 3.0)); + const Point p1(Vector3(1.3, -1.7, 2.8)); + const Vector3 v0(0.5, -0.25, 0.75); + const Vector3 v1(0.25, 0.1, 0.5); + const double dt = 0.2, tau = 0.08; + Interpolator interpolator(Vector3::Ones()); + + std::vector H(8); + (void)interpolator.interpolatePoseAndVelocity( + TimestampedPoseVelocity(p0, v0, 0.0), + TimestampedPoseVelocity(p1, v1, dt), tau, &H); + + auto position = [&](const Point& a, const Vector3& b, const Point& c, + const Vector3& d) -> Vector3 { + const auto result = interpolator.interpolatePoseAndVelocity( + TimestampedPoseVelocity(a, b, 0.0), + TimestampedPoseVelocity(c, d, dt), tau); + return result.pose.vector(); + }; + auto velocity = [&](const Point& a, const Vector3& b, const Point& c, + const Vector3& d) -> Vector3 { + const auto result = interpolator.interpolatePoseAndVelocity( + TimestampedPoseVelocity(a, b, 0.0), + TimestampedPoseVelocity(c, d, dt), tau); + return result.vel; + }; + + const double delta = 1e-6, tol = 1e-5; + EXPECT(assert_equal(numericalDerivative41(position, p0, v0, p1, v1, + delta), + H[0], tol)); + EXPECT(assert_equal(numericalDerivative42(position, p0, v0, p1, v1, + delta), + H[1], tol)); + EXPECT(assert_equal(numericalDerivative43(position, p0, v0, p1, v1, + delta), + H[2], tol)); + EXPECT(assert_equal(numericalDerivative44(position, p0, v0, p1, v1, + delta), + H[3], tol)); + EXPECT(assert_equal(numericalDerivative41(velocity, p0, v0, p1, v1, + delta), + H[4], tol)); + EXPECT(assert_equal(numericalDerivative42(velocity, p0, v0, p1, v1, + delta), + H[5], tol)); + EXPECT(assert_equal(numericalDerivative43(velocity, p0, v0, p1, v1, + delta), + H[6], tol)); + EXPECT(assert_equal(numericalDerivative44(velocity, p0, v0, p1, v1, + delta), + H[7], tol)); +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, InterpolatedSquaredRange) { + using Point = AugmentedPoint<3>; + const double dt = 0.2; + const Point p0(Vector3(1.0, 0.0, 0.0)); + const Vector3 velocity(1.0, 0.0, 0.0); + const Point p1 = p0 + dt * velocity; + + const std::set estimatedStates = { + StateData(P(0), V(0), 0.0), StateData(P(1), V(1), dt)}; + const std::set interpolatedStates = { + StateData(P(2), V(2), 0.08)}; + + auto range = std::make_shared>( + P(2), Vector3::Zero().eval(), 1.0, + noiseModel::Isotropic::Sigma(1, 0.1)); + WnoaInterpFactor factor(range, estimatedStates, interpolatedStates, + Vector3::Ones(), true); + + Values values; + values.insert(P(0), p0); + values.insert(V(0), velocity); + values.insert(P(1), p1); + values.insert(V(1), velocity); + + CHECK(factor.keys().size() == 4); + const Vector1 expected(1.08 * 1.08 - 1.0); + CHECK(assert_equal(expected, factor.unwhitenedError(values), 1e-9)); +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, InterpolatedSquaredRangeJacobians) { + using Point = AugmentedPoint<3>; + const Point p0(Vector3(1.0, -2.0, 3.0)); + const Point p1(Vector3(1.3, -1.7, 2.8)); + const Vector3 v0(0.5, -0.25, 0.75); + const Vector3 v1(0.25, 0.1, 0.5); + + const std::set estimatedStates = { + StateData(P(0), V(0), 0.0), StateData(P(1), V(1), 0.2)}; + const std::set interpolatedStates = { + StateData(P(2), V(2), 0.08)}; + + auto range = std::make_shared>( + P(2), Vector3(2.0, 0.5, -1.0), 2.5, + noiseModel::Isotropic::Sigma(1, 0.1)); + WnoaInterpFactor factor(range, estimatedStates, interpolatedStates, + Vector3::Ones(), true); + + Values values; + values.insert(P(0), p0); + values.insert(V(0), v0); + values.insert(P(1), p1); + values.insert(V(1), v1); + + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-6, 1e-5); +} + +/* ************************************************************************* */ +TEST(WnoaAugmentedPoint, RangeOnlyOptimization) { + using Point = AugmentedPoint<3>; + const Vector3 p0(0.6, -0.2, 0.7); + const Vector3 velocity(0.35, 0.1, -0.15); + const Vector3 q = Vector3::Constant(0.2); + const std::set knots = { + StateData(P(0), V(0), 0.0), StateData(P(1), V(1), 1.0), + StateData(P(2), V(2), 2.0)}; + const std::array landmarks = { + Vector3(0.0, 0.0, 0.0), Vector3(3.0, 0.0, 0.0), + Vector3(0.0, 3.0, 0.0), Vector3(0.0, 0.0, 3.0)}; + const std::array measurementTimes = {0.25, 0.75, 1.25, 1.75}; + + NonlinearFactorGraph graph; + graph.emplace_shared>( + P(0), Point(p0), noiseModel::Isotropic::Sigma(3, 0.05)); + for (size_t k = 0; k < 2; ++k) { + graph.emplace_shared>( + StateData(P(k), V(k), double(k)), + StateData(P(k + 1), V(k + 1), double(k + 1)), q); + } + + const auto rangeNoise = noiseModel::Isotropic::Sigma(1, 0.02); + for (size_t j = 0; j < measurementTimes.size(); ++j) { + const double t = measurementTimes[j]; + const Vector3 p = p0 + t * velocity; + const StateData interp(P(10 + j), V(10 + j), t); + for (const Vector3& landmark : landmarks) { + const double range = (p - landmark).norm(); + auto inner = std::make_shared>( + interp.pose, landmark, range, rangeNoise); + graph.emplace_shared>( + inner, knots, std::set{interp}, q, true); + } + } + + Values initial; + const Vector3 positionOffset(0.12, -0.08, 0.1); + const Vector3 velocityOffset(-0.08, 0.06, 0.1); + for (size_t k = 0; k < 3; ++k) { + const Vector3 p = p0 + double(k) * velocity; + initial.insert(P(k), Point(p + positionOffset)); + initial.insert(V(k), velocity + velocityOffset); + } + + const double initialError = graph.error(initial); + const Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + const double finalError = graph.error(result); + + CHECK(initialError > 0.0); + CHECK(finalError < initialError); + CHECK(finalError < 1e-4); + CHECK(result.size() == 6); // No states added at the range timestamps. + for (size_t k = 0; k < 3; ++k) { + const Vector3 expected = p0 + double(k) * velocity; + CHECK((result.at(P(k)).vector() - expected).norm() < 1e-2); + CHECK((result.at(V(k)) - velocity).norm() < 1e-2); + } +} + +/* ************************************************************************* */ +namespace qcqp_fixture { + +using Point = AugmentedPoint<3>; + +const Vector3 kPsd = Vector3::Constant(0.2); +const double kDt = 0.2, kTau = 0.08; +const Key kInterp = P(2); + +// Knot states of one interval, as nonlinear Values. +Values intervalValues() { + Values values; + values.insert(P(0), Point(Vector3(1.0, -2.0, 3.0))); + values.insert(V(0), Vector3(0.5, -0.25, 0.75)); + values.insert(P(1), Point(Vector3(1.3, -1.7, 2.8))); + values.insert(V(1), Vector3(0.25, 0.1, 0.5)); + return values; +} + +// Exact D=1 QCQP vectors of all AugmentedPoint and Vector3 entries. +Values lift(const Values& typed) { + Values lifted; + for (const auto& [key, point] : typed.extract()) { + insertQcqpValue(key, point, lifted); + } + for (const auto& [key, velocity] : typed.extract()) { + insertQcqpValue(key, velocity, lifted); + } + return lifted; +} + +// WNOA interpolation of the interval's knots at kTau. +Interpolator::PoseVel interpolate(const Values& values, + const Vector3& psd = kPsd) { + return Interpolator(psd).interpolatePoseAndVelocity( + TimestampedPoseVelocity(values.at(P(0)), + values.at(V(0)), 0.0), + TimestampedPoseVelocity(values.at(P(1)), + values.at(V(1)), kDt), + kTau); +} + +// Interpolation wrapper of a squared-range factor on the interpolated key. +std::shared_ptr> rangeFactor( + const Vector3& landmark, double range, size_t index = 2, + bool fixedNoise = true, const Vector3& psd = kPsd) { + auto inner = std::make_shared>( + P(index), landmark, range, noiseModel::Isotropic::Sigma(1, 0.1)); + return std::make_shared>( + inner, + std::set{StateData(P(0), V(0), 0.0), + StateData(P(1), V(1), kDt)}, + std::set{StateData(P(index), V(index), kTau)}, psd, + fixedNoise); +} + +// Unary linear cost on a velocity key, with QCQP support, for testing the +// interpolated-velocity coupling. +class VelocityPrior : public NoiseModelFactorN { + Vector3 measured_; + + public: + VelocityPrior(Key key, const Vector3& measured, + const SharedNoiseModel& model) + : NoiseModelFactorN(model, key), measured_(measured) {} + + Vector evaluateError(const Vector3& velocity, + OptionalMatrixType H) const override { + if (H) *H = Matrix3::Identity(); + return velocity - measured_; + } + + void qcqpFactors(NonlinearFactorGraph& costs, NonlinearEqualityConstraints&, + QcqpVariableConstraints& variableConstraints, + size_t) const override { + Matrix B(3, 4); // [A b] for the residual A*v - b. + B << Matrix3::Identity(), measured_; + const Matrix whitened = noiseModel_->Whiten(B); + costs.push_back(std::make_shared(HessianFactor( + KeyVector{key()}, SymmetricBlockMatrix(std::vector{3, 1}, + whitened.transpose() * + whitened)))); + variableConstraints.add(key(), traits::QcqpConstraints<1>); + } +}; + +// Verifies that the motion cost is exact on the canonical lift and that only +// the pose keys add lift constraints. +TEST(WnoaAugmentedPointQcqp, MotionCostMatchesNonlinear) { + const Values values = intervalValues(); + NonlinearFactorGraph graph; + graph.emplace_shared>(P(0), V(0), P(1), V(1), kDt, + kPsd); + const QcqpProblem problem(graph); + + const Values lifted = lift(values); + CHECK(graph.error(values) > 1.0); + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(lifted), + 1e-9); + EXPECT_LONGS_EQUAL(1, problem.costs().size()); + EXPECT_LONGS_EQUAL(4, problem.eConstraints().size()); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(lifted), + 1e-12); +} + +// Verifies the coordinate offset: plain Point3 poses have no lifted rows. +TEST(WnoaAugmentedPointQcqp, MotionCostWithPlainPoint3) { + Values values; + values.insert(P(0), Point3(1.0, -2.0, 3.0)); + values.insert(V(0), Vector3(0.5, -0.25, 0.75)); + values.insert(P(1), Point3(1.3, -1.7, 2.8)); + values.insert(V(1), Vector3(0.25, 0.1, 0.5)); + NonlinearFactorGraph graph; + graph.emplace_shared>(P(0), V(0), P(1), V(1), kDt, + kPsd); + const QcqpProblem problem(graph); + + Values lifted; + for (const auto& [key, vector] : values.extract()) { + insertQcqpValue(key, vector, lifted); + } + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(lifted), + 1e-9); + EXPECT_LONGS_EQUAL(0, problem.eConstraints().size()); +} + +// Verifies the interpolated range: exact cost on the lifted interpolated point, +// its lift constraints, and the three linear coupling equations. +TEST(WnoaAugmentedPointQcqp, InterpolatedRangeMatchesNonlinear) { + Values values = intervalValues(); + NonlinearFactorGraph graph; + graph.push_back(rangeFactor(Vector3(2.0, 0.5, -1.0), 2.5)); + const QcqpProblem problem(graph); + + const Point interpolated = interpolate(values).pose; + Values lifted = lift(values); + insertQcqpValue(kInterp, interpolated, lifted); + + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(lifted), + 1e-9); + // Lift constraints of P0, P1 and the interpolated key, plus one coupling. + EXPECT_LONGS_EQUAL(7, problem.eConstraints().size()); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(lifted), + 1e-9); + + // A lifted point that is not the interpolation violates the coupling. + Values moved = lift(values); + insertQcqpValue(kInterp, interpolated + Vector3(0.1, 0.0, 0.0), moved); + EXPECT(problem.eConstraints().violationNorm(moved) > 1e-3); +} + +// Verifies that factors sharing an interpolated key add its coupling once. +TEST(WnoaAugmentedPointQcqp, SharedInterpolatedKeyCouplesOnce) { + const std::array landmarks = { + Vector3(2.0, 0.5, -1.0), Vector3(-1.0, 3.0, 0.5), Vector3(0.0, 0.0, 4.0)}; + NonlinearFactorGraph shared, separate; + for (size_t i = 0; i < landmarks.size(); ++i) { + shared.push_back(rangeFactor(landmarks[i], 2.5)); + separate.push_back(rangeFactor(landmarks[i], 2.5, 2 + i)); + } + const QcqpProblem sharedProblem(shared), separateProblem(separate); + EXPECT_LONGS_EQUAL(3, sharedProblem.costs().size()); + EXPECT_LONGS_EQUAL(4 + 2 + 1, sharedProblem.eConstraints().size()); + EXPECT_LONGS_EQUAL(3, separateProblem.costs().size()); + EXPECT_LONGS_EQUAL(4 + 3 * 2 + 3, separateProblem.eConstraints().size()); +} + +// Verifies the interpolated-velocity coupling against the Interpolator. +TEST(WnoaAugmentedPointQcqp, InterpolatedVelocityKeyCoupling) { + Values values = intervalValues(); + const Key velocityKey = V(2); + auto inner = std::make_shared( + velocityKey, Vector3(0.3, 0.2, 0.1), noiseModel::Isotropic::Sigma(3, 0.1)); + NonlinearFactorGraph graph; + graph.emplace_shared>( + inner, + std::set{StateData(P(0), V(0), 0.0), + StateData(P(1), V(1), kDt)}, + std::set{StateData(P(2), velocityKey, kTau)}, kPsd, true); + const QcqpProblem problem(graph); + + const Vector3 velocity = interpolate(values).vel; + Values lifted = lift(values); + insertQcqpValue(velocityKey, velocity, lifted); + EXPECT_DOUBLES_EQUAL(0.5 * (velocity - Vector3(0.3, 0.2, 0.1)).squaredNorm() / + 0.01, + problem.costs().error(lifted), 1e-9); + EXPECT_LONGS_EQUAL(4 + 1, problem.eConstraints().size()); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(lifted), + 1e-9); + Values moved = lift(values); + insertQcqpValue(velocityKey, Vector3(velocity + Vector3(0.0, 0.1, 0.0)), + moved); + EXPECT(problem.eConstraints().violationNorm(moved) > 1e-3); +} + +// Verifies that anisotropic WNOA power spectral densities, which make the +// whitened motion cost and the interpolation differ per axis, stay exact. +TEST(WnoaAugmentedPointQcqp, AnisotropicPsdMatchesNonlinear) { + const Vector3 psd(0.05, 0.4, 1.5); + Values values = intervalValues(); + NonlinearFactorGraph graph; + graph.emplace_shared>(P(0), V(0), P(1), V(1), kDt, + psd); + graph.push_back(rangeFactor(Vector3(2.0, 0.5, -1.0), 2.5, 2, true, psd)); + const QcqpProblem problem(graph); + + Values lifted = lift(values); + insertQcqpValue(kInterp, interpolate(values, psd).pose, lifted); + CHECK(graph.error(values) > 1.0); + EXPECT_DOUBLES_EQUAL(graph.error(values), problem.costs().error(lifted), + 1e-9); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(lifted), + 1e-9); +} + +// Verifies the sign gauge: negating a lifted knot column still satisfies its +// quadratic lift constraints but violates the linear coupling, which is why +// every AugmentedPoint needs the unary constraint h = 1. +TEST(WnoaAugmentedPointQcqp, NegativeHomogeneousCoordinateBreaksCoupling) { + const Values values = intervalValues(); + NonlinearFactorGraph graph; + graph.push_back(rangeFactor(Vector3(2.0, 0.5, -1.0), 2.5)); + const QcqpProblem problem(graph); + + Values lifted = lift(values); + insertQcqpValue(kInterp, interpolate(values).pose, lifted); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(lifted), + 1e-9); + + // The negated column is s*[1, x, |x|^2] with s = -1. + Values flipped = lifted; + flipped.update(P(0), Matrix(-lifted.at(P(0)))); + EXPECT(problem.eConstraints().violationNorm(flipped) > 1e-3); +} + +// Verifies that unsupported configurations fail loudly rather than +// approximating. +TEST(WnoaAugmentedPointQcqp, RejectsUnsupportedConfigurations) { + NonlinearFactorGraph costs; + NonlinearEqualityConstraints constraints; + QcqpVariableConstraints variableConstraints; + + const auto adaptiveNoise = rangeFactor(Vector3(2.0, 0.5, -1.0), 2.5, 2, false); + CHECK_EXCEPTION(adaptiveNoise->qcqpFactors(costs, constraints, + variableConstraints), + std::runtime_error); + const auto range = rangeFactor(Vector3(2.0, 0.5, -1.0), 2.5); + CHECK_EXCEPTION(range->qcqpFactors(costs, constraints, variableConstraints, 2), + std::invalid_argument); + WnoaMotionFactor motion(P(0), V(0), P(1), V(1), kDt, kPsd); + CHECK_EXCEPTION(motion.qcqpFactors(costs, constraints, variableConstraints, 2), + std::invalid_argument); + WnoaMotionFactor pose2Motion(Key(0), Key(1), Key(2), Key(3), kDt, + Vector3::Ones()); + CHECK_EXCEPTION( + pose2Motion.qcqpFactors(costs, constraints, variableConstraints), + std::runtime_error); +} + +// Verifies that the QCQP solved locally with ALM, with h = 1 on every point, +// reproduces the Levenberg-Marquardt solution of the nonlinear range-only +// problem with shared interpolated keys. +TEST(WnoaAugmentedPointQcqp, RangeOnlyAugmentedLagrangianMatchesNonlinear) { + const Vector3 p0(0.6, -0.2, 0.7), velocity(0.35, 0.1, -0.15); + const std::set knots = {StateData(P(0), V(0), 0.0), + StateData(P(1), V(1), 1.0), + StateData(P(2), V(2), 2.0)}; + const std::array landmarks = { + Vector3(0.0, 0.0, 0.0), Vector3(3.0, 0.0, 0.0), Vector3(0.0, 3.0, 0.0), + Vector3(0.0, 0.0, 3.0)}; + const std::array times = {0.25, 0.75, 1.25, 1.75}; + + NonlinearFactorGraph graph; + for (size_t k = 0; k < 2; ++k) { + graph.emplace_shared>( + StateData(P(k), V(k), double(k)), + StateData(P(k + 1), V(k + 1), double(k + 1)), kPsd); + } + for (size_t j = 0; j < times.size(); ++j) { + const StateData interp(P(10 + j), V(10 + j), times[j]); + for (const Vector3& landmark : landmarks) { + auto inner = std::make_shared>( + interp.pose, landmark, (p0 + times[j] * velocity - landmark).norm(), + noiseModel::Isotropic::Sigma(1, 0.05)); + graph.emplace_shared>( + inner, knots, std::set{interp}, kPsd, true); + } + } + + Values initial; + for (size_t k = 0; k < 3; ++k) { + initial.insert(P(k), Point(p0 + double(k) * velocity + + Vector3(0.05, -0.04, 0.03))); + initial.insert(V(k), Vector3(velocity + Vector3(-0.04, 0.03, 0.05))); + } + const Values nonlinear = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // Lifted initial values: knots plus one interpolated point per time. + Values liftedInitial = lift(initial); + Interpolator interpolator(kPsd); + QcqpProblem problem(graph); + for (size_t j = 0; j < times.size(); ++j) { + const size_t k = times[j] < 1.0 ? 0 : 1; + const auto pv = interpolator.interpolatePoseAndVelocity( + TimestampedPoseVelocity(initial.at(P(k)), + initial.at(V(k)), double(k)), + TimestampedPoseVelocity(initial.at(P(k + 1)), + initial.at(V(k + 1)), + double(k + 1)), + times[j]); + insertQcqpValue(P(10 + j), pv.pose, liftedInitial); + } + Matrix homogeneous = Matrix::Zero(1, 5); + homogeneous(0, 0) = 1.0; + for (const auto& [key, lifted] : liftedInitial.extract()) { + if (lifted.rows() != 5) continue; + problem.addConstraint(LinearConstraint::Equal(JacobianFactor( + key, homogeneous, Vector::Ones(1), noiseModel::Unit::Create(1)))); + } + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(liftedInitial), + 1e-9); + + auto params = std::make_shared(); + params->maxIterations = 200; + params->absoluteViolationTolerance = 1e-9; + params->relativeViolationTolerance = 1e-9; + params->relativeCostTolerance = 1e-12; + params->absoluteStationarityTolerance = 1e-8; + const Values solved = + AugmentedLagrangianOptimizer(problem, liftedInitial, params).optimize(); + + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(solved), 1e-6); + for (size_t k = 0; k < 3; ++k) { + const Point recovered = fromQcqpValue(solved.at(P(k))); + EXPECT(assert_equal(nonlinear.at(P(k)), recovered, 1e-4)); + const Vector3 recoveredVelocity = solved.at(V(k)).col(0); + EXPECT(assert_equal(nonlinear.at(V(k)), recoveredVelocity, 1e-4)); + } + EXPECT_DOUBLES_EQUAL(graph.error(nonlinear), problem.costs().error(solved), + 1e-6); +} + +} // namespace qcqp_fixture +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/slam/doc/RangeOnlyWnoaAugmentedPoint.ipynb b/gtsam_unstable/slam/doc/RangeOnlyWnoaAugmentedPoint.ipynb new file mode 100644 index 0000000000..82b6b8a661 --- /dev/null +++ b/gtsam_unstable/slam/doc/RangeOnlyWnoaAugmentedPoint.ipynb @@ -0,0 +1,446 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "113f7744", + "metadata": {}, + "source": [ + "# Continuous-time range-only localization with WNOA\n", + "\n", + "This example estimates a 3D trajectory from range measurements to known landmarks. The trajectory is represented by positions and velocities at four knot times. A white-noise-on-acceleration (WNOA) prior connects consecutive knots, while interpolation factors incorporate ranges measured between them.\n", + "\n", + "The positions use `AugmentedPoint3` and the measurements use `KnownLandmarkSquaredRangeFactorAugmentedPoint3`, both from `gtsam_unstable`. We first solve the resulting nonlinear least-squares problem with GTSAM's Levenberg–Marquardt optimizer. We then rewrite the same problem as a quadratically constrained quadratic program (QCQP) and solve its semidefinite relaxation, which provides a lower bound on the cost. If a feasible solution reaches that bound to within the solver tolerance, it is a global minimizer of the nonlinear problem." + ] + }, + { + "cell_type": "markdown", + "id": "9d271bc0", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415. All Rights Reserved. Authors: Frank Dellaert, et al. (see THANKS for the full author list). See LICENSE for the license information." + ] + }, + { + "cell_type": "markdown", + "id": "ec66e589", + "metadata": {}, + "source": [ + "[![Open in Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam_unstable/slam/doc/RangeOnlyWnoaAugmentedPoint.ipynb)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f62b24f7", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "try:\n", + " import google.colab\n", + "except ImportError:\n", + " pass" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "aadfec55", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import plotly.graph_objects as go\n", + "import gtsam\n", + "import gtsam_unstable as gu" + ] + }, + { + "cell_type": "markdown", + "id": "8517dcb0", + "metadata": {}, + "source": [ + "## Range measurements and trajectory\n", + "\n", + "We simulate a smooth trajectory observed by four fixed landmarks. Range measurements are taken at the first knot and at two times inside each subsequent knot interval. The knot positions and velocities are the only unknowns; landmark locations remain fixed.\n", + "\n", + "For a landmark at $L_i$ and measured distance $d_i$, the squared-range factor uses\n", + "\n", + "$$\n", + "e_i(p(t)) = \\|p(t)-L_i\\|^2-d_i^2.\n", + "$$\n", + "\n", + "If the raw range has standard deviation $\\sigma_r$, first-order uncertainty propagation gives $\\sigma_{d^2}\\approx 2d_i\\sigma_r$. We use this value for the one-dimensional factor noise model, since the residual is expressed in squared meters." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ed1c12b9", + "metadata": {}, + "outputs": [], + "source": [ + "rng = np.random.default_rng(7)\n", + "\n", + "knot_times = np.array([0.0, 1.0, 2.0, 3.0])\n", + "measurement_times = np.array([0.0, 0.25, 0.75, 1.25, 1.75, 2.25, 2.75])\n", + "landmarks = np.array([\n", + " [0.0, 0.0, 0.0],\n", + " [3.0, 0.0, 0.0],\n", + " [0.0, 3.0, 0.0],\n", + " [0.0, 0.0, 3.0],\n", + "])\n", + "range_sigma = 0.02 # m\n", + "q_psd = np.full(3, 0.2) # WNOA acceleration power spectral density\n", + "\n", + "\n", + "def true_position(t):\n", + " t = np.asarray(t)\n", + " return np.stack((\n", + " 0.6 + 0.35 * t + 0.06 * np.sin(0.9 * t),\n", + " -0.2 + 0.1 * t - 0.07 * (1 - np.cos(0.7 * t)),\n", + " 0.7 - 0.15 * t + 0.06 * np.sin(1.1 * t),\n", + " ), axis=-1)\n", + "\n", + "\n", + "def true_velocity(t):\n", + " t = np.asarray(t)\n", + " return np.stack((\n", + " 0.35 + 0.054 * np.cos(0.9 * t),\n", + " 0.1 - 0.049 * np.sin(0.7 * t),\n", + " -0.15 + 0.066 * np.cos(1.1 * t),\n", + " ), axis=-1)\n", + "\n", + "\n", + "distances = np.linalg.norm(\n", + " true_position(measurement_times)[:, None, :] - landmarks[None, :, :],\n", + " axis=2,\n", + ")\n", + "measured_ranges = distances + rng.normal(0, range_sigma, distances.shape)" + ] + }, + { + "cell_type": "markdown", + "id": "b343b9f7", + "metadata": {}, + "source": [ + "## Build the factor graph\n", + "\n", + "Each knot has a position key and a velocity key. `WnoaMotionFactorAugmentedPoint3` constrains consecutive knots using the WNOA prior.\n", + "\n", + "A range at the first knot is connected directly to its position. For each intermediate measurement, `WnoaInterpFactorAugmentedPoint3` wraps the squared-range factor and propagates its constraint to the surrounding knot positions and velocities. There are no optimization variables at the intermediate measurement times." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d39b8d85", + "metadata": {}, + "outputs": [], + "source": "states = [\n gtsam.StateData(gtsam.symbol(\"p\", i), gtsam.symbol(\"v\", i), float(t))\n for i, t in enumerate(knot_times)\n]\n\ninitial = gtsam.Values()\nfor state, t in zip(states, knot_times):\n position = true_position(t) + np.array([0.15, -0.10, 0.13])\n velocity = true_velocity(t) + np.array([-0.10, 0.08, -0.06])\n gu.insertAugmentedPoint3(state.pose, gu.AugmentedPoint3(position), initial)\n initial.insert(state.velocity, velocity)\n\n\ndef build_graph(keep=lambda index, landmark: True):\n \"\"\"Build the WNOA graph with the ranges selected by keep(time_index, landmark_index).\"\"\"\n graph = gtsam.NonlinearFactorGraph()\n for left, right in zip(states[:-1], states[1:]):\n graph.add(gu.WnoaMotionFactorAugmentedPoint3(left, right, q_psd))\n\n for index, (t, ranges) in enumerate(zip(measurement_times, measured_ranges)):\n # Ranges at the first knot connect directly to its position; the others are interpolated.\n interpolated_state = gtsam.StateData(\n gtsam.symbol(\"p\", 100 + index), gtsam.symbol(\"v\", 100 + index), float(t))\n position_key = states[0].pose if index == 0 else interpolated_state.pose\n for j, (landmark, distance) in enumerate(zip(landmarks, ranges)):\n if not keep(index, j):\n continue\n noise = gtsam.noiseModel.Isotropic.Sigma(1, 2 * distance * range_sigma)\n range_factor = gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3(\n position_key, landmark, float(distance), noise)\n graph.add(range_factor if index == 0 else gu.WnoaInterpFactorAugmentedPoint3(\n range_factor, set(states), {interpolated_state}, q_psd, True, True))\n return graph\n\n\ngraph = build_graph()\nprint(f\"Knot states: {len(states)}\")\nprint(f\"Range measurements: {measured_ranges.size}\")\nprint(f\"Factor graph: {graph.size()} factors, {initial.size()} variables\")" + }, + { + "cell_type": "markdown", + "id": "4059f01a", + "metadata": {}, + "source": [ + "## Estimate the knot states\n", + "\n", + "Levenberg–Marquardt minimizes the sum of the WNOA motion costs and squared-range measurement costs. Because the problem is nonlinear, the result depends on the initial estimate; the perturbed knot states above provide a nearby starting trajectory." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9774ce66", + "metadata": {}, + "outputs": [], + "source": [ + "result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()\n", + "\n", + "\n", + "def knot_values(values):\n", + " positions = np.array([\n", + " np.asarray(gu.atAugmentedPoint3(state.pose, values).vector()).reshape(3)\n", + " for state in states\n", + " ])\n", + " velocities = np.array([\n", + " np.asarray(values.atVector(state.velocity)).reshape(3)\n", + " for state in states\n", + " ])\n", + " return positions, velocities\n", + "\n", + "\n", + "initial_positions, initial_velocities = knot_values(initial)\n", + "estimated_positions, estimated_velocities = knot_values(result)\n", + "\n", + "print(f\"Initial graph error: {graph.error(initial):.4f}\")\n", + "print(f\"Final graph error: {graph.error(result):.4f}\")\n", + "print(f\"Knot position RMSE: {np.sqrt(np.mean(np.sum((estimated_positions - true_position(knot_times))**2, axis=1))):.4f} m\")" + ] + }, + { + "cell_type": "markdown", + "id": "4d4ca86b", + "metadata": {}, + "source": [ + "## Trajectory estimate\n", + "\n", + "The estimated knot positions and velocities define a continuous trajectory between the knots. For the Euclidean WNOA model, the conditional mean follows cubic Hermite interpolation. The 3D plot compares that trajectory with ground truth and the initial estimate; the landmarks are shown for spatial reference." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "020fdd8d", + "metadata": {}, + "outputs": [], + "source": [ + "def interpolate_positions(times, positions, velocities):\n", + " segment = np.clip(\n", + " np.searchsorted(knot_times, times, side=\"right\") - 1,\n", + " 0, len(knot_times) - 2,\n", + " )\n", + " dt = knot_times[segment + 1] - knot_times[segment]\n", + " u = (times - knot_times[segment]) / dt\n", + " return (\n", + " (2 * u**3 - 3 * u**2 + 1)[:, None] * positions[segment]\n", + " + (u**3 - 2 * u**2 + u)[:, None] * dt[:, None] * velocities[segment]\n", + " + (-2 * u**3 + 3 * u**2)[:, None] * positions[segment + 1]\n", + " + (u**3 - u**2)[:, None] * dt[:, None] * velocities[segment + 1]\n", + " )\n", + "\n", + "\n", + "plot_times = np.linspace(knot_times[0], knot_times[-1], 200)\n", + "true_path = true_position(plot_times)\n", + "initial_path = interpolate_positions(plot_times, initial_positions, initial_velocities)\n", + "estimated_path = interpolate_positions(plot_times, estimated_positions, estimated_velocities)\n", + "\n", + "fig = go.Figure()\n", + "fig.add_scatter3d(x=true_path[:, 0], y=true_path[:, 1], z=true_path[:, 2], mode=\"lines\", name=\"Ground truth\", line=dict(width=5))\n", + "fig.add_scatter3d(x=initial_path[:, 0], y=initial_path[:, 1], z=initial_path[:, 2], mode=\"lines\", name=\"Initial estimate\", line=dict(width=3, dash=\"dash\"))\n", + "fig.add_scatter3d(x=estimated_path[:, 0], y=estimated_path[:, 1], z=estimated_path[:, 2], mode=\"lines\", name=\"WNOA estimate\", line=dict(width=5))\n", + "fig.add_scatter3d(x=estimated_positions[:, 0], y=estimated_positions[:, 1], z=estimated_positions[:, 2], mode=\"markers\", name=\"Estimated knots\", marker=dict(size=5))\n", + "fig.add_scatter3d(x=landmarks[:, 0], y=landmarks[:, 1], z=landmarks[:, 2], mode=\"markers\", name=\"Known landmarks\", marker=dict(size=6, symbol=\"diamond\"))\n", + "fig.update_layout(title=\"Range-only trajectory estimate\", scene=dict(xaxis_title=\"x (m)\", yaxis_title=\"y (m)\", zaxis_title=\"z (m)\", aspectmode=\"data\"), margin=dict(l=0, r=0, b=0, t=45))\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "aa68e56e", + "metadata": {}, + "source": [ + "## Position error\n", + "\n", + "The Euclidean distance between the estimated and true positions measures trajectory accuracy in meters. This plot evaluates the continuous trajectories, not just their knot positions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5e36a8db", + "metadata": {}, + "outputs": [], + "source": [ + "initial_error = np.linalg.norm(initial_path - true_path, axis=1)\n", + "estimated_error = np.linalg.norm(estimated_path - true_path, axis=1)\n", + "\n", + "fig = go.Figure()\n", + "fig.add_scatter(x=plot_times, y=initial_error, mode=\"lines\", name=\"Initial estimate\", line=dict(dash=\"dash\"))\n", + "fig.add_scatter(x=plot_times, y=estimated_error, mode=\"lines\", name=\"WNOA estimate\")\n", + "fig.update_layout(title=\"Trajectory position error\", xaxis_title=\"Time (s)\", yaxis_title=\"Position error (m)\")\n", + "fig.show()\n", + "\n", + "print(f\"Mean position error, initial: {initial_error.mean():.4f} m\")\n", + "print(f\"Mean position error, WNOA: {estimated_error.mean():.4f} m\")" + ] + }, + { + "cell_type": "markdown", + "id": "7098f908", + "metadata": {}, + "source": [ + "## Quadratic lifting and semidefinite relaxation\n", + "\n", + "A squared-range residual is quadratic in the position, so each position can be written with the lifted vector $X=[h, x, u]$, where $x$ holds the coordinates, $u=x^\\top x$, and $h=1$. The lift is exact when $x^\\top x = hu$ and $h^2=1$. In this form the squared-range cost is quadratic in $X$, and the WNOA motion cost is already quadratic in the knot positions and velocities, which stay as ordinary 3D vectors. Calling `gtsam.QcqpProblem(graph)` performs this conversion for the whole factor graph without approximation.\n", + "\n", + "An interpolated position $x(\\tau)$, a linear combination of the surrounding knot positions and velocities, is not a state, so its squared norm cannot be read from a knot variable. For each measurement time the conversion therefore introduces an auxiliary `AugmentedPoint3` with its own lift constraints, linked to the surrounding knot positions and velocities by a linear equality. Each measurement time uses one auxiliary point, shared by all ranges taken at that time. The quadratic constraints alone allow $h=\\pm 1$ separately for every point, which would flip the sign of the coordinates in these linear links, so we also fix $h=1$.\n", + "\n", + "The semidefinite relaxation replaces the rank-one matrix $[1;y][1;y]^\\top$, built from all lifted variables $y$, with a positive semidefinite matrix whose first entry is 1. Its optimal cost is a lower bound on the QCQP cost. If a feasible point of the QCQP attains that lower bound, the point is a global minimizer and the relaxation is tight for this instance. A numerical solver only reaches the bound up to its tolerance, so the bound is meaningful only when the solver reports an optimal status, and a point that matches it to within that tolerance is optimal only to the same accuracy. If the gap is clearly nonzero, either the relaxation is not tight or the solver did not converge; the bound then only limits how far from optimal any solution can be. Solving the relaxation requires MOSEK, which GTSAM uses through `MosekMonolithicSDP` when GTSAM is configured with `GTSAM_WITH_MOSEK=ON`." + ] + }, + { + "cell_type": "markdown", + "id": "49219705", + "metadata": {}, + "source": [ + "## Sparse intermediate-time measurements\n", + "\n", + "To make the problem sparse, we keep all four ranges at the first knot but only two ranges, to alternating landmarks, at each intermediate time. This leaves 16 of the 28 measurements. Levenberg–Marquardt solves the sparse nonlinear problem first so the relaxation has a reference." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c5a092d8", + "metadata": {}, + "outputs": [], + "source": [ + "def sparse_ranges(index, landmark):\n", + " return index == 0 or landmark in (index % 4, (index + 1) % 4)\n", + "\n", + "\n", + "sparse_graph = build_graph(sparse_ranges)\n", + "sparse_result = gtsam.LevenbergMarquardtOptimizer(sparse_graph, initial).optimize()\n", + "\n", + "# One auxiliary lifted point per interpolated measurement time.\n", + "interpolated_keys = [gtsam.symbol(\"p\", 100 + i) for i in range(1, len(measurement_times))]\n", + "position_keys = [state.pose for state in states] + interpolated_keys\n", + "\n", + "problem = gtsam.QcqpProblem(sparse_graph)\n", + "fix_h = np.array([[1.0, 0.0, 0.0, 0.0, 0.0]])\n", + "for key in position_keys:\n", + " problem.addConstraint(gtsam.LinearConstraint.Equal(\n", + " gtsam.JacobianFactor(key, fix_h, np.ones(1), gtsam.noiseModel.Unit.Create(1))))\n", + "\n", + "print(f\"Sparse graph: {sparse_graph.size() - len(states) + 1} range factors\")\n", + "print(f\"Nonlinear error after Levenberg–Marquardt: {sparse_graph.error(sparse_result):.4f}\")\n", + "print(f\"QCQP (cost, equality, inequality) dimensions: {problem.dim()}\")" + ] + }, + { + "cell_type": "markdown", + "id": "2de16d6f", + "metadata": {}, + "source": [ + "## Solve the relaxation\n", + "\n", + "MOSEK solves the relaxation and returns one lifted column per variable. The relaxation's objective is a lower bound on the QCQP cost, and, for the fixed-noise interpolation factors used here, the QCQP is the original nonlinear problem rewritten exactly. Any set of knot states is feasible for the nonlinear problem, so its nonlinear cost is an upper bound. We evaluate that cost at the knot states read from the SDP solution and also keep the Levenberg–Marquardt cost. A gap near zero, within the solver's accuracy, means the best upper bound is optimal to that accuracy and the relaxation is tight for this instance. A clearly positive gap means the relaxation is not tight or the solver did not converge. Because the solver stops at a finite tolerance, the gap can be slightly negative; a magnitude near that tolerance counts as zero, and a clearly negative gap indicates a problem with the solve or the model. The cell below skips the solve with an explanation if this GTSAM build has no MOSEK support or no usable license." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6639a1d4", + "metadata": {}, + "outputs": [], + "source": [ + "sdp = None\n", + "if not hasattr(gtsam, \"MosekMonolithicSDP\"):\n", + " print(\"This GTSAM build has no MOSEK support (configure GTSAM with GTSAM_WITH_MOSEK=ON); skipping the SDP.\")\n", + "else:\n", + " try:\n", + " sdp = gtsam.MosekMonolithicSDP(problem)\n", + " if not sdp.solve():\n", + " raise RuntimeError(sdp.problemStatus())\n", + " except RuntimeError as error:\n", + " print(f\"MOSEK could not solve the relaxation: {error}\")\n", + " sdp = None" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "43e78f94", + "metadata": {}, + "outputs": [], + "source": [ + "def knot_errors(values):\n", + " positions, _ = knot_values(values)\n", + " return np.linalg.norm(positions - true_position(knot_times), axis=1)\n", + "\n", + "\n", + "if sdp is not None:\n", + " qcqp_values = sdp.qcqpValues()\n", + " sdp_values = gtsam.Values() # knot states in the original (unlifted) variables\n", + " for state in states:\n", + " sdp_values.insert(state.velocity, qcqp_values.atMatrix(state.velocity)[:, 0])\n", + " gu.insertAugmentedPoint3(\n", + " state.pose, gu.fromQcqpValue3(qcqp_values.atMatrix(state.pose)), sdp_values)\n", + "\n", + " lower_bound = sdp.objectiveValue()\n", + " sdp_cost = sparse_graph.error(sdp_values)\n", + " lm_cost = sparse_graph.error(sparse_result)\n", + " _, violation, _ = problem.evaluate(qcqp_values)\n", + " print(f\"MOSEK problem status: {sdp.problemStatus()}\")\n", + " print(f\"SDP lower bound: {lower_bound:.6f}\")\n", + " print(f\"Nonlinear cost at the SDP knot states: {sdp_cost:.6f}\")\n", + " print(f\"Nonlinear cost, Levenberg–Marquardt: {lm_cost:.6f}\")\n", + " print(f\"Relative gap to the best upper bound: {(min(sdp_cost, lm_cost) - lower_bound) / lower_bound:.2e}\")\n", + " print(f\"Constraint violation of the lifted SDP solution: {violation:.2e}\")\n", + "\n", + " fig = go.Figure()\n", + " for name, values in ((\"Initial estimate\", initial), (\"Levenberg–Marquardt\", sparse_result), (\"SDP relaxation\", sdp_values)):\n", + " fig.add_bar(x=[f\"Knot {i}\" for i in range(len(states))], y=knot_errors(values), name=name)\n", + " fig.update_layout(title=\"Knot position error, sparse measurements\", yaxis_title=\"Position error (m)\", barmode=\"group\")\n", + " fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "ce2958ff", + "metadata": {}, + "source": [ + "## Rank diagnostics\n", + "\n", + "For each variable, the eigenvalue ratio (EVR) divides the two largest eigenvalues of its block of the relaxed matrix, $[1\\ y^\\top; y\\ Y]$. A very large ratio means the block is numerically rank one, so the relaxation produced an exact lifted point for that variable. The constraint violation printed above measures how far the lifted SDP solution is from satisfying the lift constraints and the interpolation links; a block that is exactly rank one satisfies them, but a small violation does not by itself prove that a block is rank one. These diagnostics describe the solution returned by the solver, not every optimal solution, because an optimal relaxation can have several solutions of different rank.\n", + "\n", + "A knot position that has no direct range measurement enters the costs only through its coordinates $x$ in the motion and interpolation terms, so nothing in the objective constrains the second moments involving its $u$ entry. Its ratio can therefore be small even when its cost is unaffected. For such blocks, the gap between the lower bound and the upper bound is the more reliable indicator of tightness than the ratio." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "575da5df", + "metadata": {}, + "outputs": [], + "source": [ + "if sdp is not None:\n", + " names = [gtsam.Symbol(key).string() for key in sdp.orderedKeys()]\n", + " ratios = sdp.variableEVRs()\n", + " fig = go.Figure(go.Bar(x=names, y=ratios))\n", + " fig.update_layout(title=\"Eigenvalue ratio per variable\", yaxis_title=\"EVR\", yaxis_type=\"log\")\n", + " fig.show()\n", + " for name, ratio in zip(names, ratios):\n", + " print(f\"{name:>5}: EVR {ratio:.2e}\")" + ] + }, + { + "cell_type": "markdown", + "id": "f42a2adc", + "metadata": {}, + "source": [ + "The range observations constrain the trajectory at their actual measurement times, even though only the knot states are estimated. The WNOA prior couples neighboring positions and velocities, giving a smooth trajectory between observations. Unlike single-point range localization, this graph estimates **a continuous path** from asynchronous range measurements." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.5" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index aa818402b4..5f68c16e7b 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -120,9 +120,16 @@ if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) endif() - # Wrap - matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam_unstable" - "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}" "${GTSAM_ENABLE_BOOST_SERIALIZATION}") + # Include the MATLAB-only WNOA adapter in the unstable wrapper. + set(unstable_matlab_interface_files + ${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i + ${GTSAM_SOURCE_DIR}/matlab/wnoa_unstable.i) + + # The fourth argument adds matlab/ to the include path for + # MatlabWnoaInterpFactor.h. + matlab_wrap("${unstable_matlab_interface_files}" "gtsam_unstable" + "${GTSAM_ADDITIONAL_LIBRARIES}" "${GTSAM_SOURCE_DIR}/matlab" + "${mexFlags}" "${ignore}" "${GTSAM_ENABLE_BOOST_SERIALIZATION}") set_target_properties( gtsam_unstable_matlab_wrapper PROPERTIES BUILD_RPATH diff --git a/matlab/MatlabWnoaInterpFactor.h b/matlab/MatlabWnoaInterpFactor.h new file mode 100644 index 0000000000..cfb1d1001a --- /dev/null +++ b/matlab/MatlabWnoaInterpFactor.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + +// MATLAB-only constructor bridge for the AugmentedPoint3 WNOA factor. +// MATLAB's generated wrappers cannot directly construct std::set +// or pass NoiseModelFactor::shared_ptr as a constructor argument. +class MatlabWnoaInterpFactorAugmentedPoint3 + : public WnoaInterpFactor> { + private: + using Base = WnoaInterpFactor>; + + static NoiseModelFactor::shared_ptr cloneFactor( + const NoiseModelFactor& factor) { + auto copy = std::dynamic_pointer_cast(factor.clone()); + if (!copy) { + throw std::invalid_argument( + "MATLAB WNOA interpolation requires a cloneable NoiseModelFactor"); + } + return copy; + } + + static Eigen::Matrix checkedPSD(const Vector& q_psd_diag) { + if (q_psd_diag.size() != 3) { + throw std::invalid_argument( + "MATLAB WNOA interpolation expects a 3-element PSD vector"); + } + return q_psd_diag; + } + + public: + MatlabWnoaInterpFactorAugmentedPoint3( + const NoiseModelFactor& inner_factor, const StateData& left, + const StateData& interpolated, const StateData& right, + const Vector& q_psd_diag, bool fixed_noise_model = false, + bool precomp_interp_mats = true) + : Base(cloneFactor(inner_factor), std::set{left, right}, + std::set{interpolated}, checkedPSD(q_psd_diag), + fixed_noise_model, precomp_interp_mats) {} +}; + +} // namespace gtsam diff --git a/matlab/gtsam_unstable_tests/testWnoaAugmentedPoint.m b/matlab/gtsam_unstable_tests/testWnoaAugmentedPoint.m new file mode 100644 index 0000000000..de9be16de1 --- /dev/null +++ b/matlab/gtsam_unstable_tests/testWnoaAugmentedPoint.m @@ -0,0 +1,213 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Tests WNOA factors with unstable AugmentedPoint3 states and their QCQP conversion. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function testWnoaAugmentedPoint() +% Regression tests for unstable AugmentedPoint3 WNOA MATLAB bindings. +% Run with the stable and unstable MATLAB wrapper directories on the path. + +assert(exist('gtsam_wrapper', 'file') == 3, ... + 'Add the build-x64/wrap/gtsam_mex directory to the MATLAB path.'); +assert(exist('gtsam_unstable_wrapper', 'file') == 3, ... + 'Add the build-x64/wrap/gtsam_unstable_mex directory to the MATLAB path.'); + +testMotion(); +testInterpolatedRange(); +testRangeOnlyOptimization(); +testQcqpConversion(); +testQcqpLiftedValues(); +fprintf('WNOA AugmentedPoint3 MATLAB tests passed (5 tests).\n'); +end + +function testMotion() +left = gtsam.StateData(uint64(1), uint64(2), 0.0); +right = gtsam.StateData(uint64(3), uint64(4), 0.2); +factor = gtsam.WnoaMotionFactorAugmentedPoint3(left, right, ones(3, 1)); + +values = gtsam.Values(); +p = [1; -2; 3]; +v = [0.5; -0.25; 0.75]; +insertState(values, uint64(1), uint64(2), p, v); +insertState(values, uint64(3), uint64(4), p + 0.2 * v, v); +assert(abs(factor.error(values)) < 1e-9, 'Motion residual is not zero.'); +fprintf(' Motion residual: passed\n'); +end + +function testInterpolatedRange() +left = gtsam.StateData(uint64(1), uint64(2), 0.0); +mid = gtsam.StateData(uint64(5), uint64(6), 0.5); +right = gtsam.StateData(uint64(3), uint64(4), 1.0); +noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.1); +inner = gtsam.KnownLandmarkSquaredRangeFactorAugmentedPoint3( ... + uint64(5), [2; 0; 0], 1.5, noise); +factor = gtsam.MatlabWnoaInterpFactorAugmentedPoint3( ... + inner, left, mid, right, 0.1 * ones(3, 1), true, true); + +kv = factor.keys(); +keys = zeros(1, double(kv.size()), 'uint64'); +for i = 0:numel(keys)-1 + keys(i+1) = kv.at(i); +end +assert(isequal(keys, uint64([1, 2, 3, 4])), ... + 'Interpolated factor uses unexpected optimization keys.'); +assert(factor.dim() == 1, 'Squared-range factor should have one residual.'); + +values = gtsam.Values(); +insertState(values, uint64(1), uint64(2), [1; 0; 0], [1; 0; 0]); +insertState(values, uint64(3), uint64(4), [2; 0; 0], [1; 0; 0]); +expected = 0.5 * ((0.5^2 - 1.5^2) / 0.1)^2; +assert(abs(factor.error(values) - expected) < 1e-7, ... + 'Interpolated squared-range error does not match the expected value.'); +fprintf(' Interpolated range residual and keys: passed\n'); +end + +function testRangeOnlyOptimization() +p0 = [0.6; -0.2; 0.7]; +v = [0.35; 0.10; -0.15]; +q = 0.2 * ones(3, 1); +landmarks = [0, 3, 0, 0; ... + 0, 0, 3, 0; ... + 0, 0, 0, 3]; + +states = cell(1, 3); +for i = 1:3 + states{i} = gtsam.StateData(uint64(2*i - 1), uint64(2*i), i - 1); +end + +graph = gtsam.NonlinearFactorGraph(); +for i = 1:2 + graph.add(gtsam.WnoaMotionFactorAugmentedPoint3( ... + states{i}, states{i+1}, q)); +end + +rangeNoise = gtsam.noiseModel.Isotropic.Sigma(1, 0.02); +% Anchor the first knot with measured ranges, without an AugmentedPoint prior. +for j = 1:size(landmarks, 2) + landmark = landmarks(:, j); + measured = norm(p0 - landmark); + graph.add(gtsam.KnownLandmarkSquaredRangeFactorAugmentedPoint3( ... + uint64(1), landmark, measured, rangeNoise)); +end + +% Measurements are between knots; only the knot states are optimized. +times = [0.25, 0.75, 1.25, 1.75]; +for i = 1:numel(times) + t = times(i); + mid = gtsam.StateData(uint64(100 + i), uint64(200 + i), t); + if t < 1 + left = states{1}; + right = states{2}; + else + left = states{2}; + right = states{3}; + end + position = p0 + t * v; + for j = 1:size(landmarks, 2) + landmark = landmarks(:, j); + measured = norm(position - landmark); + inner = gtsam.KnownLandmarkSquaredRangeFactorAugmentedPoint3( ... + uint64(100 + i), landmark, measured, rangeNoise); + graph.add(gtsam.MatlabWnoaInterpFactorAugmentedPoint3( ... + inner, left, mid, right, q, true, true)); + end +end + +initial = gtsam.Values(); +for i = 1:3 + position = p0 + (i - 1) * v + [0.12; -0.08; 0.10]; + velocity = v + [-0.08; 0.06; 0.10]; + insertState(initial, uint64(2*i - 1), uint64(2*i), ... + position, velocity); +end + +initialError = graph.error(initial); +result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize(); +finalError = graph.error(result); +assert(finalError < initialError, 'Optimization failed to reduce error.'); +assert(finalError < 1e-4, 'Optimization did not recover the trajectory.'); +assert(result.size() == 6, 'Unexpected number of optimized states.'); + +for i = 1:3 + position = gtsam.atAugmentedPoint3(uint64(2*i - 1), result).vector(); + velocity = result.atPoint3(uint64(2*i)); + assert(norm(position - (p0 + (i-1)*v)) < 1e-2, ... + 'Recovered knot position is inaccurate.'); + assert(norm(velocity - v) < 1e-2, ... + 'Recovered knot velocity is inaccurate.'); +end +fprintf(' Range-only optimization: passed (final error %.3g)\n', finalError); +end + +function testQcqpConversion() +% Motion and interpolated range factors convert to QCQPs; unsupported +% configurations are rejected with catchable errors. +left = gtsam.StateData(uint64(1), uint64(2), 0.0); +mid = gtsam.StateData(uint64(5), uint64(6), 0.5); +right = gtsam.StateData(uint64(3), uint64(4), 1.0); +q = 0.2 * ones(3, 1); +noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.1); + +motionGraph = gtsam.NonlinearFactorGraph(); +motionGraph.add(gtsam.WnoaMotionFactorAugmentedPoint3(left, right, q)); +assert(isa(gtsam.QcqpProblem(motionGraph), 'gtsam.QcqpProblem')); +assertThrows(@() gtsam.QcqpProblem(motionGraph, 2), 'only supports D=1'); + +inner = gtsam.KnownLandmarkSquaredRangeFactorAugmentedPoint3( ... + uint64(5), [2; 0; 0], 1.5, noise); +interpolated = gtsam.NonlinearFactorGraph(); +interpolated.add(gtsam.MatlabWnoaInterpFactorAugmentedPoint3( ... + inner, left, mid, right, q, true, true)); +assert(isa(gtsam.QcqpProblem(interpolated), 'gtsam.QcqpProblem')); +assertThrows(@() gtsam.QcqpProblem(interpolated, 2), 'only supports D=1'); + +% Interpolation noise that depends on the linearization point has no exact QCQP. +adaptive = gtsam.NonlinearFactorGraph(); +adaptive.add(gtsam.MatlabWnoaInterpFactorAugmentedPoint3( ... + inner, left, mid, right, q, false, true)); +assertThrows(@() gtsam.QcqpProblem(adaptive), 'fixed_noise_model = true'); +fprintf(' QCQP conversion and rejected configurations: passed\n'); +end + +function testQcqpLiftedValues() +% The lifted position [h; x; u] used by the interpolated-range QCQP round-trips. +point = gtsam.AugmentedPoint3([1; -2; 3]); +lifted = gtsam.qcqpValue3(point); +assert(isequal(size(lifted), [5, 1]), 'Lifted position must have 5 rows.'); +assert(norm(lifted - [1; 1; -2; 3; 14]) < 1e-12, 'Unexpected lifted position.'); +recovered = gtsam.fromQcqpValue3(lifted); +assert(norm(recovered.vector() - [1; -2; 3]) < 1e-12, 'Lift round trip failed.'); +% A negative homogeneous coordinate flips the whole column; recovery divides by h. +flipped = gtsam.fromQcqpValue3(-lifted); +assert(norm(flipped.vector() - [1; -2; 3]) < 1e-12, 'Sign-flipped lift not recovered.'); + +values = gtsam.Values(); +gtsam.insertQcqpValue3(uint64(7), point, values); +assert(norm(values.atMatrix(uint64(7)) - lifted) < 1e-12, 'Inserted lift mismatch.'); +fprintf(' Lifted position values: passed\n'); +end + +function assertThrows(action, expectedMessage) +% Ensure C++ validation failures become catchable MATLAB errors. +try + action(); +catch ME + assert(contains(ME.message, expectedMessage), ... + 'Expected an error containing "%s"; received: %s', ... + expectedMessage, ME.message); + return; +end +error('Expected an error containing "%s", but none was thrown.', expectedMessage); +end + +function insertState(values, positionKey, velocityKey, position, velocity) +gtsam.insertAugmentedPoint3(positionKey, ... + gtsam.AugmentedPoint3(position), values); +values.insertPoint3(velocityKey, velocity); +end diff --git a/matlab/wnoa_unstable.i b/matlab/wnoa_unstable.i new file mode 100644 index 0000000000..de0b40d1e5 --- /dev/null +++ b/matlab/wnoa_unstable.i @@ -0,0 +1,19 @@ +// MATLAB-only constructor bridge for the unstable WNOA interpolation factor. +// Kept separate from gtsam_unstable.i so Python bindings remain unchanged. +namespace gtsam { + +#include + +virtual class MatlabWnoaInterpFactorAugmentedPoint3 + : gtsam::NoiseModelFactor { + MatlabWnoaInterpFactorAugmentedPoint3( + const gtsam::NoiseModelFactor& inner_factor, + const gtsam::StateData& left, + const gtsam::StateData& interpolated, + const gtsam::StateData& right, + const gtsam::Vector& q_psd_diag, + bool fixed_noise_model = false, + bool precomp_interp_mats = true); +}; + +} // namespace gtsam diff --git a/python/gtsam_unstable/tests/test_wnoa_augmented_point.py b/python/gtsam_unstable/tests/test_wnoa_augmented_point.py new file mode 100644 index 0000000000..562ad26310 --- /dev/null +++ b/python/gtsam_unstable/tests/test_wnoa_augmented_point.py @@ -0,0 +1,318 @@ +"""Python regression tests for WNOA with unstable AugmentedPoint3.""" + +import os +import sys +import unittest +from pathlib import Path + +# Use the local build whether run from the source tree or by CMake's test target. +_test_path = Path(__file__).resolve() +_build_candidates = ( + _test_path.parents[2], # build-x64/python when copied to the build tree + _test_path.parents[3] / 'build-x64' / 'python', # source-tree tests +) +_dll_handles = [] +for _build_python in _build_candidates: + if any((_build_python / 'gtsam').glob('*.pyd')) or any( + (_build_python / 'gtsam').glob('*.so')): + sys.path.insert(0, str(_build_python)) + if os.name == 'nt': + _dll_handles = [os.add_dll_directory(str(_build_python / package)) + for package in ('gtsam', 'gtsam_unstable')] + break + +import numpy as np +import gtsam +import gtsam_unstable as gu + + +def pose_key(index): + return gtsam.symbol('p', index) + + +def velocity_key(index): + return gtsam.symbol('v', index) + + +def insert_state(values, index, position, velocity): + # AugmentedPoint3 is defined in unstable, so use its typed Values helper. + gu.insertAugmentedPoint3(pose_key(index), gu.AugmentedPoint3(np.asarray(position)), values) + values.insert(velocity_key(index), np.asarray(velocity, dtype=float)) + + +class TestWnoaAugmentedPoint(unittest.TestCase): + def test_motion_constant_velocity(self): + left = gtsam.StateData(pose_key(0), velocity_key(0), 0.0) + right = gtsam.StateData(pose_key(1), velocity_key(1), 0.2) + velocity = np.array([0.5, -0.25, 0.75]) + position = np.array([1.0, -2.0, 3.0]) + factor = gu.WnoaMotionFactorAugmentedPoint3( + left, right, np.ones(3) + ) + + values = gtsam.Values() + insert_state(values, 0, position, velocity) + insert_state(values, 1, position + 0.2 * velocity, velocity) + self.assertAlmostEqual(factor.error(values), 0.0, places=9) + + def test_interpolated_squared_range(self): + left = gtsam.StateData(pose_key(0), velocity_key(0), 0.0) + right = gtsam.StateData(pose_key(1), velocity_key(1), 0.2) + middle = gtsam.StateData(pose_key(2), velocity_key(2), 0.08) + noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.1) + inner = gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3( + middle.pose, np.zeros(3), 1.0, noise + ) + factor = gu.WnoaInterpFactorAugmentedPoint3( + inner, {left, right}, {middle}, np.ones(3), True, True + ) + + values = gtsam.Values() + insert_state(values, 0, [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]) + insert_state(values, 1, [1.2, 0.0, 0.0], [1.0, 0.0, 0.0]) + self.assertEqual(set(factor.keys()), + {left.pose, left.velocity, right.pose, right.velocity}) + self.assertEqual(factor.dim(), 1) + residual = 1.08**2 - 1.0 + self.assertAlmostEqual(factor.error(values), + 0.5 * (residual / 0.1)**2, places=7) + + def test_range_only_optimization(self): + p0 = np.array([0.6, -0.2, 0.7]) + velocity = np.array([0.35, 0.1, -0.15]) + q = np.full(3, 0.2) + knots = [gtsam.StateData(pose_key(i), velocity_key(i), float(i)) + for i in range(3)] + landmarks = [np.array(p, dtype=float) for p in ( + (0, 0, 0), (3, 0, 0), (0, 3, 0), (0, 0, 3) + )] + + graph = gtsam.NonlinearFactorGraph() + for left, right in zip(knots[:-1], knots[1:]): + graph.add(gu.WnoaMotionFactorAugmentedPoint3(left, right, q)) + + range_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.02) + # Ranges at the first knot anchor the trajectory without requiring + # an AugmentedPoint prior-factor binding. + for landmark in landmarks: + measured = np.linalg.norm(p0 - landmark) + graph.add(gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3( + knots[0].pose, landmark, measured, range_noise + )) + + for i, t in enumerate((0.25, 0.75, 1.25, 1.75)): + at_time = gtsam.StateData(pose_key(10 + i), + velocity_key(10 + i), t) + position = p0 + t * velocity + for landmark in landmarks: + measured = np.linalg.norm(position - landmark) + inner = gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3( + at_time.pose, landmark, measured, range_noise + ) + graph.add(gu.WnoaInterpFactorAugmentedPoint3( + inner, set(knots), {at_time}, q, True, True + )) + + initial = gtsam.Values() + for i in range(3): + insert_state(initial, i, + p0 + i * velocity + np.array([0.12, -0.08, 0.10]), + velocity + np.array([-0.08, 0.06, 0.10])) + + initial_error = graph.error(initial) + result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() + final_error = graph.error(result) + + self.assertGreater(initial_error, 0.0) + self.assertLess(final_error, initial_error) + self.assertLess(final_error, 1e-4) + self.assertEqual(result.size(), 6) + for i in range(3): + position = np.asarray(gu.atAugmentedPoint3( + pose_key(i), result).vector()).reshape(3) + actual_velocity = np.asarray(result.atVector(velocity_key(i))) + np.testing.assert_allclose(position, p0 + i * velocity, atol=1e-2) + np.testing.assert_allclose(actual_velocity, velocity, atol=1e-2) + + +def lifted_state(values, index, position, velocity): + """Insert the exact QCQP columns [h, x, u] and an unlifted 3x1 velocity.""" + gu.insertQcqpValue3(pose_key(index), gu.AugmentedPoint3(np.asarray(position)), values) + values.insert(velocity_key(index), np.asarray(velocity, dtype=float).reshape(3, 1)) + + +def fix_homogeneous_coordinate(problem, keys): + """Pin h = 1 on every lifted position, removing the per-point sign gauge.""" + h = np.array([[1.0, 0.0, 0.0, 0.0, 0.0]]) + for key in keys: + problem.addConstraint(gtsam.LinearConstraint.Equal( + gtsam.JacobianFactor(key, h, np.ones(1), gtsam.noiseModel.Unit.Create(1)))) + + +class TestWnoaQcqp(unittest.TestCase): + """QCQP conversion of the AugmentedPoint3 WNOA factors.""" + + Q = np.full(3, 0.2) + LANDMARK = np.array([2.0, 0.0, 0.0]) + + def interpolated_graph(self, ranges=(1.5,), fixed_noise=True): + left = gtsam.StateData(pose_key(0), velocity_key(0), 0.0) + right = gtsam.StateData(pose_key(1), velocity_key(1), 1.0) + middle = gtsam.StateData(pose_key(2), velocity_key(2), 0.4) + graph = gtsam.NonlinearFactorGraph() + for measured in ranges: + inner = gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3( + middle.pose, self.LANDMARK, measured, + gtsam.noiseModel.Isotropic.Sigma(1, 0.1)) + graph.add(gu.WnoaInterpFactorAugmentedPoint3( + inner, {left, right}, {middle}, self.Q, fixed_noise, True)) + return graph + + def constant_velocity_values(self, position, velocity): + """Typed knot values and lifted values (with the interpolated point at t=0.4).""" + typed, lifted = gtsam.Values(), gtsam.Values() + for index in (0, 1): + insert_state(typed, index, position + index * velocity, velocity) + lifted_state(lifted, index, position + index * velocity, velocity) + gu.insertQcqpValue3(pose_key(2), gu.AugmentedPoint3(position + 0.4 * velocity), lifted) + return typed, lifted + + def test_motion_qcqp_matches_nonlinear_cost(self): + left = gtsam.StateData(pose_key(0), velocity_key(0), 0.0) + right = gtsam.StateData(pose_key(1), velocity_key(1), 0.3) + graph = gtsam.NonlinearFactorGraph() + graph.add(gu.WnoaMotionFactorAugmentedPoint3(left, right, self.Q)) + problem = gtsam.QcqpProblem(graph) + + # Two lift equalities per AugmentedPoint, no inequalities. + _, equality_dim, inequality_dim = problem.dim() + self.assertEqual((equality_dim, inequality_dim), (4, 0)) + + typed, lifted = gtsam.Values(), gtsam.Values() + states = [([0.1, -0.2, 0.3], [0.5, 0.1, -0.4]), ([0.4, 0.0, 0.1], [0.3, -0.2, 0.2])] + for index, (position, velocity) in enumerate(states): + insert_state(typed, index, position, velocity) + lifted_state(lifted, index, position, velocity) + cost, equality_violation, _ = problem.evaluate(lifted) + self.assertGreater(cost, 0.0) + self.assertAlmostEqual(cost, graph.error(typed), places=9) + self.assertAlmostEqual(equality_violation, 0.0, places=12) + + # A lifted norm that disagrees with the coordinates violates a constraint. + broken = gtsam.Values() + lifted_state(broken, 1, *states[1]) + broken.insert(pose_key(0), np.array([[1.0], [0.1], [-0.2], [0.3], [5.0]])) + broken.insert(velocity_key(0), np.array(states[0][1]).reshape(3, 1)) + self.assertGreater(problem.evaluate(broken)[1], 1.0) + + def test_interpolated_range_qcqp_matches_nonlinear_cost(self): + graph = self.interpolated_graph() + problem = gtsam.QcqpProblem(graph) + # Lift constraints for two knots and the interpolated point (6 rows) plus + # the 3-row linear link of the interpolated coordinates. + self.assertEqual(problem.dim()[1:], (9, 0)) + + position = np.array([0.5, -0.1, 0.2]) + velocity = np.array([0.4, 0.2, -0.1]) + typed, lifted = self.constant_velocity_values(position, velocity) + cost, equality_violation, _ = problem.evaluate(lifted) + residual = np.linalg.norm(position + 0.4 * velocity - self.LANDMARK)**2 - 1.5**2 + self.assertAlmostEqual(cost, 0.5 * (residual / 0.1)**2, places=7) + self.assertAlmostEqual(cost, graph.error(typed), places=7) + self.assertAlmostEqual(equality_violation, 0.0, places=9) + + # Moving the interpolated point away from the knot interpolation breaks the link. + moved = gtsam.Values() + lifted_state(moved, 0, position, velocity) + lifted_state(moved, 1, position + velocity, velocity) + gu.insertQcqpValue3(pose_key(2), gu.AugmentedPoint3(position + 0.4 * velocity + 0.05), moved) + self.assertGreater(problem.evaluate(moved)[1], 1e-3) + + def test_negated_lift_violates_coupling(self): + # Negating a lifted point keeps its own lift constraints satisfied but + # breaks the linear coupling, so every point needs the constraint h = 1. + problem = gtsam.QcqpProblem(self.interpolated_graph()) + position = np.array([0.5, -0.1, 0.2]) + velocity = np.array([0.4, 0.2, -0.1]) + _, lifted = self.constant_velocity_values(position, velocity) + self.assertAlmostEqual(problem.evaluate(lifted)[1], 0.0, places=9) + + flipped = gtsam.Values() + lifted_state(flipped, 1, position + velocity, velocity) + gu.insertQcqpValue3(pose_key(2), gu.AugmentedPoint3(position + 0.4 * velocity), flipped) + flipped.insert(pose_key(0), -gu.qcqpValue3(gu.AugmentedPoint3(position))) + flipped.insert(velocity_key(0), velocity) + self.assertGreater(problem.evaluate(flipped)[1], 1e-3) + + def test_shared_interpolated_key_is_coupled_once(self): + # Several ranges at one time share the interpolated point and its constraints. + problem = gtsam.QcqpProblem(self.interpolated_graph(ranges=(1.5, 1.4, 1.6))) + self.assertEqual(problem.dim()[1:], (9, 0)) + + def test_unsupported_configurations_are_rejected(self): + with self.assertRaises(RuntimeError): + gtsam.QcqpProblem(self.interpolated_graph(fixed_noise=False)) + with self.assertRaises(ValueError): + gtsam.QcqpProblem(self.interpolated_graph(), 2) + left = gtsam.StateData(pose_key(0), velocity_key(0), 0.0) + right = gtsam.StateData(pose_key(1), velocity_key(1), 1.0) + motion = gtsam.NonlinearFactorGraph() + motion.add(gu.WnoaMotionFactorAugmentedPoint3(left, right, self.Q)) + with self.assertRaises(ValueError): + gtsam.QcqpProblem(motion, 2) + + def test_range_only_qcqp_matches_levenberg_marquardt(self): + p0 = np.array([0.6, -0.2, 0.7]) + velocity = np.array([0.35, 0.1, -0.15]) + knots = [gtsam.StateData(pose_key(i), velocity_key(i), float(i)) for i in range(3)] + landmarks = [np.array(p, dtype=float) for p in ((0, 0, 0), (3, 0, 0), (0, 3, 0), (0, 0, 3))] + times = (0.25, 0.75, 1.25, 1.75) + noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.05) + + graph = gtsam.NonlinearFactorGraph() + for left, right in zip(knots[:-1], knots[1:]): + graph.add(gu.WnoaMotionFactorAugmentedPoint3(left, right, self.Q)) + for i, t in enumerate(times): + at_time = gtsam.StateData(pose_key(10 + i), velocity_key(10 + i), t) + for landmark in landmarks: + inner = gu.KnownLandmarkSquaredRangeFactorAugmentedPoint3( + at_time.pose, landmark, float(np.linalg.norm(p0 + t * velocity - landmark)), noise) + graph.add(gu.WnoaInterpFactorAugmentedPoint3( + inner, set(knots), {at_time}, self.Q, True, True)) + + initial, lifted = gtsam.Values(), gtsam.Values() + for i in range(3): + position = p0 + i * velocity + np.array([0.05, -0.04, 0.03]) + guess = velocity + np.array([-0.04, 0.03, 0.05]) + insert_state(initial, i, position, guess) + lifted_state(lifted, i, position, guess) + for i, t in enumerate(times): + k = int(t) + gu.insertQcqpValue3(pose_key(10 + i), gu.AugmentedPoint3( + p0 + k * velocity + np.array([0.05, -0.04, 0.03]) + (t - k) * velocity), lifted) + nonlinear = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() + + problem = gtsam.QcqpProblem(graph) + fix_homogeneous_coordinate(problem, [pose_key(i) for i in range(3)] + [pose_key(10 + i) for i in range(4)]) + params = gtsam.AugmentedLagrangianParams() + params.updatePolicy = gtsam.AugmentedLagrangianUpdatePolicy.Aggressive + params.maxIterations = 200 + params.absoluteViolationTolerance = 1e-9 + params.relativeViolationTolerance = 1e-9 + params.relativeCostTolerance = 1e-12 + params.absoluteStationarityTolerance = 1e-8 + solved = gtsam.AugmentedLagrangianOptimizer(problem, lifted, params).optimize() + + cost, equality_violation, _ = problem.evaluate(solved) + self.assertAlmostEqual(equality_violation, 0.0, places=5) + self.assertAlmostEqual(cost, graph.error(nonlinear), places=5) + for i in range(3): + np.testing.assert_allclose( + gu.fromQcqpValue3(solved.atMatrix(pose_key(i))).vector().reshape(3), + gu.atAugmentedPoint3(pose_key(i), nonlinear).vector().reshape(3), atol=1e-4) + np.testing.assert_allclose( + solved.atVector(velocity_key(i)), nonlinear.atVector(velocity_key(i)), atol=1e-4) + + +if __name__ == '__main__': + unittest.main()