参考:http://www.voidcn.com/article/p-aovciemr-bpr.html
步骤
对无序点云在空间变化上的检测,其实是对前后两幅点云在八叉树结构下的差异检测。因此我们要首先载入一个原始点云,并生成第一个八叉树结构;然后切缓冲,载入第二个点云,生成第二个八叉树结构;最后进行比较,如果有一些叶子结点在第二个八叉树上,但是不在第一个八叉树上,那么就认为这些叶子节点内的点是空间上变化多出来的点。
思路
读取一个点云并复制,在赋值的点云中添加1000个随机点,对两个点云进行0ctree检测,找出新增的点,并将三个点云可视化
代码
#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
//octree_change_detction
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
int main(int argc, char** argv) {
srand((unsigned int) time(NULL));//随机种子
//Octree resolution - side length of octree voxels
float resolution = 15.0f;
//实例化基于octree的点云检测类
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);//创建检测类的对象
//创建两个XYZ点云指针,想办法将文件中的数据赋值给该指针
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud3 (new pcl::PointCloud<pcl::PointXYZ>);//用来类型转换的
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result (new pcl::PointCloud<pcl::PointXYZ>);//使用这个指针将获得的索引强制转换并输出
//加载点云文件给cloud指针
if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd",*cloud1)==-1)//??????
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return(-1);
}
std::cout<<"loaded room_scan1.pcd " <<cloud1->size() <<std::endl;
//改造原始点云
pcl::PointCloud<pcl::PointXYZ> handledCloud(*cloud1);//拷贝复制
//随机增加10000个点云,1024.f * rand() / (RAND_MAX + 1.0f)中的1024控制点云的范围
for(int i = 0;i < 10000; i++)
{
handledCloud.push_back(
pcl::PointXYZ(50.0f * rand() / (RAND_MAX + 1.0f), 50.0f * rand() / (RAND_MAX + 1.0f), 1.0f * rand() / (RAND_MAX + 1.0f))
);
}
std::cout<<"handledCloud points loaded "<<handledCloud.size()<<endl;
//pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan2.pcd",*cloud2);
//std::cout<<"loaded maize1"<<std::endl;
//add cloud_point from cloud1 to octree
octree.setInputCloud(cloud1);//输入点云cloud1
octree.addPointsFromInputCloud();//从输入点云cloud1构建八叉树
//Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers();//交换八叉树缓存,但是cloud1对应的八叉树结构仍在内存中
//add points from cloud2 to octree
octree.setInputCloud(handledCloud.makeShared());//makeShared()返回一个指针
octree.addPointsFromInputCloud();//从输入点云cloud2构建八叉树
std::vector<int> newPointIdxVector;//存储新加点的索引的向量
//Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels(newPointIdxVector);//获取新增点的索引
//将新增点的放到cloud_result所指向的内存中
cloud_result->width = 50000;
cloud_result->height = 1;
cloud_result->is_dense = false;
cloud_result->points.resize(cloud_result->width * cloud_result->height);
for(size_t i = 0;i < newPointIdxVector.size();++i)
{
cloud_result->points[i].x = handledCloud.points[newPointIdxVector[i]].x;
cloud_result->points[i].y = handledCloud.points[newPointIdxVector[i]].y;
cloud_result->points[i].z = handledCloud.points[newPointIdxVector[i]].z;
}
//保存文件
pcl::io::savePCDFileASCII("result.pcd",*cloud_result);//将计算结果存放到result.pcd
std::cout<<"result.pcd saved "<< cloud_result->size()<< "points"<<endl;
//不转换直接输出2
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
std::cout << newPointIdxVector.size () <<" points changed "<<std::endl;
for (size_t i = 0; i < newPointIdxVector.size (); ++i)
std::cout << i << "# Index:" << newPointIdxVector[i]
<< " Point:" << handledCloud.points[newPointIdxVector[i]].x << " "
<< handledCloud.points[newPointIdxVector[i]].y << " "
<< handledCloud.points[newPointIdxVector[i]].z << std::endl;
std::cout<<"-----------------------------result end-------------------------"<<std::endl;
//指针赋值
//pcl::io::loadPCDFile("result.pcd",*cloud3);//将新得到的result.pcd赋值给*cloud_3,读取
//可视化模块
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewwe"));//创建指针viewer
viewer->initCameraParameters();//初始化相机参数
int v1(0);//第二个窗口的参数
viewer->createViewPort(0.0,0.0,0.33,1,v1); //设置第一个窗口的大小,位于屏幕左侧
viewer->setBackgroundColor(255,0,255,v1);//background of first port
viewer->addText("cloud1",10,10,"cloud1",v1);//好像是一个便签
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud1,0,255,0);//设置第一个点云的颜色
viewer->addPointCloud<pcl::PointXYZ>(cloud1,single_color1,"simple_cloud1",v1);//显示第一个点云
int v2(0);//第一个窗口的参数
viewer->createViewPort(0.33,0,0.66,1,v2);//设置第二个窗口的大小,位于屏幕右侧
viewer->setBackgroundColor(0,255,255,v2);//background of second port
viewer->addText("handledCloud",10,9,"handledCloud",v2);//输出一行文字
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(handledCloud.makeShared(),255,1,1);//设置第二个点云的颜色
viewer->addPointCloud<pcl::PointXYZ>(handledCloud.makeShared(),single_color2,"simple_cloud2",v2);//显示第二个点云
//第三个点云窗口
int v3(0);//第三个窗口的参数
viewer->createViewPort(0.66,0,1,1,v3);//窗口大小
viewer->setBackgroundColor(0,0,0,v3);//背景颜色
viewer->addText("cloud_result",10,8,"cloud_result",v3);//好像是一个便签
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud_result,255,255,255);//点云颜色
viewer->addPointCloud<pcl::PointXYZ>(cloud_result,single_color3,"simple_cloud3",v3);//显示点云
//viewer->createViewPort();
viewer->addCoordinateSystem(2);//添加坐标系
// while(!viewer->wasStopped())
// viewer->spinOnce(100);
viewer->spin();
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(poind_cloud_change_detected)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
set(CMAKE_CXX_STANDARD 11)
add_executable(${PROJECT_NAME} main.cpp)
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
结果
遇到的问题
1、不能检测到新增点,最后输出点的时候显示没有新增点0个
解决:随机点赋值的时候没有加上"f"(原本为64.0f,写成了64)
2、读取pcd文件失败
解决:文件中的点的SIZE和实际的点的数目不匹配
3、随机点赋值的时候64.0f 代表长宽高,如果随机点的体积过小(在原点云内部)是检测不出来的
4、新增点是通过push_back()函数添加的。
// 改造原始点云
pcl::PointCloud<pcl::PointXYZ> handledCloud(sourceCloud); // 拷贝复制
// 随机增加100个点
for (int i = 0; i < 100000; i++)
{
handledCloud.push_back(
pcl::PointXYZ(1.0f*rand() / (RAND_MAX + 1.0f),
10.0f*rand() / (RAND_MAX + 1.0f),
10.0f*rand() / (RAND_MAX + 1.0f)));
}
5、最后输出检测到的点是通过强制类型转换为points类型再输出的
//不转换直接输出2
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
std::cout << newPointIdxVector.size () <<" points changed "<<std::endl;
for (size_t i = 0; i < newPointIdxVector.size (); ++i)
std::cout << i << "# Index:" << newPointIdxVector[i]
<< " Point:" << handledCloud.points[newPointIdxVector[i]].x << " "
<< handledCloud.points[newPointIdxVector[i]].y << " "
<< handledCloud.points[newPointIdxVector[i]].z << std::endl;
std::cout<<"-----------------------------result end-------------------------"<<std::endl;