|
| 1 | +// |
| 2 | +// Tutorial Author: shapelim@mit.edu (임형태) |
| 3 | +// |
| 4 | +// Runs ICP/GICP/Normal pipelines once and writes their inputs/outputs as PCD |
| 5 | +// files into web/public/precomputed/ for the React tutorial site to load. |
| 6 | + |
| 7 | +#include <filesystem> |
| 8 | +#include <iostream> |
| 9 | +#include <pcl/point_types.h> |
| 10 | +#include <pcl/io/pcd_io.h> |
| 11 | +#include <pcl/common/transforms.h> |
| 12 | +#include <pcl/registration/icp.h> |
| 13 | +#include <pcl/registration/gicp.h> |
| 14 | + |
| 15 | +namespace fs = std::filesystem; |
| 16 | + |
| 17 | +static pcl::PointCloud<pcl::PointXYZ>::Ptr load_kitti_bin( const std::string &filename) { |
| 18 | + FILE *file = fopen(filename.c_str(), "rb"); |
| 19 | + if (!file) { |
| 20 | + std::cerr << "[export] failed to open " << filename << std::endl; |
| 21 | + return nullptr; |
| 22 | + } |
| 23 | + std::vector<float> buffer(1'000'000); |
| 24 | + size_t num_points = |
| 25 | + fread(reinterpret_cast<char *>(buffer.data()), sizeof(float), buffer.size(), file) / 4; |
| 26 | + fclose(file); |
| 27 | + |
| 28 | + pcl::PointCloud<pcl::PointXYZ>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZ>()); |
| 29 | + cloud->resize(num_points); |
| 30 | + for (size_t i = 0; i < num_points; ++i) { |
| 31 | + cloud->points[i].x = buffer[i * 4]; |
| 32 | + cloud->points[i].y = buffer[i * 4 + 1]; |
| 33 | + cloud->points[i].z = buffer[i * 4 + 2]; |
| 34 | + } |
| 35 | + return cloud; |
| 36 | +} |
| 37 | + |
| 38 | +template <typename Reg> |
| 39 | +static void run_registration(const std::string &name, |
| 40 | + Reg ®, |
| 41 | + pcl::PointCloud<pcl::PointXYZ>::Ptr src, |
| 42 | + pcl::PointCloud<pcl::PointXYZ>::Ptr tgt, |
| 43 | + const fs::path &outDir) { |
| 44 | + reg.setMaxCorrespondenceDistance(1.0); |
| 45 | + reg.setTransformationEpsilon(0.003); |
| 46 | + reg.setMaximumIterations(1000); |
| 47 | + reg.setInputSource(src); |
| 48 | + reg.setInputTarget(tgt); |
| 49 | + |
| 50 | + pcl::PointCloud<pcl::PointXYZ>::Ptr aligned( new pcl::PointCloud<pcl::PointXYZ>); |
| 51 | + reg.align(*aligned); |
| 52 | + |
| 53 | + pcl::io::savePCDFileBinary((outDir / (name + "_src.pcd")).string(), *src); |
| 54 | + pcl::io::savePCDFileBinary((outDir / (name + "_tgt.pcd")).string(), *tgt); |
| 55 | + pcl::io::savePCDFileBinary((outDir / (name + "_aligned.pcd")).string(), *aligned); |
| 56 | + |
| 57 | + std::cout << "[export] " << name |
| 58 | + << " fitness=" << reg.getFitnessScore() |
| 59 | + << " converged=" << reg.hasConverged() << std::endl; |
| 60 | +} |
| 61 | + |
| 62 | +int main(int argc, char **argv) { |
| 63 | + fs::path outDir = (argc > 1) ? fs::path(argv[1]) |
| 64 | + : fs::path("../web/public/precomputed"); |
| 65 | + fs::create_directories(outDir); |
| 66 | + std::cout << "[export] writing to " << outDir << std::endl; |
| 67 | + |
| 68 | + auto src = load_kitti_bin("./auxiliary/kitti00_000000.bin"); |
| 69 | + if (!src) return 1; |
| 70 | + |
| 71 | + // Synthetic target = src translated +2m along x. |
| 72 | + pcl::PointCloud<pcl::PointXYZ>::Ptr tgt( new pcl::PointCloud<pcl::PointXYZ>); |
| 73 | + Eigen::Matrix4f tf = Eigen::Matrix4f::Identity(); |
| 74 | + tf(0, 3) = 2.0f; |
| 75 | + pcl::transformPointCloud(*src, *tgt, tf); |
| 76 | + |
| 77 | + { |
| 78 | + pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; |
| 79 | + run_registration("icp", icp, src, tgt, outDir); |
| 80 | + } |
| 81 | + { |
| 82 | + pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp; |
| 83 | + run_registration("gicp", gicp, src, tgt, outDir); |
| 84 | + } |
| 85 | + |
| 86 | + std::cout << "[export] done" << std::endl; |
| 87 | + return 0; |
| 88 | +} |
0 commit comments