Skip to content

Commit

Permalink
Merge pull request #40 from koide3/maha
Browse files Browse the repository at this point in the history 
Add FusedCovCacheMode for GICP and VGICP
  • Loading branch information
@koide3
koide3 authored Jan 14, 2025
2 parents 3509828 + f596303 commit b68b388
Show file tree
Hide file tree
Showing 6 changed files with 330 additions and 25 deletions.
71 changes: 58 additions & 13 deletions include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <gtsam/linear/HessianFactor.h>
#include <gtsam_points/config.hpp>
#include <gtsam_points/ann/kdtree2.hpp>
#include <gtsam_points/util/compact.hpp>
#include <gtsam_points/util/parallelism.hpp>
#include <gtsam_points/types/frame_traits.hpp>
#include <gtsam_points/factors/impl/scan_matching_reduction.hpp>
Expand All @@ -27,6 +28,7 @@ IntegratedGICPFactor_<TargetFrame, SourceFrame>::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),
Expand Down Expand Up @@ -67,6 +69,7 @@ IntegratedGICPFactor_<TargetFrame, SourceFrame>::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),
Expand Down Expand Up @@ -102,6 +105,8 @@ IntegratedGICPFactor_<TargetFrame, SourceFrame>::~IntegratedGICPFactor_() {}

template <typename TargetFrame, typename SourceFrame>
void IntegratedGICPFactor_<TargetFrame, SourceFrame>::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;
Expand All @@ -117,7 +122,17 @@ void IntegratedGICPFactor_<TargetFrame, SourceFrame>::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) {
Expand All @@ -129,15 +144,29 @@ void IntegratedGICPFactor_<TargetFrame, SourceFrame>::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;
}
};

Expand Down Expand Up @@ -193,7 +222,23 @@ double IntegratedGICPFactor_<TargetFrame, SourceFrame>::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;
}
Expand All @@ -206,8 +251,8 @@ double IntegratedGICPFactor_<TargetFrame, SourceFrame>::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<double, 6, 4> J_target_mahalanobis = J_target.transpose() * mahalanobis[i];
Eigen::Matrix<double, 6, 4> J_source_mahalanobis = J_source.transpose() * mahalanobis[i];
Eigen::Matrix<double, 6, 4> J_target_mahalanobis = J_target.transpose() * mahalanobis;
Eigen::Matrix<double, 6, 4> J_source_mahalanobis = J_source.transpose() * mahalanobis;

*H_target += J_target_mahalanobis * J_target;
*H_source += J_source_mahalanobis * J_source;
Expand Down
71 changes: 61 additions & 10 deletions include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <gtsam_points/config.hpp>
#include <gtsam_points/types/gaussian_voxelmap_cpu.hpp>
#include <gtsam_points/util/parallelism.hpp>
#include <gtsam_points/util/compact.hpp>
#include <gtsam_points/factors/impl/scan_matching_reduction.hpp>

#ifdef GTSAM_POINTS_USE_TBB
Expand All @@ -24,6 +25,7 @@ IntegratedVGICPFactor_<SourceFrame>::IntegratedVGICPFactor_(
const std::shared_ptr<const SourceFrame>& source)
: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key),
num_threads(1),
mahalanobis_cache_mode(FusedCovCacheMode::FULL),
target_voxels(std::dynamic_pointer_cast<const GaussianVoxelMapCPU>(target_voxels)),
source(source) {
//
Expand Down Expand Up @@ -51,6 +53,7 @@ IntegratedVGICPFactor_<SourceFrame>::IntegratedVGICPFactor_(
const std::shared_ptr<const SourceFrame>& source)
: gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key),
num_threads(1),
mahalanobis_cache_mode(FusedCovCacheMode::FULL),
target_voxels(std::dynamic_pointer_cast<const GaussianVoxelMapCPU>(target_voxels)),
source(source) {
//
Expand All @@ -75,8 +78,19 @@ IntegratedVGICPFactor_<SourceFrame>::~IntegratedVGICPFactor_() {}

template <typename SourceFrame>
void IntegratedVGICPFactor_<SourceFrame>::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);
Expand All @@ -85,16 +99,37 @@ void IntegratedVGICPFactor_<SourceFrame>::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;
}
}
};

Expand Down Expand Up @@ -153,7 +188,23 @@ double IntegratedVGICPFactor_<SourceFrame>::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;
Expand All @@ -167,8 +218,8 @@ double IntegratedVGICPFactor_<SourceFrame>::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<double, 6, 4> J_target_mahalanobis = J_target.transpose() * mahalanobis[i];
Eigen::Matrix<double, 6, 4> J_source_mahalanobis = J_source.transpose() * mahalanobis[i];
Eigen::Matrix<double, 6, 4> J_target_mahalanobis = J_target.transpose() * mahalanobis;
Eigen::Matrix<double, 6, 4> J_source_mahalanobis = J_source.transpose() * mahalanobis;

*H_target += J_target_mahalanobis * J_target;
*H_source += J_source_mahalanobis * J_source;
Expand Down
17 changes: 16 additions & 1 deletion include/gtsam_points/factors/integrated_gicp_factor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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<const NearestNeighborSearch> 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<long> correspondences;
mutable std::vector<Eigen::Matrix4d> mahalanobis;
mutable std::vector<Eigen::Matrix4d> mahalanobis_full;
mutable std::vector<Eigen::Matrix<float, 6, 1>> mahalanobis_compact;

std::shared_ptr<const TargetFrame> target;
std::shared_ptr<const SourceFrame> source;
Expand Down
9 changes: 8 additions & 1 deletion include/gtsam_points/factors/integrated_vgicp_factor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/types/gaussian_voxelmap_cpu.hpp>
#include <gtsam_points/factors/integrated_matching_cost_factor.hpp>
#include <gtsam_points/factors/integrated_gicp_factor.hpp>

namespace gtsam_points {

Expand Down Expand Up @@ -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);
Expand All @@ -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<const GaussianVoxel*> correspondences;
mutable std::vector<Eigen::Matrix4d> mahalanobis;
mutable std::vector<Eigen::Matrix4d> mahalanobis_full;
mutable std::vector<Eigen::Matrix<float, 6, 1>> mahalanobis_compact;

std::shared_ptr<const GaussianVoxelMapCPU> target_voxels;
std::shared_ptr<const SourceFrame> source;
Expand Down
27 changes: 27 additions & 0 deletions include/gtsam_points/util/compact.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Kenji Koide ([email protected])
#pragma once

#include <Eigen/Core>

namespace gtsam_points {

template <int D>
inline Eigen::Matrix<float, 6, 1> compact_cov(const Eigen::Matrix<double, D, D>& cov) {
Eigen::Matrix<float, 6, 1> 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<float, 6, 1>& 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
Loading

0 comments on commit b68b388

Please sign in to comment.