From 1c6f0abf0c7c562934c25fc4fb5ab328d0158d6b Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Fri, 3 Oct 2025 11:19:51 -0400 Subject: [PATCH 1/2] Add an FEJ factor to allow for easier control of linearization points of the problem --- include/factors/FEJFactor.h | 158 ++++++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 include/factors/FEJFactor.h diff --git a/include/factors/FEJFactor.h b/include/factors/FEJFactor.h new file mode 100644 index 0000000..0b383c8 --- /dev/null +++ b/include/factors/FEJFactor.h @@ -0,0 +1,158 @@ +#pragma once + +#include +#include +#include + +/** + * @brief Wrapper for Ceres cost functions that enables the use of FEJ + * (first estimate Jacobians), by allowing the Jacobians to be evaluated at different + * linearization points than the current parameter estimates. + * + * To use this class, inherit from it and implement the evaluateResiduals and + * evaluateJacobians methods. + * + * To set a fixed linearization point for a parameter block, call + * setLinearizationPoint with the parameter block index and the desired + * linearization point. + */ +template +class FEJFactor : public ceres::SizedCostFunction { +public: + static constexpr int kNumParameterBlocks = sizeof...(Ns); + static constexpr int kNumResidualsStatic = kNumResiduals; + + FEJFactor() { + // Initialize the parameter block sizes array + parameter_block_sizes_ = {Ns...}; + + // Initialize the other vectors + linearization_points_.resize(kNumParameterBlocks); + is_linearization_point_set_.resize(kNumParameterBlocks, false); + use_fixed_linearization_.resize(kNumParameterBlocks, false); + } + virtual ~FEJFactor() = default; + + /** + * @brief For a given parameter block index, set a fixed linearization point. + */ + bool setLinearizationPoint(int param_idx, + const Eigen::VectorXd linearization_point) { + if (param_idx < 0 || param_idx >= kNumParameterBlocks) { + LOG(ERROR) << "Parameter index out of range"; + return false; + } + + // Validate that the linearization point size matches the expected parameter block size + int expected_size = parameter_block_sizes_[param_idx]; + if (linearization_point.size() != expected_size) { + LOG(ERROR) << "Linearization point size (" << linearization_point.size() + << ") does not match expected parameter block size (" + << expected_size << ")"; + return false; + } + + is_linearization_point_set_[param_idx] = true; + use_fixed_linearization_[param_idx] = true; + linearization_points_[param_idx] = linearization_point; + return true; + } + + /** + * @brief For a given parameter block index, get the currently set + * linearization point. + */ + bool getLinearizationPoint(int param_idx, + Eigen::VectorXd &linearization_point) const { + if (param_idx < 0 || param_idx >= kNumParameterBlocks) { + LOG(ERROR) << "Parameter index out of range"; + return false; + } + + if (!is_linearization_point_set_[param_idx]) { + return false; + } + + linearization_point = linearization_points_[param_idx]; + return true; + } + + /** + * @brief See if we've already set a linearization point for a given parameter block. + */ + bool hasLinearizationPoint(int param_idx) const { + if (param_idx < 0 || param_idx >= kNumParameterBlocks) { + throw std::out_of_range("Parameter index out of range"); + } + return is_linearization_point_set_[param_idx]; + } + + /** + * @brief Disable the use of a fixed linearization point for a given parameter block. + * The Jacobians will be evaluated at the current parameter estimate. + */ + bool resetLinearizationPoint(int param_idx) { + if (param_idx < 0 || param_idx >= kNumParameterBlocks) { + LOG(ERROR) << "Parameter index out of range"; + return false; + } + + is_linearization_point_set_[param_idx] = false; + use_fixed_linearization_[param_idx] = false; + linearization_points_[param_idx].resize(0); + return true; + } + + /** + * @brief Wraps the Evaluate function to handle FEJ logic. + * + * We evaluate the residuals at the current parameter estimates, + * but the Jacobians at either the current estimates or the fixed + * estimates, if a linearization point has been set for that + * parameter block. + */ + bool Evaluate(double const *const *parameters, double *residuals, + double **jacobians) const override { + // Always evaluate the residuals at the current state + if (!evaluateResiduals(parameters, residuals)) { + return false; + } + + if (!jacobians) { + return true; + } + + // Evaluate the Jacobians at the correct evaluation points + std::vector eval_points(kNumParameterBlocks); + for (int i = 0; i < kNumParameterBlocks; i++) { + if (use_fixed_linearization_[i]) { + eval_points[i] = linearization_points_[i].data(); + } else { + eval_points[i] = parameters[i]; + } + } + + return evaluateJacobians(eval_points.data(), jacobians); + } + +protected: + /** + * @brief Implement this function to evaluate the residuals given the + * parameters. + */ + virtual bool evaluateResiduals(double const *const *parameters, + double *residuals) const = 0; + + /** + * @brief Implement this function to evaluate the Jacobians for a given set of parameters. + */ + virtual bool evaluateJacobians(double const *const *parameters, + double **jacobians) const = 0; + + // Linearization point management + std::vector linearization_points_; + // Sizes of each parameter block + std::vector parameter_block_sizes_; + std::vector is_linearization_point_set_; + std::vector use_fixed_linearization_; +}; From da637bcd25f296c2fa48efb29e162ebfa2069e60 Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Fri, 3 Oct 2025 11:34:00 -0400 Subject: [PATCH 2/2] Add test for FEJFactor --- tests/CMakeLists.txt | 6 +- tests/test_fej_factor.cpp | 214 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 219 insertions(+), 1 deletion(-) create mode 100644 tests/test_fej_factor.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ac79306..af6895d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -14,10 +14,14 @@ target_link_libraries(test_state_collection PRIVATE ${PROJECT_NAME} Catch2::Catc add_executable(test_vector_math test_vector_math.cpp) target_link_libraries(test_vector_math PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain) +add_executable(test_fej_factor test_fej_factor.cpp) +target_link_libraries(test_fej_factor PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain) + include(CTest) include(Catch) catch_discover_tests(test_factor_graph) catch_discover_tests(test_jacobians) catch_discover_tests(test_parameter_blocks) catch_discover_tests(test_state_collection) -catch_discover_tests(test_vector_math) \ No newline at end of file +catch_discover_tests(test_vector_math) +catch_discover_tests(test_fej_factor) \ No newline at end of file diff --git a/tests/test_fej_factor.cpp b/tests/test_fej_factor.cpp new file mode 100644 index 0000000..957124c --- /dev/null +++ b/tests/test_fej_factor.cpp @@ -0,0 +1,214 @@ +#include + +#include + +#include "utils/CostFunctionUtils.h" +#include "factors/FEJFactor.h" + +// A simple implementation of FEJFactor for testing purposes +// The dimension of the residual is 2, with two parameter blocks of sizes 3 and 4 +class TestFEJFactor : public FEJFactor<2, 3, 4> { +public: + // Track how many times each method is called and with what parameters + mutable std::vector residual_evaluation_points_; + mutable std::vector jacobian_evaluation_points_; + mutable int residual_call_count_ = 0; + mutable int jacobian_call_count_ = 0; + +protected: + bool evaluateResiduals(double const *const *parameters, + double *residuals) const override { + residual_call_count_++; + // Store the evaluation points for verification + residual_evaluation_points_.clear(); + + residual_evaluation_points_.push_back( + Eigen::VectorXd::Map(parameters[0], 3)); + residual_evaluation_points_.push_back( + Eigen::VectorXd::Map(parameters[1], 4)); + + // Simple residual: f(x,y) = [x[0] + y[0], x[1] + y[1]] + residuals[0] = parameters[0][0] + parameters[1][0]; + residuals[1] = parameters[0][1] + parameters[1][1]; + return true; + } + + bool evaluateJacobians(double const *const *parameters, + double **jacobians) const override { + + jacobian_call_count_++; + + // Store the evaluation points for verification + jacobian_evaluation_points_.clear(); + jacobian_evaluation_points_.push_back( + Eigen::VectorXd::Map(parameters[0], 3)); + jacobian_evaluation_points_.push_back( + Eigen::VectorXd::Map(parameters[1], 4)); + + if (jacobians[0] != nullptr) { + // Jacobian w.r.t. first parameter block (3x2 matrix, stored row-major) + jacobians[0][0] = 1.0; + jacobians[0][1] = 0.0; + jacobians[0][2] = 0.0; + jacobians[0][3] = 0.0; + jacobians[0][4] = 1.0; + jacobians[0][5] = 0.0; + } + + if (jacobians[1] != nullptr) { + // Jacobian w.r.t. second parameter block (4x2 matrix, stored row-major) + jacobians[1][0] = 1.0; + jacobians[1][1] = 0.0; + jacobians[1][2] = 0.0; + jacobians[1][3] = 0.0; + jacobians[1][4] = 0.0; + jacobians[1][5] = 1.0; + jacobians[1][6] = 0.0; + jacobians[1][7] = 0.0; + } + + return true; + } +}; + +TEST_CASE("FEJCostFunction Basic Functionality", "[FEJCostFunction]") { + TestFEJFactor factor; + + SECTION("Constructor initializes correctly") { + REQUIRE(factor.kNumParameterBlocks == 2); + REQUIRE(factor.kNumResidualsStatic == 2); + } + + SECTION("Set and get linearization points") { + // Set two linearization points + Eigen::Vector3d lin_point_0(1.0, 2.0, 3.0); + Eigen::Vector4d lin_point_1(4.0, 5.0, 6.0, 7.0); + factor.setLinearizationPoint(0, lin_point_0); + factor.setLinearizationPoint(1, lin_point_1); + + Eigen::VectorXd retrieved_point; + REQUIRE(factor.getLinearizationPoint(0, retrieved_point)); + REQUIRE(retrieved_point.isApprox(lin_point_0)); + + REQUIRE(factor.getLinearizationPoint(1, retrieved_point)); + REQUIRE(retrieved_point.isApprox(lin_point_1)); + } + + SECTION("Error handling for invalid parameter indices") { + Eigen::Vector3d lin_point(1.0, 2.0, 3.0); + + REQUIRE(!factor.setLinearizationPoint(-1, lin_point)); + REQUIRE(!factor.setLinearizationPoint(2, lin_point)); + + Eigen::VectorXd retrieved_point; + REQUIRE(!factor.getLinearizationPoint(-1, retrieved_point)); + REQUIRE(!factor.getLinearizationPoint(2, retrieved_point)); + } + + SECTION("Get linearization point returns false when not set") { + Eigen::VectorXd retrieved_point; + REQUIRE(!factor.getLinearizationPoint(0, retrieved_point)); + REQUIRE(!factor.getLinearizationPoint(1, retrieved_point)); + } + + SECTION("Wrong size linearization point should return false") { + Eigen::Vector2d wrong_size_point(1.0, 2.0); + REQUIRE(!factor.setLinearizationPoint(0, wrong_size_point)); + } +} + +TEST_CASE("FEJCostFunction Evaluate Method", "[FEJCostFunction]") { + TestFEJFactor factor; + + // Set up test parameters + std::vector param0 = {1.0, 2.0, 3.0}; + std::vector param1 = {4.0, 5.0, 6.0, 7.0}; + double const *parameters[2] = {param0.data(), param1.data()}; + + std::vector residuals(2); + + SECTION("Evaluate residuals only") { + REQUIRE(factor.Evaluate(parameters, residuals.data(), nullptr)); + + // Verify residuals were evaluated at current parameters + REQUIRE(factor.residual_call_count_ == 1); + REQUIRE(factor.jacobian_call_count_ == 0); + } + + SECTION("Evaluate residuals and Jacobians without FEJ") { + std::vector jac0(6), jac1(8); + double *jacobians[2] = {jac0.data(), jac1.data()}; + + REQUIRE(factor.Evaluate(parameters, residuals.data(), jacobians)); + + // Verify both residuals and Jacobians were computed + REQUIRE(factor.residual_call_count_ == 1); + REQUIRE(factor.jacobian_call_count_ == 1); + + // Verify Jacobians were evaluated at current parameters (same as residuals) + REQUIRE(factor.jacobian_evaluation_points_[0] == + factor.residual_evaluation_points_[0]); + REQUIRE(factor.jacobian_evaluation_points_[1] == + factor.residual_evaluation_points_[1]); + } + + SECTION("Evaluate residuals and Jacobians with FEJ") { + // Set linearization points + Eigen::Vector3d lin_point_0(10.0, 20.0, 30.0); + Eigen::Vector4d lin_point_1(40.0, 50.0, 60.0, 70.0); + + factor.setLinearizationPoint(0, lin_point_0); + factor.setLinearizationPoint(1, lin_point_1); + + std::vector jac0(6), jac1(8); + double *jacobians[2] = {jac0.data(), jac1.data()}; + + REQUIRE(factor.Evaluate(parameters, residuals.data(), jacobians)); + + // Verify residuals were evaluated at current parameters + REQUIRE(factor.residual_evaluation_points_[0].isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(factor.residual_evaluation_points_[1].isApprox(Eigen::Vector4d(4.0, 5.0, 6.0, 7.0))); + + // Verify Jacobians were evaluated at linearization points + REQUIRE(factor.jacobian_evaluation_points_[0].isApprox(Eigen::Vector3d(10.0, 20.0, 30.0))); + REQUIRE(factor.jacobian_evaluation_points_[1].isApprox(Eigen::Vector4d(40.0, 50.0, 60.0, 70.0))); + + std::cout << factor.jacobian_evaluation_points_[0].transpose() << std::endl; + std::cout << factor.jacobian_evaluation_points_[1].transpose() << std::endl; + } + + SECTION("Evaluate with partial FEJ (only one parameter has linearization " + "point)") { + // Set linearization point only for parameter 0 + Eigen::Vector3d lin_point_0(10.0, 20.0, 30.0); + factor.setLinearizationPoint(0, lin_point_0); + + std::vector jac0(6), jac1(8); + double *jacobians[2] = {jac0.data(), jac1.data()}; + + REQUIRE(factor.Evaluate(parameters, residuals.data(), jacobians)); + + // Verify Jacobians: param 0 at linearization point, param 1 at current + // point + REQUIRE(factor.jacobian_evaluation_points_[0].isApprox(Eigen::Vector3d(10.0, 20.0, 30.0))); + REQUIRE(factor.jacobian_evaluation_points_[1].isApprox(Eigen::Vector4d(4.0, 5.0, 6.0, 7.0))); + } + + SECTION("Test resetting linearization points") { + // Set and then reset linearization point for parameter 0 + Eigen::Vector3d lin_point_0(10.0, 20.0, 30.0); + factor.setLinearizationPoint(0, lin_point_0); + REQUIRE(factor.hasLinearizationPoint(0)); + factor.resetLinearizationPoint(0); + REQUIRE(!factor.hasLinearizationPoint(0)); + + std::vector jac0(6), jac1(8); + double *jacobians[2] = {jac0.data(), jac1.data()}; + + REQUIRE(factor.Evaluate(parameters, residuals.data(), jacobians)); + + // Verify Jacobians - both should be at the current points now + REQUIRE(factor.jacobian_evaluation_points_[0].isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); + REQUIRE(factor.jacobian_evaluation_points_[1].isApprox(Eigen::Vector4d(4.0, 5.0, 6.0, 7.0))); + } +} \ No newline at end of file