mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

pcl

This commit is contained in:
k.koide 2024-03-22 20:20:11 +09:00
parent 3b0f422bcb
commit e09e5b8aea
4 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/small_gicp/ann/traits.hpp
View File

@ -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);

4
include/small_gicp/points/pcl_point.hpp
View File

@ -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;

3
include/small_gicp/util/read_points.hpp
View File

@ -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;
}

23
src/small_gicp_test.cpp
View File

@ -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::Matrix4d> covs;
for (const auto& voxel : voxelmap->flat_voxels) {
means.emplace_back(voxel.mean);
covs.emplace_back(voxel.cov);
}
viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
// std::vector<Eigen::Vector4d> means;
// std::vector<Eigen::Matrix4d> covs;
// for (const auto& voxel : voxelmap->flat_voxels) {
// means.emplace_back(voxel.mean);
// covs.emplace_back(voxel.cov);
// }
// 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;