雷达相机标定（一）----点云提取标定板角点
目录
-
- 思路
- 源码
- 效果
- 数据
图像提取角点：https://blog.csdn.net/xx970829/article/details/123233609
思路
velodyne16线点云稀疏，与相机标定时不利于角点的提取，本文先提取出平面，再把平面边缘点投影到yoz平面，拟合出交点，然后结合空间平面方程计算出其空间位置。
源码
//--------------------------------------------------------------------------------------------
// source /home/xx/catkin_ws/devel/setup.bash && rosrun my_cam_lidar_calib cloud 1.pcd
//
// 提取点云中的标定板
//--------------------------------------------------------------------------------------------
#include
#include
#include
#include
//常用点云类
#include
#include
#include //直通滤波器头文件
#include //滤波相关
#include //类cloud_viewer头文件申明
//分割类
#include
#include
#include
#include
#include
//图像类
#include
#include
#include
#include
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
//标定板长、宽
#define LONG 0.6
#define WIDE 0.67
#define DisThre 0.03//平面分割阈值
#define SLEEP 2 //睡眠时间
using namespace pcl;
using namespace cv;
using namespace std;
float plane_A ,plane_B ,plane_C,plane_D;
void cloudPassThrough(pcl::PointCloud::Ptr cloud,const char *zhou,int min,int max);
void Plane_fitting(pcl::PointCloud::Ptr cloud_input );
void cloudStatisticalOutlierRemoval(pcl::PointCloud::Ptr cloud);
void output_plane(pcl::PointCloud::Ptr cloud_plane ,pcl::ModelCoefficients::Ptr coefficients,pcl::PointIndices::Ptr inliers);
bool choicePlane(pcl::PointCloud::Ptr cloud_plane);
void cloudEdge(pcl::PointCloud::Ptr cloud);
bool Collinear(pcl::PointXYZ A,pcl::PointXYZ B,pcl::PointXYZ C);
void LineFitLeastFit(const std::vector &_points,float & _k,float & _b,float & _r);
void intersection(cv::Point2f &point,float & A_k,float & A_b,float & B_k,float & B_b);
void SpatialPoint(cv::Point2f &point2D,pcl::PointCloud::Ptr cloud);
void addSpatialPoint(cv::Point2f &point2A,cv::Point2f &point2B,float & B_k,float & B_b,pcl::PointCloud::Ptr cloud);
pcl::visualization::CloudViewer viewer("viewer");
int main(int argc,char** argv)
{
string pcd_mame = argv[1];
//打开PCD
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
if (pcl::io::loadPCDFile(pcd_mame, *cloud) == -1)
{
PCL_ERROR("Could not read pcd file!\n");
}
//直通滤波
cloudPassThrough(cloud,"y",-1.8,1.8);
cloudPassThrough(cloud,"x",1.8,7);
cloudPassThrough(cloud,"z",-2,2);
viewer.showCloud(cloud);
sleep(SLEEP);
//去除离群点
cloudStatisticalOutlierRemoval( cloud);
viewer.showCloud(cloud);
pcl::io::savePCDFileASCII("myRemoval.pcd",*cloud);
cout<<"save myRemoval.pcd success !!"<::Ptr cloud_input)
{
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation seg;
pcl::PointCloud::Ptr cloud_plane(new pcl::PointCloud());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(300);
seg.setDistanceThreshold(DisThre);
while (cloud_input->size() > 100)
{
pcl::PointCloud::Ptr cloud_p(new pcl::PointCloud);
seg.setInputCloud(cloud_input);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
break;
}
pcl::ExtractIndices extract;
extract.setInputCloud(cloud_input);
extract.setIndices(inliers);
extract.filter(*cloud_plane);//输出平面
if (cloud_plane->size()>100)
{
output_plane(cloud_plane ,coefficients,inliers);
}
// 移除plane
extract.setNegative(true);
extract.filter(*cloud_p);
*cloud_input = *cloud_p;
}
}
void output_plane(pcl::PointCloud::Ptr cloud_plane ,pcl::ModelCoefficients::Ptr coeff,pcl::PointIndices::Ptr inliers)
{
if(choicePlane(cloud_plane))
{
plane_A=coeff->values[0];
plane_B=coeff->values[1];
plane_C=coeff->values[2];
plane_D=coeff->values[3];
cout<< coeff->values[0]<<" "<values[1]<<" "<values[2]<<" "<values[3]<::Ptr cloud)
{
double maxDistance=sqrt(pow(LONG,2)+pow(WIDE,2));
double oldDistance=0;
for(std::size_t i=0; isize(); i++)
{
double thisDistance=sqrt(pow(cloud->points[i].x-cloud->points[0].x,2)+pow(cloud->points[i].y-cloud->points[0].y,2)+pow(cloud->points[i].z-cloud->points[0].z,2));
if(oldDistancemaxDistance+0.05)
return false;
}
}
if(oldDistance::Ptr cloud,const char *zhou,int min,int max)
{
pcl::PassThrough passthrough;
passthrough.setInputCloud(cloud);//输入点云
passthrough.setFilterFieldName(zhou);//对z轴进行操作
passthrough.setFilterLimits(min,max);//设置直通滤波器操作范围
passthrough.filter(*cloud);//);//执行滤波
}
//去除离群点
void cloudStatisticalOutlierRemoval(pcl::PointCloud::Ptr cloud)
{
pcl::StatisticalOutlierRemoval sor; //创建滤波器对象
sor.setInputCloud (cloud); //设置待滤波的点云
sor.setMeanK (20); //设置在进行统计时考虑的临近点个数
sor.setStddevMulThresh (1.0); //设置判断是否为离群点的阀值
sor.filter (*cloud);
}
void cloudEdge(pcl::PointCloud::Ptr cloud)
{
int index[16][2];
memset(index,-1,sizeof(index));//赋值
pcl::PointCloud::Ptr cloud_all(new pcl::PointCloud);
for(std::size_t i=0 ; i< cloud->size(); i++)
{
//计算垂直俯仰角
float angle = atan(cloud->points[i].z / sqrt( pow(cloud->points[i].x,2) + pow(cloud->points[i].y,2))) * 180 / M_PI;
int scanID = 0;
scanID = int((angle + 15) / 2 + 0.5);// + 0.5 用于四舍五入
if(0<=scanID && scanID<16)
{
if(index[scanID][0]==-1)
index[scanID][0]=i;
else
index[scanID][1]=i;
}
pcl::PointXYZRGB thisColor;
thisColor.x=cloud->points[i].x;
thisColor.y=cloud->points[i].y;
thisColor.z=cloud->points[i].z;
thisColor.r=255;
thisColor.g=255;
thisColor.b=255;
cloud_all->push_back(thisColor);
}
//提取边缘
pcl::PointCloud::Ptr cloud_edge(new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_edge_left(new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_edge_right(new pcl::PointCloud);
for(int i=0 ;i<16;i++)
{
if(index[i][0]!=-1)
{
pcl::PointXYZRGB thisColor;
thisColor.x=cloud->points[index[i][0]].x;
thisColor.y=cloud->points[index[i][0]].y;
thisColor.z=cloud->points[index[i][0]].z;
thisColor.r=255;
thisColor.g=255;
thisColor.b=255;
//cloud_all->push_back(thisColor);
cloud_edge->push_back(thisColor);
cloud_edge_left->push_back(cloud->points[index[i][0]]);
}
}
for(int i=15;i>=0;i--)
{
if(index[i][1]!=-1)
{
pcl::PointXYZRGB thisColor;
thisColor.x=cloud->points[index[i][1]].x;
thisColor.y=cloud->points[index[i][1]].y;
thisColor.z=cloud->points[index[i][1]].z;
thisColor.r=255;
thisColor.g=255;
thisColor.b=255;
//cloud_all->push_back(thisColor);
cloud_edge->push_back(thisColor);
cloud_edge_right->push_back(cloud->points[index[i][1]]);
}
}
//划分4条线
int d_a=0;
int b_c=0;
vector pointsA;
vector pointsB;
vector pointsC;
vector pointsD;
for(std::size_t i=0; isize()-2; i++)
{
//左侧第三个点到前两个点的距离
if(Collinear(cloud_edge_left->points[i],cloud_edge_left->points[i+1],cloud_edge_left->points[i+2]))
{
d_a=i+1;
break;
}
}
for(std::size_t i=0; isize()-2; i++)
{
//右侧第三个点到前两个点的距离
if(Collinear(cloud_edge_right->points[i],cloud_edge_right->points[i+1],cloud_edge_right->points[i+2]))
{
b_c=i+1;
break;
}
}
for(std::size_t i=0; isize(); i++)
{
if(ipoints[i].y;
thisPoint.y=cloud_edge_left->points[i].z;
pointsD.push_back(thisPoint);
}
else
{
cv::Point2f thisPoint;
thisPoint.x=cloud_edge_left->points[i].y;
thisPoint.y=cloud_edge_left->points[i].z;
pointsA.push_back(thisPoint);
}
}
for(std::size_t i=0; isize(); i++)
{
if(ipoints[i].y;
thisPoint.y=cloud_edge_right->points[i].z;
pointsB.push_back(thisPoint);
}
else
{
cv::Point2f thisPoint;
thisPoint.x=cloud_edge_right->points[i].y;
thisPoint.y=cloud_edge_right->points[i].z;
pointsC.push_back(thisPoint);
}
}
float A_k,A_b,A_r;
float B_k,B_b,B_r;
float C_k,C_b,C_r;
float D_k,D_b,D_r;
LineFitLeastFit(pointsA,A_k,A_b, A_r);
LineFitLeastFit(pointsB,B_k,B_b, B_r);
LineFitLeastFit(pointsC,C_k,C_b, C_r);
LineFitLeastFit(pointsD,D_k,D_b, D_r);
//求yoz平面交点
cv::Point2f pointAb;
cv::Point2f pointBc;
cv::Point2f pointCd;
cv::Point2f pointDa;
intersection(pointAb, A_k, A_b, B_k, B_b);
intersection(pointBc, B_k, B_b, C_k, C_b);
intersection(pointCd, C_k, C_b, D_k, D_b);
intersection(pointDa, D_k, D_b, A_k, A_b);
//求空间点
SpatialPoint(pointAb,cloud_all);
SpatialPoint(pointBc,cloud_all);
SpatialPoint(pointCd,cloud_all);
SpatialPoint(pointDa,cloud_all);
//增加空间点
addSpatialPoint(pointAb,pointBc,B_k,B_b,cloud_all);
addSpatialPoint(pointBc,pointCd,C_k,C_b,cloud_all);
addSpatialPoint(pointCd,pointDa,D_k,D_b,cloud_all);
addSpatialPoint(pointDa,pointAb,A_k,A_b,cloud_all);
// 保存边界点云和拟合点云
viewer.showCloud(cloud_edge);
sleep(SLEEP);
pcl::io::savePCDFileASCII("myEdge.pcd",*cloud_edge);
cout<<"save myEdge.pcd success !!"< &_points,float & _k,float & _b,float & _r)
{
//https://blog.csdn.net/jjjstephen/article/details/108053148?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_title~default-0.pc_relevant_default&spm=1001.2101.3001.4242.1&utm_relevant_index=3
float B = 0.0f;
float A = 0.0f;
float D = 0.0f;
float C = 0.0f;
int N = _points.size();
for (int i = 0; i < N; i++)
{
B += _points[i].x;
A += _points[i].x * _points[i].x;
D += _points[i].y;
C += _points[i].x * _points[i].y;
}
if ((N * A - B * B) == 0)
return;
_k = (N * C - B * D) / (N * A - B * B);
_b = (A * D - C * B) / (N * A - B * B);
//计算相关系数
float Xmean = B / N;
float Ymean = D / N;
float tempX = 0.0f;
float tempY = 0.0f;
float rDenominator = 0.0;
for (int i = 0; i < N; i++)
{
tempX += (_points[i].x - Xmean) * (_points[i].x - Xmean);
tempY += (_points[i].y - Ymean) * (_points[i].y - Ymean);
rDenominator += (_points[i].x - Xmean) * (_points[i].y - Ymean);
}
float SigmaXY = sqrt(tempX) * sqrt(tempY);
if (SigmaXY == 0)
return;
_r = rDenominator / SigmaXY;
//cout<<"r:"<<_r< A;
A(0,0)=1;
A(0,1)=-A_k;
A(1,0)=1;
A(1,1)=-B_k;
Eigen::Matrix B;
B(0,0)=A_b;
B(1,0)=B_b;
Eigen::Matrix xy=A.fullPivHouseholderQr().solve(B);
point.x=xy(1,0);
point.y=xy(0,0);
//cout<::Ptr cloud_all)
{
pcl::PointXYZRGB thisColor;
thisColor.x=-(plane_B*point2D.x+plane_C*point2D.y+plane_D)/plane_A;
thisColor.y=point2D.x;
thisColor.z=point2D.y;
thisColor.r=0;
thisColor.g=255;
thisColor.b=0;
cloud_all->push_back(thisColor);
}
//添加空间直线上的点
void addSpatialPoint(cv::Point2f &point2A,cv::Point2f &point2B,float & _k,float & _b,pcl::PointCloud::Ptr cloud_all)
{
float step=0.01;
if(point2A.x < point2B.x)
{
for(float a=point2A.x ;a 效果
数据
下载：https://download.csdn.net/download/xx970829/85161423
注意：根据应用场景，需自己调整部分参数
【转载请标明出处：https://blog.csdn.net/xx970829/article/details/123233029】