diff --git a/gtsam/base/ProductLieGroup-inl.h b/gtsam/base/ProductLieGroup-inl.h index 905e104724..42a0681459 100644 --- a/gtsam/base/ProductLieGroup-inl.h +++ b/gtsam/base/ProductLieGroup-inl.h @@ -38,22 +38,38 @@ template ProductLieGroup ProductLieGroup::retract(const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { - if (H1 || H2) { - throw std::runtime_error( - "ProductLieGroup::retract derivatives not implemented yet"); - } const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); if (static_cast(v.size()) != - combinedDimension(firstDimension, secondDimension)) { + productDimension) { throw std::invalid_argument( "ProductLieGroup::retract tangent dimension does not match product " "dimension"); } - G g = - traits::Retract(this->first, tangentSegment(v, 0, firstDimension)); + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + G g = traits::Retract(this->first, tangentSegment(v, 0, firstDimension), + H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); H h = traits::Retract( - this->second, tangentSegment(v, firstDimension, secondDimension)); + this->second, tangentSegment(v, firstDimension, secondDimension), + H1 ? &D_h_first : nullptr, H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } return ProductLieGroup(g, h); } @@ -62,16 +78,33 @@ typename ProductLieGroup::TangentVector ProductLieGroup::localCoordinates(const ProductLieGroup& g, ChartJacobian H1, ChartJacobian H2) const { - if (H1 || H2) { - throw std::runtime_error( - "ProductLieGroup::localCoordinates derivatives not implemented yet"); - } checkMatchingDimensions(g, "localCoordinates"); const size_t firstDimension = firstDim(); const size_t secondDimension = secondDim(); - typename traits::TangentVector v1 = traits::Local(this->first, g.first); - typename traits::TangentVector v2 = - traits::Local(this->second, g.second); + const size_t productDimension = + combinedDimension(firstDimension, secondDimension); + Jacobian1 D_g_first; + Jacobian1 D_g_second; + Jacobian2 D_h_first; + Jacobian2 D_h_second; + typename traits::TangentVector v1 = + traits::Local(this->first, g.first, H1 ? &D_g_first : nullptr, + H2 ? &D_g_second : nullptr); + typename traits::TangentVector v2 = traits::Local( + this->second, g.second, H1 ? &D_h_first : nullptr, + H2 ? &D_h_second : nullptr); + if (H1) { + *H1 = zeroJacobian(productDimension); + H1->block(0, 0, firstDimension, firstDimension) = D_g_first; + H1->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_first; + } + if (H2) { + *H2 = zeroJacobian(productDimension); + H2->block(0, 0, firstDimension, firstDimension) = D_g_second; + H2->block(firstDimension, firstDimension, secondDimension, + secondDimension) = D_h_second; + } return makeTangentVector(v1, v2, firstDimension, secondDimension); } @@ -469,16 +502,18 @@ template Derived PowerLieGroupBase::retract(const TangentVector& v, ChartJacobian H1, ChartJacobian H2) const { - if (H1 || H2) { - throw std::runtime_error( - "PowerLieGroup::retract derivatives not implemented yet"); - } const size_t count = componentCount(); checkDynamicTangentSize(v, count, "retract"); + JacobianStorage firstJacobians = makeJacobianStorage(count); + JacobianStorage secondJacobians = makeJacobianStorage(count); Derived result = makeResult(count); for (size_t i = 0; i < count; ++i) { - result[i] = traits::Retract(derived()[i], tangentSegment(v, i)); + result[i] = traits::Retract(derived()[i], tangentSegment(v, i), + H1 ? &firstJacobians[i] : nullptr, + H2 ? &secondJacobians[i] : nullptr); } + fillJacobianBlocks(H1, firstJacobians, count); + fillJacobianBlocks(H2, secondJacobians, count); return result; } @@ -487,15 +522,19 @@ typename PowerLieGroupBase::TangentVector PowerLieGroupBase::localCoordinates(const Derived& g, ChartJacobian H1, ChartJacobian H2) const { - if (H1 || H2) { - throw std::runtime_error( - "PowerLieGroup::localCoordinates derivatives not implemented yet"); - } checkMatchingCounts(g, "localCoordinates"); + const size_t count = componentCount(); + JacobianStorage firstJacobians = makeJacobianStorage(count); + JacobianStorage secondJacobians = makeJacobianStorage(count); TangentVector v = zeroTangent(componentCount()); - for (size_t i = 0; i < componentCount(); ++i) { - assignTangentSegment(v, i, traits::Local(derived()[i], g[i])); + for (size_t i = 0; i < count; ++i) { + assignTangentSegment( + v, i, + traits::Local(derived()[i], g[i], H1 ? &firstJacobians[i] : nullptr, + H2 ? &secondJacobians[i] : nullptr)); } + fillJacobianBlocks(H1, firstJacobians, count); + fillJacobianBlocks(H2, secondJacobians, count); return v; } diff --git a/gtsam/navigation/EquivariantFilter.h b/gtsam/navigation/EquivariantFilter.h index 187f0d3cb1..27bcabccb2 100644 --- a/gtsam/navigation/EquivariantFilter.h +++ b/gtsam/navigation/EquivariantFilter.h @@ -66,10 +66,45 @@ class EquivariantFilter : public ManifoldEKF { M xi_ref_; // Origin (reference) state on the manifold typename Symmetry::Orbit act_on_ref_; // Orbit of the reference state MatrixMG Dphi0_; // Differential of state action at identity - MatrixGM InnovationLift_; // Innovation lift matrix ((Dphi0)^+) + MatrixGM InnovationLift_; // Innovation lift matrix ((Dphi0)^+) G g_; // Group element estimate + protected: + /// @return Current reference manifold state. + const M& referenceState() const { return xi_ref_; } + + /// Recompute Dphi0 and innovation lift matrix from current reference state. + void recomputeActionDifferentials() { + act_on_ref_ = typename Symmetry::Orbit(xi_ref_); + const G identity_at_g = traits::Compose(g_.inverse(), g_); + act_on_ref_(identity_at_g, &Dphi0_); + InnovationLift_ = Dphi0_.completeOrthogonalDecomposition().pseudoInverse(); + } + + /// Reset reference, covariance, and group estimate; sync manifold state. + void resetReferenceAndGroup(const M& xi_ref, const CovarianceM& P, + const G& g) { + xi_ref_ = xi_ref; + g_ = g; + recomputeActionDifferentials(); + if constexpr (DimM == Eigen::Dynamic) { + this->n_ = traits::GetDimension(xi_ref_); + this->I_ = MatrixM::Identity(this->n_, this->n_); + } + this->P_ = P; + this->X_ = act_on_ref_(g_); + } + + /// Set group estimate and update manifold state through the action. + void setGroupEstimateAndSyncState(const G& g) { + g_ = g; + this->X_ = act_on_ref_(g_); + } + + /// Set error covariance directly. + void setErrorCovariance(const CovarianceM& P) { this->P_ = P; } + public: /** * @brief Initialize the Equivariant Filter. @@ -82,7 +117,8 @@ class EquivariantFilter : public ManifoldEKF { const G& X0 = traits::Identity()) : Base(xi_ref, Sigma), xi_ref_(xi_ref), act_on_ref_(xi_ref), g_(X0) { // Compute differential of action phi at identity (Dphi0) - act_on_ref_(traits::Identity(), &Dphi0_); + const G identity_at_g = traits::Compose(g_.inverse(), g_); + act_on_ref_(identity_at_g, &Dphi0_); // Precompute the Innovation Lift matrix (pseudo-inverse of Dphi0) InnovationLift_ = Dphi0_.completeOrthogonalDecomposition().pseudoInverse(); @@ -284,6 +320,23 @@ class EquivariantFilter : public ManifoldEKF { this->validateInputs(prediction, H, z, R); update(prediction, H, z, R); } + + /// Vector measurement update using a custom innovation lift delta_x=f(delta_xi). + template + void updateWithVector(const gtsam::Vector& prediction, const Matrix& H, + const gtsam::Vector& z, const Matrix& R, + InnovationLiftFn&& innovationLift) { + this->validateInputs(prediction, H, z, R); + + const gtsam::Vector innovation = traits::Local(z, prediction); + const Matrix K = this->KalmanGain(H, R); + const TangentM delta_xi = -K * innovation; + const TangentG delta_x = innovationLift(delta_xi); + + g_ = traits::Compose(traits::Expmap(delta_x), g_); + this->X_ = act_on_ref_(g_); + this->JosephUpdate(K, H, R); + } }; } // namespace gtsam diff --git a/gtsam/navigation/doc/ImuFactor.ipynb b/gtsam/navigation/doc/ImuFactor.ipynb index c82d31beaf..50ad3d0910 100644 --- a/gtsam/navigation/doc/ImuFactor.ipynb +++ b/gtsam/navigation/doc/ImuFactor.ipynb @@ -29,7 +29,7 @@ "Instead of connecting five variables (`Pose_i`, `Vel_i`, `Pose_j`, `Vel_j`, `Bias_i`), the `ImuFactor2` connects three:\n", "\n", "1. `NavState` at time $i$ (`NavState` combines pose and velocity)\n", - "2. `NavState` at time $j`\n", + "2. `NavState` at time $j$\n", "3. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n", "\n", "### Modeling Bias\n", @@ -48,7 +48,13 @@ ] }, "source": [ - "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information" + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" ] }, { @@ -330,4 +336,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} \ No newline at end of file +} diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index bbf25da814..a41e87d566 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam_unstable LANGUAGES CXX) set (gtsam_unstable_subdirs base geometry + navigation linear discrete dynamics @@ -69,6 +70,7 @@ install(FILES "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_IN set(gtsam_unstable_srcs ${base_srcs} ${geometry_srcs} + ${navigation_srcs} ${linear_srcs} ${discrete_srcs} ${dynamics_srcs} diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt index 120f293e71..72045a8304 100644 --- a/gtsam_unstable/examples/CMakeLists.txt +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -1,4 +1,5 @@ set (excluded_examples "") +list(APPEND excluded_examples "EqVIOFilterRosbagReplay.cpp") # Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set if (NOT GTSAM_USE_BOOST_FEATURES) @@ -10,3 +11,24 @@ endif() gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) + +option(GTSAM_UNSTABLE_BUILD_EQVIO_ROSBAG_EXAMPLE + "Build EqVIO rosbag replay example (requires ROS1 rosbag + cv_bridge)" OFF) + +if(GTSAM_UNSTABLE_BUILD_EQVIO_ROSBAG_EXAMPLE) + find_package(OpenCV REQUIRED) + find_package(rosbag REQUIRED) + find_package(cv_bridge REQUIRED) + + add_executable(EqVIOFilterRosbagReplay EqVIOFilterRosbagReplay.cpp) + target_link_libraries(EqVIOFilterRosbagReplay + gtsam_unstable + ${OpenCV_LIBS} + ${rosbag_LIBRARIES} + ${cv_bridge_LIBRARIES}) + target_include_directories(EqVIOFilterRosbagReplay PRIVATE + ${OpenCV_INCLUDE_DIRS} + ${rosbag_INCLUDE_DIRS} + ${cv_bridge_INCLUDE_DIRS}) + list(APPEND examples_list EqVIOFilterRosbagReplay) +endif() diff --git a/gtsam_unstable/examples/EqVIOFilterCsvReplay.cpp b/gtsam_unstable/examples/EqVIOFilterCsvReplay.cpp new file mode 100644 index 0000000000..476e89cf0b --- /dev/null +++ b/gtsam_unstable/examples/EqVIOFilterCsvReplay.cpp @@ -0,0 +1,234 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOFilterCsvReplay.cpp +/// @brief Replay preprocessed EqVIO CSV data without feature tracking. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace { + +struct HardcodedGroundTruth { + static constexpr double t = 1403715283.3121430874; + static Vector3 position() { return Vector3(-0.954631, -0.101702, 0.179862); } + static Vector3 velocity() { return Vector3(-0.120739, -0.314283, 0.119599); } +}; + +double metadataFiniteDouble(const EqVIOCsvLog& log, const std::string& key, + double fallback) { + const double v = metadataDouble(log, key, fallback); + if (!std::isfinite(v)) return fallback; + return v; +} + +void propagateBufferedImu(EqVIOFilter& filter, std::vector& imuBuffer, + double& currentTime, double targetTime) { + if (!filter.isInitialized() || imuBuffer.empty() || + targetTime <= currentTime) { + return; + } + + const double tRef = currentTime; + double accumulatedTime = 0.0; + IMUInput accumulatedVelocity = IMUInput::Zero(); + for (size_t i = 0; i < imuBuffer.size(); ++i) { + const double t0 = std::max(imuBuffer[i].stamp, tRef); + const double t1 = + i + 1 < imuBuffer.size() ? std::min(imuBuffer[i + 1].stamp, targetTime) + : targetTime; + const double dt = std::max(t1 - t0, 0.0); + accumulatedTime += dt; + accumulatedVelocity = accumulatedVelocity + imuBuffer[i] * dt; + } + + if (accumulatedTime > 0.0) { + accumulatedVelocity = accumulatedVelocity * (1.0 / accumulatedTime); + filter.propagateCovariance(accumulatedVelocity, accumulatedTime); + + for (size_t i = 0; i < imuBuffer.size(); ++i) { + const double t0 = std::max(imuBuffer[i].stamp, tRef); + const double t1 = + i + 1 < imuBuffer.size() ? std::min(imuBuffer[i + 1].stamp, targetTime) + : targetTime; + const double dt = std::max(t1 - t0, 0.0); + if (dt > 0.0) { + filter.propagateState(imuBuffer[i], dt); + currentTime += dt; + } + } + } + + auto it = std::find_if(imuBuffer.begin(), imuBuffer.end(), + [targetTime](const IMUInput& imu) { + return imu.stamp >= targetTime; + }); + if (it != imuBuffer.begin()) { + --it; + imuBuffer.erase(imuBuffer.begin(), it); + } +} + +} // namespace + +int main(int argc, char** argv) { + if (argc < 2) { + std::cerr << "Usage:\n " << argv[0] << " \n"; + return 1; + } + + const std::string csvPath = argv[1]; + + try { + const EqVIOCsvLog log = readEqVIOCsv(csvPath); + + EqVIOFilterParams params; + params.initialPointDepth = metadataFiniteDouble( + log, "eqf.initial_point_depth", params.initialPointDepth); + params.initialPointVariance = metadataFiniteDouble( + log, "eqf.initial_point_variance", params.initialPointVariance); + params.measurementNoiseVariance = metadataFiniteDouble( + log, "eqf.measurement_noise_variance_norm", + params.measurementNoiseVariance); + params.outlierThresholdAbs = metadataFiniteDouble( + log, "eqf.outlier_threshold_abs", params.outlierThresholdAbs); + params.outlierThresholdAbs = metadataFiniteDouble( + log, "eqf.outlier_threshold_abs_norm", params.outlierThresholdAbs); + params.outlierThresholdProb = metadataFiniteDouble( + log, "eqf.outlier_threshold_prob", params.outlierThresholdProb); + params.featureRetention = metadataFiniteDouble( + log, "eqf.feature_retention", params.featureRetention); + + params.biasOmegaProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_bias_omega", params.biasOmegaProcessVariance); + params.biasAccelProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_bias_accel", params.biasAccelProcessVariance); + params.attitudeProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_attitude", params.attitudeProcessVariance); + params.positionProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_position", params.positionProcessVariance); + params.velocityProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_velocity", params.velocityProcessVariance); + params.cameraAttitudeProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_cam_attitude", params.cameraAttitudeProcessVariance); + params.cameraPositionProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_cam_position", params.cameraPositionProcessVariance); + params.pointProcessVariance = metadataFiniteDouble( + log, "eqf.process_var_point", params.pointProcessVariance); + + params.inputNoise.setZero(); + params.inputNoise.block<3, 3>(0, 0).setIdentity(); + params.inputNoise.block<3, 3>(3, 3).setIdentity(); + params.inputNoise.block<3, 3>(6, 6).setIdentity(); + params.inputNoise.block<3, 3>(9, 9).setIdentity(); + params.inputNoise.block<3, 3>(0, 0) *= + metadataFiniteDouble(log, "eqf.input_var_gyr", + params.inputNoise(0, 0)); + params.inputNoise.block<3, 3>(3, 3) *= + metadataFiniteDouble(log, "eqf.input_var_acc", + params.inputNoise(3, 3)); + params.inputNoise.block<3, 3>(6, 6) *= + metadataFiniteDouble(log, "eqf.input_var_gyr_bias_walk", + params.inputNoise(6, 6)); + params.inputNoise.block<3, 3>(9, 9) *= + metadataFiniteDouble(log, "eqf.input_var_acc_bias_walk", + params.inputNoise(9, 9)); + + SensorState sensor; + sensor.inputBias = Bias::Identity(); + sensor.pose = Pose3::Identity(); + sensor.velocity.setZero(); + sensor.cameraOffset = cameraExtrinsicsFromMetadata(log).value_or(Pose3::Identity()); + State xi0(sensor, {}); + Matrix Sigma0 = Matrix::Identity(xi0.dim(), xi0.dim()); + Sigma0.block<3, 3>(0, 0) *= metadataFiniteDouble( + log, "eqf.initial_var_bias_omega", 0.1); + Sigma0.block<3, 3>(3, 3) *= metadataFiniteDouble( + log, "eqf.initial_var_bias_accel", 0.1); + Sigma0.block<3, 3>(6, 6) *= metadataFiniteDouble( + log, "eqf.initial_var_attitude", 1e-4); + Sigma0.block<3, 3>(9, 9) *= metadataFiniteDouble( + log, "eqf.initial_var_position", 1e-4); + Sigma0.block<3, 3>(12, 12) *= metadataFiniteDouble( + log, "eqf.initial_var_velocity", 1e-2); + Sigma0.block<3, 3>(15, 15) *= metadataFiniteDouble( + log, "eqf.initial_var_cam_attitude", 1e-5); + Sigma0.block<3, 3>(18, 18) *= metadataFiniteDouble( + log, "eqf.initial_var_cam_position", 1e-4); + + EqVIOFilter filter(params); + filter.setReferenceState(xi0, Sigma0); + auto camera = std::make_shared( + Pose3::Identity(), Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); + + size_t imuCount = 0; + size_t visionFrameCount = 0; + size_t visionFeatureCount = 0; + double currentTime = -1.0; + std::vector imuBuffer; + for (const EqVIOCsvEvent& event : log.events) { + if (event.type == EqVIOCsvEvent::Type::Imu) { + if (!filter.isInitialized()) { + filter.initializeFromIMU(event.imu); + currentTime = event.imu.stamp; + } + if (filter.isInitialized()) imuBuffer.push_back(event.imu); + ++imuCount; + } else { + propagateBufferedImu(filter, imuBuffer, currentTime, event.tAbs); + const Matrix R = + Matrix::Identity(static_cast(2 * event.vision.size()), + static_cast(2 * event.vision.size())) * + params.measurementNoiseVariance; + filter.correct(event.vision, camera, R); + ++visionFrameCount; + visionFeatureCount += event.vision.size(); + } + } + + const State estimate = filter.stateEstimate(); + std::cout << "CSV replay complete.\n"; + std::cout << "Events: " << log.events.size() << ", IMU: " << imuCount + << ", vision frames: " << visionFrameCount + << ", vision features: " << visionFeatureCount << "\n"; + std::cout << "Measurement noise variance (normalized): " + << params.measurementNoiseVariance << "\n"; + std::cout << std::setprecision(17); + std::cout << "Filter time: " << currentTime << "\n"; + std::cout << std::setprecision(10); + std::cout << "Landmarks: " << estimate.n() << "\n"; + std::cout << "Pose translation: " + << estimate.sensor.pose.translation().transpose() << "\n"; + std::cout << "GT pose translation: " + << HardcodedGroundTruth::position().transpose() << "\n"; + std::cout << "Velocity: " << estimate.sensor.velocity.transpose() << "\n"; + std::cout << "GT velocity: " + << HardcodedGroundTruth::velocity().transpose() << "\n"; + + } catch (const std::exception& e) { + std::cerr << "EqVIOFilterCsvReplay failed: " << e.what() << "\n"; + return 2; + } + + return 0; +} diff --git a/gtsam_unstable/geometry/ABCEquivariantFilter.h b/gtsam_unstable/geometry/ABCEquivariantFilter.h index fb6a7695e0..ca6497cdb8 100644 --- a/gtsam_unstable/geometry/ABCEquivariantFilter.h +++ b/gtsam_unstable/geometry/ABCEquivariantFilter.h @@ -1,7 +1,10 @@ /** * @file ABCEquivariantFilter.h * @brief Attitude-Bias-Calibration Equivariant Filter for state estimation + * @author Jennifer Oum + * @author Darshan Rajasekaran * @author Rohan Bansal + * @author Frank Dellaert * @date 2026 * * This file extends the EquivariantFilter class to provide a more user-friendly diff --git a/gtsam_unstable/navigation/CMakeLists.txt b/gtsam_unstable/navigation/CMakeLists.txt new file mode 100644 index 0000000000..73026a11b2 --- /dev/null +++ b/gtsam_unstable/navigation/CMakeLists.txt @@ -0,0 +1,7 @@ +# Install headers +file(GLOB navigation_headers "*.h") +install(FILES ${navigation_headers} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam_unstable/navigation) + +# Add all tests +add_subdirectory(tests) diff --git a/gtsam_unstable/navigation/EqVIOCommon.h b/gtsam_unstable/navigation/EqVIOCommon.h new file mode 100644 index 0000000000..886d0ae8a4 --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOCommon.h @@ -0,0 +1,320 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOCommon.h + * @brief Common EqVIO math/data types for unstable navigation. + * @author Rohan Bansal + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { +namespace eqvio { + +using SOT3 = ProductLieGroup; + +using Se23 = ExtendedPose3<2>; +using Bias = imuBias::ConstantBias; +using LandmarkGroup = PowerLieGroup; +using SensorCore = ProductLieGroup; +using LandmarkCore = ProductLieGroup; +using VioGroup = ProductLieGroup; + +/// Approximate gravitational acceleration magnitude in m/s^2. +constexpr double GRAVITY_CONSTANT = 9.80665; + +/// Return positive scale component of SOT3. +inline double SOT3Scale(const SOT3& Q) { return std::exp(Q.second); } + +/// Return SO3 rotation component of SOT3. +inline const SO3& SOT3Rotation(const SOT3& Q) { return Q.first; } + +/// Return scaled-rotation matrix a*R for SOT3 element (R,log(a)). +inline Matrix3 SOT3ScaledRotation(const SOT3& Q) { + return SOT3Rotation(Q).matrix() * SOT3Scale(Q); +} + +/// Apply inverse SOT3 transform to a 3D point. +inline Vector3 SOT3ApplyInverse(const SOT3& Q, const Vector3& p) { + return (1.0 / SOT3Scale(Q)) * (SOT3Rotation(Q).matrix().transpose() * p); +} + +/// Construct SOT3 from rotation and positive scale. +inline SOT3 MakeSOT3(const SO3& R, double scale) { + if (scale <= 0.0) { + throw std::invalid_argument("MakeSOT3: scale must be strictly positive"); + } + return SOT3(R, std::log(scale)); +} + +/// IMU input bundle used by EqVIO propagation. +struct GTSAM_UNSTABLE_EXPORT IMUInput { + static constexpr int CompDim = 12; + using Vector12 = Eigen::Matrix; + + double stamp = -1.0; + Vector3 gyr = Vector3::Zero(); + Vector3 acc = Vector3::Zero(); + Vector3 gyrBiasVel = Vector3::Zero(); + Vector3 accBiasVel = Vector3::Zero(); + + /// Return a zero-initialized input with invalid timestamp. + static IMUInput Zero() { return IMUInput(); } + + IMUInput() = default; + + /// Construct from stacked [gyr, acc, gyrBiasVel, accBiasVel]. + explicit IMUInput(const Vector12& vec) { + gyr = vec.segment<3>(0); + acc = vec.segment<3>(3); + gyrBiasVel = vec.segment<3>(6); + accBiasVel = vec.segment<3>(9); + } + + /// Component-wise addition. + IMUInput operator+(const IMUInput& other) const { + IMUInput out; + out.stamp = stamp >= 0.0 ? stamp : other.stamp; + out.gyr = gyr + other.gyr; + out.acc = acc + other.acc; + out.gyrBiasVel = gyrBiasVel + other.gyrBiasVel; + out.accBiasVel = accBiasVel + other.accBiasVel; + return out; + } + + /// Subtract a ConstantBias from [gyr, acc]. + IMUInput operator-(const Bias& bias) const { + IMUInput out(*this); + out.gyr -= bias.gyroscope(); + out.acc -= bias.accelerometer(); + return out; + } + + /// Scale all components. + IMUInput operator*(double c) const { + IMUInput out(*this); + out.gyr *= c; + out.acc *= c; + out.gyrBiasVel *= c; + out.accBiasVel *= c; + return out; + } +}; + +/** + * @brief Camera model used by EqVIO measurement functions. + * + * We reuse GTSAM's existing `PinholeCamera` instead of defining a new + * camera class here. + * + * More general camera models (distortion, fisheye, etc.) are intentionally not + * used here because EqVIO also needs: + * 1. an explicit "undistort to normalized bearing" operation, and + * 2. a closed-form projection Jacobian with respect to the 3D ray. + * Those operations are model-specific for non-pinhole cameras and would add + * extra branches and conversion code. + */ +using CameraModel = PinholeCamera; + +/** + * @brief Convert image coordinates into a normalized bearing-like vector. + * + * For `Cal3_S2`, calibration inversion maps pixel coordinates to normalized + * image coordinates `(x/z, y/z)`. EqVIO represents this as a 3D direction-like + * vector `[x/z, y/z, 1]`. + */ +inline Vector3 undistortPoint(const CameraModel& camera, const Point2& y) { + const Point2 p = camera.calibration().calibrate(y); + return Vector3(p.x(), p.y(), 1.0); +} + +/** + * @brief Jacobian of pixel projection with respect to a camera-frame 3D point. + * + * This returns `d pi(y) / d y` for the `Cal3_S2` pinhole projection used by + * EqVIO's linearized output model. + * + * @throws std::invalid_argument if `y.z()` is numerically near zero. + */ +inline Matrix23 projectionJacobian(const CameraModel& camera, const Vector3& y) { + if (std::abs(y.z()) < 1e-12) { + throw std::invalid_argument("projectionJacobian: z is near zero"); + } + + const double invz = 1.0 / y.z(); + const double invz2 = invz * invz; + const double fx = camera.calibration().fx(); + const double fy = camera.calibration().fy(); + const double s = camera.calibration().skew(); + + Matrix23 J; + J << fx * invz, s * invz, -(fx * y.x() + s * y.y()) * invz2, 0.0, fy * invz, + -fy * y.y() * invz2; + return J; +} + +/// Vision measurement keyed by landmark id. +using VisionMeasurement = std::map; + +/// Ordered landmark ids matching map iteration order. +inline std::vector measurementIds(const VisionMeasurement& measurement) { + std::vector ids; + ids.reserve(measurement.size()); + for (const auto& item : measurement) { + ids.push_back(item.first); + } + return ids; +} + +/// Remove a contiguous row block from a matrix. +inline void removeRows(Matrix& mat, int startRow, int numRows) { + const int rows = mat.rows(); + const int cols = mat.cols(); + assert(startRow >= 0 && numRows >= 0 && startRow + numRows <= rows); + mat.block(startRow, 0, rows - numRows - startRow, cols) = + mat.block(startRow + numRows, 0, rows - numRows - startRow, cols); + mat.conservativeResize(rows - numRows, Eigen::NoChange); +} + +/// Remove a contiguous column block from a matrix. +inline void removeCols(Matrix& mat, int startCol, int numCols) { + const int rows = mat.rows(); + const int cols = mat.cols(); + assert(startCol >= 0 && numCols >= 0 && startCol + numCols <= cols); + mat.block(0, startCol, rows, cols - numCols - startCol) = + mat.block(0, startCol + numCols, rows, cols - numCols - startCol); + mat.conservativeResize(Eigen::NoChange, cols - numCols); +} + +/// Readable accessors for the composed ProductLieGroup VioGroup. +inline const Se23& A_sensorKinematics(const VioGroup& X) { + return X.first.first; +} + +inline const Bias& Beta_biasOffset(const VioGroup& X) { + return X.first.second; +} + +inline const Pose3& B_cameraExtrinsics(const VioGroup& X) { + return X.second.first; +} + +inline const LandmarkGroup& Q_landmarkTransforms(const VioGroup& X) { + return X.second.second; +} + +inline size_t N_landmarkCount(const VioGroup& X) { + return Q_landmarkTransforms(X).size(); +} +inline size_t Dim_groupTangent(const VioGroup& X) { + return 21 + 4 * N_landmarkCount(X); +} + +inline VioGroup makeVioGroup(const Se23& sensor_kinematics, + const Bias& bias_offset, + const Pose3& camera_extrinsics, + const LandmarkGroup& landmark_transforms) { + return VioGroup(SensorCore(sensor_kinematics, bias_offset), + LandmarkCore(camera_extrinsics, landmark_transforms)); +} + +inline VioGroup makeVioGroupIdentity(size_t n = 0) { + return makeVioGroup(Se23::Identity(), Bias::Identity(), Pose3::Identity(), + LandmarkGroup(n)); +} + +} // namespace eqvio + +} // namespace gtsam + +namespace gtsam { + +template +inline decltype(auto) get(eqvio::VioGroup& X) { + static_assert(I < 4, "VioGroup index out of range"); + if constexpr (I == 0) { + return (X.first.first); + } else if constexpr (I == 1) { + return (X.first.second); + } else if constexpr (I == 2) { + return (X.second.first); + } else { + return (X.second.second); + } +} + +template +inline decltype(auto) get(const eqvio::VioGroup& X) { + static_assert(I < 4, "VioGroup index out of range"); + if constexpr (I == 0) { + return (X.first.first); + } else if constexpr (I == 1) { + return (X.first.second); + } else if constexpr (I == 2) { + return (X.second.first); + } else { + return (X.second.second); + } +} + +} // namespace gtsam + +namespace std { + +template <> +struct tuple_size : std::integral_constant {}; + +template <> +struct tuple_element<0, gtsam::eqvio::VioGroup> { + using type = gtsam::eqvio::Se23; +}; + +template <> +struct tuple_element<1, gtsam::eqvio::VioGroup> { + using type = gtsam::eqvio::Bias; +}; + +template <> +struct tuple_element<2, gtsam::eqvio::VioGroup> { + using type = gtsam::Pose3; +}; + +template <> +struct tuple_element<3, gtsam::eqvio::VioGroup> { + using type = gtsam::eqvio::LandmarkGroup; +}; + +} // namespace std diff --git a/gtsam_unstable/navigation/EqVIOCsv.cpp b/gtsam_unstable/navigation/EqVIOCsv.cpp new file mode 100644 index 0000000000..4ebe84e298 --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOCsv.cpp @@ -0,0 +1,238 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOCsv.cpp +/// @brief CSV utilities for replaying preprocessed EqVIO streams. + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { +namespace eqvio { + +namespace { + +std::string trim(const std::string& s) { + size_t b = 0; + while (b < s.size() && std::isspace(static_cast(s[b]))) ++b; + size_t e = s.size(); + while (e > b && std::isspace(static_cast(s[e - 1]))) --e; + return s.substr(b, e - b); +} + +std::vector splitCsvLine(const std::string& line) { + std::vector cols; + std::stringstream ss(line); + std::string cell; + while (std::getline(ss, cell, ',')) { + cols.push_back(trim(cell)); + } + return cols; +} + +double parseDoubleOr(const std::string& s, double fallback = 0.0) { + if (s.empty()) return fallback; + return std::stod(s); +} + +size_t parseSizeOr(const std::string& s, size_t fallback = 0) { + if (s.empty()) return fallback; + return static_cast(std::stoull(s)); +} + +int parseIntOr(const std::string& s, int fallback = 0) { + if (s.empty()) return fallback; + return std::stoi(s); +} + +std::unordered_map buildHeaderMap( + const std::vector& header) { + std::unordered_map index; + for (size_t i = 0; i < header.size(); ++i) { + index[header[i]] = i; + } + return index; +} + +std::string getCell(const std::vector& cells, + const std::unordered_map& idx, + const std::string& key) { + const auto it = idx.find(key); + if (it == idx.end()) { + throw std::invalid_argument("EqVIOCsv: missing required column: " + key); + } + return (it->second < cells.size()) ? cells[it->second] : ""; +} + +} // namespace + +EqVIOCsvLog readEqVIOCsv(const std::string& csvPath) { + std::ifstream in(csvPath); + if (!in.is_open()) { + throw std::invalid_argument("EqVIOCsv: cannot open file: " + csvPath); + } + + std::string headerLine; + if (!std::getline(in, headerLine)) { + throw std::invalid_argument("EqVIOCsv: empty file: " + csvPath); + } + + const std::vector header = splitCsvLine(headerLine); + const auto index = buildHeaderMap(header); + const std::vector required = { + "row_type", "t_abs", "seq", "frame_idx", "landmark_id", "gx", "gy", + "gz", "ax", "ay", "az", "bgx", "bgy", "bgz", + "bax", "bay", "baz", "u_norm", "v_norm", "key", "value"}; + for (const auto& key : required) { + if (index.count(key) == 0) { + throw std::invalid_argument("EqVIOCsv: missing required column: " + key); + } + } + + EqVIOCsvLog log; + std::unordered_map seqToVisionEvent; + + std::string line; + while (std::getline(in, line)) { + if (trim(line).empty()) continue; + + const std::vector cells = splitCsvLine(line); + const std::string rowType = getCell(cells, index, "row_type"); + + if (rowType == "meta") { + const std::string key = getCell(cells, index, "key"); + const std::string value = getCell(cells, index, "value"); + if (!key.empty()) { + log.metadata[key] = value; + } + continue; + } + + if (rowType == "imu") { + EqVIOCsvEvent event; + event.type = EqVIOCsvEvent::Type::Imu; + event.seq = parseSizeOr(getCell(cells, index, "seq")); + event.tAbs = parseDoubleOr(getCell(cells, index, "t_abs")); + event.imu.stamp = event.tAbs; + event.imu.gyr = Vector3(parseDoubleOr(getCell(cells, index, "gx")), + parseDoubleOr(getCell(cells, index, "gy")), + parseDoubleOr(getCell(cells, index, "gz"))); + event.imu.acc = Vector3(parseDoubleOr(getCell(cells, index, "ax")), + parseDoubleOr(getCell(cells, index, "ay")), + parseDoubleOr(getCell(cells, index, "az"))); + event.imu.gyrBiasVel = + Vector3(parseDoubleOr(getCell(cells, index, "bgx")), + parseDoubleOr(getCell(cells, index, "bgy")), + parseDoubleOr(getCell(cells, index, "bgz"))); + event.imu.accBiasVel = + Vector3(parseDoubleOr(getCell(cells, index, "bax")), + parseDoubleOr(getCell(cells, index, "bay")), + parseDoubleOr(getCell(cells, index, "baz"))); + log.events.push_back(event); + continue; + } + + if (rowType == "vision_feature") { + const size_t seq = parseSizeOr(getCell(cells, index, "seq")); + const double tAbs = parseDoubleOr(getCell(cells, index, "t_abs")); + const int frameIdx = parseIntOr(getCell(cells, index, "frame_idx"), -1); + const int id = parseIntOr(getCell(cells, index, "landmark_id")); + const double u = parseDoubleOr(getCell(cells, index, "u_norm")); + const double v = parseDoubleOr(getCell(cells, index, "v_norm")); + + auto it = seqToVisionEvent.find(seq); + if (it == seqToVisionEvent.end()) { + EqVIOCsvEvent event; + event.type = EqVIOCsvEvent::Type::VisionFrame; + event.seq = seq; + event.tAbs = tAbs; + event.frameIdx = frameIdx; + event.vision[id] = Point2(u, v); + seqToVisionEvent.emplace(seq, log.events.size()); + log.events.push_back(std::move(event)); + } else { + EqVIOCsvEvent& event = log.events[it->second]; + if (event.frameIdx != frameIdx) { + throw std::invalid_argument( + "EqVIOCsv: inconsistent frame_idx for vision seq=" + + std::to_string(seq)); + } + if (std::abs(event.tAbs - tAbs) > 1e-9) { + throw std::invalid_argument( + "EqVIOCsv: inconsistent t_abs for vision seq=" + + std::to_string(seq)); + } + event.vision[id] = Point2(u, v); + } + continue; + } + + throw std::invalid_argument("EqVIOCsv: unknown row_type: " + rowType); + } + + std::stable_sort(log.events.begin(), log.events.end(), + [](const EqVIOCsvEvent& a, const EqVIOCsvEvent& b) { + return a.seq < b.seq; + }); + return log; +} + +double metadataDouble(const EqVIOCsvLog& log, const std::string& key, + double fallback) { + const auto it = log.metadata.find(key); + if (it == log.metadata.end() || it->second.empty()) return fallback; + try { + return std::stod(it->second); + } catch (...) { + return fallback; + } +} + +std::optional cameraExtrinsicsFromMetadata(const EqVIOCsvLog& log) { + const std::vector keys = { + "T_ci_tx", "T_ci_ty", "T_ci_tz", "T_ci_qw", + "T_ci_qx", "T_ci_qy", "T_ci_qz"}; + for (const auto& key : keys) { + const auto it = log.metadata.find(key); + if (it == log.metadata.end() || it->second.empty()) { + return std::nullopt; + } + } + + const double tx = std::stod(log.metadata.at("T_ci_tx")); + const double ty = std::stod(log.metadata.at("T_ci_ty")); + const double tz = std::stod(log.metadata.at("T_ci_tz")); + double qw = std::stod(log.metadata.at("T_ci_qw")); + double qx = std::stod(log.metadata.at("T_ci_qx")); + double qy = std::stod(log.metadata.at("T_ci_qy")); + double qz = std::stod(log.metadata.at("T_ci_qz")); + + const double qnorm = std::sqrt(qw * qw + qx * qx + qy * qy + qz * qz); + if (qnorm <= 1e-12) { + return std::nullopt; + } + qw /= qnorm; + qx /= qnorm; + qy /= qnorm; + qz /= qnorm; + + return Pose3(Rot3::Quaternion(qw, qx, qy, qz), Point3(tx, ty, tz)); +} + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOCsv.h b/gtsam_unstable/navigation/EqVIOCsv.h new file mode 100644 index 0000000000..8102573bdc --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOCsv.h @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOCsv.h +/// @brief CSV utilities for replaying preprocessed EqVIO streams. + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { +namespace eqvio { + +/// Parsed replay event from a typed EqVIO CSV stream. +struct GTSAM_UNSTABLE_EXPORT EqVIOCsvEvent { + enum class Type { Imu, VisionFrame }; + + Type type = Type::Imu; + size_t seq = 0; + double tAbs = 0.0; + int frameIdx = -1; + IMUInput imu; + VisionMeasurement vision; +}; + +/// Parsed EqVIO CSV log: metadata and merged event stream. +struct GTSAM_UNSTABLE_EXPORT EqVIOCsvLog { + std::map metadata; + std::vector events; +}; + +/// Parse a typed EqVIO CSV file. +GTSAM_UNSTABLE_EXPORT EqVIOCsvLog readEqVIOCsv(const std::string& csvPath); + +/// Parse metadata value as double with fallback. +GTSAM_UNSTABLE_EXPORT double metadataDouble(const EqVIOCsvLog& log, + const std::string& key, + double fallback); + +/// Parse camera extrinsics Pose3 from metadata keys, if present. +GTSAM_UNSTABLE_EXPORT std::optional cameraExtrinsicsFromMetadata( + const EqVIOCsvLog& log); + +} // namespace eqvio +} // namespace gtsam + diff --git a/gtsam_unstable/navigation/EqVIOFilter.cpp b/gtsam_unstable/navigation/EqVIOFilter.cpp new file mode 100644 index 0000000000..af31c5377f --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOFilter.cpp @@ -0,0 +1,477 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOFilter.cpp +/// @brief Standalone equivariant VIO filter for gtsam_unstable. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { +namespace eqvio { + +namespace { + +Vector measurementVector(const VisionMeasurement& measurement) { + Vector v = Vector::Zero(static_cast(2 * measurement.size())); + int i = 0; + for (const auto& item : measurement) { + v.segment<2>(2 * i) = item.second; + ++i; + } + return v; +} + +Vector measurementDifference(const VisionMeasurement& lhs, + const VisionMeasurement& rhs) { + if (lhs.size() != rhs.size()) { + throw std::invalid_argument("measurementDifference: size mismatch"); + } + + Vector diff = Vector::Zero(static_cast(2 * lhs.size())); + auto itL = lhs.begin(); + auto itR = rhs.begin(); + int i = 0; + for (; itL != lhs.end(); ++itL, ++itR) { + if (itL->first != itR->first) { + throw std::invalid_argument("measurementDifference: id mismatch"); + } + diff.segment<2>(2 * i) = itL->second - itR->second; + ++i; + } + return diff; +} + +} // namespace + +EqVIOFilter::EqVIOFilter() : EqVIOFilter(EqVIOFilterParams()) {} + +EqVIOFilter::EqVIOFilter(const EqVIOFilterParams& params) + : Base(State(), defaultCovariance(0), makeVioGroupIdentity()), + params_(params) { + view_.xi0 = State(); + view_.xi0.sensor.inputBias = Bias::Identity(); + view_.xi0.sensor.pose = Pose3::Identity(); + view_.xi0.sensor.velocity.setZero(); + view_.xi0.sensor.cameraOffset = Pose3::Identity(); + view_.X = makeVioGroupIdentity(); + view_.Sigma = defaultCovariance(0); + syncBase(true); +} + +EqVIOFilter::EqVIOFilter(const State& xi0, const Matrix& Sigma0, + const EqVIOFilterParams& params) + : Base(State(), defaultCovariance(0), makeVioGroupIdentity()), + params_(params) { + view_.xi0 = xi0; + view_.X = makeVioGroupIdentity(view_.xi0.n()); + view_.Sigma = Sigma0; + initialized_ = true; + syncBase(true); +} + +void EqVIOFilter::initializeFromIMU(const IMUInput& imu) { + view_.xi0.sensor.inputBias = Bias::Identity(); + view_.xi0.sensor.pose = Pose3::Identity(); + view_.xi0.sensor.velocity.setZero(); + + Vector3 approxGravity = imu.acc; + if (approxGravity.norm() < 1e-9) approxGravity = Vector3::UnitZ(); + // Build initial attitude that aligns measured gravity with +Z. + Quaternion q; + q.setFromTwoVectors(approxGravity.normalized(), Vector3::UnitZ()); + const Rot3 R0(q); + view_.xi0.sensor.pose = Pose3(R0, Point3::Zero()); + initialized_ = true; + syncBase(true); +} + +void EqVIOFilter::setReferenceState(const State& xi0, const Matrix& Sigma0) { + if (Sigma0.rows() != xi0.dim() || Sigma0.cols() != xi0.dim()) { + throw std::invalid_argument( + "EqVIOFilter::setReferenceState: covariance dimension mismatch"); + } + view_.xi0 = xi0; + view_.X = makeVioGroupIdentity(xi0.n()); + view_.Sigma = Sigma0; + syncBase(true); +} + +void EqVIOFilter::propagate(const IMUInput& imu, double dt) { + propagateCovariance(imu, dt); + propagateState(imu, dt); +} + +void EqVIOFilter::propagateCovariance(const IMUInput& imu, double dt) { + if (!initialized_ || dt <= 0.0) { + return; + } + const Matrix A0t = + EqFCoordinateSuite_invdepth.stateMatrixA(view_.X, view_.xi0, imu); + const Matrix Bt = EqFCoordinateSuite_invdepth.inputMatrixB(view_.X, view_.xi0); + const Matrix A0tExp = + Matrix::Identity(view_.xi0.dim(), view_.xi0.dim()) + dt * A0t; + view_.Sigma = A0tExp * view_.Sigma * A0tExp.transpose() + + dt * (Bt * params_.inputNoise * Bt.transpose() + + stateProcessNoise(view_.xi0.n())); + setErrorCovariance(view_.Sigma); +} + +void EqVIOFilter::propagateState(const IMUInput& imu, double dt) { + if (!initialized_ || dt <= 0.0) { + return; + } + auto liftFunctor = [imu, dt](const State& xi) -> Vector { + return (VioGroup::Logmap(liftVelocityDiscrete(xi, imu, dt)) / dt).eval(); + }; + const Matrix A = Matrix::Zero(view_.xi0.dim(), view_.xi0.dim()); + const Matrix Qc = Matrix::Zero(view_.xi0.dim(), view_.xi0.dim()); + Base::template predictWithJacobian<1>(liftFunctor, A, Qc, dt); + syncFromBase(); +} + +void EqVIOFilter::correct(const VisionMeasurement& measurement, + const std::shared_ptr& camera, + const Matrix& R) { + if (!initialized_) { + return; + } + + removeOldLandmarks(measurementIds(measurement)); + + VisionMeasurement matchedMeasurement = measurement; + removeOutliers(matchedMeasurement, camera); + addNewLandmarks(matchedMeasurement, camera); + + if (matchedMeasurement.empty()) { + syncBase(false); + return; + } + + update(matchedMeasurement, camera, R); + removeInvalidLandmarksNow(); + + syncBase(false); + + assert(!view_.Sigma.hasNaN()); +} + +State EqVIOFilter::stateEstimate() const { + return stateGroupAction(view_.X, view_.xi0); +} + +Matrix EqVIOFilter::defaultCovariance(size_t nLandmarks) { + const int d = SensorState::CompDim + 3 * static_cast(nLandmarks); + return Matrix::Identity(d, d); +} + +void EqVIOFilter::syncBase(bool resetReference) { + if (resetReference) { + resetReferenceAndGroup(view_.xi0, view_.Sigma, view_.X); + return; + } + // ensure referenceState().n() == N_landmarkCount(view_.X)) + setGroupEstimateAndSyncState(view_.X); + setErrorCovariance(view_.Sigma); +} + +void EqVIOFilter::syncFromBase() { + view_.X = groupEstimate(); + view_.Sigma = errorCovariance(); +} + +Matrix EqVIOFilter::stateProcessNoise(size_t nLandmarks) const { + Matrix Q = Matrix::Identity( + SensorState::CompDim + 3 * static_cast(nLandmarks), + SensorState::CompDim + 3 * static_cast(nLandmarks)); + Q.block<3, 3>(0, 0) *= params_.biasOmegaProcessVariance; + Q.block<3, 3>(3, 3) *= params_.biasAccelProcessVariance; + Q.block<3, 3>(6, 6) *= params_.attitudeProcessVariance; + Q.block<3, 3>(9, 9) *= params_.positionProcessVariance; + Q.block<3, 3>(12, 12) *= params_.velocityProcessVariance; + Q.block<3, 3>(15, 15) *= params_.cameraAttitudeProcessVariance; + Q.block<3, 3>(18, 18) *= params_.cameraPositionProcessVariance; + if (nLandmarks > 0) { + Q.block(SensorState::CompDim, SensorState::CompDim, + 3 * static_cast(nLandmarks), 3 * static_cast(nLandmarks)) *= + params_.pointProcessVariance; + } + return Q; +} + +void EqVIOFilter::addNewLandmarks( + const VisionMeasurement& measurement, + const std::shared_ptr& camera) { + if (measurement.empty()) return; + if (!camera) { + throw std::invalid_argument("EqVIOFilter::addNewLandmarks: null camera"); + } + + std::vector newLandmarks; + const std::vector existingIds = view_.xi0.ids(); + for (const auto& [id, coord] : measurement) { + if (std::find(existingIds.begin(), existingIds.end(), id) != existingIds.end()) { + continue; + } + const Vector3 bearing = undistortPoint(*camera, coord); + newLandmarks.push_back(Landmark{bearing, id}); + } + + if (newLandmarks.empty()) return; + + const double initialDepth = params_.initialPointDepth; + for (Landmark& lm : newLandmarks) lm.p *= initialDepth; + + const Matrix newLandmarksCov = + Matrix::Identity(3 * static_cast(newLandmarks.size()), + 3 * static_cast(newLandmarks.size())) * + params_.initialPointVariance; + addLandmarksInternal(newLandmarks, newLandmarksCov); +} + +void EqVIOFilter::addLandmarksInternal(std::vector& newLandmarks, + const Matrix& newLandmarkCov) { + if (newLandmarks.empty()) return; + + view_.xi0.cameraLandmarks.insert(view_.xi0.cameraLandmarks.end(), + newLandmarks.begin(), newLandmarks.end()); + + const auto& [A, Beta, B, Q] = view_.X; + std::vector q; + q.reserve(Q.size() + newLandmarks.size()); + for (size_t i = 0; i < Q.size(); ++i) { + q.push_back(Q[i]); + } + for (size_t i = 0; i < newLandmarks.size(); ++i) { + q.push_back(SOT3::Identity()); + } + + view_.X = makeVioGroup(A, Beta, B, LandmarkGroup(q)); + + const int oldSize = view_.Sigma.rows(); + const int newN = static_cast(newLandmarks.size()); + view_.Sigma.conservativeResize(oldSize + 3 * newN, oldSize + 3 * newN); + view_.Sigma.block(oldSize, 0, 3 * newN, oldSize).setZero(); + view_.Sigma.block(0, oldSize, oldSize, 3 * newN).setZero(); + view_.Sigma.block(oldSize, oldSize, 3 * newN, 3 * newN) = newLandmarkCov; + + syncBase(true); +} + +void EqVIOFilter::removeOldLandmarks(const std::vector& measurementIds) { + const std::vector existingIds = view_.xi0.ids(); + std::vector lostIndices(existingIds.size()); + std::iota(lostIndices.begin(), lostIndices.end(), 0); + if (lostIndices.empty()) return; + + const auto lostIndicesEnd = std::remove_if( + lostIndices.begin(), lostIndices.end(), [&](const int& lidx) { + const int oldId = existingIds[static_cast(lidx)]; + return std::any_of(measurementIds.begin(), measurementIds.end(), + [&oldId](const int& measId) { return measId == oldId; }); + }); + lostIndices.erase(lostIndicesEnd, lostIndices.end()); + + if (lostIndices.empty()) return; + + std::reverse(lostIndices.begin(), lostIndices.end()); + for (const int idx : lostIndices) { + removeLandmarkByIndex(idx); + } +} + +void EqVIOFilter::removeLandmarkByIndex(int idx) { + view_.xi0.cameraLandmarks.erase(view_.xi0.cameraLandmarks.begin() + idx); + + const auto& [A, Beta, B, Q] = view_.X; + std::vector q; + q.reserve(Q.size() - 1); + for (size_t i = 0; i < Q.size(); ++i) { + if (static_cast(i) == idx) continue; + q.push_back(Q[i]); + } + + view_.X = makeVioGroup(A, Beta, B, LandmarkGroup(q)); + + removeRows(view_.Sigma, SensorState::CompDim + 3 * idx, 3); + removeCols(view_.Sigma, SensorState::CompDim + 3 * idx, 3); + + syncBase(true); +} + +void EqVIOFilter::removeLandmarkById(int id) { + const auto it = std::find_if( + view_.xi0.cameraLandmarks.begin(), view_.xi0.cameraLandmarks.end(), + [&id](const Landmark& lm) { return lm.id == id; }); + assert(it != view_.xi0.cameraLandmarks.end()); + removeLandmarkByIndex( + static_cast(std::distance(view_.xi0.cameraLandmarks.begin(), it))); +} + +Matrix3 EqVIOFilter::getLandmarkCovById(int id) const { + const auto it = std::find_if( + view_.xi0.cameraLandmarks.begin(), view_.xi0.cameraLandmarks.end(), + [&id](const Landmark& lm) { return lm.id == id; }); + assert(it != view_.xi0.cameraLandmarks.end()); + const int i = + static_cast(std::distance(view_.xi0.cameraLandmarks.begin(), it)); + return view_.Sigma.block<3, 3>(SensorState::CompDim + 3 * i, + SensorState::CompDim + 3 * i); +} + +Matrix2 EqVIOFilter::outputCovarianceById( + int id, const Point2&, + const std::shared_ptr& camera) const { + const Matrix3 lmCov = getLandmarkCovById(id); + const auto it = std::find_if( + view_.xi0.cameraLandmarks.begin(), view_.xi0.cameraLandmarks.end(), + [&id](const Landmark& lm) { return lm.id == id; }); + assert(it != view_.xi0.cameraLandmarks.end()); + + const size_t i = + static_cast(std::distance(view_.xi0.cameraLandmarks.begin(), it)); + const auto& Q = gtsam::get<3>(view_.X); + const SOT3& Q_i = Q[i]; + + const Matrix23 C0i = EqFCoordinateSuite_invdepth.outputMatrixCi(it->p, Q_i, camera); + return C0i * lmCov * C0i.transpose(); +} + +void EqVIOFilter::removeInvalidLandmarksNow() { + const auto& Q = gtsam::get<3>(view_.X); + std::set invalidIds; + for (size_t i = 0; i < Q.size(); ++i) { + const double a = SOT3Scale(Q[i]); + if (!std::isfinite(a) || a <= 1e-8 || a > 1e8) { + invalidIds.insert(view_.xi0.cameraLandmarks[i].id); + } + } + for (const int id : invalidIds) { + removeLandmarkById(id); + } +} + +void EqVIOFilter::removeOutliers( + VisionMeasurement& measurement, + const std::shared_ptr& camera) { + const size_t maxOutliers = static_cast( + (1.0 - params_.featureRetention) * measurement.size()); + if (!camera) return; + + const VisionMeasurement yHat = measureSystemState(stateEstimate(), camera); + + std::vector proposedOutliers; + std::map absoluteOutliers; + for (const auto& [lmId, yHatI] : yHat) { + if (measurement.count(lmId) == 0) continue; + const double errAbs = (measurement.at(lmId) - yHatI).norm(); + if (errAbs > params_.outlierThresholdAbs) { + absoluteOutliers[lmId] = errAbs; + proposedOutliers.push_back(lmId); + } + } + + std::map probabilisticOutliers; + for (const auto& [lmId, yI] : measurement) { + if (absoluteOutliers.count(lmId)) continue; + const auto itHat = yHat.find(lmId); + if (itHat == yHat.end()) continue; + const Point2 yTildeI = yI - itHat->second; + + const Matrix2 outputCov = outputCovarianceById(lmId, measurement.at(lmId), camera); + const double errProb = yTildeI.transpose() * outputCov.inverse() * yTildeI; + if (errProb > params_.outlierThresholdProb) { + probabilisticOutliers[lmId] = errProb; + proposedOutliers.push_back(lmId); + } + } + + std::sort(proposedOutliers.begin(), proposedOutliers.end(), + [&absoluteOutliers, &probabilisticOutliers](int lmId1, int lmId2) { + if (absoluteOutliers.count(lmId1)) { + if (absoluteOutliers.count(lmId2)) { + return absoluteOutliers.at(lmId1) < absoluteOutliers.at(lmId2); + } + return false; + } + if (absoluteOutliers.count(lmId2)) return true; + return probabilisticOutliers.at(lmId1) < probabilisticOutliers.at(lmId2); + }); + std::reverse(proposedOutliers.begin(), proposedOutliers.end()); + if (proposedOutliers.size() > maxOutliers) { + proposedOutliers.erase(proposedOutliers.begin() + maxOutliers, + proposedOutliers.end()); + } + + for (const int lmId : proposedOutliers) { + removeLandmarkById(lmId); + measurement.erase(lmId); + } +} + +double EqVIOFilter::getMedianSceneDepth() const { + const std::vector landmarks = stateEstimate().cameraLandmarks; + if (landmarks.empty()) return params_.initialPointDepth; + + std::vector depthsSquared(landmarks.size()); + std::transform(landmarks.begin(), landmarks.end(), depthsSquared.begin(), + [](const Landmark& lm) { return lm.p.squaredNorm(); }); + const auto midway = depthsSquared.begin() + depthsSquared.size() / 2; + std::nth_element(depthsSquared.begin(), midway, depthsSquared.end()); + + double medianDepth = params_.initialPointDepth; + if (midway != depthsSquared.end()) { + medianDepth = std::pow(*midway, 0.5); + } + return medianDepth; +} + +void EqVIOFilter::update(const VisionMeasurement& measurement, + const std::shared_ptr& camera, + const Matrix& outputGainMatrix) { + if (measurement.empty()) return; + if (!camera) { + throw std::invalid_argument("EqVIOFilter::update: null camera"); + } + const VisionMeasurement estimatedMeasurement = + measureSystemState(stateEstimate(), camera); + Vector yTilde; + try { + yTilde = measurementDifference(measurement, estimatedMeasurement); + } catch (const std::exception& e) { + throw std::invalid_argument(std::string("EqVIOFilter::update: ") + e.what()); + } + const Matrix Ct = EqFCoordinateSuite_invdepth.outputMatrixC( + view_.xi0, view_.X, measurement, camera, true); + + const Matrix Rused = + (outputGainMatrix.rows() == Ct.rows() && outputGainMatrix.cols() == Ct.rows()) + ? outputGainMatrix + : Matrix::Identity(Ct.rows(), Ct.rows()) * params_.measurementNoiseVariance; + + const Vector zhat = measurementVector(estimatedMeasurement); + const Vector z = measurementVector(measurement); + Base::updateWithVector(zhat, Ct, z, Rused, [this](const Vector& delta_xi) -> Vector { + return EqFCoordinateSuite_invdepth.liftInnovation(delta_xi, view_.xi0); + }); + syncFromBase(); +} + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOFilter.h b/gtsam_unstable/navigation/EqVIOFilter.h new file mode 100644 index 0000000000..107ca50461 --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOFilter.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOFilter.h +/// @brief Standalone equivariant VIO filter for gtsam_unstable. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { +namespace eqvio { + +/// Runtime parameters for the standalone EqVIO filter. +struct GTSAM_UNSTABLE_EXPORT EqVIOFilterParams { + double initialPointDepth = 10.0; + double initialPointVariance = 1.0; + double measurementNoiseVariance = 1e-4; + double outlierThresholdAbs = 1e8; + double outlierThresholdProb = 1e8; + double featureRetention = 0.3; + double biasOmegaProcessVariance = 0.001; + double biasAccelProcessVariance = 0.001; + double attitudeProcessVariance = 0.001; + double positionProcessVariance = 0.001; + double velocityProcessVariance = 0.001; + double cameraAttitudeProcessVariance = 0.001; + double cameraPositionProcessVariance = 0.001; + double pointProcessVariance = 0.001; + Eigen::Matrix inputNoise = + Eigen::Matrix::Identity() * + 1e-3; +}; + +/// Standalone EqVIO filter. +class GTSAM_UNSTABLE_EXPORT EqVIOFilter + : public EquivariantFilter { + public: + using Base = EquivariantFilter; + + /// Internal filter state view. + struct View { + State xi0; + VioGroup X = makeVioGroupIdentity(); + Matrix Sigma = + Matrix::Identity(SensorState::CompDim, SensorState::CompDim); + }; + + private: + EqVIOFilterParams params_; + View view_; + bool initialized_ = false; + + public: + EqVIOFilter(); + explicit EqVIOFilter(const EqVIOFilterParams& params); + EqVIOFilter(const State& xi0, const Matrix& Sigma0, + const EqVIOFilterParams& params); + + /// Initialize orientation from gravity in the first IMU sample. + void initializeFromIMU(const IMUInput& imu); + /// Set manifold reference/origin and covariance. + void setReferenceState(const State& xi0, const Matrix& Sigma0); + + /// Propagate observer by one IMU hold over `dt`. + void propagate(const IMUInput& imu, double dt); + /// Propagate covariance only over `dt` with one IMU hold. + void propagateCovariance(const IMUInput& imu, double dt); + /// Propagate observer state only over `dt` with one IMU hold. + void propagateState(const IMUInput& imu, double dt); + /// Apply one visual correction at current observer time. + void correct(const VisionMeasurement& measurement, + const std::shared_ptr& camera, + const Matrix& R = Matrix()); + + /// Current full state estimate. + State stateEstimate() const; + /// True after IMU-based initialization. + bool isInitialized() const { return initialized_; } + /// Access internal reference/group/covariance state. + const View& view() const { return view_; } + + private: + static Matrix defaultCovariance(size_t nLandmarks); + + void syncBase(bool resetReference); + void syncFromBase(); + + Matrix stateProcessNoise(size_t nLandmarks) const; + double getMedianSceneDepth() const; + + void addNewLandmarks(const VisionMeasurement& measurement, + const std::shared_ptr& camera); + void addLandmarksInternal(std::vector& newLandmarks, + const Matrix& newLandmarkCov); + void removeLandmarkByIndex(int idx); + void removeLandmarkById(int id); + void removeOldLandmarks(const std::vector& measurementIds); + void removeOutliers(VisionMeasurement& measurement, + const std::shared_ptr& camera); + void removeInvalidLandmarksNow(); + Matrix3 getLandmarkCovById(int id) const; + Matrix2 outputCovarianceById( + int id, const Point2& y, + const std::shared_ptr& camera) const; + + void update(const VisionMeasurement& measurement, + const std::shared_ptr& camera, + const Matrix& outputGainMatrix); +}; + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOState.cpp b/gtsam_unstable/navigation/EqVIOState.cpp new file mode 100644 index 0000000000..97d743a02c --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOState.cpp @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOState.cpp + * @brief Dynamic EqVIO manifold state implementation. + * @author Rohan Bansal + */ + +#include + +#include +#include + +namespace gtsam { +namespace eqvio { + + +void Landmark::print(const std::string& s) const { + if (!s.empty()) std::cout << s << " "; + std::cout << "Landmark{id=" << id << ", p=" << p.transpose() << "}" + << std::endl; +} + +bool Landmark::equals(const Landmark& other, double tol) const { + return id == other.id && equal_with_abs_tol(p, other.p, tol); +} + +Vector3 SensorState::gravityDir() const { + return pose.rotation().unrotate(Vector3::UnitZ()); +} + +void SensorState::print(const std::string& s) const { + if (!s.empty()) std::cout << s << std::endl; + gtsam::print(Vector(inputBias.vector()), " inputBias"); + pose.print(" pose"); + gtsam::print(Vector(velocity), " velocity"); + cameraOffset.print(" cameraOffset"); +} + +bool SensorState::equals(const SensorState& other, double tol) const { + return inputBias.equals(other.inputBias, tol) && + pose.equals(other.pose, tol) && + equal_with_abs_tol(velocity, other.velocity, tol) && + cameraOffset.equals(other.cameraOffset, tol); +} + +State::State(const SensorState& sensor_, + const std::vector& lms) + : sensor(sensor_), cameraLandmarks(lms) {} + +size_t State::n() const { return cameraLandmarks.size(); } + +int State::dim() const { + return SensorState::CompDim + Landmark::CompDim * static_cast(n()); +} + +std::vector State::ids() const { + std::vector out; + out.reserve(cameraLandmarks.size()); + for (const Landmark& lm : cameraLandmarks) out.push_back(lm.id); + return out; +} + +State State::retract(const TangentVector& v, ChartJacobian H1, + ChartJacobian H2) const { + const int d = dim(); + + Matrix66 Hpose1, Hpose2, Hcam1, Hcam2; + State out(*this); + + out.sensor.inputBias = sensor.inputBias.retract(v.segment<6>(0)); + out.sensor.pose = + sensor.pose.retract(v.segment<6>(6), H1 ? &Hpose1 : nullptr, + H2 ? &Hpose2 : nullptr); + out.sensor.velocity += v.segment<3>(12); + out.sensor.cameraOffset = + sensor.cameraOffset.retract(v.segment<6>(15), H1 ? &Hcam1 : nullptr, + H2 ? &Hcam2 : nullptr); + + for (size_t i = 0; i < n(); ++i) { + out.cameraLandmarks[i].p += + v.segment<3>(SensorState::CompDim + 3 * static_cast(i)); + } + + if (H1) { + H1->setZero(d, d); + H1->block<6, 6>(0, 0).setIdentity(); + H1->block<6, 6>(6, 6) = Hpose1; + H1->block<3, 3>(12, 12).setIdentity(); + H1->block<6, 6>(15, 15) = Hcam1; + for (size_t i = 0; i < n(); ++i) { + const int row = SensorState::CompDim + 3 * static_cast(i); + H1->block<3, 3>(row, row).setIdentity(); + } + } + + if (H2) { + H2->setZero(d, d); + H2->block<6, 6>(0, 0).setIdentity(); + H2->block<6, 6>(6, 6) = Hpose2; + H2->block<3, 3>(12, 12).setIdentity(); + H2->block<6, 6>(15, 15) = Hcam2; + for (size_t i = 0; i < n(); ++i) { + const int row = SensorState::CompDim + 3 * static_cast(i); + H2->block<3, 3>(row, row).setIdentity(); + } + } + + return out; +} + +State::TangentVector State::localCoordinates(const State& other, + ChartJacobian H1, + ChartJacobian H2) const { + const int d = dim(); + + Matrix66 Hpose1, Hpose2, Hcam1, Hcam2; + TangentVector out = Vector::Zero(d); + + out.segment<6>(0) = sensor.inputBias.localCoordinates(other.sensor.inputBias); + out.segment<6>(6) = sensor.pose.localCoordinates( + other.sensor.pose, H1 ? &Hpose1 : nullptr, H2 ? &Hpose2 : nullptr); + out.segment<3>(12) = other.sensor.velocity - sensor.velocity; + out.segment<6>(15) = sensor.cameraOffset.localCoordinates( + other.sensor.cameraOffset, H1 ? &Hcam1 : nullptr, H2 ? &Hcam2 : nullptr); + + for (size_t i = 0; i < n(); ++i) { + out.segment<3>(SensorState::CompDim + 3 * static_cast(i)) = + other.cameraLandmarks[i].p - cameraLandmarks[i].p; + } + + if (H1) { + H1->setZero(d, d); + H1->block<6, 6>(0, 0) = -Matrix66::Identity(); + H1->block<6, 6>(6, 6) = Hpose1; + H1->block<3, 3>(12, 12) = -Matrix3::Identity(); + H1->block<6, 6>(15, 15) = Hcam1; + for (size_t i = 0; i < n(); ++i) { + const int row = SensorState::CompDim + 3 * static_cast(i); + H1->block<3, 3>(row, row) = -Matrix3::Identity(); + } + } + + if (H2) { + H2->setZero(d, d); + H2->block<6, 6>(0, 0).setIdentity(); + H2->block<6, 6>(6, 6) = Hpose2; + H2->block<3, 3>(12, 12).setIdentity(); + H2->block<6, 6>(15, 15) = Hcam2; + for (size_t i = 0; i < n(); ++i) { + const int row = SensorState::CompDim + 3 * static_cast(i); + H2->block<3, 3>(row, row).setIdentity(); + } + } + + return out; +} + +void State::print(const std::string& s) const { + if (!s.empty()) std::cout << s << std::endl; + std::cout << "State(dim=" << dim() << ", n=" << n() << ")" << std::endl; + sensor.print(" sensor"); + for (size_t i = 0; i < cameraLandmarks.size(); ++i) { + cameraLandmarks[i].print(" landmark[" + std::to_string(i) + "]"); + } +} + +bool State::equals(const State& other, double tol) const { + if (cameraLandmarks.size() != other.cameraLandmarks.size()) return false; + if (!sensor.equals(other.sensor, tol)) return false; + for (size_t i = 0; i < cameraLandmarks.size(); ++i) { + if (!cameraLandmarks[i].equals(other.cameraLandmarks[i], tol)) return false; + } + return true; +} + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOState.h b/gtsam_unstable/navigation/EqVIOState.h new file mode 100644 index 0000000000..02c7ef2aca --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOState.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOState.h + * @brief Dynamic EqVIO manifold state. + * @author Rohan Bansal + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam { +namespace eqvio { + +/** + * @brief One visual landmark state block. + * + * Stores a 3D landmark position plus its integer id. The Lie/chart dimension + * contribution is 3. + */ +struct GTSAM_UNSTABLE_EXPORT Landmark { + static constexpr int CompDim = 3; + + /// Landmark position in world coordinates. + Point3 p = Point3::Zero(); + /// Stable landmark identifier used for ordering/alignment. + int id = -1; + + void print(const std::string& s = "") const; + bool equals(const Landmark& other, double tol = 1e-9) const; +}; + +struct GTSAM_UNSTABLE_EXPORT SensorState { + static constexpr int CompDim = 21; + + Bias inputBias = Bias::Identity(); + Pose3 pose = Pose3::Identity(); + /// Body/IMU translational velocity in world frame. + Vector3 velocity = Vector3::Zero(); + /// Camera pose relative to IMU/body frame. + Pose3 cameraOffset = Pose3::Identity(); + + /// Unit gravity direction expressed in the body frame. + Vector3 gravityDir() const; + + void print(const std::string& s = "") const; + bool equals(const SensorState& other, double tol = 1e-9) const; +}; + +/// Dynamic VIO state manifold with dimension 21 + 3n. +class GTSAM_UNSTABLE_EXPORT State { + public: + static constexpr int dimension = Eigen::Dynamic; + + using TangentVector = Vector; + using Jacobian = Matrix; + using ChartJacobian = OptionalJacobian; + + SensorState sensor; + std::vector cameraLandmarks; + + /// Construct default-initialized state. + State() = default; + /// Construct from explicit sensor and landmark blocks. + State(const SensorState& sensor_, const std::vector& lms); + + /// Number of landmarks. + size_t n() const; + int dim() const; + std::vector ids() const; + + /// Retract in the state chart. + State retract(const TangentVector& v, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + /// Local coordinates in the state chart. + TangentVector localCoordinates(const State& other, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) const; + + void print(const std::string& s = "") const; + bool equals(const State& other, double tol = 1e-9) const; +}; + +} // namespace eqvio + +/** + * @brief Traits specialization for State. + * + * This allows State to be used as a manifold type in GTSAM. + */ +template <> +struct traits { + static constexpr int dimension = Eigen::Dynamic; + using TangentVector = Vector; + using ManifoldType = eqvio::State; + using structure_category = manifold_tag; + + static int GetDimension(const eqvio::State& xi) { return xi.dim(); } + + static eqvio::State Retract( + const eqvio::State& xi, const TangentVector& v, + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { + return xi.retract(v, H1, H2); + } + + static TangentVector Local( + const eqvio::State& xi, const eqvio::State& other, + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { + return xi.localCoordinates(other, H1, H2); + } + + static void Print(const eqvio::State& xi, const std::string& s = "") { + xi.print(s); + } + + static bool Equals(const eqvio::State& xi1, const eqvio::State& xi2, + double tol = 1e-9) { + return xi1.equals(xi2, tol); + } +}; + +template <> +struct traits : traits {}; + +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOSymmetry.cpp b/gtsam_unstable/navigation/EqVIOSymmetry.cpp new file mode 100644 index 0000000000..aa5c15d4f0 --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOSymmetry.cpp @@ -0,0 +1,788 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOSymmetry.cpp + * @brief EqVIO symmetry actions and lift helpers. + * @author Rohan Bansal + */ + + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { +namespace eqvio { + +namespace { + +Pose3 APose(const VioGroup& X) { + const auto& A = gtsam::get<0>(X); + return Pose3(A.rotation(), A.x(0)); +} + +Vector3 AW(const VioGroup& X) { + const auto& A = gtsam::get<0>(X); + return A.x(1); +} + +Se23 MakeA(const Rot3& R, const Point3& x0, const Vector3& w) { + Se23::Matrix3K x; + x.col(0) = x0; + x.col(1) = w; + return Se23(R, x); +} + +/// Construct the shortest-arc rotation that maps first vector to second vector. +Rot3 RotationFromTwoVectors(const Vector3& from, const Vector3& to) { + gtsam::Quaternion q; + q.setFromTwoVectors(from, to); + return Rot3(q); +} + +std::vector QIdsForMeasurement(const VioGroup& X, + const VisionMeasurement& measurement) { + if (N_landmarkCount(X) == 0) return {}; + if (measurement.size() != N_landmarkCount(X)) { + throw std::invalid_argument( + "outputGroupAction: measurement count must match group landmark count"); + } + return measurementIds(measurement); +} + +/// Numerical derivative of the state action with respect to group coordinates. +Matrix NumericalDerivativeActionWrtGroup( + const std::function& f, const VioGroup& X, + const State& y0, double h = 1e-6) { + const int n = static_cast(Dim_groupTangent(X)); + const int m = y0.dim(); + Matrix H = Matrix::Zero(m, n); + + for (int j = 0; j < n; ++j) { + Vector dx = Vector::Zero(n); + dx(j) = h; + const State yPlus = f(X.retract(dx)); + dx(j) = -h; + const State yMinus = f(X.retract(dx)); + + H.col(j) = + (y0.localCoordinates(yPlus) - y0.localCoordinates(yMinus)) / (2.0 * h); + } + + return H; +} + +Matrix NumericalDifferential(const std::function& f, + const Vector& x, double h = 1e-6) { + const int n = static_cast(x.size()); + const Vector y = f(x); + const int m = static_cast(y.size()); + + Matrix J = Matrix::Zero(m, n); + for (int j = 0; j < n; ++j) { + Vector dx = Vector::Zero(n); + dx(j) = h; + J.col(j) = (f(x + dx) - f(x - dx)) / (2.0 * h); + } + return J; +} + +void CheckStateChartAlignment(const State& Xi, const State& Xi0, + const char* context) { + if (Xi.n() != Xi0.n()) { + throw std::invalid_argument(std::string(context) + + ": landmark counts do not match"); + } + for (size_t i = 0; i < Xi.n(); ++i) { + if (Xi.cameraLandmarks[i].id != Xi0.cameraLandmarks[i].id) { + throw std::invalid_argument(std::string(context) + + ": landmark ids are not aligned"); + } + } +} + +Vector2 E3ProjectSphere(const Vector3& eta) { + static const Matrix23 I23 = Matrix23::Identity(); + static const Vector3 e3 = Vector3::UnitZ(); + return I23 * (eta - e3) / (1.0 - e3.dot(eta)); +} + +Vector3 E3ProjectSphereInv(const Vector2& y) { + static const Vector3 e3 = Vector3::UnitZ(); + const Vector3 yBar = (Vector3() << y, 0.0).finished(); + return e3 + 2.0 / (yBar.squaredNorm() + 1.0) * (yBar - e3); +} + +Matrix23 E3ProjectSphereDiff(const Vector3& eta) { + static const Matrix23 I23 = Matrix23::Identity(); + static const Vector3 e3 = Vector3::UnitZ(); + Matrix23 diff = + I23 * (Matrix3::Identity() * (1.0 - eta.z()) + (eta - e3) * e3.transpose()); + diff /= std::pow(1.0 - e3.dot(eta), 2.0); + return diff; +} + +Matrix32 E3ProjectSphereInvDiff(const Vector2& y) { + Matrix32 diff; + diff.block<2, 2>(0, 0) = + Matrix2::Identity() * (y.squaredNorm() + 1.0) - 2.0 * y * y.transpose(); + diff.block<1, 2>(2, 0) = 2.0 * y.transpose(); + diff *= 2.0 / std::pow(y.squaredNorm() + 1.0, 2.0); + return diff; +} + +Vector2 SphereChartStereo(const Vector3& eta, const Vector3& pole) { + const Rot3 sphereRot = RotationFromTwoVectors(-pole, Vector3::UnitZ()); + const Vector3 etaRotated = sphereRot.matrix() * eta; + return E3ProjectSphere(etaRotated); +} + +Vector3 SphereChartStereoInv(const Vector2& y, const Vector3& pole) { + const Vector3 etaRotated = E3ProjectSphereInv(y); + const Rot3 sphereRot = RotationFromTwoVectors(-pole, Vector3::UnitZ()); + return sphereRot.matrix().transpose() * etaRotated; +} + +Matrix23 SphereChartStereoDiff0(const Vector3& pole) { + const Rot3 sphereRot = RotationFromTwoVectors(-pole, Vector3::UnitZ()); + const Vector3 etaRotated = sphereRot.matrix() * pole; + return E3ProjectSphereDiff(etaRotated) * sphereRot.matrix(); +} + +Matrix32 SphereChartStereoInvDiff0(const Vector3& pole) { + const Rot3 sphereRot = RotationFromTwoVectors(-pole, Vector3::UnitZ()); + return sphereRot.matrix().transpose() * E3ProjectSphereInvDiff(Vector2::Zero()); +} + +Vector3 PointChartInvDepth(const Point3& q, const Point3& q0) { + const double rho = 1.0 / q.norm(); + const double rho0 = 1.0 / q0.norm(); + const Vector3 y = q * rho; + const Vector3 y0 = q0 * rho0; + + Vector3 eps; + eps.head<2>() = SphereChartStereo(y, y0); + eps(2) = rho - rho0; + return eps; +} + +Point3 PointChartInvDepthInv(const Vector3& eps, const Point3& q0) { + const double rho0 = 1.0 / q0.norm(); + const Vector3 y0 = q0 * rho0; + const Vector3 y = SphereChartStereoInv(eps.head<2>(), y0); + + double rho = eps(2) + rho0; + if (rho <= 0.0) rho = 1e-6; + return y / rho; +} + +Matrix3 ConvEucToInvDepth(const Point3& q0) { + const double rho0 = 1.0 / q0.norm(); + const Vector3 y0 = q0 * rho0; + + Matrix3 M; + M.block<2, 3>(0, 0) = rho0 * SphereChartStereoDiff0(y0) * + (Matrix3::Identity() - y0 * y0.transpose()); + M.block<1, 3>(2, 0) = -rho0 * rho0 * y0.transpose(); + return M; +} + +Matrix3 ConvInvDepthToEuc(const Point3& q0) { + const double rho0 = 1.0 / q0.norm(); + const Vector3 y0 = q0 * rho0; + + Matrix3 M; + M.block<3, 2>(0, 0) = SphereChartStereoInvDiff0(y0) / rho0; + M.block<3, 1>(0, 2) = -y0 / (rho0 * rho0); + return M; +} + +Vector StateChartInvDepth(const State& Xi, const State& Xi0) { + CheckStateChartAlignment(Xi, Xi0, "stateChart_invdepth"); + + Vector eps = Vector::Zero(Xi0.dim()); + eps.segment<6>(0) = (Xi.sensor.inputBias - Xi0.sensor.inputBias).vector(); + eps.segment<6>(6) = Xi0.sensor.pose.localCoordinates(Xi.sensor.pose); + eps.segment<3>(12) = Xi.sensor.velocity - Xi0.sensor.velocity; + eps.segment<6>(15) = + Xi0.sensor.cameraOffset.localCoordinates(Xi.sensor.cameraOffset); + + for (size_t i = 0; i < Xi0.n(); ++i) { + eps.segment<3>(SensorState::CompDim + 3 * static_cast(i)) = + PointChartInvDepth(Xi.cameraLandmarks[i].p, Xi0.cameraLandmarks[i].p); + } + return eps; +} + +State StateChartInvDepthInv(const Vector& eps, const State& Xi0) { + if (eps.size() != Xi0.dim()) { + throw std::invalid_argument( + "stateChartInv_invdepth: chart vector dimension mismatch"); + } + + State Xi; + Xi.sensor.inputBias = Xi0.sensor.inputBias + eps.segment<6>(0); + Xi.sensor.pose = Xi0.sensor.pose.retract(eps.segment<6>(6)); + Xi.sensor.velocity = Xi0.sensor.velocity + eps.segment<3>(12); + Xi.sensor.cameraOffset = Xi0.sensor.cameraOffset.retract(eps.segment<6>(15)); + + Xi.cameraLandmarks.resize(Xi0.n()); + for (size_t i = 0; i < Xi0.n(); ++i) { + const int k = SensorState::CompDim + 3 * static_cast(i); + Xi.cameraLandmarks[i].id = Xi0.cameraLandmarks[i].id; + Xi.cameraLandmarks[i].p = + PointChartInvDepthInv(eps.segment<3>(k), Xi0.cameraLandmarks[i].p); + } + return Xi; +} + +Matrix EqFInputMatrixB_invdepth(const VioGroup& X, const State& xi0); + +Matrix EqFStateMatrixA_invdepth(const VioGroup& X, const State& xi0, + const IMUInput& imuVel) { + const int N = static_cast(xi0.n()); + Matrix A0t = Matrix::Zero(xi0.dim(), xi0.dim()); + + const Matrix B = EqFInputMatrixB_invdepth(X, xi0); + A0t.block(0, 0, xi0.dim(), 3) = -B.block(0, 3, xi0.dim(), 3); + A0t.block(0, 3, xi0.dim(), 3) = -B.block(0, 0, xi0.dim(), 3); + A0t.block<3, 3>(9, 12).setIdentity(); + A0t.block<3, 3>(12, 6) = -GRAVITY_CONSTANT * Rot3::Hat(xi0.sensor.gravityDir()); + + const State xiHat = stateGroupAction(X, xi0); + const IMUInput vEst = imuVel - xiHat.sensor.inputBias; + Vector6 U_I; + U_I << vEst.gyr, xiHat.sensor.velocity; + + const Pose3 A = APose(X); + const Vector6 commonTwist = + xi0.sensor.cameraOffset.inverse().AdjointMap() * A.AdjointMap() * U_I; + A0t.block<6, 6>(15, 15) = Pose3::adjointMap(commonTwist); + + const Matrix3 R_IC = xiHat.sensor.cameraOffset.rotation().matrix(); + const Matrix3 R_A = A.rotation().matrix(); + for (int i = 0; i < N; ++i) { + const Point3 q0 = xi0.cameraLandmarks[static_cast(i)].p; + const Matrix3 Qhat_i = + SOT3ScaledRotation(Q_landmarkTransforms(X)[static_cast(i)]); + A0t.block<3, 3>(SensorState::CompDim + 3 * i, 12) = + -ConvEucToInvDepth(q0) * Qhat_i * R_IC.transpose() * R_A.transpose(); + } + + const Matrix66 commonTerm = + B_cameraExtrinsics(X).inverse().AdjointMap() * Pose3::adjointMap(commonTwist); + for (int i = 0; i < N; ++i) { + const Point3 q0 = xi0.cameraLandmarks[static_cast(i)].p; + Matrix36 temp; + temp << Rot3::Hat(q0) * + SOT3Rotation(Q_landmarkTransforms(X)[static_cast(i)]) + .matrix(), + -SOT3Scale(Q_landmarkTransforms(X)[static_cast(i)]) * + SOT3Rotation(Q_landmarkTransforms(X)[static_cast(i)]) + .matrix(); + A0t.block<3, 6>(SensorState::CompDim + 3 * i, 15) = + ConvEucToInvDepth(q0) * temp * commonTerm; + } + + const Vector6 U_C = xiHat.sensor.cameraOffset.inverse().AdjointMap() * U_I; + const Vector3 v_C = U_C.tail<3>(); + for (int i = 0; i < N; ++i) { + const Point3 q0 = xi0.cameraLandmarks[static_cast(i)].p; + const Matrix3 Qhat_i = + SOT3ScaledRotation(Q_landmarkTransforms(X)[static_cast(i)]); + const Point3 qhat_i = xiHat.cameraLandmarks[static_cast(i)].p; + const Matrix3 A_qi = + -Qhat_i * + (Rot3::Hat(qhat_i) * Rot3::Hat(v_C) - 2.0 * v_C * qhat_i.transpose() + + qhat_i * v_C.transpose()) * + Qhat_i.inverse() * (1.0 / qhat_i.squaredNorm()); + A0t.block<3, 3>(SensorState::CompDim + 3 * i, + SensorState::CompDim + 3 * i) = + ConvEucToInvDepth(q0) * A_qi * ConvInvDepthToEuc(q0); + } + + return A0t; +} + +Matrix EqFInputMatrixB_invdepth(const VioGroup& X, const State& xi0) { + const int N = static_cast(xi0.n()); + Matrix Bt = Matrix::Zero(xi0.dim(), IMUInput::CompDim); + + const State xiHat = stateGroupAction(X, xi0); + const Pose3 A = APose(X); + + Bt.block<3, 3>(0, 9).setIdentity(); + Bt.block<3, 3>(3, 6).setIdentity(); + + const Matrix3 R_A = A.rotation().matrix(); + Bt.block<3, 3>(6, 0) = R_A; + Bt.block<3, 3>(9, 0) = Rot3::Hat(A.translation()) * R_A; + Bt.block<3, 3>(12, 0) = R_A * Rot3::Hat(xiHat.sensor.velocity); + Bt.block<3, 3>(12, 3) = R_A; + + const Matrix3 RT_IC = xiHat.sensor.cameraOffset.rotation().matrix().transpose(); + const Point3 x_IC = xiHat.sensor.cameraOffset.translation(); + for (int i = 0; i < N; ++i) { + const Point3 q0 = xi0.cameraLandmarks[static_cast(i)].p; + const Matrix3 Qhat_i = + SOT3ScaledRotation(Q_landmarkTransforms(X)[static_cast(i)]); + const Point3 qhat_i = xiHat.cameraLandmarks[static_cast(i)].p; + Bt.block<3, 3>(SensorState::CompDim + 3 * i, 0) = + ConvEucToInvDepth(q0) * + Qhat_i * (Rot3::Hat(qhat_i) * RT_IC + RT_IC * Rot3::Hat(x_IC)); + } + + return Bt; +} + +Matrix23 EqFoutputMatrixCiStar_base( + const Point3& q0, const SOT3& QHat, + const std::shared_ptr& camera, const Point2& y) { + if (!camera) { + throw std::invalid_argument("EqFoutputMatrixCiStar_base: null camera"); + } + + using Matrix43 = Eigen::Matrix; + using Matrix34 = Eigen::Matrix; + using Matrix24 = Eigen::Matrix; + + const Vector3 qHat = SOT3ApplyInverse(QHat, q0); + const Vector3 yHat = qHat.normalized(); + + Matrix43 m2g = Matrix43::Zero(); + m2g.block<3, 3>(0, 0) = -Rot3::Hat(q0); + m2g.row(3) = -q0.transpose(); + m2g /= q0.squaredNorm(); + + const auto DRho = [&camera](const Vector3& yVec) -> Matrix24 { + Matrix34 DRhoVec = Matrix34::Zero(); + DRhoVec.block<3, 3>(0, 0) = Rot3::Hat(yVec); + return projectionJacobian(*camera, yVec) * DRhoVec; + }; + + const Vector3 yTru = undistortPoint(*camera, y); + const Matrix24 drhoSym = 0.5 * (DRho(yTru) + DRho(yHat)); + const Matrix44 adjQInv = QHat.inverse().AdjointMap(); + return drhoSym * adjQInv * m2g; +} + +Matrix23 EqFoutputMatrixCiStar_invdepth( + const Point3& q0, const SOT3& QHat, + const std::shared_ptr& camera, const Point2& y) { + const double r0 = q0.norm(); + const Vector3 y0 = q0 / r0; + Matrix3 ind2euc; + ind2euc.block<3, 2>(0, 0) = r0 * SphereChartStereoInvDiff0(y0); + ind2euc.block<3, 1>(0, 2) = -r0 * q0; + return EqFoutputMatrixCiStar_base(q0, QHat, camera, y) * ind2euc; +} + +Vector liftInnovation_invdepth(const Vector& totalInnovation, + const State& xi0) { + if (totalInnovation.size() != xi0.dim()) { + throw std::invalid_argument( + "liftInnovation_invdepth: innovation dimension mismatch"); + } + + const int N = static_cast(xi0.n()); + Vector lift = Vector::Zero(21 + 4 * N); + + lift.segment<6>(9) = totalInnovation.segment<6>(0); + lift.segment<6>(0) = totalInnovation.segment<6>(6); + + const Vector3 gammaV = totalInnovation.segment<3>(12); + lift.segment<3>(6) = -gammaV - Rot3::Hat(lift.segment<3>(0)) * xi0.sensor.velocity; + + lift.segment<6>(15) = + totalInnovation.segment<6>(15) + + xi0.sensor.cameraOffset.inverse().AdjointMap() * lift.segment<6>(0); + + for (int i = 0; i < N; ++i) { + const Point3 qi0 = xi0.cameraLandmarks[static_cast(i)].p; + const Vector3 gammaQi0 = + ConvInvDepthToEuc(qi0) * + totalInnovation.segment<3>(SensorState::CompDim + 3 * i); + + lift.segment<3>(21 + 4 * i) = -qi0.cross(gammaQi0) / qi0.squaredNorm(); + lift(21 + 4 * i + 3) = -qi0.dot(gammaQi0) / qi0.squaredNorm(); + } + + return lift; +} + +VioGroup liftInnovationDiscrete_invdepth(const Vector& totalInnovation, + const State& xi0) { + if (totalInnovation.size() != xi0.dim()) { + throw std::invalid_argument( + "liftInnovationDiscrete_invdepth: innovation dimension mismatch"); + } + + const Bias beta(totalInnovation.segment<6>(0)); + const Pose3 A_pose = Pose3::Expmap(totalInnovation.segment<6>(6)); + const Vector3 w = xi0.sensor.velocity - + A_pose.rotation().matrix() * + (xi0.sensor.velocity + totalInnovation.segment<3>(12)); + + const Pose3 B = xi0.sensor.cameraOffset.inverse() + .compose(A_pose) + .compose(xi0.sensor.cameraOffset) + .compose(Pose3::Expmap(totalInnovation.segment<6>(15))); + + std::vector Q; + Q.reserve(xi0.n()); + + for (size_t i = 0; i < xi0.n(); ++i) { + const Point3 qi0 = xi0.cameraLandmarks[i].p; + const Point3 qi1 = + PointChartInvDepthInv(totalInnovation.segment<3>( + SensorState::CompDim + 3 * static_cast(i)), + qi0); + const Rot3 R = RotationFromTwoVectors(qi1.normalized(), qi0.normalized()); + const double a = qi0.norm() / qi1.norm(); + Q.emplace_back(MakeSOT3(SO3(R.matrix()), a)); + } + + return makeVioGroup(MakeA(A_pose.rotation(), A_pose.translation(), w), beta, B, + LandmarkGroup(Q)); +} + +} // namespace + +SensorState sensorStateGroupAction(const VioGroup& X, + const SensorState& sensor) { + const auto& Beta = gtsam::get<1>(X); + const auto& B = gtsam::get<2>(X); + const Pose3 A = APose(X); + const Vector3 w = AW(X); + + SensorState out; + out.inputBias = sensor.inputBias + Beta; + out.pose = sensor.pose.compose(A); + out.velocity = A.rotation().unrotate(sensor.velocity - w); + out.cameraOffset = A.inverse().compose(sensor.cameraOffset).compose(B); + return out; +} + +State stateGroupAction(const VioGroup& X, const State& state) { + const auto& Q = gtsam::get<3>(X); + if (Q.size() != state.n()) { + throw std::invalid_argument( + "stateGroupAction: group and state landmark counts do not match"); + } + + State out; + out.sensor = sensorStateGroupAction(X, state.sensor); + out.cameraLandmarks.resize(Q.size()); + + for (size_t i = 0; i < Q.size(); ++i) { + out.cameraLandmarks[i].p = + SOT3ApplyInverse(Q[i], state.cameraLandmarks[i].p); + out.cameraLandmarks[i].id = state.cameraLandmarks[i].id; + } + + return out; +} + +VisionMeasurement outputGroupAction(const VioGroup& X, + const VisionMeasurement& measurement, + const std::shared_ptr& camera) { + const auto& Q = gtsam::get<3>(X); + VisionMeasurement out; + if (!camera) { + throw std::invalid_argument("outputGroupAction: camera model is null"); + } + + const std::vector qIds = QIdsForMeasurement(X, measurement); + if (qIds.size() != Q.size()) { + throw std::invalid_argument( + "outputGroupAction: invalid Q-to-id mapping cardinality"); + } + + for (size_t i = 0; i < Q.size(); ++i) { + const int id = qIds[i]; + const auto it = measurement.find(id); + if (it == measurement.end()) continue; + + const Vector3 bearing = undistortPoint(*camera, it->second); + const Vector3 rotated = SOT3Rotation(Q[i]).matrix().transpose() * bearing; + out[id] = camera->project2(rotated); + } + + return out; +} + +Vector liftVelocity(const State& state, const IMUInput& velocity) { + const size_t N = state.n(); + Vector lift = Vector::Zero(21 + 4 * static_cast(N)); + + const SensorState& sensor = state.sensor; + const IMUInput v_est = velocity - sensor.inputBias; + + Vector6 U_A; + U_A << v_est.gyr, sensor.velocity; + const Vector6 U_B = sensor.cameraOffset.inverse().AdjointMap() * U_A; + const Vector3 u_w = -v_est.acc + sensor.gravityDir() * GRAVITY_CONSTANT; + + lift.segment<6>(0) = U_A; + lift.segment<3>(6) = u_w; + lift.segment<6>(9) << velocity.accBiasVel, velocity.gyrBiasVel; + lift.segment<6>(15) = U_B; + + const Vector6 U_C = sensor.cameraOffset.inverse().AdjointMap() * U_A; + const Vector3 omegaC = U_C.head<3>(); + const Vector3 vC = U_C.tail<3>(); + + // Lift the landmark transform velocities + for (size_t i = 0; i < N; ++i) { + const Vector3 p = state.cameraLandmarks[i].p; + Vector4 W; + W.head<3>() = omegaC + Rot3::Hat(p) * vC / p.squaredNorm(); + W(3) = p.dot(vC) / p.squaredNorm(); + lift.segment<4>(21 + 4 * static_cast(i)) = W; + } + + return lift; +} + +VioGroup liftVelocityDiscrete(const State& state, const IMUInput& velocity, + double dt) { + const SensorState& sensor = state.sensor; + const IMUInput v_est = velocity - sensor.inputBias; + + const Rot3 dR = Rot3::Expmap(dt * v_est.gyr); + const Vector3 dXWorld = + dt * (sensor.pose.rotation() * sensor.velocity) + + 0.5 * dt * dt * + (sensor.pose.rotation() * v_est.acc + Vector3(0, 0, -GRAVITY_CONSTANT)); + const Point3 dXBody = sensor.pose.rotation().unrotate(dXWorld); + const Pose3 A_pose(dR, dXBody); + + const Vector3 bodyVelocityDiff = + v_est.acc - sensor.gravityDir() * GRAVITY_CONSTANT; + const Vector3 w = sensor.velocity - (sensor.velocity + dt * bodyVelocityDiff); + + const Pose3 B = sensor.cameraOffset.inverse().compose(A_pose).compose( + sensor.cameraOffset); + const Pose3 cameraPoseChangeInv = sensor.cameraOffset.inverse() + .compose(A_pose.inverse()) + .compose(sensor.cameraOffset); + + // Construct the landmark transform velocities + std::vector q; + q.resize(state.n()); + for (size_t i = 0; i < state.n(); ++i) { + const Landmark& lm0 = state.cameraLandmarks[i]; + Landmark lm1; + lm1.id = lm0.id; + lm1.p = cameraPoseChangeInv.transformFrom(lm0.p); + + const Rot3 R = RotationFromTwoVectors(lm1.p, lm0.p); + const double a = lm0.p.norm() / lm1.p.norm(); + q[i] = MakeSOT3(SO3(R.matrix()), a); + } + + LandmarkGroup Q(q); + const Bias beta(dt * velocity.accBiasVel, dt * velocity.gyrBiasVel); + const Se23 A = MakeA(A_pose.rotation(), A_pose.translation(), w); + return makeVioGroup(A, beta, B, Q); +} + +State integrateSystemFunction(const State& state, const IMUInput& velocity, + double dt) { + State out; + const SensorState& sensor = state.sensor; + const IMUInput v_est = velocity - sensor.inputBias; + + out.sensor.inputBias = Bias( + sensor.inputBias.accelerometer() + dt * velocity.accBiasVel, + sensor.inputBias.gyroscope() + dt * velocity.gyrBiasVel); + + const Rot3 dR = Rot3::Expmap(dt * v_est.gyr); + const Vector3 dXWorld = + dt * (sensor.pose.rotation() * sensor.velocity) + + 0.5 * dt * dt * + (sensor.pose.rotation() * v_est.acc + Vector3(0, 0, -GRAVITY_CONSTANT)); + const Point3 dXBody = sensor.pose.rotation().unrotate(dXWorld); + const Pose3 poseChange(dR, dXBody); + + out.sensor.pose = sensor.pose.compose(poseChange); + + const Vector3 inertialVelocityDiff = + sensor.pose.rotation() * v_est.acc + Vector3(0, 0, -GRAVITY_CONSTANT); + out.sensor.velocity = out.sensor.pose.rotation().unrotate( + sensor.pose.rotation() * sensor.velocity + dt * inertialVelocityDiff); + + const Pose3 cameraPoseChangeInv = sensor.cameraOffset.inverse() + .compose(poseChange.inverse()) + .compose(sensor.cameraOffset); + out.cameraLandmarks.resize(state.n()); + + // Transform the body-fixed landmarks + for (size_t i = 0; i < state.n(); ++i) { + out.cameraLandmarks[i].p = + cameraPoseChangeInv.transformFrom(state.cameraLandmarks[i].p); + out.cameraLandmarks[i].id = state.cameraLandmarks[i].id; + } + + out.sensor.cameraOffset = sensor.cameraOffset; + return out; +} + +/// Predict ideal normalized image measurements from current landmark state. +VisionMeasurement measureSystemState( + const State& state, const std::shared_ptr& camera) { + if (!camera) { + throw std::invalid_argument("measureSystemState: camera model is null"); + } + + // Project each landmark through the camera model + VisionMeasurement out; + for (const Landmark& lm : state.cameraLandmarks) { + out[lm.id] = camera->project2(lm.p); + } + return out; +} + +const EqFCoordinateSuite EqFCoordinateSuite_invdepth{ + StateChartInvDepth, + StateChartInvDepthInv, + EqFStateMatrixA_invdepth, + EqFInputMatrixB_invdepth, + EqFoutputMatrixCiStar_invdepth, + liftInnovation_invdepth, + liftInnovationDiscrete_invdepth}; + +Matrix EqFCoordinateSuite::outputMatrixC( + const State& xi0, const VioGroup& X, const VisionMeasurement& y, + const std::shared_ptr& camera, + bool useEquivariance) const { + if (!camera) { + throw std::invalid_argument("EqFCoordinateSuite::outputMatrixC: null camera"); + } + const int M = static_cast(xi0.n()); + const std::vector yIds = measurementIds(y); + const int N = static_cast(yIds.size()); + const auto& Q = gtsam::get<3>(X); + + Matrix C = Matrix::Zero(2 * N, SensorState::CompDim + Landmark::CompDim * M); + + for (int i = 0; i < M; ++i) { + const int idNum = xi0.cameraLandmarks[static_cast(i)].id; + const auto itY = std::find(yIds.begin(), yIds.end(), idNum); + if (itY == yIds.end()) continue; + + const size_t k = static_cast(i); + const int j = static_cast(std::distance(yIds.begin(), itY)); + const Point3& qi0 = xi0.cameraLandmarks[static_cast(i)].p; + const SOT3& Qk = Q[k]; + + const Matrix23 Ci = useEquivariance + ? outputMatrixCiStar(qi0, Qk, camera, y.at(idNum)) + : outputMatrixCi(qi0, Qk, camera); + if (!Ci.array().isFinite().all()) { + const Point3 qHat = SOT3ApplyInverse(Qk, qi0); + const Point2 yObs = y.at(idNum); + const Vector3 yUnd = undistortPoint(*camera, yObs); + throw std::runtime_error( + "EqFCoordinateSuite::outputMatrixC: non-finite Ci for id " + + std::to_string(idNum) + ", qi0_norm=" + std::to_string(qi0.norm()) + + ", qHat_norm=" + std::to_string(qHat.norm()) + + ", Q_scale=" + std::to_string(SOT3Scale(Qk)) + + ", yUnd_norm=" + std::to_string(yUnd.norm())); + } + C.block<2, 3>(2 * j, SensorState::CompDim + 3 * i) = Ci; + } + + if (!C.array().isFinite().all()) { + throw std::runtime_error("EqFCoordinateSuite::outputMatrixC produced NaN/Inf"); + } + return C; +} + +Matrix EqFCoordinateSuite::stateMatrixADiscrete(const VioGroup& X, + const State& xi0, + const IMUInput& imuVel, + double dt) const { + auto a0Discrete = [&](const Vector& epsilon) { + const State xiE = stateChartInv(epsilon, xi0); + const State xiHat = stateGroupAction(X, xi0); + const State xi = stateGroupAction(X, xiE); + const VioGroup lambdaTilde = + liftVelocityDiscrete(xi, imuVel, dt) * + liftVelocityDiscrete(xiHat, imuVel, dt).inverse(); + const State xiE1 = stateGroupAction(X * lambdaTilde * X.inverse(), xiE); + return stateChart(xiE1, xi0); + }; + + return NumericalDifferential(a0Discrete, Vector::Zero(xi0.dim())); +} + +Matrix23 EqFCoordinateSuite::outputMatrixCi( + const Point3& q0, const SOT3& QHat, + const std::shared_ptr& camera) const { + if (!camera) { + throw std::invalid_argument("EqFCoordinateSuite::outputMatrixCi: null camera"); + } + const Vector3 qHat = SOT3ApplyInverse(QHat, q0); + const Point2 yHat = camera->project2(qHat); + return outputMatrixCiStar(q0, QHat, camera, yHat); +} + +State Symmetry::operator()( + const State& xi, const VioGroup& X, + OptionalJacobian H_xi, + OptionalJacobian H_X) const { + const State y = stateGroupAction(X, xi); + + if (H_xi) { + const int d = xi.dim(); + H_xi->setZero(d, d); + + const Pose3 A = APose(X); + + Matrix66 Hpose; + xi.sensor.pose.compose(A, Hpose, {}); + H_xi->block<6, 6>(6, 6) = Hpose; + + Matrix66 HtmpCamera, Hcamera; + const Pose3 tmp = A.inverse().compose(xi.sensor.cameraOffset, {}, + HtmpCamera); + const auto& B = gtsam::get<2>(X); + const auto& Q = gtsam::get<3>(X); + tmp.compose(B, Hcamera, {}); + H_xi->block<6, 6>(15, 15) = Hcamera * HtmpCamera; + + H_xi->block<6, 6>(0, 0).setIdentity(); + H_xi->block<3, 3>(12, 12) = A.rotation().matrix().transpose(); + + // Compute the Jacobian of the landmark transform velocities + for (size_t i = 0; i < xi.n(); ++i) { + const int row = SensorState::CompDim + 3 * static_cast(i); + H_xi->block<3, 3>(row, row) = + (1.0 / SOT3Scale(Q[i])) * SOT3Rotation(Q[i]).matrix().transpose(); + } + } + + if (H_X) { + auto f = [&xi, this](const VioGroup& g) { return this->operator()(xi, g); }; + *H_X = NumericalDerivativeActionWrtGroup(f, X, y); + } + + return y; +} + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/EqVIOSymmetry.h b/gtsam_unstable/navigation/EqVIOSymmetry.h new file mode 100644 index 0000000000..d51c180fa3 --- /dev/null +++ b/gtsam_unstable/navigation/EqVIOSymmetry.h @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 EqVIOSymmetry.h + * @brief EqVIO symmetry actions and lift helpers. + * @author Rohan Bansal + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace gtsam { +namespace eqvio { + +/// Right group action on the sensor-only block. +GTSAM_UNSTABLE_EXPORT SensorState sensorStateGroupAction( + const VioGroup& X, const SensorState& sensor); +/// Right group action on full state. +GTSAM_UNSTABLE_EXPORT State stateGroupAction(const VioGroup& X, + const State& state); +/// Right group action on vision measurements. +GTSAM_UNSTABLE_EXPORT VisionMeasurement outputGroupAction( + const VioGroup& X, const VisionMeasurement& measurement, + const std::shared_ptr& camera); + +/// Continuous-time lift map from IMU velocity to VioGroup tangent. +GTSAM_UNSTABLE_EXPORT Vector liftVelocity(const State& state, + const IMUInput& velocity); +/// Discrete-time lift map from IMU velocity to VioGroup increment. +GTSAM_UNSTABLE_EXPORT VioGroup liftVelocityDiscrete(const State& state, + const IMUInput& velocity, + double dt); + +/// Integrate system dynamics forward by dt. +GTSAM_UNSTABLE_EXPORT State integrateSystemFunction( + const State& state, const IMUInput& velocity, double dt); +/// Generate ideal camera measurements from state. +GTSAM_UNSTABLE_EXPORT VisionMeasurement measureSystemState( + const State& state, const std::shared_ptr& camera); + +/// InvDepth EqF coordinate suite and associated matrices/lifts. +struct GTSAM_UNSTABLE_EXPORT EqFCoordinateSuite { + std::function stateChart; + std::function stateChartInv; + std::function + stateMatrixA; + std::function inputMatrixB; + std::function&, + const Point2&)> + outputMatrixCiStar; + std::function liftInnovation; + std::function liftInnovationDiscrete; + + Matrix outputMatrixC(const State& xi0, const VioGroup& X, + const VisionMeasurement& y, + const std::shared_ptr& camera, + bool useEquivariance = true) const; + + Matrix stateMatrixADiscrete(const VioGroup& X, const State& xi0, + const IMUInput& imuVel, double dt) const; + + Matrix23 outputMatrixCi(const Point3& q0, const SOT3& QHat, + const std::shared_ptr& camera) + const; +}; + +extern const GTSAM_UNSTABLE_EXPORT EqFCoordinateSuite EqFCoordinateSuite_invdepth; + +/// Right action phi(xi, X) = stateGroupAction(X, xi). +struct GTSAM_UNSTABLE_EXPORT Symmetry + : public GroupAction { + static constexpr ActionType type = ActionType::Right; + + /// Evaluate right action phi(xi, X). + State operator()(const State& xi, const VioGroup& X, + OptionalJacobian H_xi = {}, + OptionalJacobian H_X = {}) + const; +}; + +} // namespace eqvio +} // namespace gtsam diff --git a/gtsam_unstable/navigation/tests/CMakeLists.txt b/gtsam_unstable/navigation/tests/CMakeLists.txt new file mode 100644 index 0000000000..86e11a6709 --- /dev/null +++ b/gtsam_unstable/navigation/tests/CMakeLists.txt @@ -0,0 +1,3 @@ +set(excluded_tests "") + +gtsamAddTestsGlob(navigation_unstable "test*.cpp" "${excluded_tests}" "gtsam_unstable") diff --git a/gtsam_unstable/navigation/tests/testEqVIOFilter.cpp b/gtsam_unstable/navigation/tests/testEqVIOFilter.cpp new file mode 100644 index 0000000000..85d0c3db58 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOFilter.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOFilter.cpp + +#include +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +TEST(EqVIOFilter, Smoke) { + EqVIOFilterParams params; + + SensorState sensor; + sensor.inputBias = Bias::Identity(); + sensor.pose = Pose3::Identity(); + sensor.velocity.setZero(); + sensor.cameraOffset = Pose3::Identity(); + + State xi0(sensor, {{Point3(0.8, -0.2, 4.5), 11}, {Point3(-0.6, 0.3, 3.8), 22}}); + Matrix Sigma0 = Matrix::Identity(xi0.dim(), xi0.dim()) * 1e-3; + + EqVIOFilter filter(xi0, Sigma0, params); + auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + + const double dt = 0.01; + double t = 0.0; + for (int k = 0; k < 100; ++k) { + t += dt; + IMUInput imu; + imu.stamp = t; + imu.gyr = Vector3::Zero(); + imu.acc = Vector3(0.0, 0.0, GRAVITY_CONSTANT); + filter.propagate(imu, dt); + + if (k % 5 == 0) { + VisionMeasurement y = measureSystemState(filter.stateEstimate(), camera); + filter.correct(y, camera); + } + } + + const State est = filter.stateEstimate(); + EXPECT_LONGS_EQUAL(2, est.n()); + EXPECT_LONGS_EQUAL(xi0.dim(), filter.view().Sigma.rows()); + EXPECT_LONGS_EQUAL(xi0.dim(), filter.view().Sigma.cols()); + EXPECT(filter.view().Sigma.array().isFinite().all()); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOFilterDynamicLandmarks.cpp b/gtsam_unstable/navigation/tests/testEqVIOFilterDynamicLandmarks.cpp new file mode 100644 index 0000000000..5b7024a76d --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOFilterDynamicLandmarks.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOFilterDynamicLandmarks.cpp + +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +TEST(EqVIOFilter, DynamicLandmarksAddRemove) { + EqVIOFilterParams params; + params.initialPointDepth = 5.0; + + EqVIOFilter filter(params); + auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + + IMUInput imu0; + imu0.stamp = 0.0; + imu0.acc = Vector3(0.0, 0.0, GRAVITY_CONSTANT); + imu0.gyr = Vector3::Zero(); + filter.initializeFromIMU(imu0); + + IMUInput imu1 = imu0; + imu1.stamp = 0.01; + filter.propagate(imu1, 0.01); + + VisionMeasurement meas1; + meas1[1] = camera->project2(Point3(0.2, -0.1, 3.5)); + meas1[2] = camera->project2(Point3(-0.3, 0.15, 4.0)); + filter.propagate(imu1, 0.01); + filter.correct(meas1, camera); + EXPECT_LONGS_EQUAL(2, filter.stateEstimate().n()); + + IMUInput imu2 = imu0; + imu2.stamp = 0.02; + filter.propagate(imu2, 0.01); + + VisionMeasurement meas2; + meas2[1] = meas1.at(1); + filter.correct(meas2, camera); + + const State est = filter.stateEstimate(); + EXPECT_LONGS_EQUAL(1, est.n()); + EXPECT_LONGS_EQUAL(1, est.cameraLandmarks.front().id); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOFilterInitAndPropagation.cpp b/gtsam_unstable/navigation/tests/testEqVIOFilterInitAndPropagation.cpp new file mode 100644 index 0000000000..871e34b697 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOFilterInitAndPropagation.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOFilterInitAndPropagation.cpp + +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +TEST(EqVIOFilter, InitAndPropagation) { + EqVIOFilterParams params; + + EqVIOFilter filter(params); + EXPECT(!filter.isInitialized()); + + IMUInput imu0; + imu0.stamp = 1.0; + imu0.acc = Vector3(0.0, 0.0, GRAVITY_CONSTANT); + imu0.gyr = Vector3::Zero(); + filter.initializeFromIMU(imu0); + EXPECT(filter.isInitialized()); + + IMUInput imu1 = imu0; + imu1.stamp = 1.01; + filter.propagate(imu1, 0.01); + filter.propagate(imu1, 0.01); + + auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + VisionMeasurement meas; + filter.correct(meas, camera); + + EXPECT(filter.view().Sigma.array().isFinite().all()); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOFilterParityShortSequence.cpp b/gtsam_unstable/navigation/tests/testEqVIOFilterParityShortSequence.cpp new file mode 100644 index 0000000000..8a5cd4a184 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOFilterParityShortSequence.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOFilterParityShortSequence.cpp + +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +TEST(EqVIOFilter, ParityShortSequence) { + EqVIOFilterParams params; + + EqVIOFilter filter(params); + auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + + IMUInput initImu; + initImu.stamp = 0.0; + initImu.gyr = Vector3::Zero(); + initImu.acc = Vector3(0.0, 0.0, GRAVITY_CONSTANT); + filter.initializeFromIMU(initImu); + + State manual = filter.stateEstimate(); + double t = 0.01; + const double dt = 0.01; + for (int k = 0; k < 8; ++k) { + IMUInput imu; + imu.stamp = t; + imu.gyr = Vector3(0.01, -0.02, 0.015); + imu.acc = Vector3(0.02, -0.01, GRAVITY_CONSTANT - 0.03); + imu.gyrBiasVel = Vector3(0.0005, -0.0002, 0.0001); + imu.accBiasVel = Vector3(-0.0004, 0.0003, -0.0001); + filter.propagate(imu, dt); + + VisionMeasurement emptyMeas; + filter.correct(emptyMeas, camera); + + manual = integrateSystemFunction(manual, imu, dt); + t += dt; + } + + const State est = filter.stateEstimate(); + const Vector eps = manual.localCoordinates(est); + EXPECT(eps.norm() < 2e-5); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOFilterVisionUpdate.cpp b/gtsam_unstable/navigation/tests/testEqVIOFilterVisionUpdate.cpp new file mode 100644 index 0000000000..13fc3258c1 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOFilterVisionUpdate.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOFilterVisionUpdate.cpp + +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace { + +State MakeState1() { + SensorState sensor; + sensor.inputBias = Bias(Vector3(0.03, -0.01, 0.02), Vector3(0.1, -0.2, 0.05)); + sensor.pose = Pose3(Rot3::RzRyRx(0.2, -0.1, 0.15), Point3(0.4, -0.2, 1.0)); + sensor.velocity = Vector3(0.5, -0.3, 0.2); + sensor.cameraOffset = + Pose3(Rot3::RzRyRx(-0.08, 0.04, -0.03), Point3(0.1, 0.0, 0.05)); + return State(sensor, {{Point3(0.3, -0.15, 4.5), 10}}); +} + +} // namespace + +TEST(EqVIOFilter, VisionUpdate) { + EqVIOFilterParams params; + + const State xi0 = MakeState1(); + const Matrix Sigma0 = Matrix::Identity(xi0.dim(), xi0.dim()) * 1e-3; + EqVIOFilter filter(xi0, Sigma0, params); + + IMUInput imu; + imu.stamp = 0.0; + imu.gyr = Vector3::Zero(); + imu.acc = Vector3(0.0, 0.0, GRAVITY_CONSTANT); + filter.propagate(imu, 0.01); + + auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + const VisionMeasurement meas = measureSystemState(filter.stateEstimate(), camera); + filter.correct(meas, camera); + + EXPECT_LONGS_EQUAL(1, filter.stateEstimate().n()); + EXPECT(filter.view().Sigma.array().isFinite().all()); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOGroup.cpp b/gtsam_unstable/navigation/tests/testEqVIOGroup.cpp new file mode 100644 index 0000000000..1365d79fc7 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOGroup.cpp @@ -0,0 +1,277 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOGroup.cpp + * @brief Unit tests for EqVIOGroup. + * @author Rohan Bansal + */ + +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace { + +template +void testLieGroupDerivativesN(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + Matrix H1, H2; + using T = traits; + using OJ = OptionalJacobian; + + OJ none; + EXPECT(assert_equal(t1.inverse(), T::Inverse(t1, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none), + H1)); + + EXPECT(assert_equal(t2.inverse(), T::Inverse(t2, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none), + H1)); + + EXPECT(assert_equal(t1 * t2, T::Compose(t1, t2, H1, H2))); + EXPECT(assert_equal( + numericalDerivative41(T::Compose, t1, t2, none, none), + H1)); + EXPECT(assert_equal( + numericalDerivative42(T::Compose, t1, t2, none, none), + H2)); + + EXPECT(assert_equal(t1.inverse() * t2, T::Between(t1, t2, H1, H2))); + EXPECT(assert_equal( + numericalDerivative41(T::Between, t1, t2, none, none), + H1)); + EXPECT(assert_equal( + numericalDerivative42(T::Between, t1, t2, none, none), + H2)); +} + +template +void testChartDerivativesN(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + Matrix H1, H2; + using T = traits; + using V = typename T::TangentVector; + using OJ = OptionalJacobian; + + OJ none; + const V w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1, w12, H1, H2))); + EXPECT(assert_equal( + numericalDerivative41(T::Retract, t1, w12, none, none), + H1)); + EXPECT(assert_equal( + numericalDerivative42(T::Retract, t1, w12, none, none), + H2)); + + EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2))); + EXPECT(assert_equal( + numericalDerivative41(T::Local, t1, t2, none, none), + H1)); + EXPECT(assert_equal( + numericalDerivative42(T::Local, t1, t2, none, none), + H2)); +} + +using SE23 = Se23; +using LandmarkGroup = LandmarkGroup; + +const Rot3 kR1 = Rot3::RzRyRx(0.1, -0.2, 0.3); +const Rot3 kR2 = Rot3::RzRyRx(-0.3, 0.2, -0.1); +const Rot3 kR3 = Rot3::RzRyRx(0.2, 0.1, -0.25); +const Rot3 kR4 = Rot3::RzRyRx(-0.15, -0.1, 0.05); + +const SE23::Matrix3K kX1 = + (SE23::Matrix3K() << 1.0, -0.4, 0.3, 0.9, -0.8, 0.2).finished(); +const SE23::Matrix3K kX2 = + (SE23::Matrix3K() << -0.5, 0.7, 0.1, -0.2, 0.4, 1.1).finished(); + +const SE23 kA1(kR1, kX1); +const SE23 kA2(kR2, kX2); + +const Bias kBeta1(Vector3(-0.4, 0.2, 0.05), Vector3(0.1, -0.2, 0.3)); +const Bias kBeta2(Vector3(0.2, -0.15, 0.07), Vector3(-0.05, 0.3, -0.1)); + +const Pose3 kB1(kR3, Point3(0.2, -0.5, 1.1)); +const Pose3 kB2(kR4, Point3(-0.6, 0.7, 0.3)); + +const SOT3 kQ1(SO3::Expmap((Vector3() << 0.08, -0.04, 0.05).finished()), + std::log(1.2)); +const SOT3 kQ2(SO3::Expmap((Vector3() << -0.03, 0.06, -0.02).finished()), + std::log(0.95)); +const SOT3 kQ3(SO3::Expmap((Vector3() << 0.04, 0.07, -0.08).finished()), + std::log(1.1)); +const SOT3 kQ4(SO3::Expmap((Vector3() << -0.06, -0.02, 0.09).finished()), + std::log(1.05)); +const SOT3 kQ5(SO3::Expmap((Vector3() << 0.01, 0.02, 0.03).finished()), + std::log(0.98)); +const SOT3 kQ6(SO3::Expmap((Vector3() << -0.02, 0.03, -0.01).finished()), + std::log(1.03)); + +LandmarkGroup MakeQ0() { return LandmarkGroup(0); } +LandmarkGroup MakeQ1A() { return LandmarkGroup({kQ1}); } +LandmarkGroup MakeQ1B() { return LandmarkGroup({kQ2}); } +LandmarkGroup MakeQ3A() { return LandmarkGroup({kQ1, kQ2, kQ3}); } +LandmarkGroup MakeQ3B() { return LandmarkGroup({kQ4, kQ5, kQ6}); } + +VioGroup MakeG0() { return makeVioGroup(kA1, kBeta1, kB1, MakeQ0()); } +VioGroup MakeG0b() { return makeVioGroup(kA2, kBeta2, kB2, MakeQ0()); } +VioGroup MakeG1() { return makeVioGroup(kA1, kBeta1, kB1, MakeQ1A()); } +VioGroup MakeG1b() { return makeVioGroup(kA2, kBeta2, kB2, MakeQ1B()); } +VioGroup MakeG3() { return makeVioGroup(kA1, kBeta1, kB1, MakeQ3A()); } +VioGroup MakeG3b() { return makeVioGroup(kA2, kBeta2, kB2, MakeQ3B()); } + +Vector Xi0() { + return (Vector(21) << 0.05, -0.04, 0.03, 0.2, -0.1, 0.15, -0.05, 0.07, + -0.09, 0.1, -0.08, 0.06, -0.04, 0.02, 0.03, 0.01, -0.02, 0.04, + -0.03, 0.05, -0.01) + .finished(); +} + +Vector Xi1() { + Vector xi(25); + xi << Xi0(), 0.03, -0.02, 0.01, 0.04; + return xi; +} + +Vector Xi3() { + Vector xi(33); + xi << Xi0(), 0.03, -0.02, 0.01, 0.04, -0.01, 0.05, -0.03, 0.02, 0.02, + 0.01, -0.04, 0.03; + return xi; +} + +} // namespace + +// Verifies VioGroup satisfies the required concept checks. +TEST(VioGroup, Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsTestable); +} + +// Verifies identity sizing and factor accessors. +TEST(VioGroup, ConstructorsAndAccessors) { + const VioGroup empty = makeVioGroupIdentity(); + EXPECT_LONGS_EQUAL(0, N_landmarkCount(empty)); + EXPECT_LONGS_EQUAL(21, Dim_groupTangent(empty)); + + const VioGroup identity3 = makeVioGroupIdentity(3); + EXPECT_LONGS_EQUAL(3, N_landmarkCount(identity3)); + EXPECT_LONGS_EQUAL(33, Dim_groupTangent(identity3)); + + const VioGroup g = MakeG3(); + EXPECT(assert_equal(kA1, A_sensorKinematics(g))); + EXPECT(assert_equal(kBeta1.vector(), Beta_biasOffset(g).vector())); + EXPECT(assert_equal(kB1, B_cameraExtrinsics(g))); + EXPECT(assert_equal(MakeQ3A(), Q_landmarkTransforms(g))); +} + +// Verifies compose, between, and inverse behavior. +TEST(VioGroup, GroupOperations) { + const VioGroup g1 = MakeG3(); + const VioGroup g2 = MakeG3b(); + const VioGroup composed = g1.compose(g2); + + EXPECT(assert_equal(A_sensorKinematics(g1).compose(A_sensorKinematics(g2)), + A_sensorKinematics(composed))); + EXPECT(assert_equal((Beta_biasOffset(g1) + Beta_biasOffset(g2)).vector(), + Beta_biasOffset(composed).vector())); + EXPECT(assert_equal(B_cameraExtrinsics(g1).compose(B_cameraExtrinsics(g2)), + B_cameraExtrinsics(composed))); + EXPECT(assert_equal(Q_landmarkTransforms(g1).compose(Q_landmarkTransforms(g2)), + Q_landmarkTransforms(composed))); + + const VioGroup between = g1.between(g2); + EXPECT(assert_equal(g1.inverse() * g2, between)); + EXPECT(assert_equal(makeVioGroupIdentity(3), g1.compose(g1.inverse()))); +} + +// Verifies Expmap/Logmap round-trip and adjoint consistency. +TEST(VioGroup, ExpmapLogmapAndAdjoint) { + const Vector xi0 = Xi0(); + const Vector xi1 = Xi1(); + const Vector xi3 = Xi3(); + + const VioGroup g0 = VioGroup::Expmap(xi0); + const VioGroup g1 = VioGroup::Expmap(xi1); + const VioGroup g3 = VioGroup::Expmap(xi3); + + EXPECT(assert_equal(xi0, VioGroup::Logmap(g0), 1e-9)); + EXPECT(assert_equal(xi1, VioGroup::Logmap(g1), 1e-9)); + EXPECT(assert_equal(xi3, VioGroup::Logmap(g3), 1e-9)); + + VioGroup core(SensorCore(A_sensorKinematics(g3), Beta_biasOffset(g3)), + LandmarkCore(B_cameraExtrinsics(g3), + Q_landmarkTransforms(g3))); + EXPECT(assert_equal(core.AdjointMap(), g3.AdjointMap(), 1e-9)); +} + +// Verifies Lie and chart derivatives for n=0 landmarks. +TEST(VioGroup, DerivativesN0) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + const VioGroup id = makeVioGroupIdentity(); + const VioGroup g = MakeG0(); + const VioGroup h = MakeG0b(); + + testLieGroupDerivativesN<21>(result_, name_, id, g); + testLieGroupDerivativesN<21>(result_, name_, g, h); + testChartDerivativesN<21>(result_, name_, id, g); + testChartDerivativesN<21>(result_, name_, g, h); +#else + EXPECT(true); +#endif +} + +// Verifies Lie and chart derivatives for n=1 landmark. +TEST(VioGroup, DerivativesN1) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + const VioGroup id = makeVioGroupIdentity(1); + const VioGroup g = MakeG1(); + const VioGroup h = MakeG1b(); + + testLieGroupDerivativesN<25>(result_, name_, id, g); + testLieGroupDerivativesN<25>(result_, name_, g, h); + testChartDerivativesN<25>(result_, name_, id, g); + testChartDerivativesN<25>(result_, name_, g, h); +#else + EXPECT(true); +#endif +} + +// Verifies Lie and chart derivatives for n=3 landmarks. +TEST(VioGroup, DerivativesN3) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + const VioGroup id = makeVioGroupIdentity(3); + const VioGroup g = MakeG3(); + const VioGroup h = MakeG3b(); + + testLieGroupDerivativesN<33>(result_, name_, id, g); + testLieGroupDerivativesN<33>(result_, name_, g, h); + testChartDerivativesN<33>(result_, name_, id, g); + testChartDerivativesN<33>(result_, name_, g, h); +#else + EXPECT(true); +#endif +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOState.cpp b/gtsam_unstable/navigation/tests/testEqVIOState.cpp new file mode 100644 index 0000000000..9a0ebac5f1 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOState.cpp @@ -0,0 +1,184 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOState.cpp + * @brief Unit tests for State manifold + * @author Rohan Bansal + */ + +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace { + +template +void testChartDerivativesN(TestResult& result_, const std::string& name_, + const State& t1, const State& t2) { + Matrix H1, H2; + using T = traits; + using V = typename T::TangentVector; + using OJ = OptionalJacobian; + + OJ none; + const V w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1, w12, H1, H2))); + EXPECT(assert_equal( + numericalDerivative41(T::Retract, t1, + w12, none, none), + H1, 1e-5)); + EXPECT(assert_equal( + numericalDerivative42(T::Retract, t1, + w12, none, none), + H2, 1e-5)); + + EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2), 1e-9)); + EXPECT(assert_equal( + numericalDerivative41(T::Local, t1, t2, + none, none), + H1, 1e-5)); + EXPECT(assert_equal( + numericalDerivative42(T::Local, t1, t2, + none, none), + H2, 1e-5)); +} + +SensorState MakeSensor1() { + SensorState s; + s.inputBias = Bias(Vector3(0.03, -0.01, 0.02), Vector3(0.1, -0.2, 0.05)); + s.pose = Pose3(Rot3::RzRyRx(0.2, -0.1, 0.15), Point3(0.4, -0.2, 1.0)); + s.velocity = Vector3(0.5, -0.3, 0.2); + s.cameraOffset = + Pose3(Rot3::RzRyRx(-0.08, 0.04, -0.03), Point3(0.1, 0.0, 0.05)); + return s; +} + +SensorState MakeSensor2() { + SensorState s; + s.inputBias = + Bias(Vector3(0.02, 0.05, -0.06), Vector3(-0.04, 0.07, -0.03)); + s.pose = Pose3(Rot3::RzRyRx(-0.1, 0.2, -0.25), Point3(-0.3, 0.5, 0.8)); + s.velocity = Vector3(-0.2, 0.4, -0.1); + s.cameraOffset = + Pose3(Rot3::RzRyRx(0.06, -0.03, 0.02), Point3(-0.05, 0.02, 0.09)); + return s; +} + +std::vector Lms0() { return {}; } + +std::vector Lms1A() { return {{Point3(1.0, -0.5, 4.0), 11}}; } + +std::vector Lms1B() { return {{Point3(0.8, -0.3, 3.7), 11}}; } + +std::vector Lms3A() { + return {{Point3(1.0, -0.5, 4.0), 11}, + {Point3(-0.6, 0.4, 3.2), 22}, + {Point3(0.2, 0.7, 5.1), 33}}; +} + +std::vector Lms3B() { + return {{Point3(0.9, -0.45, 3.8), 11}, + {Point3(-0.5, 0.35, 3.4), 22}, + {Point3(0.1, 0.65, 4.9), 33}}; +} + +State MakeState0A() { return State(MakeSensor1(), Lms0()); } +State MakeState0B() { return State(MakeSensor2(), Lms0()); } +State MakeState1A() { return State(MakeSensor1(), Lms1A()); } +State MakeState1B() { return State(MakeSensor2(), Lms1B()); } +State MakeState3A() { return State(MakeSensor1(), Lms3A()); } +State MakeState3B() { return State(MakeSensor2(), Lms3B()); } + +} // namespace + +//****************************************************************************** +// Verifies State satisfies manifold and testable concept checks. +TEST(State, Concept) { + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsTestable); +} + +//****************************************************************************** +// Verifies dynamic dimensions and landmark id accessors. +TEST(State, DimensionsAndAccessors) { + const State s0 = MakeState0A(); + const State s1 = MakeState1A(); + const State s3 = MakeState3A(); + + EXPECT_LONGS_EQUAL(0, s0.n()); + EXPECT_LONGS_EQUAL(21, s0.dim()); + + EXPECT_LONGS_EQUAL(1, s1.n()); + EXPECT_LONGS_EQUAL(24, s1.dim()); + EXPECT((s1.ids() == std::vector{11})); + + EXPECT_LONGS_EQUAL(3, s3.n()); + EXPECT_LONGS_EQUAL(30, s3.dim()); + EXPECT((s3.ids() == std::vector{11, 22, 33})); +} + +//****************************************************************************** +// Verifies localCoordinates/retract round-trip consistency. +TEST(State, RetractLocalRoundTrip) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + const State s3a = MakeState3A(); + const State s3b = MakeState3B(); + + const Vector v = s3a.localCoordinates(s3b); + const State recovered = s3a.retract(v); + EXPECT(assert_equal(s3b, recovered, 1e-9)); +#else + EXPECT(true); +#endif +} + +//****************************************************************************** +// Verifies chart Jacobians for n=0 landmarks. +TEST(State, DerivativesN0) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + testChartDerivativesN<21>(result_, name_, MakeState0A(), MakeState0B()); +#else + EXPECT(true); +#endif +} + +//****************************************************************************** +// Verifies chart Jacobians for n=1 landmark. +TEST(State, DerivativesN1) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + testChartDerivativesN<24>(result_, name_, MakeState1A(), MakeState1B()); +#else + EXPECT(true); +#endif +} + +//****************************************************************************** +// Verifies chart Jacobians for n=3 landmarks. +TEST(State, DerivativesN3) { +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + testChartDerivativesN<30>(result_, name_, MakeState3A(), MakeState3B()); +#else + EXPECT(true); +#endif +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testEqVIOSymmetry.cpp b/gtsam_unstable/navigation/tests/testEqVIOSymmetry.cpp new file mode 100644 index 0000000000..8197c76618 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testEqVIOSymmetry.cpp @@ -0,0 +1,455 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testEqVIOSymmetry.cpp + * @brief Unit tests for VIO symmetry actions and lift helpers + * @author Rohan Bansal + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace eqvio_test_util { + inline std::shared_ptr CreateDefaultCamera() { + return std::make_shared( + Pose3::Identity(), Cal3_S2(450.0, 450.0, 0.0, 400.0, 240.0)); + } + + inline Se23 MakeA(const Rot3& R, const Point3& t, const Vector3& w) { + Se23::Matrix3K x; + x.col(0) = t; + x.col(1) = w; + return Se23(R, x); + } + + inline State RandomStateElement(const std::vector& ids) { + SensorState sensor; + sensor.inputBias = Bias(Vector3::Random(), Vector3::Random()); + sensor.pose = Pose3::Expmap(Vector6::Random()); + sensor.velocity = Vector3::Random(); + sensor.cameraOffset = Pose3::Expmap(Vector6::Random()); + + std::vector lms(ids.size()); + for (size_t i = 0; i < ids.size(); ++i) { + Point3 p = 10.0 * Vector3::Random(); + p.z() = std::abs(p.z()) + 1.0; + lms[i] = Landmark{p, ids[i]}; + } + return State(sensor, lms); + } + + inline VioGroup RandomGroupElement(const std::vector& ids) { + const Pose3 Apose = Pose3::Expmap(Vector6::Random()); + const Vector3 w = Vector3::Random(); + const Pose3 B = Pose3::Expmap(Vector6::Random()); + const Bias beta(Vector3::Random(), Vector3::Random()); + + std::vector Q(ids.size()); + for (size_t i = 0; i < ids.size(); ++i) { + const double scale = 2.0 * static_cast(rand()) / RAND_MAX + 1.0; + const double yaw = 0.3 * (2.0 * static_cast(rand()) / RAND_MAX - 1.0); + Q[i] = SOT3(SO3::Expmap(Vector3(0.0, 0.0, yaw)), std::log(scale)); + } + + return makeVioGroup(MakeA(Apose.rotation(), Apose.translation(), w), beta, B, + LandmarkGroup(Q)); + } + + inline IMUInput RandomVelocityElement() { + IMUInput vel; + vel.gyr = Vector3::Random(); + vel.acc = Vector3::Random(); + vel.gyrBiasVel = Vector3::Random(); + vel.accBiasVel = Vector3::Random(); + vel.stamp = 0.0; + return vel; + } + + inline VisionMeasurement RandomVisionMeasurement( + const std::vector& ids, + const std::shared_ptr& camera) { + VisionMeasurement measurement; + + for (int id : ids) { + Vector3 p; + do { + p = Vector3::Random(); + } while (p.norm() < 1e-9); + p.normalize(); + while (p.z() < 1e-1) { + p = Vector3::Random().normalized(); + } + measurement[id] = camera->project2(p); + } + return measurement; + } + + inline Vector MeasurementVector(const VisionMeasurement& measurement) { + Vector y = Vector::Zero(2 * static_cast(measurement.size())); + int i = 0; + for (const auto& [id, p] : measurement) { + (void)id; + y.segment<2>(2 * i) << p.x(), p.y(); + ++i; + } + return y; + } + + inline Vector MeasurementDifference(const VisionMeasurement& y1, + const VisionMeasurement& y2) { + if (y1.size() != y2.size()) { + throw std::invalid_argument("MeasurementDifference: size mismatch"); + } + Vector diff = Vector::Zero(2 * static_cast(y1.size())); + int i = 0; + for (const auto& [id, p1] : y1) { + const auto it = y2.find(id); + if (it == y2.end()) { + throw std::invalid_argument("MeasurementDifference: id mismatch"); + } + diff.segment<2>(2 * i) << p1.x() - it->second.x(), p1.y() - it->second.y(); + ++i; + } + return diff; + } + + inline double LogNorm(const VioGroup& X) { return VioGroup::Logmap(X).norm(); } + + inline double StateDistance(const State& xi1, const State& xi2) { + if (xi1.n() != xi2.n()) { + throw std::invalid_argument("StateDistance: landmark count mismatch"); + } + + double dist = 0.0; + dist += xi1.sensor.inputBias.localCoordinates(xi2.sensor.inputBias).norm(); + dist += xi1.sensor.pose.localCoordinates(xi2.sensor.pose).norm(); + dist += xi1.sensor.cameraOffset.localCoordinates(xi2.sensor.cameraOffset).norm(); + dist += (xi1.sensor.velocity - xi2.sensor.velocity).norm(); + + for (size_t i = 0; i < xi1.n(); ++i) { + if (xi1.cameraLandmarks[i].id != xi2.cameraLandmarks[i].id) { + throw std::invalid_argument("StateDistance: landmark ids mismatch"); + } + dist += (xi1.cameraLandmarks[i].p - xi2.cameraLandmarks[i].p).norm(); + } + return dist; + } + + inline double MeasurementDistance(const VisionMeasurement& y1, + const VisionMeasurement& y2) { + Vector y1vec = MeasurementVector(y1); + Vector y2vec = MeasurementVector(y2); + const double scale = std::max(1.0, std::max(y1vec.norm(), y2vec.norm())); + const Vector diff = MeasurementDifference(y1, y2); + return diff.norm() / scale; + } + + inline Matrix NumericalDifferential(const std::function& f, + const Vector& x0, double h) { + const int n = static_cast(x0.size()); + const Vector y0 = f(x0); + const int m = static_cast(y0.size()); + Matrix J = Matrix::Zero(m, n); + for (int j = 0; j < n; ++j) { + Vector dx = Vector::Zero(n); + dx(j) = h; + J.col(j) = (f(x0 + dx) - f(x0 - dx)) / (2.0 * h); + } + return J; + } + + inline bool MatrixClose(const Matrix& A, const Matrix& B, double h = -1.0) { + if (A.rows() != B.rows() || A.cols() != B.cols()) return false; + if (!A.array().isFinite().all() || !B.array().isFinite().all()) return false; + + if (h < 0.0) h = std::cbrt(std::numeric_limits::epsilon()); + + for (int i = 0; i < A.rows(); ++i) { + for (int j = 0; j < A.cols(); ++j) { + const double tol = std::max(h, h * 1e1 * std::abs(A(i, j))); + if (std::abs(A(i, j) - B(i, j)) > tol) return false; + } + } + return true; + } + +} // namespace eqvio_test_util + +using namespace eqvio_test_util; + +namespace { + +constexpr int kEqvioActionReps = 25; +constexpr double kEqvioNearZero = 1e-12; + + +std::vector Lms0() { return {}; } +std::vector Lms3() { + return {{Point3(1.0, -0.3, 4.2), 11}, + {Point3(-0.7, 0.4, 3.1), 22}, + {Point3(0.2, 0.8, 5.5), 33}}; +} + +SensorState SensorA() { + SensorState s; + s.inputBias = Bias(Vector3(0.01, -0.02, 0.04), Vector3(0.05, -0.04, 0.03)); + s.pose = Pose3(Rot3::RzRyRx(0.1, -0.2, 0.25), Point3(0.4, -0.1, 1.0)); + s.velocity = Vector3(0.3, -0.2, 0.1); + s.cameraOffset = + Pose3(Rot3::RzRyRx(-0.03, 0.05, -0.02), Point3(0.1, 0.02, 0.07)); + return s; +} + +State State0() { return State(SensorA(), Lms0()); } +State State3() { return State(SensorA(), Lms3()); } + +VioGroup Group0() { + const Rot3 R = Rot3::RzRyRx(0.02, -0.03, 0.04); + const Point3 t(0.05, -0.02, 0.03); + const Vector3 w(0.01, -0.03, 0.02); + return makeVioGroup( + MakeA(R, t, w), + Bias(Vector3(-0.02, 0.01, 0.0), Vector3(0.01, -0.01, 0.02)), + Pose3(Rot3::RzRyRx(-0.02, 0.01, 0.03), Point3(0.02, 0.0, -0.01)), + LandmarkGroup(0)); +} + +VioGroup Group3() { + const Rot3 R = Rot3::RzRyRx(0.02, -0.03, 0.04); + const Point3 t(0.05, -0.02, 0.03); + const Vector3 w(0.01, -0.03, 0.02); + const SOT3 q1(SO3::Expmap((Vector3() << 0.03, -0.02, 0.01).finished()), + std::log(1.1)); + const SOT3 q2(SO3::Expmap((Vector3() << -0.01, 0.04, -0.02).finished()), + std::log(0.95)); + const SOT3 q3(SO3::Expmap((Vector3() << 0.02, 0.01, 0.03).finished()), + std::log(1.05)); + return makeVioGroup( + MakeA(R, t, w), + Bias(Vector3(-0.02, 0.01, 0.0), Vector3(0.01, -0.01, 0.02)), + Pose3(Rot3::RzRyRx(-0.02, 0.01, 0.03), Point3(0.02, 0.0, -0.01)), + LandmarkGroup({q1, q2, q3})); +} + +VioGroup Group3b() { + const Rot3 R = Rot3::RzRyRx(-0.04, 0.01, -0.02); + const Point3 t(-0.03, 0.04, -0.01); + const Vector3 w(-0.02, 0.01, 0.03); + const SOT3 q1(SO3::Expmap((Vector3() << -0.02, 0.01, 0.04).finished()), + std::log(1.02)); + const SOT3 q2(SO3::Expmap((Vector3() << 0.02, -0.03, 0.01).finished()), + std::log(0.98)); + const SOT3 q3(SO3::Expmap((Vector3() << 0.01, 0.02, -0.02).finished()), + std::log(1.08)); + return makeVioGroup( + MakeA(R, t, w), + Bias(Vector3(0.02, 0.01, -0.01), Vector3(-0.02, 0.03, -0.01)), + Pose3(Rot3::RzRyRx(0.01, -0.02, 0.04), Point3(-0.01, 0.03, 0.02)), + LandmarkGroup({q1, q2, q3})); +} + +Matrix NumericalDerivativeWrtGroup(const Symmetry& phi, const VioGroup& X, + const State& xi, const State& y0, + double h = 1e-6) { + Matrix H = Matrix::Zero(y0.dim(), static_cast(Dim_groupTangent(X))); + for (int j = 0; j < static_cast(Dim_groupTangent(X)); ++j) { + Vector dx = Vector::Zero(static_cast(Dim_groupTangent(X))); + dx(j) = h; + const State yPlus = phi(xi, X.retract(dx)); + dx(j) = -h; + const State yMinus = phi(xi, X.retract(dx)); + H.col(j) = + (y0.localCoordinates(yPlus) - y0.localCoordinates(yMinus)) / (2.0 * h); + } + return H; +} + +Matrix NumericalDerivativeWrtState(const Symmetry& phi, const VioGroup& X, + const State& xi, const State& y0, + double h = 1e-6) { + Matrix H = Matrix::Zero(y0.dim(), xi.dim()); + for (int j = 0; j < xi.dim(); ++j) { + Vector dxi = Vector::Zero(xi.dim()); + dxi(j) = h; + const State yPlus = phi(xi.retract(dxi), X); + dxi(j) = -h; + const State yMinus = phi(xi.retract(dxi), X); + H.col(j) = + (y0.localCoordinates(yPlus) - y0.localCoordinates(yMinus)) / (2.0 * h); + } + return H; +} + +} // namespace + +//****************************************************************************** +// Verifies the right-action law for Symmetry. +TEST(Symmetry, RightActionLaw) { + const Symmetry phi; + const State xi = State3(); + const VioGroup X1 = Group3(); + const VioGroup X2 = Group3b(); + EXPECT_RIGHT_ACTION(phi, xi, X1, X2); +} + +//****************************************************************************** +// Verifies action Jacobians against numerical derivatives for n=0. +TEST(Symmetry, JacobiansN0) { + const Symmetry phi; + const VioGroup X = Group0(); + const State xi = State0(); + + Matrix HX, HXi; + const State y = phi(xi, X, HXi, HX); + const Matrix HXNum = NumericalDerivativeWrtGroup(phi, X, xi, y); + const Matrix HXiNum = NumericalDerivativeWrtState(phi, X, xi, y); + + EXPECT(assert_equal(HXNum, HX, 1e-5)); + EXPECT(assert_equal(HXiNum, HXi, 1e-5)); +} + +//****************************************************************************** +// Verifies action Jacobians against numerical derivatives for n=3. +TEST(Symmetry, JacobiansN3) { + const Symmetry phi; + const VioGroup X = Group3(); + const State xi = State3(); + + Matrix HX, HXi; + const State y = phi(xi, X, HXi, HX); + const Matrix HXNum = NumericalDerivativeWrtGroup(phi, X, xi, y); + const Matrix HXiNum = NumericalDerivativeWrtState(phi, X, xi, y); + + EXPECT(assert_equal(HXNum, HX, 1e-5)); + EXPECT(assert_equal(HXiNum, HXi, 1e-5)); +} + +//****************************************************************************** +// Verifies eqvio-ported state action identity and composition checks. +TEST(Symmetry, StateActionEqvioPort) { + srand(0); + const std::vector ids = {0, 1, 2, 3, 4}; + const VioGroup groupId = makeVioGroupIdentity(ids.size()); + + for (int rep = 0; rep < kEqvioActionReps; ++rep) { + const VioGroup X1 = RandomGroupElement(ids); + const VioGroup X2 = RandomGroupElement(ids); + const State xi0 = RandomStateElement(ids); + + const State xi0Id = stateGroupAction(groupId, xi0); + const double dist0Id = StateDistance(xi0Id, xi0); + EXPECT(dist0Id <= kEqvioNearZero); + + const State xi1 = stateGroupAction(X2, stateGroupAction(X1, xi0)); + const State xi2 = stateGroupAction(X1 * X2, xi0); + const double dist12 = StateDistance(xi1, xi2); + EXPECT(dist12 <= kEqvioNearZero); + } +} + +//****************************************************************************** +// Verifies eqvio-ported output action identity and composition checks. +TEST(Symmetry, OutputActionEqvioPort) { + srand(0); + const std::vector ids = {0, 1, 2, 3, 4}; + const VioGroup groupId = makeVioGroupIdentity(ids.size()); + const auto camera = CreateDefaultCamera(); + + for (int rep = 0; rep < kEqvioActionReps; ++rep) { + const VioGroup X1 = RandomGroupElement(ids); + const VioGroup X2 = RandomGroupElement(ids); + const VisionMeasurement y0 = RandomVisionMeasurement(ids, camera); + + try { + const VisionMeasurement y0Id = outputGroupAction(groupId, y0, camera); + const double dist0Id = MeasurementDistance(y0Id, y0); + EXPECT(dist0Id <= 1e-5); + + const VisionMeasurement y1 = + outputGroupAction(X2, outputGroupAction(X1, y0, camera), camera); + const VisionMeasurement y2 = outputGroupAction(X1 * X2, y0, camera); + const double dist12 = MeasurementDistance(y1, y2); + EXPECT(dist12 <= 1e-5); + } catch (const CheiralityException&) { + continue; + } + } +} + +//****************************************************************************** +// Verifies measurement equivariance under group action. +TEST(Symmetry, OutputEquivarianceEqvioPort) { + srand(0); + const std::vector ids = {0, 1, 2, 3, 4, 5}; + const auto camera = CreateDefaultCamera(); + + for (int rep = 0; rep < kEqvioActionReps; ++rep) { + const VioGroup X = RandomGroupElement(ids); + const State xi0 = RandomStateElement(ids); + + try { + const VisionMeasurement y1 = + measureSystemState(stateGroupAction(X, xi0), camera); + const VisionMeasurement y2 = + outputGroupAction(X, measureSystemState(xi0, camera), camera); + const double dist12 = MeasurementDistance(y1, y2); + EXPECT(dist12 <= 1e-5); + } catch (const CheiralityException&) { + continue; + } + } +} + +//****************************************************************************** +// Verifies discrete lift update matches direct integration. +TEST(Symmetry, LiftAndIntegrationSanity) { + const State xi = State3(); + IMUInput imu; + imu.gyr = Vector3(0.03, -0.02, 0.01); + imu.acc = Vector3(0.2, -0.1, 9.75); + imu.gyrBiasVel = Vector3(0.01, 0.0, -0.02); + imu.accBiasVel = Vector3(-0.01, 0.02, 0.0); + + const Vector l = liftVelocity(xi, imu); + EXPECT_LONGS_EQUAL(33, l.size()); + + const double dt = 1e-3; + const VioGroup delta = liftVelocityDiscrete(xi, imu, dt); + const State xiLifted = stateGroupAction(delta, xi); + const State xiIntegrated = integrateSystemFunction(xi, imu, dt); + EXPECT(assert_equal(xiIntegrated, xiLifted, 1e-7)); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam_unstable/navigation/tests/testVIOEqFMatricesInvDepth.cpp b/gtsam_unstable/navigation/tests/testVIOEqFMatricesInvDepth.cpp new file mode 100644 index 0000000000..fd32a74294 --- /dev/null +++ b/gtsam_unstable/navigation/tests/testVIOEqFMatricesInvDepth.cpp @@ -0,0 +1,186 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 testVIOEqFMatricesInvDepth.cpp + * @brief Unit tests for inverse-depth VIO EqF matrix suite + */ + +#include +#include +#include + +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::eqvio; + +namespace { + +Se23 MakeA(const Rot3& R, const Point3& t, const Vector3& w) { + Se23::Matrix3K x; + x.col(0) = t; + x.col(1) = w; + return Se23(R, x); +} + +SensorState SensorFixture() { + SensorState s; + s.inputBias = Bias( + (Vector3() << 0.01, -0.02, 0.03).finished(), + (Vector3() << 0.04, -0.01, 0.02).finished()); + s.pose = Pose3(Rot3::RzRyRx(0.1, -0.05, 0.2), Point3(0.3, -0.4, 1.2)); + s.velocity = Vector3(0.2, -0.1, 0.05); + s.cameraOffset = + Pose3(Rot3::RzRyRx(-0.02, 0.03, -0.01), Point3(0.1, 0.02, 0.08)); + return s; +} + +State State0() { return State(SensorFixture(), {}); } +State State1() { return State(SensorFixture(), {{Point3(0.8, -0.2, 4.5), 11}}); } +State State3() { + return State(SensorFixture(), + {{Point3(0.8, -0.2, 4.5), 11}, + {Point3(-0.6, 0.3, 3.8), 22}, + {Point3(0.1, 0.7, 5.2), 33}}); +} + +VioGroup Group0() { return makeVioGroupIdentity(0); } +VioGroup Group1() { + const SOT3 q1(SO3::Expmap((Vector3() << 0.02, -0.01, 0.03).finished()), + std::log(1.1)); + return makeVioGroup( + MakeA(Rot3::RzRyRx(0.03, -0.02, 0.01), Point3(0.05, -0.01, 0.02), + Vector3(0.01, -0.02, 0.03)), + Bias((Vector3() << 0.01, 0.0, -0.01).finished(), + (Vector3() << 0.02, -0.01, 0.03).finished()), + Pose3(Rot3::RzRyRx(-0.01, 0.02, -0.03), Point3(0.02, 0.01, -0.01)), + LandmarkGroup({q1})); +} +VioGroup Group3() { + const SOT3 q1(SO3::Expmap((Vector3() << 0.02, -0.01, 0.03).finished()), + std::log(1.1)); + const SOT3 q2(SO3::Expmap((Vector3() << -0.01, 0.03, -0.02).finished()), + std::log(0.95)); + const SOT3 q3(SO3::Expmap((Vector3() << 0.01, 0.02, 0.01).finished()), + std::log(1.05)); + return makeVioGroup( + MakeA(Rot3::RzRyRx(0.03, -0.02, 0.01), Point3(0.05, -0.01, 0.02), + Vector3(0.01, -0.02, 0.03)), + Bias((Vector3() << 0.01, 0.0, -0.01).finished(), + (Vector3() << 0.02, -0.01, 0.03).finished()), + Pose3(Rot3::RzRyRx(-0.01, 0.02, -0.03), Point3(0.02, 0.01, -0.01)), + LandmarkGroup({q1, q2, q3})); +} + +IMUInput ImuFixture() { + IMUInput u; + u.gyr = Vector3(0.02, -0.01, 0.03); + u.acc = Vector3(0.1, -0.05, 9.7); + u.gyrBiasVel = Vector3(0.01, 0.0, -0.01); + u.accBiasVel = Vector3(-0.02, 0.01, 0.0); + return u; +} + +bool IsFinite(const Matrix& M) { return M.array().isFinite().all(); } + +} // namespace + +//****************************************************************************** +TEST(VIOEqFMatricesInvDepth, Selector) { + const EqFCoordinateSuite* suite = &EqFCoordinateSuite_invdepth; + EXPECT(suite != nullptr); +} + +//****************************************************************************** +TEST(VIOEqFMatricesInvDepth, ShapesAndFinite) { + const auto camera = + std::make_shared(Pose3::Identity(), Cal3_S2(1, 1, 0, 0, 0)); + const EqFCoordinateSuite& suite = EqFCoordinateSuite_invdepth; + + for (const auto& pair : + std::vector>{{State0(), Group0()}, + {State1(), Group1()}, + {State3(), Group3()}}) { + const State& xi0 = pair.first; + const VioGroup& X = pair.second; + const IMUInput imu = ImuFixture(); + const VisionMeasurement y = + measureSystemState(stateGroupAction(X, xi0), camera); + + const Matrix A = suite.stateMatrixA(X, xi0, imu); + const Matrix B = suite.inputMatrixB(X, xi0); + const Matrix C = suite.outputMatrixC(xi0, X, y, camera, true); + + EXPECT_LONGS_EQUAL(xi0.dim(), A.rows()); + EXPECT_LONGS_EQUAL(xi0.dim(), A.cols()); + EXPECT_LONGS_EQUAL(xi0.dim(), B.rows()); + EXPECT_LONGS_EQUAL(IMUInput::CompDim, B.cols()); + EXPECT_LONGS_EQUAL(2 * static_cast(y.size()), C.rows()); + EXPECT_LONGS_EQUAL(xi0.dim(), C.cols()); + + EXPECT(IsFinite(A)); + EXPECT(IsFinite(B)); + EXPECT(IsFinite(C)); + } +} + +//****************************************************************************** +TEST(VIOEqFMatricesInvDepth, StateChartRoundTrip) { + const EqFCoordinateSuite& suite = EqFCoordinateSuite_invdepth; + + for (const State& xi0 : std::vector{State1(), State3()}) { + const Vector eps = Vector::LinSpaced(xi0.dim(), -1e-3, 1e-3); + const State xi = suite.stateChartInv(eps, xi0); + const Vector epsRecovered = suite.stateChart(xi, xi0); + EXPECT(assert_equal(eps, epsRecovered, 1e-8)); + } +} + +//****************************************************************************** +TEST(VIOEqFMatricesInvDepth, SmallStepDiscreteConsistency) { + const EqFCoordinateSuite& suite = EqFCoordinateSuite_invdepth; + + const State xi0 = State3(); + const VioGroup X = Group3(); + const IMUInput imu = ImuFixture(); + + const double dt = 1e-6; + const Matrix A = suite.stateMatrixA(X, xi0, imu); + const Matrix Phi = suite.stateMatrixADiscrete(X, xi0, imu, dt); + const Matrix PhiApprox = Matrix::Identity(xi0.dim(), xi0.dim()) + dt * A; + + EXPECT(assert_equal(PhiApprox, Phi, 1e-4)); +} + +//****************************************************************************** +TEST(VIOEqFMatricesInvDepth, InnovationLiftConsistency) { + const EqFCoordinateSuite& suite = EqFCoordinateSuite_invdepth; + + for (const State& xi0 : std::vector{State1(), State3()}) { + const Vector gamma = Vector::LinSpaced(xi0.dim(), -0.1, 0.1); + const double eps = 1e-3; + const Vector lift = suite.liftInnovation(eps * gamma, xi0); + const VioGroup dCont = VioGroup::Expmap(lift); + const VioGroup dDisc = suite.liftInnovationDiscrete(eps * gamma, xi0); + + const Vector err = dCont.localCoordinates(dDisc); + EXPECT(err.norm() < 5e-3); + } +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +}