|
点云处理的第一步是先获取点云。 随着技术的进步，场景3D数据的获取以及从耗时耗力的线性激光扫描发展到通过3D深度相机直接获取。但是3D深度相机一般直接获取到的是一张包含深度数据的图像，需要进行处理后才能获取点云数据。 主要技术包括双目立体视觉，结构光和光飞行时间TOF。 以TOF相机为例，点云转换步骤为：  完整代码为：
//使用了PCL库和OpenCV库
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
//将16位深度图像转为点云
//depthMat：16位深度图像
//输出：点云
PointCloudT::Ptr matToPointcloud(cv::Mat depthMat)
{
//相机内参，请根据相机调整
double fx = 296.0412471;
double fy = 296.839046;
double cx = 178.091392;
double cy = 129.480179;
double a = 1;
double k1 = -0.145637047;
double k2 = 0.276467521;
double p1 = 0;
double p2 = 0;
double k3 = 0;
//畸变矫正后图像
cv::Mat img;
//内参矩阵
cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F); //3*3单位矩阵
cameraMatrix.at<double>(0, 0) = fx;
cameraMatrix.at<double>(0, 1) = 0;
cameraMatrix.at<double>(0, 2) = cx;
cameraMatrix.at<double>(1, 1) = fy;
cameraMatrix.at<double>(1, 2) = cy;
cameraMatrix.at<double>(2, 2) = 1;
//畸变参数
cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64F); //5*1全0矩阵
distCoeffs.at<double>(0, 0) = k1;
distCoeffs.at<double>(1, 0) = k2;
distCoeffs.at<double>(2, 0) = p1;
distCoeffs.at<double>(3, 0) = p2;
distCoeffs.at<double>(4, 0) = k3;
cv::Size imageSize = depthMat.size();
cv::Mat map1, map2;
//参数1：相机内参矩阵
//参数2：畸变矩阵
//参数3：可选输入，第一和第二相机坐标之间的旋转矩阵
//参数4：校正后的3X3相机矩阵
//参数5：无失真图像尺寸
//参数6：map1数据类型，CV_32FC1或CV_16SC2
//参数7、8：输出X/Y坐标重映射参数
initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix, imageSize,
CV_32FC1, map1, map2); //计算畸变映射
//参数1：畸变原始图像
//参数2：输出图像
//参数3、4：X\Y坐标重映射
//参数5：图像的插值方式
//参数6：边界填充方式
remap(depthMat, img, map1, map2, cv::INTER_NEAREST); //畸变矫正
//点云变换
int imgWidth = img.size().width;
int imgHeight = img.size().height;
PointCloudT::Ptr pointcloud(new PointCloudT);
for (int i = 0; i < imgHeight; i++)
{
for (int j = 0; j < imgWidth; j++)
{
//图像上x,y和中心点角度关系
float picAngle = atan2(fx*(i - imgHeight / 2.0), fy*(j - imgWidth / 2.0));
float angle = atan(sqrt((j - imgWidth / 2.0)*(j - imgWidth / 2.0) / fx / fx +
(i - imgHeight / 2.0)*(i - imgHeight / 2.0) / fy / fy)); if(img.at<ushort>(i,j) > 3000 || img.at<ushort>(i,j) == 0)
continue;
float dist = img.at<ushort>(i, j) / a; //原始图像深度
pcl::PointXYZRGBA p;
p.z = dist*cos(angle); //坐标变换后的深度
p.x = -dist*sin(angle)*cos(picAngle);
p.y = -dist*sin(angle)*sin(picAngle);
p.r = 250;
p.g = 250;
p.b = 250;
p.a = 255;
pointcloud->points.push_back(p);
}
}
reuturn pointcloud;
}
获取图像： 首先获取图像，根据不同相机方式不同。一般深度相机生成的图像都是16位单通道图像。 畸变矫正： 具体原理可以见：http://sunhx.cn/a/tuxiang/2019/0420/48.html 相机内参和畸变系数每个相机都是不一样的，且不动硬件的情况下固定不变，可以通过棋盘格等方式标定、获取。
initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix, imageSize,
通过OpenCV函数计算即便映射，得到映射map1和map2。CV_32FC1, map1, map2); //计算畸变映射
remap(depthMat, img, map1, map2, cv::INTER_NEAREST); //畸变矫正
得到畸变矫正后的图像。需要注意最后一个参数边界填充方式。因为深度图像获取原理的限制可能会出现某些像素无法确定准确距离，一般深度相机会给定一个比较大的值或者0来表示。但是畸变矫正时无法做到矫正后的的每个像素都在原图上有对应的点，需要估算两个像素之间的值，对于彩色图像来说一般是连续的。但是对于深度图像，有效点和无效点之间的像素值估计可能会引入噪声，这里使用最近点估计，避免生成的点云中物体和无限远之间出现噪点，或者可以采用无效点附近的像素都记为无效点来消除噪声。 坐标系转换： 具体原理可以见：http://sunhx.cn/a/tuxiang/2019/0420/49.html  根据小孔成像原理，需要先计算soc角度：
float angle = atan(sqrt((j - imgWidth / 2.0)*(j - imgWidth / 2.0) / fx / fx +
(i - imgHeight / 2.0)*(i - imgHeight / 2.0) / fy / fy)); 计算c'x'和c'y'之间的夹角，用以计算真实cx和cy：
float picAngle = atan2(fx*(i - imgHeight / 2.0), fy*(j - imgWidth / 2.0));
过滤像素，部分像素数据无效或者过曝等不做变换，直接丢弃：
if(img.at<ushort>(i,j) > 3000 || img.at<ushort>(i,j) == 0)
continue;
注意一般获取到的图像像素距离和真实距离一般有一个放大系数，即这里的a，默认1。计算像素距离，即os距离：
float dist = img.at<ushort>(i, j) / a;
最后计算相机坐标系下坐标，cx，cy和oc，加入到点云结构体中：
pcl::PointXYZRGBA p;
p.z = dist*cos(angle); //坐标变换后的深度
p.x = -dist*sin(angle)*cos(picAngle);
p.y = -dist*sin(angle)*sin(picAngle);
p.r = 250;
p.g = 250;
p.b = 250;
p.a = 255;
pointcloud->points.push_back(p);
建议 最后建议，在显示、处理点云数据之前检查点云中的点个数，防止因为遮挡镜头等各种原因造成数据无效，点云中没有点，程序异常。
if(pointcloud->size() == 0)
pointcloud->resize(1);
|
