PCL中ICP配准点云(计算出RT,并且设置不同颜色来显示三个点云)
来源:互联网 发布:淘宝代购链接怎么做 编辑:程序博客网 时间:2024/06/11 14:27
根据PCL英语官网教程(http://pointclouds.org/documentation/tutorials/)ICP两个例子更改:
PCL中ICP配准点云(计算出RT,并且设置不同颜色来显示三个点云),代码如下:
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
include <iostream>
void
print4x4Matrix(const Eigen::Matrix4d & matrix)
{
printf("Rotation matrix :\n");
printf(" | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
printf(" | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
printf("Translation vector :\n");
printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("0XYZ.pcd", *cloud_in);//cloud_in:初始PCD点云
pcl::io::loadPCDFile("00XYZ.pcd", *cloud_out); //cloud_out:目标PCD点云
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//icp.setMaximumIterations(25);
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
PointCloudT::Ptr cloud_icp(new PointCloudT); //cloud_icp: ICP生成的结果点云
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
transformation_matrix = icp.getFinalTransformation().cast<double>();
pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
////////////////////////////以下是三个pcd分别加入颜色并且合并成一个,并显示:
PointCloud::Ptr MergeCloud(new PointCloud);
for (int j = 0; j < cloud_in->points.size(); j += 1)
{
pcl::PointXYZRGB p;
p.x = cloud_in->points[j].x;
p.y = cloud_in->points[j].y;
p.z = cloud_in->points[j].z;
p.r = 255;//红色
p.g = 0;
p.b = 0;
MergeCloud->points.push_back(p);
}
for (int j = 0; j < cloud_out->points.size(); j += 1)
{
pcl::PointXYZRGB p;
p.x = cloud_out->points[j].x;
p.y = cloud_out->points[j].y;
p.z = cloud_out->points[j].z;
p.r = 0;//黑色
p.g = 0;
p.b = 0;
MergeCloud->points.push_back(p);
}
for (int j = 0; j < cloud_icp->points.size(); j += 1)
{
pcl::PointXYZRGB p;
p.x = cloud_icp->points[j].x;
p.y = cloud_icp->points[j].y;
p.z = cloud_icp->points[j].z;
p.r = 150;//灰色
p.g = 150;
p.b = 150;
MergeCloud->points.push_back(p);
}
// 设置并保存点云
MergeCloud->height = 1;
MergeCloud->width = MergeCloud->points.size();
MergeCloud->is_dense = false;
pcl::io::savePCDFile("ICPmerge3-in-out-icp.pcd", *MergeCloud);
// 清除数据并退出
MergeCloud->points.clear();
std::cout << "cloud_in(input1-0XYZ):" << "红色" << std::endl;
std::cout << "cloud_out(input2-00XYZ):" << "黑色" << std::endl;
std::cout << "cloud_icp(icp-result):" << "灰色" << std::endl;
return (0);
}
- PCL中ICP配准点云(计算出RT,并且设置不同颜色来显示三个点云)
- PCL 学习笔记-使用 PCL 来显示点云
- PCL:点云的可视化(程序中在线显示)
- PCL可视化显示点云
- PCL显示点云[微记]
- PCL点云的显示
- PCL可视化显示点云
- PCL中RANSAC使用,点云平面检测,显示,存储
- ply格式点云在PCL中显示
- PCL中计算点云的法向量并显示
- PCL同时在一个窗口中显示两个(多个)点云
- pcl点云显示(ubuntu14.04下qt打开)
- PCL:点云的法线估计(并显示)
- PCL+VS2010+Qt显示点云
- pcl点云mfc单文档显示
- PCL+QT+VS显示点云
- KinectV2 qt pcl 实现点云显示
- PCL编程->三维彩色点云显示
- django中 authenticate返回None的情况解决办法
- Android测试--Monkey
- Bootstrap网格系统
- Feign Hystrix 支持
- A^B Mod C(快速幂算法)
- PCL中ICP配准点云(计算出RT,并且设置不同颜色来显示三个点云)
- 双关系递推
- 【Java学习笔记】Comparable接口的实现和使用
- linux 安装netcdf 库
- 悼念512汶川大地震遇难同胞——珍惜现在,感恩生活(HDU-2191)
- 01背包问题
- SpringMVC+Hibernate+Junit4+json基本框架近乎0配置
- git submodule
- Bootstrap菜单、导航、按钮