Updated Ground Alignment and Add Local Map-Based Evaluation of Closures

cpp/map_closures/AlignRansac2D.cpp

@@ -38,7 +38,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D(
     const std::vector<map_closures::PointPair> &keypoint_pairs) {
     map_closures::PointPair mean =
         std::reduce(keypoint_pairs.cbegin(), keypoint_pairs.cend(), map_closures::PointPair(),
-                    [](auto lhs, const auto &rhs) {
+                    [](map_closures::PointPair lhs, const map_closures::PointPair &rhs) {
                         lhs.ref += rhs.ref;
                         lhs.query += rhs.query;
                         return lhs;
@@ -47,7 +47,7 @@ Eigen::Isometry2d KabschUmeyamaAlignment2D(
     mean.ref /= static_cast<double>(keypoint_pairs.size());
     Eigen::Matrix2d covariance_matrix = std::transform_reduce(
         keypoint_pairs.cbegin(), keypoint_pairs.cend(), Eigen::Matrix2d().setZero(),
-        std::plus<Eigen::Matrix2d>(), [&](const auto &keypoint_pair) {
+        std::plus<Eigen::Matrix2d>(), [&](const map_closures::PointPair &keypoint_pair) {
             return (keypoint_pair.ref - mean.ref) *
                    ((keypoint_pair.query - mean.query).transpose());
         });
@@ -93,11 +93,11 @@ std::pair<Eigen::Isometry2d, std::size_t> RansacAlignment2D(
 
         std::sample(keypoint_pairs.begin(), keypoint_pairs.end(), sample_keypoint_pairs.begin(), 2,
                     std::mt19937{std::random_device{}()});
-        const auto &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
+        const Eigen::Isometry2d &T = KabschUmeyamaAlignment2D(sample_keypoint_pairs);
 
         int index = 0;
         std::for_each(keypoint_pairs.cbegin(), keypoint_pairs.cend(),
-                      [&](const auto &keypoint_pair) {
+                      [&](const PointPair &keypoint_pair) {
                           if ((T * keypoint_pair.ref - keypoint_pair.query).norm() <
                               inliers_distance_threshold)
                               inlier_indices.emplace_back(index);

cpp/map_closures/CMakeLists.txt

@@ -22,7 +22,7 @@
 # SOFTWARE.
 
 add_library(map_closures STATIC)
-target_sources(map_closures PRIVATE DensityMap.cpp AlignRansac2D.cpp GroundAlign.cpp
+target_sources(map_closures PRIVATE VoxelMap.cpp DensityMap.cpp AlignRansac2D.cpp GroundAlign.cpp
                                     MapClosures.cpp)
 target_include_directories(map_closures PUBLIC ${hbst_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/..)
 target_link_libraries(map_closures PUBLIC Eigen3::Eigen ${OpenCV_LIBS} Sophus::Sophus)

cpp/map_closures/DensityMap.cpp

@@ -65,26 +65,26 @@ DensityMap GenerateDensityMap(const std::vector<Eigen::Vector3d> &pcd,
     };
     std::vector<Eigen::Array2i> pixels(pcd.size());
     std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) {
-        const auto &pixel = Discretize2D(point);
+        const Eigen::Array2i &pixel = Discretize2D(point);
         lower_bound_coordinates = lower_bound_coordinates.min(pixel);
         upper_bound_coordinates = upper_bound_coordinates.max(pixel);
         return pixel;
     });
-    const auto rows_and_columns = upper_bound_coordinates - lower_bound_coordinates;
-    const auto n_rows = rows_and_columns.x() + 1;
-    const auto n_cols = rows_and_columns.y() + 1;
+    const Eigen::Array2i rows_and_columns = upper_bound_coordinates - lower_bound_coordinates;
+    const int n_rows = rows_and_columns.x() + 1;
+    const int n_cols = rows_and_columns.y() + 1;
 
     cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0);
-    std::for_each(pixels.cbegin(), pixels.cend(), [&](const auto &pixel) {
-        const auto px = pixel - lower_bound_coordinates;
+    std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) {
+        const Eigen::Array2i px = pixel - lower_bound_coordinates;
         counting_grid.at<double>(px.x(), px.y()) += 1;
         max_points = std::max(max_points, counting_grid.at<double>(px.x(), px.y()));
         min_points = std::min(min_points, counting_grid.at<double>(px.x(), px.y()));
     });
 
     DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates);
     counting_grid.forEach<double>([&](const double count, const int pos[]) {
-        auto density = (count - min_points) / (max_points - min_points);
+        double density = (count - min_points) / (max_points - min_points);
         density = density > density_threshold ? density : 0.0;
         density_map(pos[0], pos[1]) = static_cast<uint8_t>(255 * density);
     });

cpp/map_closures/DensityMap.hpp

@@ -38,7 +38,7 @@ struct DensityMap {
     DensityMap(DensityMap &&other) = default;
     DensityMap &operator=(DensityMap &&other) = default;
     DensityMap &operator=(const DensityMap &other) = delete;
-    inline auto &operator()(const int x, const int y) { return grid.at<uint8_t>(x, y); }
+    uint8_t &operator()(const int x, const int y) { return grid.at<uint8_t>(x, y); }
     Eigen::Vector2i lower_bound;
     double resolution;
     cv::Mat grid;

cpp/map_closures/GroundAlign.cpp

@@ -24,6 +24,8 @@
 #include "GroundAlign.hpp"
 
 #include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <Eigen/Geometry>
 #include <algorithm>
 #include <numeric>
 #include <sophus/se3.hpp>
@@ -32,22 +34,92 @@
 #include <vector>
 
 namespace {
+using Vector3dVector = std::vector<Eigen::Vector3d>;
+using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
+
 struct PixelHash {
     size_t operator()(const Eigen::Vector2i &pixel) const {
         const uint32_t *vec = reinterpret_cast<const uint32_t *>(pixel.data());
         return (vec[0] * 73856093 ^ vec[1] * 19349669);
     }
 };
 
-void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &pointcloud) {
+void TransformPoints(const Sophus::SE3d &T, Vector3dVector &pointcloud) {
     std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(),
-                   [&](const auto &point) { return T * point; });
+                   [&](const Eigen::Vector3d &point) { return T * point; });
 }
 
-using LinearSystem = std::pair<Eigen::Matrix3d, Eigen::Vector3d>;
-LinearSystem BuildLinearSystem(const std::vector<Eigen::Vector3d> &points,
-                               const double resolution) {
-    auto compute_jacobian_and_residual = [](const auto &point) {
+struct VoxelMeanAndNormal {
+    Eigen::Vector3d mean;
+    Eigen::Vector3d normal;
+};
+
+auto PointToPixel = [](const Eigen::Vector3d &pt) -> Eigen::Vector2i {
+    return Eigen::Vector2i(static_cast<int>(std::floor(pt.x())),
+                           static_cast<int>(std::floor(pt.y())));
+};
+
+std::pair<Vector3dVector, Sophus::SE3d> SampleGroundPoints(const Vector3dVector &voxel_means,
+                                                           const Vector3dVector &voxel_normals) {
+    std::unordered_map<Eigen::Vector2i, VoxelMeanAndNormal, PixelHash> lowest_voxel_hash_map;
+
+    for (size_t index = 0; index < voxel_means.size(); ++index) {
+        const Eigen::Vector3d &mean = voxel_means[index];
+        const Eigen::Vector3d &normal = voxel_normals[index];
+        const Eigen::Vector2i pixel = PointToPixel(mean);
+
+        auto it = lowest_voxel_hash_map.find(pixel);
+        if (it == lowest_voxel_hash_map.end()) {
+            lowest_voxel_hash_map.emplace(pixel, VoxelMeanAndNormal{mean, normal});
+        } else if (mean.z() < it->second.mean.z()) {
+            it->second = VoxelMeanAndNormal{mean, normal};
+        }
+    }
+
+    std::vector<VoxelMeanAndNormal> low_lying_voxels(lowest_voxel_hash_map.size());
+    std::transform(lowest_voxel_hash_map.cbegin(), lowest_voxel_hash_map.cend(),
+                   low_lying_voxels.begin(), [](const auto &entry) { return entry.second; });
+
+    const Eigen::Matrix3d covariance_matrix =
+        std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(),
+                              Eigen::Matrix3d().setZero(), std::plus<Eigen::Matrix3d>(),
+                              [&](const VoxelMeanAndNormal &voxel) {
+                                  return voxel.normal * voxel.normal.transpose();
+                              }) /
+        static_cast<double>(low_lying_voxels.size() - 1);
+
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance_matrix);
+    Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors().col(2);
+    largest_eigenvector =
+        (largest_eigenvector.z() < 0) ? -1.0 * largest_eigenvector : largest_eigenvector;
+    const Eigen::Vector3d axis = largest_eigenvector.cross(Eigen::Vector3d(0.0, 0.0, 1.0));
+    const double angle = std::acos(std::clamp(largest_eigenvector.z(), -1.0, 1.0));
+    const double axis_norm = axis.norm();
+
+    const Eigen::Matrix3d R = axis_norm > 1e-3
+                                  ? Eigen::AngleAxisd(angle, axis / axis_norm).toRotationMatrix()
+                                  : Eigen::Matrix3d::Identity();
+
+    Eigen::Vector3d ground_centroid(0.0, 0.0, 0.0);
+    Vector3dVector ground_samples;
+    ground_samples.reserve(low_lying_voxels.size());
+    std::for_each(low_lying_voxels.cbegin(), low_lying_voxels.cend(),
+                  [&](const VoxelMeanAndNormal &voxel) {
+                      if (std::abs(voxel.normal.dot(largest_eigenvector)) > 0.95) {
+                          ground_centroid += voxel.mean;
+                          ground_samples.emplace_back(voxel.mean);
+                      }
+                  });
+    ground_samples.shrink_to_fit();
+    ground_centroid /= static_cast<double>(ground_samples.size());
+
+    const double z_shift = R.row(2) * ground_centroid;
+    const Sophus::SE3d T_init(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift));
+    return std::make_pair(ground_samples, T_init);
+}
+
+LinearSystem BuildLinearSystem(const Vector3dVector &points) {
+    auto compute_jacobian_and_residual = [](const Eigen::Vector3d &point) {
         const double residual = point.z();
         Eigen::Matrix<double, 1, 3> J;
         J(0, 0) = 1.0;
@@ -65,55 +137,31 @@ LinearSystem BuildLinearSystem(const std::vector<Eigen::Vector3d> &points,
     const auto &[H, b] =
         std::transform_reduce(points.cbegin(), points.cend(),
                               LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()),
-                              sum_linear_systems, [&](const auto &point) {
+                              sum_linear_systems, [&](const Eigen::Vector3d &point) {
                                   const auto &[J, residual] = compute_jacobian_and_residual(point);
-                                  const double w = std::abs(residual) <= resolution ? 1.0 : 0.0;
+                                  const double w = std::exp(-1.0 * residual * residual);
                                   return LinearSystem(J.transpose() * w * J,          // JTJ
                                                       J.transpose() * w * residual);  // JTr
                               });
     return {H, b};
 }
 
 static constexpr double convergence_threshold = 1e-3;
-static constexpr int max_iterations = 20;
-
-std::vector<Eigen::Vector3d> ComputeLowestPoints(const std::vector<Eigen::Vector3d> &pointcloud,
-                                                 const double resolution) {
-    std::unordered_map<Eigen::Vector2i, Eigen::Vector3d, PixelHash> lowest_point_hash_map;
-    auto PointToPixel = [&resolution](const Eigen::Vector3d &pt) -> Eigen::Vector2i {
-        return Eigen::Vector2i(static_cast<int>(std::floor(pt.x() / resolution)),
-                               static_cast<int>(std::floor(pt.y() / resolution)));
-    };
-
-    std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3d &point) {
-        const auto &pixel = PointToPixel(point);
-        if (lowest_point_hash_map.find(pixel) == lowest_point_hash_map.cend()) {
-            lowest_point_hash_map.emplace(pixel, point);
-        } else if (point.z() < lowest_point_hash_map[pixel].z()) {
-            lowest_point_hash_map[pixel] = point;
-        }
-    });
-
-    std::vector<Eigen::Vector3d> low_lying_points(lowest_point_hash_map.size());
-    std::transform(lowest_point_hash_map.cbegin(), lowest_point_hash_map.cend(),
-                   low_lying_points.begin(), [](const auto &entry) { return entry.second; });
-    return low_lying_points;
-}
+static constexpr int max_iterations = 10;
 }  // namespace
 
 namespace map_closures {
-Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &pointcloud,
-                                   const double resolution) {
-    Sophus::SE3d T = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero());
-    auto low_lying_points = ComputeLowestPoints(pointcloud, resolution);
-
+Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means,
+                                   const Vector3dVector &voxel_normals) {
+    auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals);
+    TransformPoints(T, ground_samples);
     for (int iters = 0; iters < max_iterations; iters++) {
-        const auto &[H, b] = BuildLinearSystem(low_lying_points, resolution);
+        const auto &[H, b] = BuildLinearSystem(ground_samples);
         const Eigen::Vector3d &dx = H.ldlt().solve(-b);
         Eigen::Matrix<double, 6, 1> se3 = Eigen::Matrix<double, 6, 1>::Zero();
         se3.block<3, 1>(2, 0) = dx;
         Sophus::SE3d estimation(Sophus::SE3d::exp(se3));
-        TransformPoints(estimation, low_lying_points);
+        TransformPoints(estimation, ground_samples);
         T = estimation * T;
         if (dx.norm() < convergence_threshold) break;
     }

cpp/map_closures/GroundAlign.hpp

@@ -27,6 +27,6 @@
 #include <vector>
 
 namespace map_closures {
-Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &pointcloud,
-                                   const double resolution);
+Eigen::Matrix4d AlignToLocalGround(const std::vector<Eigen::Vector3d> &voxel_means,
+                                   const std::vector<Eigen::Vector3d> &voxel_normals);
 }  // namespace map_closures