还没有自己测试,在郭浩老师的书中看见了这个例程,先记下来。
应用RANSAC算法实现基于球型靶标的自动标定方法,连个深度摄像头获取的点云数据中检测并识别出各个圆球,检测识别出3个圆球后,拥有两组对应的3个坐标点,根据变换矩阵求解方程,估计出变换矩阵,实现双深度摄像头的标定。
包括数据获取模块、直通滤波模块、平面检测及删除模块、圆球检测模块、刚体变换矩阵估计模块。
(1)数据获取模块
从不同方向采集3个小球的场景点云,使用Grabber接口。
(2)直通滤波模块
采集到的点云通常包含Z轴方向2m以外的点云,可以使用直通滤波的方式滤除Z轴方向的点云数据。
(3)平面检测及删除模块
直通滤波后的点云有近似于平面形状的地面点云,利用SACSegmentation类检测地面所在平面点云,并删除,其中setModelType()函数设置模型类型为平面,setMethodType()函数设置为随机采样一致性。
pcl::SACSegmentation<pcl::PointXYZRGB>seg;
seg.setModelType(pcl::SANMODEL_PLANE);//平面模型
seg.setMethodType(pcl::SAC_RANSAC);
(4)删除场景地面点云数据后,创建SACSegmentation对象检测圆球,设置检测形状及相关参数;
pcl::SACSegmentation<pcl::PointXYZRGB>seg2;
seg2.setOptimizeCoefficients(true);
seg2.setModelType(pcl::SACMODEL_SPHERE);//球模型
seg2.setMethodType(pcl::SAC_RANSAC);
seg2.setMaxIterations(100);//迭代次数
seg2.setDistanceThreshold(0.01);
循环检测,直到把3个球表面点云都检测到,并记录三个圆球的球心
for(int i = 0;i < 3; i++)
{
pcl::ModelCoefficients coefficients2;
seg2.setInputCloud(cloud_filtered);
seg2.segment(*tmpinliers2,coefficients2);
coefficients_all.push_back(coefficients2);
pcl::ExtractIndices<pcl::PointXYZRGB>extract2;
extract2.setInputCloud(cloud_filtered);
extract2.setIndices(tmpinliers2);
extract2.setNegative(false);//内点
extract2.filter(*cloud_sphere);
viewer.showCloud(cloud_sphere);
extract2.setNegative(true);//外点
extract2.filter(*cloud_f2);
cloud_filtered = cloud_f2;//剩下点云再检测
}
(5)刚体变换矩阵估计
基于3对对应点,利用TransformationFromCorrespondences类估计变换矩阵,利用transformationFormCorr类的add函数添加对应点,利用getTransformation函数获取变换矩阵,实现双深度摄像头标定。