pcl

include/small_gicp/ann/traits.hpp

@@ -9,6 +9,11 @@ namespace traits {
 template <typename T>
 struct Traits;
 
+template <typename T>
+size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) {
+  return Traits<T>::nearest_neighbor_search(tree, point, k_index, k_sq_dist);
+}
+
 template <typename T>
 size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
   return Traits<T>::knn_search(tree, point, k, k_indices, k_sq_dists);

include/small_gicp/points/pcl_point.hpp

@@ -7,7 +7,7 @@ namespace pcl {
 using Matrix4fMap = Eigen::Map<Eigen::Matrix4f, Eigen::Aligned>;
 using Matrix4fMapConst = const Eigen::Map<const Eigen::Matrix4f, Eigen::Aligned>;
 
-struct EIGEN_ALIGN16 PointCovariance {
+struct PointCovariance {
   PCL_ADD_POINT4D;
   Eigen::Matrix4f cov;
 
@@ -17,7 +17,7 @@ struct EIGEN_ALIGN16 PointCovariance {
   PCL_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-struct EIGEN_ALIGN16 PointNormalCovariance {
+struct PointNormalCovariance {
   PCL_ADD_POINT4D;
   PCL_ADD_NORMAL4D
   Eigen::Matrix4f cov;

include/small_gicp/util/read_points.hpp

@@ -18,6 +18,9 @@ static std::vector<Eigen::Vector4f> read_points(const std::string& filename) {
   ifs.seekg(0, std::ios::beg);
   std::vector<Eigen::Vector4f> points(num_points);
   ifs.read(reinterpret_cast<char*>(points.data()), sizeof(Eigen::Vector4f) * num_points);
+  for (auto& pt : points) {
+    pt(3) = 1.0;
+  }
 
   return points;
 }

src/small_gicp_test.cpp

@@ -1,10 +1,10 @@
 #include <iostream>
 #include <filesystem>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 #include <small_gicp/util/read_points.hpp>
 
-#include <small_gicp/points/pcl_point.hpp>
-#include <small_gicp/points/pcl_point_traits.hpp>
 #include <small_gicp/points/point_cloud.hpp>
 #include <small_gicp/points/gaussian_voxelmap.hpp>
 #include <small_gicp/ann/kdtree.hpp>
@@ -21,7 +21,6 @@
 #include <small_gicp/factors/plane_icp_factor.hpp>
 
 #include <glk/io/ply_io.hpp>
-#include <glk/pointcloud_buffer_pcl.hpp>
 #include <glk/normal_distributions.hpp>
 #include <guik/viewer/light_viewer.hpp>
 
@@ -81,15 +80,17 @@ int main(int argc, char** argv) {
     voxelmap->insert(*points, T);
 
     prof.push("show");
-    viewer->update_points("current", points->points, guik::FlatOrange(T).set_point_scale(2.0f));
+    // viewer->update_points("current", raw_points[0].data(), sizeof(float) * 4, raw_points.size(), guik::FlatOrange(T).set_point_scale(2.0f));
 
-    std::vector<Eigen::Vector4d> means;
+    // std::vector<Eigen::Vector4d> means;
-    std::vector<Eigen::Matrix4d> covs;
+    // std::vector<Eigen::Matrix4d> covs;
-    for (const auto& voxel : voxelmap->flat_voxels) {
+    // for (const auto& voxel : voxelmap->flat_voxels) {
-      means.emplace_back(voxel.mean);
+    //   means.emplace_back(voxel.mean);
-      covs.emplace_back(voxel.cov);
+    //   covs.emplace_back(voxel.cov);
-    }
+    // }
-    viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
+    // viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
+
+    std::cout << "--- T ---" << std::endl << T.matrix() << std::endl;
 
     if (!viewer->spin_once()) {
       break;