前言
在机器人比赛中时常会需要涉及到对圆柱进行识别，机器人的运动姿态需要根据识别出的圆柱中心点来进行调整。因此，设计算法来识别中心点是非常必要的。
笔者利用Kinect相机得到了深度图和彩色图，首先利用OpenCV来确定特殊颜色的连通域，其次利用深度图和彩色图构建PCL点云并得到拟合圆柱的参数，最终使用了点云——中心轴线投影法确定出最终的圆柱中心点的位置。
接下来让我们一起来看一下具体的操作。
一、OpenCV确定特定区域的连通域
首先我们来思考一下，既然有了整张图的color_image和depth_image，我们为什么不直接构建点云呢？因为这样会引入大量的噪声，以至于拟合效果极差甚至根本得不到结果；并且遍历全图的代价太高，性能和帧率会很差。
为了解决上述的问题，我们首先对全图进行颜色滤波，即用特定的颜色区间过滤出我们想要的点，再通过设置面积和距离条件（深度图的信息），筛选出真正有效的像素信息。
常见的方法是将GBRA的格式转换为GBR再转换为HSV来进行处理，这里有一个小技巧，我们可以设置几个参数滑动条来手动的调整参数，十分的直观。
int Hlow=136,Hhigh=182;
int Hlowb=94,Hhighb=160;
int Slow=160,Shigh=284;
int Vlow=89,Vhigh=222;
cv::Mat red_origin = *color_image;
cv::Mat red_image = *color_image;
cv::Mat blue_origin = *color_image;
cv::Mat blue_image = *color_image;
cv::Mat hsv_red,hsv_blue,red_final,blue_final;
cv::cvtColor(red_image, red_image, cv::COLOR_BGRA2BGR);
cv::cvtColor(red_origin, red_origin, cv::COLOR_BGRA2BGR);
cv::cvtColor(red_image, hsv_red, cv::COLOR_BGR2HSV);
cv::cvtColor(blue_image, red_image, cv::COLOR_BGRA2BGR);
cv::cvtColor(blue_origin, red_origin, cv::COLOR_BGRA2BGR);
cv::cvtColor(blue_image, hsv_blue, cv::COLOR_BGR2HSV);
cv::Mat blue_mask, red_mask0,red_mask1;
// cv::namedWindow("trackbar", cv::WINDOW_AUTOSIZE);
// cv::createTrackbar("Hlow", "trackbar", &Hlow, 360);
// cv::createTrackbar("Hhigh", "trackbar", &Hhigh, 360);
// cv::createTrackbar("Hlowb", "trackbar", &Hlowb, 360);
// cv::createTrackbar("Hhighb", "trackbar", &Hhighb, 360);
// cv::createTrackbar("Slow", "trackbar", &Slow, 360);
// cv::createTrackbar("Shigh", "trackbar", &Shigh, 360);
// cv::createTrackbar("Vlow", "trackbar", &Vlow, 360);
// cv::createTrackbar("Vhigh", "trackbar", &Vhigh, 360);
cv::Scalar red_scalarL_1 = cv::Scalar(136,160, 89);
cv::Scalar red_scalarH_1 = cv::Scalar(182, 284,222);
cv::Scalar red_scalarL_2 = cv::Scalar(155,160, 89);
cv::Scalar red_scalarH_2 = cv::Scalar(360, 284,222);
// cv::Scalar blue_scalarL = cv::Scalar(93,160, 89);
// cv::Scalar blue_scalarH = cv::Scalar(149,284, 222);
cv::Scalar blue_scalarL = cv::Scalar(101,160,89);
cv::Scalar blue_scalarH = cv::Scalar(114,284,222);
cv::inRange(hsv_red, red_scalarL_1, red_scalarH_1,red_mask0);
cv::inRange(hsv_red, red_scalarL_2, red_scalarH_2,red_mask1);
red_final = red_mask0 + red_mask1;
cv::inRange(hsv_blue, blue_scalarL, blue_scalarH, blue_mask);
blue_final = blue_mask;设定好参数后，我们固定参数得到特定的Scalar,利用inRange进行过滤。注意像红色这种颜色在HSV体系下是分开的（请自行查阅HSV色域），因此需要加在一起（逻辑并）。那么这里我们需要分析并找到最大连通域，利用rectangle函数画框确定需要遍历的范围，作为点云输入。
腐蚀膨胀与连通域分析
Mat labels_red = Mat::zeros(red_final.size(), CV_32S);
Mat labels_blue = Mat::zeros(blue_final.size(), CV_32S);
//连通域分析
Mat stats_red, centroids_red;//统计信息存放
Mat stats_blue, centroids_blue;//统计信息存放
int num_labels_red = connectedComponentsWithStats(red_final, labels_red, stats_red, centroids_red, 8, CV_16U);
int num_labels_blue = connectedComponentsWithStats(blue_final, labels_blue, stats_blue, centroids_blue, 8, CV_16U);
int i1=1;
for (int j1 = 1; j1 < num_labels_red; j1++) {
if(stats_red.at<int>(i1, CC_STAT_AREA)<stats_red.at<int>(j1, CC_STAT_AREA))
i1=j1;
}
int i2=1;
for (int j2 = 1; j2 < num_labels_blue; j2++) {
if(stats_blue.at<int>(i2, CC_STAT_AREA)<stats_blue.at<int>(j2, CC_STAT_AREA))
i2=j2;
}
Vec2d pt1 = centroids_red.at<Vec2d>(i1, 0);
int x1 = stats_red.at<int>(i1, CC_STAT_LEFT);
int y1 = stats_red.at<int>(i1, CC_STAT_TOP);
int width1 = stats_red.at<int>(i1, CC_STAT_WIDTH);
int height1 = stats_red.at<int>(i1, CC_STAT_HEIGHT);
//int area1 = stats_red.at<int>(i1, CC_STAT_AREA);
//printf("red_area : %d, center point(%.2f, %.2f)\n", area1, pt1[0], pt1[1]);//面积信息
circle(red_origin, Point(pt1[0], pt1[1]), 2, Scalar(0, 0, 255), -1, 8, 0);//中心点坐标
rectangle(red_origin, Rect(x1, y1, width1, height1), Scalar(0, 0, 255), 5, 8, 0);//外接矩形
Vec2d pt2 = centroids_blue.at<Vec2d>(i2, 0);
int x2 = stats_blue.at<int>(i2, CC_STAT_LEFT);
int y2 = stats_blue.at<int>(i2, CC_STAT_TOP);
int width2 = stats_blue.at<int>(i2, CC_STAT_WIDTH);
int height2 = stats_blue.at<int>(i2, CC_STAT_HEIGHT);
//int area2 = stats_blue.at<int>(i2, CC_STAT_AREA);
//printf("blue_area : %d, center point(%.2f, %.2f)\n", area2, pt2[0], pt2[1]);//面积信息
circle(red_origin, Point(pt2[0], pt2[1]), 2, Scalar(0, 0, 255), -1, 8, 0);//中心点坐标
rectangle(red_origin, Rect(x2, y2, width2, height2), Scalar(255, 0, 0), 5, 8, 0);//外接矩形
//imshow("red",red_final);
//imshow("blue",blue_final);
imshow("连通域", red_origin);二、利用color_image和depth_image构建点云
这一部分非常的简单，我们直接采用循环即可。
//处理红色的点云
for(int y=y1; y<y1+height1; y++)
{
for(int x=x1; x<x1+width1; x++){
hitcrt::Kinectv3::Point flatPoint; //这个驱动特有的三维点的类型
// (x,y)是图像中心点的像素坐标，也可以换成你识别到的对应颜色区域的像素坐标
// 使用short访问深度图中的深度值的信息
if ((float)(*depth_image).at<short>(y, x) <= 0 || (float)(*depth_image).at<short>(y, x) >= 5000.0)
{
//贴心地帮您去掉丢深度信息点或者深度信息太远的点的深度值，深度信息单位为毫米mm
}
else
{
cam.get3DPoint(hitcrt::Kinectv3::Point((float)x, (float)y, (float)(*depth_image).at<short>(y, x)), flatPoint);
//从深度图图片的像素位置索引及其深度大小来获取三维点位置信息。
// flatPoint当中装的是3D点，3D点的单位为米m
pcl::PointXYZ p;
p.x = flatPoint.x;
p.y = flatPoint.y;
p.z = flatPoint.z;
cloud->points.push_back(p); //将点p保存到点云当中
}
}
}
//显示每一帧图像的点云个数
//std::cerr << "red_PointCloud has: " << cloud->size () << " data points." << std::endl;
//viewer.showCloud(cloud);//在这个窗口显示点云
三、利用PCL拟合圆柱并获得拟合圆柱的参数
有了点云，那么只需要利用PCL的流程即可迅速获得圆柱参数。当然，参数一共有7位，value[0],value[1],value[2]，是中轴线上的任意一点，value[3],value[4],value[5]，是中轴线的方向向量，value[6]是圆柱的半径。
那么如何拟合呢？请见之前发表的文章PCL点云初步_HIT-LQM的博客-CSDN博客
大致就是先拟合法线，再拟合平面，运用SAC_RANSAC方法来拟合圆柱
//利用pcl进行法线拟合
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud5_blue(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud5_blue(new pcl::PointCloud<pcl::PointXYZ>);
cloud5_blue = cloud_blue;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_blue(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2_blue(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_blue;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_blue(new pcl::search::KdTree<pcl::PointXYZ>());
ne_blue.setInputCloud(cloud5_blue);
ne_blue.setSearchMethod(tree_blue); //设置搜索方法
ne_blue.setKSearch (50);
ne_blue.compute(*cloud_normals_blue);
//拟合平面
pcl::ExtractIndices<pcl::PointXYZ> extract_blue;
pcl::ExtractIndices<pcl::Normal> extract_normals_blue;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_blue (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficents_blue(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_blue(new pcl::PointIndices);
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg_blue;
seg_blue.setOptimizeCoefficients(true);
seg_blue.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_blue.setNormalDistanceWeight (0.1);
seg_blue.setMethodType(pcl::SAC_RANSAC);
seg_blue.setMaxIterations (100);
seg_blue.setDistanceThreshold(0.03);
seg_blue.setInputCloud(cloud5_blue);
seg_blue.setInputNormals (cloud_normals_blue);
seg_blue.segment(*inliers_blue, *coefficents_blue);
//平面提取
pcl::copyPointCloud(*cloud5_blue, inliers_blue->indices, *output_cloud5_blue);
//从输入云中提取平面内点
extract_blue.setInputCloud(cloud5_blue);
extract_blue.setIndices(inliers_blue);
extract_blue.setNegative(false);
//将平面内点写入
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_blue (new pcl::PointCloud<pcl::PointXYZ> ());
extract_blue.filter (*cloud_plane_blue);
//std::cerr << "PointCloud representing the planar component: " << cloud_plane->size () << " data points." << std::endl;
//删除平面内点，提取其余部分
extract_blue.setNegative(true);
extract_blue.filter(*cloud_filtered_blue);
extract_normals_blue.setNegative(true);
extract_normals_blue.setInputCloud(cloud_normals_blue);
extract_normals_blue.setIndices(inliers_blue);
extract_normals_blue.filter(*cloud_normals2_blue);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud6_blue(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients_cylinder_blue(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_cylinder_blue(new pcl::PointIndices);
seg_blue.setOptimizeCoefficients(true);
seg_blue.setModelType(pcl::SACMODEL_CYLINDER); //设置成圆柱模型
seg_blue.setMethodType(pcl::SAC_RANSAC);
seg_blue.setNormalDistanceWeight(0.1); //法线在估计的权重
seg_blue.setMaxIterations(10000); //迭代次数
seg_blue.setDistanceThreshold(0.05);
seg_blue.setRadiusLimits(0, 0.3);
seg_blue.setInputCloud(cloud_filtered_blue);
seg_blue.setInputNormals(cloud_normals2_blue);
seg_blue.segment(*inliers_cylinder_blue, *coefficients_cylinder_blue);
pcl::copyPointCloud(*cloud5_blue, inliers_cylinder_blue->indices, *output_cloud6_blue);
std::cerr << "Cylinder_Blue: " << coefficients_cylinder_blue->values[6] << std::endl;四、计算中心点（待优化）
那么现在我只有一个点云、一条法线（任意一点及其方向向量），却要求找到中心坐标，这该如何实现呢？
苦思冥想，我设计出一种方案，那就是把所有的点云投影到中心轴线上，这样的话可以确定圆柱上下表面的中点start和end，这两个点的中点不就是问题的答案吗！
立即写出如下代码：
//根据中心轴线计算圆柱体中心点的位置
//将所有的点投影到中心轴线上，则可以计算出起点和终点，那么中心轴线线段上的中点即为圆柱体的中心点
Eigen::Vector3d axis_start_point_blue(coefficients_cylinder_blue->values[0], coefficients_cylinder_blue->values[1], coefficients_cylinder_blue->values[2]);
Eigen::Vector3f axis_direct_vec_blue(coefficients_cylinder_blue->values[3], coefficients_cylinder_blue->values[4], coefficients_cylinder_blue->values[5]);
pcl::PCA<PointT> pca_blue;
pca_blue.setInputCloud(output_cloud6_blue);
//修改第一成分方向向量(提高分量准确度，及分量沿着圆柱方向)
Eigen::Vector3f direct_vec_blue(coefficients_cylinder_blue->values[3], coefficients_cylinder_blue->values[4], coefficients_cylinder_blue->values[5]);
pca_blue.getEigenVectors().block<3, 1>(0, 0) = direct_vec_blue;
//获取圆柱点云在各个成分向量上的分量
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection_blue(new pcl::PointCloud<pcl::PointXYZ>);
pca_blue.project(*output_cloud6_blue, *cloudPCAprojection_blue);
//或得质心
Eigen::Vector4f pcaCentroid_blue = pca_blue.getMean();
Eigen::Matrix4f tm_blue = Eigen::Matrix4f::Identity();
Eigen::Matrix4f tm_inv_blue = Eigen::Matrix4f::Identity();
//矩阵的转置
tm_blue.block<3, 3>(0, 0) = pca_blue.getEigenVectors().transpose(); //R
tm_blue.block<3, 1>(0, 3) = -1.0f * (pca_blue.getEigenVectors().transpose()) * (pcaCentroid_blue.head<3>());
tm_inv_blue = tm_blue.inverse();
pcl::PointCloud<PointT>::Ptr transformedCloud_blue(new pcl::PointCloud<PointT>);
pcl::transformPointCloud(*cloud_blue, *transformedCloud_blue, tm_blue);
PointT min_p1_blue, max_p1_blue;
Eigen::Vector3f c1_blue, c_blue;
pcl::getMinMax3D(*transformedCloud_blue, min_p1_blue, max_p1_blue);
PointT start_blue, end_blue;
pcl::getMinMax3D(*transformedCloud_blue, start_blue, end_blue);
//保留第一主成分
start_blue.y = start_blue.z = 0;
end_blue.y = end_blue.z = 0;
//重投影点在点云表面
pcl::PointCloud<PointT>::Ptr reconstruct_points_blue(new pcl::PointCloud<PointT>);
reconstruct_points_blue->points.push_back(start_blue);
reconstruct_points_blue->points.push_back(end_blue);
pca_blue.reconstruct(*reconstruct_points_blue, *reconstruct_points_blue);
//std::cout << "start" << reconstruct_points->points[0] << ",end" << reconstruct_points->points[1] << std::endl;
float x_blue = (reconstruct_points_blue->points[0].x + reconstruct_points_blue->points[1].x)/2;
float y_blue = (reconstruct_points_blue->points[0].y + reconstruct_points_blue->points[1].y)/2;
float z_blue = (reconstruct_points_blue->points[0].z + reconstruct_points_blue->points[1].z)/2;
std::cout << "middle_blue:"<< x_blue<< "\t" << y_blue<<"\t"<<z_blue<<std::endl;
效果非常的好，与预期的测试结果基本对应上了。
但是，这种方案在理论上某种时刻会失灵，比如，深度相机正对着这个底面，我此时能获取的点云信息大部分是这个底面的，而另一侧底面完全无法获取，在这种情况下，势必会造成错误的中心点估计，因此，后续我将继续探索并优化此算法。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
