diff --git a/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp index b9ebe8e..fca83df 100644 --- a/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -27,6 +28,7 @@ IntegratedGICPFactor_::IntegratedGICPFactor_( : gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), max_correspondence_distance_sq(1.0), + mahalanobis_cache_mode(FusedCovCacheMode::FULL), correspondence_update_tolerance_rot(0.0), correspondence_update_tolerance_trans(0.0), target(target), @@ -67,6 +69,7 @@ IntegratedGICPFactor_::IntegratedGICPFactor_( : gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key), num_threads(1), max_correspondence_distance_sq(1.0), + mahalanobis_cache_mode(FusedCovCacheMode::FULL), correspondence_update_tolerance_rot(0.0), correspondence_update_tolerance_trans(0.0), target(target), @@ -102,6 +105,8 @@ IntegratedGICPFactor_::~IntegratedGICPFactor_() {} template void IntegratedGICPFactor_::update_correspondences(const Eigen::Isometry3d& delta) const { + linearization_point = delta; + bool do_update = true; if (correspondences.size() == frame::size(*source) && (correspondence_update_tolerance_trans > 0.0 || correspondence_update_tolerance_rot > 0.0)) { Eigen::Isometry3d diff = delta.inverse() * last_correspondence_point; @@ -117,7 +122,17 @@ void IntegratedGICPFactor_::update_correspondences(con } correspondences.resize(frame::size(*source)); - mahalanobis.resize(frame::size(*source)); + + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + mahalanobis_full.resize(frame::size(*source)); + break; + case FusedCovCacheMode::COMPACT: + mahalanobis_compact.resize(frame::size(*source)); + break; + case FusedCovCacheMode::NONE: + break; + } const auto perpoint_task = [&](int i) { if (do_update) { @@ -129,15 +144,29 @@ void IntegratedGICPFactor_::update_correspondences(con correspondences[i] = (num_found && k_sq_dist < max_correspondence_distance_sq) ? k_index : -1; } - if (correspondences[i] < 0) { - mahalanobis[i].setZero(); - } else { - const auto& target_cov = frame::cov(*target, correspondences[i]); - Eigen::Matrix4d RCR = (target_cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); - - RCR(3, 3) = 1.0; - mahalanobis[i] = RCR.inverse(); - mahalanobis[i](3, 3) = 0.0; + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + if (correspondences[i] < 0) { + mahalanobis_full[i].setZero(); + } else { + const auto& target_cov = frame::cov(*target, correspondences[i]); + const Eigen::Matrix4d RCR = (target_cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); + mahalanobis_full[i].setZero(); + mahalanobis_full[i].topLeftCorner<3, 3>() = RCR.topLeftCorner<3, 3>().inverse(); + } + break; + case FusedCovCacheMode::COMPACT: + if (correspondences[i] < 0) { + mahalanobis_compact[i].setZero(); + } else { + const auto& target_cov = frame::cov(*target, correspondences[i]); + const Eigen::Matrix4d RCR = (target_cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); + const Eigen::Matrix3d maha = RCR.topLeftCorner<3, 3>().inverse(); + mahalanobis_compact[i] = compact_cov(maha); + } + break; + case FusedCovCacheMode::NONE: + break; } }; @@ -193,7 +222,23 @@ double IntegratedGICPFactor_::evaluate( const Eigen::Vector4d transed_mean_A = delta * mean_A; const Eigen::Vector4d residual = mean_B - transed_mean_A; - const double error = residual.transpose() * mahalanobis[i] * residual; + Eigen::Matrix4d mahalanobis; + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + mahalanobis = mahalanobis_full[i]; + break; + case FusedCovCacheMode::COMPACT: + mahalanobis = uncompact_cov(mahalanobis_compact[i]); + break; + case FusedCovCacheMode::NONE: { + const auto& delta_l = linearization_point; // Delta at the linearization point + const Eigen::Matrix4d RCR = (cov_B + delta_l.matrix() * cov_A * delta_l.matrix().transpose()); + mahalanobis.setZero(); + mahalanobis.topLeftCorner<3, 3>() = RCR.topLeftCorner<3, 3>().inverse(); + } break; + } + + const double error = residual.transpose() * mahalanobis * residual; if (H_target == nullptr) { return error; } @@ -206,8 +251,8 @@ double IntegratedGICPFactor_::evaluate( J_source.block<3, 3>(0, 0) = delta.linear() * gtsam::SO3::Hat(mean_A.template head<3>()); J_source.block<3, 3>(0, 3) = -delta.linear(); - Eigen::Matrix J_target_mahalanobis = J_target.transpose() * mahalanobis[i]; - Eigen::Matrix J_source_mahalanobis = J_source.transpose() * mahalanobis[i]; + Eigen::Matrix J_target_mahalanobis = J_target.transpose() * mahalanobis; + Eigen::Matrix J_source_mahalanobis = J_source.transpose() * mahalanobis; *H_target += J_target_mahalanobis * J_target; *H_source += J_source_mahalanobis * J_source; diff --git a/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp index 4f82cbd..fa7f641 100644 --- a/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #ifdef GTSAM_POINTS_USE_TBB @@ -24,6 +25,7 @@ IntegratedVGICPFactor_::IntegratedVGICPFactor_( const std::shared_ptr& source) : gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), + mahalanobis_cache_mode(FusedCovCacheMode::FULL), target_voxels(std::dynamic_pointer_cast(target_voxels)), source(source) { // @@ -51,6 +53,7 @@ IntegratedVGICPFactor_::IntegratedVGICPFactor_( const std::shared_ptr& source) : gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key), num_threads(1), + mahalanobis_cache_mode(FusedCovCacheMode::FULL), target_voxels(std::dynamic_pointer_cast(target_voxels)), source(source) { // @@ -75,8 +78,19 @@ IntegratedVGICPFactor_::~IntegratedVGICPFactor_() {} template void IntegratedVGICPFactor_::update_correspondences(const Eigen::Isometry3d& delta) const { + linearization_point = delta; correspondences.resize(frame::size(*source)); - mahalanobis.resize(frame::size(*source)); + + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + mahalanobis_full.resize(frame::size(*source)); + break; + case FusedCovCacheMode::COMPACT: + mahalanobis_compact.resize(frame::size(*source)); + break; + case FusedCovCacheMode::NONE: + break; + } const auto perpoint_task = [&](int i) { Eigen::Vector4d pt = delta * frame::point(*source, i); @@ -85,16 +99,37 @@ void IntegratedVGICPFactor_::update_correspondences(const Eigen::Is if (voxel_id < 0) { correspondences[i] = nullptr; - mahalanobis[i].setIdentity(); + + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + mahalanobis_full[i].setZero(); + break; + case FusedCovCacheMode::COMPACT: + mahalanobis_compact[i].setZero(); + break; + case FusedCovCacheMode::NONE: + break; + } } else { const auto voxel = &target_voxels->lookup_voxel(voxel_id); - correspondences[i] = voxel; - Eigen::Matrix4d RCR = (voxel->cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); - RCR(3, 3) = 1.0; - mahalanobis[i] = RCR.inverse(); - mahalanobis[i](3, 3) = 0.0; + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: { + const Eigen::Matrix4d RCR = (voxel->cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); + mahalanobis_full[i].setZero(); + mahalanobis_full[i].topLeftCorner<3, 3>() = RCR.topLeftCorner<3, 3>().inverse(); + break; + } + case FusedCovCacheMode::COMPACT: { + const Eigen::Matrix4d RCR = (voxel->cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); + const Eigen::Matrix3d maha = RCR.topLeftCorner<3, 3>().inverse(); + mahalanobis_compact[i] = compact_cov(maha); + break; + } + case FusedCovCacheMode::NONE: + break; + } } }; @@ -153,7 +188,23 @@ double IntegratedVGICPFactor_::evaluate( Eigen::Vector4d transed_mean_A = delta * mean_A; Eigen::Vector4d residual = mean_B - transed_mean_A; - const double error = residual.transpose() * mahalanobis[i] * residual; + Eigen::Matrix4d mahalanobis; + switch (mahalanobis_cache_mode) { + case FusedCovCacheMode::FULL: + mahalanobis = mahalanobis_full[i]; + break; + case FusedCovCacheMode::COMPACT: + mahalanobis = uncompact_cov(mahalanobis_compact[i]); + break; + case FusedCovCacheMode::NONE: { + const auto& delta_l = linearization_point; // Delta at the linearization point + const Eigen::Matrix4d RCR = (cov_B + delta_l.matrix() * cov_A * delta_l.matrix().transpose()); + mahalanobis.setZero(); + mahalanobis.topLeftCorner<3, 3>() = RCR.topLeftCorner<3, 3>().inverse(); + } break; + } + + const double error = residual.transpose() * mahalanobis * residual; if (!H_target) { return error; @@ -167,8 +218,8 @@ double IntegratedVGICPFactor_::evaluate( J_source.block<3, 3>(0, 0) = delta.linear() * gtsam::SO3::Hat(mean_A.template head<3>()); J_source.block<3, 3>(0, 3) = -delta.linear(); - Eigen::Matrix J_target_mahalanobis = J_target.transpose() * mahalanobis[i]; - Eigen::Matrix J_source_mahalanobis = J_source.transpose() * mahalanobis[i]; + Eigen::Matrix J_target_mahalanobis = J_target.transpose() * mahalanobis; + Eigen::Matrix J_source_mahalanobis = J_source.transpose() * mahalanobis; *H_target += J_target_mahalanobis * J_target; *H_source += J_source_mahalanobis * J_source; diff --git a/include/gtsam_points/factors/integrated_gicp_factor.hpp b/include/gtsam_points/factors/integrated_gicp_factor.hpp index 60c1458..39d7e6f 100644 --- a/include/gtsam_points/factors/integrated_gicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_gicp_factor.hpp @@ -13,6 +13,15 @@ namespace gtsam_points { struct NearestNeighborSearch; +/** + * @brief Cache mode for fused covariance matrices (i.e., mahalanobis) + */ +enum class FusedCovCacheMode { + FULL, // Full matrix (4x4 double : 128 bit per point, fast) + COMPACT, // Compact matrix (upper-trianguler of 3x3 float, 24 bit per point, intermediate) + NONE // No cache (0 bit per point, slow) +}; + /** * @brief Generalized ICP matching cost factor * Segal et al., "Generalized-ICP", RSS2005 @@ -85,6 +94,9 @@ class IntegratedGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor correspondence_update_tolerance_trans = trans; } + /// @brief Set the cache mode for fused covariance matrices (i.e., mahalanobis). + void set_fused_cov_cache_mode(FusedCovCacheMode mode) { mahalanobis_cache_mode = mode; } + /// @brief Compute the fraction of inlier points that have correspondences with a distance smaller than the trimming threshold. double inlier_fraction() const { const int outliers = std::count(correspondences.begin(), correspondences.end(), -1); @@ -106,15 +118,18 @@ class IntegratedGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor private: int num_threads; double max_correspondence_distance_sq; + FusedCovCacheMode mahalanobis_cache_mode; std::shared_ptr target_tree; // I'm unhappy to have mutable members... double correspondence_update_tolerance_rot; double correspondence_update_tolerance_trans; + mutable Eigen::Isometry3d linearization_point; mutable Eigen::Isometry3d last_correspondence_point; mutable std::vector correspondences; - mutable std::vector mahalanobis; + mutable std::vector mahalanobis_full; + mutable std::vector> mahalanobis_compact; std::shared_ptr target; std::shared_ptr source; diff --git a/include/gtsam_points/factors/integrated_vgicp_factor.hpp b/include/gtsam_points/factors/integrated_vgicp_factor.hpp index 85bf334..e17b40c 100644 --- a/include/gtsam_points/factors/integrated_vgicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_vgicp_factor.hpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace gtsam_points { @@ -58,6 +59,9 @@ class IntegratedVGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor /// and setting n>1 can rather affect the processing speed. void set_num_threads(int n) { num_threads = n; } + /// @brief Set the cache mode for fused covariance matrices (i.e., mahalanobis). + void set_fused_cov_cache_mode(FusedCovCacheMode mode) { mahalanobis_cache_mode = mode; } + /// @brief Compute the fraction of inlier points that have correspondences with a distance smaller than the trimming threshold. double inlier_fraction() const { const int outliers = std::count(correspondences.begin(), correspondences.end(), nullptr); @@ -78,10 +82,13 @@ class IntegratedVGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor private: int num_threads; + FusedCovCacheMode mahalanobis_cache_mode; // I'm unhappy to have mutable members... + mutable Eigen::Isometry3d linearization_point; mutable std::vector correspondences; - mutable std::vector mahalanobis; + mutable std::vector mahalanobis_full; + mutable std::vector> mahalanobis_compact; std::shared_ptr target_voxels; std::shared_ptr source; diff --git a/include/gtsam_points/util/compact.hpp b/include/gtsam_points/util/compact.hpp new file mode 100644 index 0000000..932bce5 --- /dev/null +++ b/include/gtsam_points/util/compact.hpp @@ -0,0 +1,27 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) +#pragma once + +#include + +namespace gtsam_points { + +template +inline Eigen::Matrix compact_cov(const Eigen::Matrix& cov) { + Eigen::Matrix compact; + compact << cov(0, 0), cov(1, 0), cov(1, 1), cov(2, 0), cov(2, 1), cov(2, 2); + return compact; +} + +inline Eigen::Matrix4d uncompact_cov(const Eigen::Matrix& compact) { + Eigen::Matrix4d cov = Eigen::Matrix4d::Zero(); + cov(0, 0) = compact(0); + cov(1, 0) = cov(0, 1) = compact(1); + cov(1, 1) = compact(2); + cov(2, 0) = cov(0, 2) = compact(3); + cov(2, 1) = cov(1, 2) = compact(4); + cov(2, 2) = compact(5); + return cov; +} + +} // namespace gtsam_points diff --git a/src/test/test_compact_mahalanobis.cpp b/src/test/test_compact_mahalanobis.cpp new file mode 100644 index 0000000..467d47e --- /dev/null +++ b/src/test/test_compact_mahalanobis.cpp @@ -0,0 +1,160 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +TEST(CompactTest, CompactUncompact) { + for (int i = 0; i < 10; i++) { + const Eigen::Matrix3d rand3 = Eigen::Matrix3d::Random(); + Eigen::Matrix4d cov = Eigen::Matrix4d::Zero(); + cov.topLeftCorner<3, 3>() = rand3 * rand3.transpose(); + + const Eigen::Matrix compact = gtsam_points::compact_cov(cov); + const Eigen::Matrix4d cov2 = gtsam_points::uncompact_cov(compact); + + EXPECT_NEAR((cov - cov2).norm(), 0.0, 1e-6); + } +} + +TEST(CompactTest, UnusedElements) { + for (int i = 0; i < 10; i++) { + Eigen::Matrix4d cov = Eigen::Matrix4d::Random(); + cov = cov * cov.transpose(); + + const Eigen::Matrix compact = gtsam_points::compact_cov(cov); + const Eigen::Matrix4d cov2 = gtsam_points::uncompact_cov(compact); + + EXPECT_NEAR(cov2.bottomRows<1>().norm(), 0.0, 1e-6); + EXPECT_NEAR(cov2.rightCols<1>().norm(), 0.0, 1e-6); + } +} + +class CompactMahalanobisTestBase : public testing::Test { +public: + virtual void SetUp() override { + const std::string data_path = "./data/kitti_00"; + + const auto load_points = [](const std::string& filename) -> gtsam_points::PointCloudCPU::Ptr { + const auto raw = gtsam_points::read_points(filename); + if (raw.empty()) { + std::cerr << "Failed to read " << filename << std::endl; + return nullptr; + } + + auto points = std::make_shared(raw); + points = gtsam_points::voxelgrid_sampling(points, 0.25); + points->add_covs(gtsam_points::estimate_covariances(points->points, points->size())); + return points; + }; + + target = load_points(data_path + "/000000.bin"); + source = load_points(data_path + "/000001.bin"); + if (target) { + target_tree = std::make_shared(target->points, target->size()); + + auto voxels = std::make_shared(1.0); + voxels->insert(*target); + target_voxels = voxels; + } + } + + gtsam_points::PointCloudCPU::ConstPtr target; + gtsam_points::PointCloudCPU::ConstPtr source; + gtsam_points::KdTree::ConstPtr target_tree; + gtsam_points::GaussianVoxelMap::ConstPtr target_voxels; +}; + +TEST_F(CompactMahalanobisTestBase, LoadCheck) { + ASSERT_NE(target, nullptr); + ASSERT_NE(source, nullptr); + ASSERT_NE(target_tree, nullptr); +} + +class CompactMahalanobisTest : public CompactMahalanobisTestBase, public testing::WithParamInterface { +public: + std::tuple create_factor() { + const auto method = GetParam(); + + if (method == "GICP") { + auto factor_full = gtsam::make_shared(0, 1, target, source, target_tree); + factor_full->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::FULL); + + auto factor_compact = gtsam::make_shared(0, 1, target, source, target_tree); + factor_compact->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::COMPACT); + + auto factor_none = gtsam::make_shared(0, 1, target, source, target_tree); + factor_none->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::NONE); + + return {factor_full, factor_compact, factor_none}; + } else if (method == "VGICP") { + auto factor_full = gtsam::make_shared(0, 1, target_voxels, source); + factor_full->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::FULL); + + auto factor_compact = gtsam::make_shared(0, 1, target_voxels, source); + factor_compact->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::COMPACT); + + auto factor_none = gtsam::make_shared(0, 1, target_voxels, source); + factor_none->set_fused_cov_cache_mode(gtsam_points::FusedCovCacheMode::NONE); + + return {factor_full, factor_compact, factor_none}; + } else { + std::cerr << "Unknown method: " << method << std::endl; + } + + return std::tuple(); + } +}; + +INSTANTIATE_TEST_SUITE_P(gtsam_points, CompactMahalanobisTest, testing::Values("GICP", "VGICP"), [](const auto& info) { return info.param; }); + +TEST_P(CompactMahalanobisTest, FactorTest) { + const auto factors = create_factor(); + const auto factor_full = std::get<0>(factors); + const auto factor_compact = std::get<1>(factors); + const auto factor_none = std::get<2>(factors); + + std::mt19937 mt; + std::uniform_real_distribution dist(-1.0, 1.0); + + for (int i = 0; i < 3; i++) { + gtsam::Vector6 noise1, noise2; + std::generate(noise1.data(), noise1.data() + noise1.size(), [&] { return dist(mt) * 0.5; }); + std::generate(noise2.data(), noise2.data() + noise2.size(), [&] { return dist(mt) * 0.5; }); + + gtsam::Values values; + values.insert(0, gtsam::Pose3::Expmap(noise1)); + values.insert(1, gtsam::Pose3::Expmap(noise2)); + + const auto linear_full = factor_full->linearize(values); + const auto linear_compact = factor_compact->linearize(values); + const auto linear_none = factor_none->linearize(values); + + const auto info_full = linear_full->augmentedInformation(); + const auto info_compact = linear_compact->augmentedInformation(); + const auto info_none = linear_none->augmentedInformation(); + + EXPECT_NEAR((info_full - info_compact).cwiseAbs2().maxCoeff(), 0.0, 1e-3) << "Large augmented info error (full vs. compact)"; + EXPECT_NEAR((info_full - info_none).cwiseAbs2().maxCoeff(), 0.0, 1e-3) << "Large augmented info error (full vs. none)"; + + gtsam::Vector6 noise3; + std::generate(noise3.data(), noise3.data() + noise3.size(), [&] { return dist(mt) * 0.1; }); + values.insert_or_assign(0, gtsam::Pose3::Expmap(noise2) * gtsam::Pose3::Expmap(noise3)); + + const double error_full = factor_full->error(values); + const double error_compact = factor_compact->error(values); + const double error_none = factor_none->error(values); + + EXPECT_NEAR(error_full, error_compact, 1e-3) << "Large error (full vs. compact)"; + EXPECT_NEAR(error_full, error_none, 1e-3) << "Large error (full vs. none)"; + } +}