change accessing order for column major (#112)

Author: koide3

include/small_gicp/ann/projective_search.hpp

@@ -110,15 +110,15 @@ struct UnsafeProjectiveSearch {
     const int u = uv[0] * index_map.cols();
     const int v = uv[1] * index_map.rows();
 
-    for (int dv = -search_window_v; dv <= search_window_v; dv++) {
-      const int v_clamped = border_v(v + dv, index_map.rows());
-      if (v_clamped < 0 || v_clamped >= index_map.rows()) {
+    for (int du = -search_window_h; du <= search_window_h; du++) {
+      const int u_clamped = border_h(u + du, index_map.cols());
+      if (u_clamped < 0 || u_clamped >= index_map.cols()) {
         continue;
       }
 
-      for (int du = -search_window_h; du <= search_window_h; du++) {
-        const int u_clamped = border_h(u + du, index_map.cols());
-        if (u_clamped < 0 || u_clamped >= index_map.cols()) {
+      for (int dv = -search_window_v; dv <= search_window_v; dv++) {
+        const int v_clamped = border_v(v + dv, index_map.rows());
+        if (v_clamped < 0 || v_clamped >= index_map.rows()) {
           continue;
         }
 

include/small_gicp/points/eigen.hpp

@@ -2,6 +2,7 @@
 // SPDX-License-Identifier: MIT
 #pragma once
 
+#include <vector>
 #include <Eigen/Core>
 #include <small_gicp/points/traits.hpp>
 
@@ -15,5 +16,30 @@ struct Traits<Eigen::MatrixXd> {
   static Eigen::Vector4d point(const Eigen::MatrixXd& points, size_t i) { return Eigen::Vector4d(points(i, 0), points(i, 1), points(i, 2), 1.0); }
 };
 
+template <typename Scalar, int D>
+struct Traits<std::vector<Eigen::Matrix<Scalar, D, 1>>> {
+  static_assert(D == 3 || D == 4, "Only 3D and 4D (homogeneous) points are supported");
+  static_assert(std::is_floating_point<Scalar>::value, "Only floating point types are supported");
+
+  using Point = Eigen::Matrix<Scalar, D, 1>;
+  static size_t size(const std::vector<Point>& points) { return points.size(); }
+  static bool has_points(const std::vector<Point>& points) { return points.size(); }
+  static Eigen::Vector4d point(const std::vector<Point>& points, size_t i) {
+    if constexpr (std::is_same_v<Scalar, double>) {
+      if constexpr (D == 3) {
+        return points[i].homogeneous();
+      } else {
+        return points[i];
+      }
+    } else {
+      if constexpr (D == 3) {
+        return points[i].homogeneous().template cast<double>();
+      } else {
+        return points[i].template cast<double>();
+      }
+    }
+  }
+};
+
 }  // namespace traits
 }  // namespace small_gicp