参考
双目立体成像（一）双目图片生成点云
双目三维重建：双目摄像头实现双目测距（Python）
精选课：C++完整的实现双目摄像头图像采集、双目摄像头畸变矫正、前景物体提取、生成视差图、深度图、PCL点云图
一、原理
视差到深度的转换公式：
基本流程：
二、参数确定
在生成点云前，要先确定参数。
根据单目标定和双目标定的结果，确定相机内参矩阵K，相机焦距f，基线b
单目标定得到的参数
内参矩阵K
焦距 (fx, fy): 相机镜头的焦距，影响图像的放大程度
光心 (cx, cy): 图像平面中相机镜头的中心点。
第一行和第二行的第一和第二个元素分别是沿图像平面水平和垂直轴的焦距。
c_x 和 c_y分别是图像平面上光心的水平和垂直坐标。
矩阵的第三行为 [0, 0, 1]，用于保持坐标的同质性。
双目标定得到的参数
直接截图了，可以求出基线b的值
其一 ：
其二 ：
三、程序
3.1 视差图生成点云
由双目立体视差图重建三维点云【Win10+OpenCV3.4.1+PCL1.8】-VIP文章
使用PCL库来将视差图转换为点云图，可以使用pcl::PointCloud类来表示点云数据，然后使用pcl::PointCloudpcl::PointXYZ类型的变量来存储点云信息。
主要步骤：
- 加载视差图及原彩色图
- 基于相机内参和视差值计算每个像素的3D坐标，包括深度z。
- 将每个像素的3D坐标转换为PCL点云中的点
- 将点云保存为PCL文件格式
cpp
/*
相机参数：
cam0 = [4152.073 0 1288.147; 0 4152.073 973.571; 0 0 1]
cam1 = [4152.073 0 1501.231; 0 4152.073 973.571; 0 0 1]
doffs = 213.084
baseline = 176.252
width = 2872
height = 1984
相机内参数矩阵：
K=[fx 0 u0; 0 fy v0; 0 0 1]
doffs = |u1 - u0|
*/
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
using namespace cv;
using namespace std;
using namespace pcl;
int user_data;
// 相机内参
const double u0 = 1288.147;
const double v0 = 973.571;
const double fx = 4152.073;
const double fy = 4152.073;
const double baseline = 176.252;
const double doffs = 213.084; // 代表两个相机主点在x方向上的差距, doffs = |u1 - u0|
void viewerOneOff(visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor(0.0, 0.0, 0.0);
}
int main()
{
// 读入数据
Mat color = imread("im0.png"); // 读入RGB色彩信息
Mat depth = imread("disp0.png",IMREAD_UNCHANGED);// 读入depth信息，即视差图
if (color.empty() || depth.empty()){
cout << "The image is empty, please check it!" << endl;
return -1;
}
// 相机坐标系下的点云
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
for (int row = 0; row < depth.rows; row++) {
for (int col = 0; col < depth.cols; col++){
ushort d = depth.ptr<ushort>(row)[col];
if (d==0)
continue;
PointXYZRGB p;
// 计算深度信息depth
p.z = fx * baseline / (d + doffs); // Zc = baseline * f / (d + doffs)
p.x = (col - u0) * p.z / fx; // Xc向右，Yc向下为正
p.y = (row - v0) * p.z / fy;
p.y = -p.y; // 为便于显示，绕x轴三维旋转180°
p.z = -p.z;
// 读入RGB信息
p.b = color.ptr<uchar>(row)[col * 3];
p.g = color.ptr<uchar>(row)[col * 3 + 1];
p.r = color.ptr<uchar>(row)[col * 3 + 2];
cloud->points.push_back(p);
}
}
cloud->height = depth.rows;
cloud->width = depth.cols;
cloud->points.resize(cloud->height * cloud->width);
visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud);
viewer.runOnVisualizationThreadOnce(viewerOneOff);
while (!viewer.wasStopped()){
user_data = 9;
}
return 0;
}3.2 素材及处理
middlebury提供的groundtruth是.pfm格式的文件，而OpenCV是不支持.pfm格式文件操作的，因此就需要自己造"轮子"或者转成OpenCV支持的格式。
解决方法：
1.使用的是图片格式在线转换工具，将.pfm格式图片转成.png格式
2.github上大神也提供了开源的"轮子"，可直接读入.pfm文件并转成Mat格式，下面是一个例子
cpp
/**
* Example of how to read and save PFM files.
*/
#include <iostream>
#include <string>
#include "PFMReadWrite.h"
using namespace std;
using namespace cv;
int main(void)
{
//Example that loads an image and stores it under another name
Mat image = loadPFM(string("image.pfm"));
//Display the image
imshow("Image", image);
waitKey(0);
savePFM(image, "image_saved.pfm");
return 0;
}四、点云操作
4.1 点云可视化
pcl_viewer 是 Point Cloud Library (PCL) 提供的一个用于可视化点云数据的工具。
CloudViewer 和 PCLVisualizer 是 PCL 中两个不同的类，都用于创建和管理点云数据的可视化窗口。
4.2 点云分层、断层
立体匹配视差图生成的PCL点云分层、放射状等杂乱效果的解决方法_视差图效果不好-VIP文章
点云出现下面这样像是很多2d平面图像层叠出来的问题，问题在于：转换点云时使用的视差图格式不对。
以OpenCV-sgbm为例，先调sgbm中的参数、至视差图既平滑又少空洞。
用于计算距离的视差图(CV_32F) 和用于 肉眼看的视差图(CV_8U) 使用的格式不同，并且用于计算的视差图无需进行裁剪和归一化，这些只是为了显示的可读性和美观。
所以，在对sgbm进行compute之后得到视差图disparty_sgbm，除以16得到用于计算的视差图disparity(除以16是因为每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16)。
将compute后的视差图，转为8位，得到可视的视差图disparity CV_8U，可以作为sgbm调参的直观依据，没有后续的作用了。
转换了正确的视差图格式之后，在计算点云时还要注意计算深度值的取点语句，数据类型也要对应好。
也就是说：进行匹配后得到的视差图，除以16得到用于计算的视差图（CV_32F），转换为8为得到用于显示的视差图（CV_8U）
cpp
cv::Mat disparity_sgbm, disparity, disparityCV_8U;
sgbm->compute(rectifyImageL, rectifyImageR, disparity_sgbm);
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //用来测距
disparity_sgbm.convertTo(disparityCV_8U, CV_8U, 255 / (NumDisparities * 16.)); //用来调参直观参考
imshow("disparityCV_8U",disparityCV_8U);
// 计算点云的程序内容
......
for (int i = 0; i < disparity.rows; i++){
for (int j = 0; j < disparity.cols; j++) {
PointXYZRGB p;
double d = disparity.ptr<float>(i)[j]; //注意数据类型
......
}
}
......相关知识：
图像格式（矩阵类型）
cpp
Mat(int rows, int cols, int type)
// rows为行数，cols为列数，type为类型| type()=0 | CV_8U | 八位无符号整型 (uchar) | 0--255 |
|---|---|---|---|
| 1 | CV_8S | 八位有符号整型 (schar) | -128--127 |
| 2 | CV_16U | 十六位无符号整型 (ushort) | 0--65535 |
| 3 | CV_16S | 十六位有符号整型 (short) | -32768--32767 |
| 4 | CV_32S | 三十二位有符号整型 (int) | 0--65535 |
| 5 | CV_32F | 三十二位浮点数 (float) | 0.0--1.0 |
| 6 | CV_64F | 六十四位浮点数 (double) | 0.0--1.0 |
convertTo函数
cpp
xxx.convertTo(dst, type, scale, shift)dst：目的矩阵；
type：需要的输出矩阵类型，或者更明确的，是输出矩阵的深度，如果是负值（常用-1）则输出矩阵和输入矩阵类型相同。转换位深度本质上就是对原深度下的数据做线性变换，使原位深度下的最小值和最大值分别对应转换后位深度下的最小值和最大值。
scale：范围比例因子；eg. 原来的图是255的灰度图，转成float型数据并归一化到0~1,那么就是scale=1.0/255
shift：将输入数组元素按比例缩放后添加的值；
目前OpenCV主要只支持单通道和3通道的图像，并要求其深度为8bit和16bit无符号(即CV_16U)，所以其他是不支持的，比如说float型等。
如果Mat类型数据的深度不满足上面的要求，则需要使用convertTo()函数来进行转换。convertTo()函数负责转换数据类型不同的Mat，即可以将类似float型的Mat转换到imwrite()函数能够接受的类型。
4.3 点云放射、发散、锥形
立体匹配视差图生成的PCL点云分层、放射状等杂乱效果的解决方法_视差图效果不好-VIP文章
效果如图，PCL点云是发散、放射、圆锥形的问题，结论：点云可能是对的，但发散出来大多都是无效点，除掉即可。
这里的点云锥顶部份比较稠密，而且分层也不是那么规律。再考虑到相机硬件本身的局限性，基线的长度决定了测距的有效范围，超过范围的点误差就会很大，这些点也是没有意义的。
解决的方法也很简单，在计算点云的双for循环中添加以下语句，把视差值小于80或者深度大于60cm的点剔除掉即可。（这里的数字不一定适配你的硬件，自行修改）
4.4 保存及加载点云
保存成不同格式对应不同函数，加载同理
cpp
pcl::io::savePCDFile("name.pcd", *cloud); // 保存点云
pcl::io::loadPCDFile("test.pcd", *cloud); // 加载点云4.5 PCL点云图3D建模
使用C++实现PCL点云图3D建模的基本步骤如下：
- 加载点云数据：使用PCL的PointCloud读取点云数据，支持多种点云格式，如PCD、PLY、OBJ等。
- 数据预处理：对点云数据进行过滤、降采样等处理，以提高建模效率和准确性。可使用PCL中的滤波器、采样器等工具。
- 特征提取：提取点云数据中的特征，例如表面法线、曲率、关键点等。可使用PCL提供的特征计算工具。
- 点云配准：将多个点云数据进行配准，以使它们处于相同的坐标系中。可使用PCL中的配准算法，如ICP、NDT等。
- 点云重构：使用已配准的点云数据进行重构，生成3D模型。可使用PCL中的重构算法，如贪心投影三角化（Greedy Projection Triangulation）、泊松重建（Poisson Reconstruction）等。
- 模型可视化：使用PCL中的可视化工具，如PCLVisualizer，将生成的3D模型可视化。
下面是一个简单的PCL点云图3D建模示例程序：
cpp
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
int main ()
{
// 加载点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile ("input_cloud.pcd", *cloud);
// 对点云数据进行降采样
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud);
// 计算法线
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
ne.setKSearch (20);
ne.compute (*normals);
// 配准
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource (cloud);
icp.setInputTarget (cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned (new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_aligned);
// 重构
pcl::GreedyProjectionTriangulation<pcl::PointXYZ> gp3;
pcl::PolygonMesh triangles;
gp3.setInputCloud (cloud_aligned);
gp3.setSearchRadius (0.025);
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
gp3.setMaximumSurfaceAngle(M_PI/4);
gp3.setMinimumAngle(M_PI/18);
gp3.setMaximumAngle(2*M_PI/3);
gp3.setNormalConsistency(false);
gp3.compute (triangles);
// 可视化
pcl::visualization::PCLVisualizer viewer("3D Model");
viewer.addPolygonMesh(triangles, "model");
viewer.spin();
return 0;
}4.6 PCL点云图实现选点、点到点、点到线、周长、面积的测量
使用C++实现PCL点云图上选点，点到点的距离测量，点到线的垂直距离测量，多线段长度测量，周长测量，面积测试。
以下是使用PCL库在C++中实现上述功能的代码示例，在实际应用中可能需要对代码进行一定的修改才能满足具体的需求，具体的实现细节需要根据应用场景进行调整。
cpp
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
using namespace std;
using namespace pcl;
int main()
{
// 读取点云数据
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>());
if (io::loadPCDFile("test.pcd", *cloud) == -1)
{
cout << "load pcd file error!" << endl;
return -1;
}
cout << "load pcd file success!" << endl;
// 可视化点云数据
visualization::CloudViewer viewer("PointCloud Viewer");
viewer.showCloud(cloud);
// 选定点
PointIndices::Ptr selected_indices(new PointIndices());
selected_indices->indices.push_back(0); // 选取第一个点
// 计算点到点距离
float dist = 0.0;
for (int i = 1; i < cloud->points.size(); i++)
{
if (selected_indices->indices[0] == i)
continue;
dist = sqrt(pow(cloud->points[i].x - cloud->points[selected_indices->indices[0]].x, 2)
+ pow(cloud->points[i].y - cloud->points[selected_indices->indices[0]].y, 2)
+ pow(cloud->points[i].z - cloud->points[selected_indices->indices[0]].z, 2));
cout << "Distance from point " << selected_indices->indices[0] << " to point " << i << ": " << dist << endl;
}
// 选定线段
PointCloud<PointXYZ>::Ptr line_cloud(new PointCloud<PointXYZ>());
line_cloud->points.push_back(cloud->points[0]);
line_cloud->points.push_back(cloud->points[10]); // 假设选取第1个点到第10个点作为线段
// 计算点到线垂直距离
Eigen::Vector3f direction_vector = line_cloud->points[1].getVector3fMap() - line_cloud->points[0].getVector3fMap(); // 计算线段的方向向量
direction_vector.normalize(); // 将方向向量单位化
PointXYZ projection_point;
float min_dist_to_line = FLT_MAX;
for (int i = 0; i < cloud->points.size(); i++)
{
if (i == 0 || i == 10) // 不对线段的两个端点进行计算
continue;
float dist_to_1st_point = sqrt(pow(cloud->points[i].x - line_cloud->points[0].x, 2)
+ pow(cloud->points[i].y - line_cloud->points[0].y, 2)
+ pow(cloud->points[i].z - line_cloud->points[0].z, 2));
float dist_to_2nd_point = sqrt(pow(cloud->points[i].x - line_cloud->points[1].x, 2)
+ pow(cloud->points[i].y - line_cloud->points[1].y, 2)
+ pow(cloud->points[i].z - line_cloud->points[1].z, 2));
if (dist_to_1st_point < 0.01 || dist_to_2nd_point < 0.01) // 如果点在线段的两个端点附近，则不进行计算
continue;
float dist_to_line = abs(direction_vector.dot(cloud->points[i].getVector3fMap() - line_cloud->points[0].getVector3fMap()));
if (dist_to_line < min_dist_to_line)
{
min_dist_to_line = dist_to_line;
projection_point = cloud->points[i];
}
}
cout << "Min distance from line " << line_cloud->points[0] << " to " << line_cloud->points[1] << " is: " << min_dist_to_line << endl;
// 计算多线段长度
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> euclidean_cluster_extraction;
euclidean_cluster_extraction.setInputCloud(cloud);
euclidean_cluster_extraction.setClusterTolerance(0.1);
euclidean_cluster_extraction.setMinClusterSize(2);
euclidean_cluster_extraction.setMaxClusterSize(1000);
euclidean_cluster_extraction.extract(cluster_indices);
vector<PointCloud<PointXYZ>::Ptr> lines;
for (int i = 0; i < cluster_indices.size(); i++)
{
PointCloud<PointXYZ>::Ptr line(new PointCloud<PointXYZ>());
line->points.push_back(cloud->points[cluster_indices[i].indices[0]]);
line->points.push_back(cloud->points[cluster_indices[i].indices[1]]);
lines.push_back(line);
cout << "Line " << i << " length is: " << sqrt(pow(line->points[1].x - line->points[0].x, 2)
+ pow(line->points[1].y - line->points[0].y, 2)
+ pow(line->points[1].z - line->points[0].z, 2)) << endl;
}
// 计算周长和面积
double perimeter = 0.0;
double area = 0.0;
PointCloud<PointXYZ>::Ptr polygon_cloud(new PointCloud<PointXYZ>());
for (int i = 0; i < lines.size(); i++)
{
perimeter += sqrt(pow(lines[i]->points[1].x - lines[i]->points[0].x, 2)
+ pow(lines[i]->points[1].y - lines[i]->points[0].y, 2)
+ pow(lines[i]->points[1].z - lines[i]->points[0].z, 2));
polygon_cloud->points.push_back(lines[i]->points[0]);
}
polygon_cloud->points.push_back(lines[lines.size() - 1]->points[1]);
for (int i = 0; i < polygon_cloud->points.size() - 1; i++)
{
area += (polygon_cloud->points[i].x * polygon_cloud->points[i + 1].y
- polygon_cloud->points[i + 1].x * polygon_cloud->points[i].y);
}
area += (polygon_cloud->points[polygon_cloud->points.size() - 1].x * polygon_cloud->points[0].y
- polygon_cloud->points[0].x * polygon_cloud->points[polygon_cloud->points.size() - 1].y);
cout << "Perimeter is: " << perimeter << endl;
cout << "Area is: " << abs(area) / 2 << endl;
return 0;
}4.7 点云滤波
点云需要滤波的情况：
- 点云数据密度不规则，需要平滑
- 因遮挡等问题造成离群点，需要去除
- 数据量大，需要下采样
- 噪声数据，需要去除
对应的方法：
- 按照具体给定的规则限制滤除
- 通过常用滤波算法修改点的部分属性
- 对数据下采样
4.7.1.使用VoxelGrid滤波器对点云进行下采样
参考2
使用Voxel Grid （体素格滤波）实现下采样，即减少点的数量。减少点云数据，并同时保存点云的形状特征，在提高配准，曲面重建，形状识别等算法速度中非常实用，PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格，容纳后每个体素内用体素中所有点的重心来近似显示体素中其他点，这样该体素内所有点都用一个重心点最终表示，对于所有体素处理后得到的过滤后的点云，这种方法比用体素中心逼近的方法更慢，但是对于采样点对应曲面的表示更为准确。
cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
int
main(int argc, char** argv)
{
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());
pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
//点云对象的读取
pcl::PCDReader reader;
reader.read("squ4r.pcd", *cloud); //读取点云到cloud中
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList(*cloud) << ").";
/******************************************************************************
创建一个叶大小为1cm的pcl::VoxelGrid滤波器，
**********************************************************************************/
pcl::VoxelGrid<pcl::PCLPointCloud2> sor; //创建滤波对象
sor.setInputCloud(cloud); //设置需要过滤的点云给滤波对象
sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
sor.filter(*cloud_filtered); //执行滤波处理，存储输出
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ").";
pcl::PCDWriter writer;
writer.write("squ4r filter.pcd", *cloud_filtered,Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);
return (0);
}4.7.2.使用statisticalOutlierRemoval滤波器移除离群点
参考2
Statistical Outlier Removal （统计离群点滤波），从一个点云数据中集中移除测量噪声点（也就是离群点）。
比如：激光扫描通常会产生密度不均匀的点云数据集，另外测量中的误差也会产生稀疏的离群点，使效果不好，估计局部点云特征（例如采样点处法向量或曲率变化率）的运算复杂，这会导致错误的数值，反过来就会导致点云配准等后期的处理失败。
解决办法：每个点的邻域进行一个统计分析，并修剪掉一些不符合一定标准的点，稀疏离群点移除方法基于在输入数据中对点到临近点的距离分布的计算，对每一个点，计算它到它的所有临近点的平均距离，，假设得到的结果是一个高斯分布，其形状是由均值和标准差决定，平均距离在标准范围之外的点，可以被定义为离群点并可从数据中去除。
cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
// 定义读取对象
pcl::PCDReader reader;
// 读取点云文件
reader.read<pcl::PointXYZ>("sun2.pcd", *cloud);
std::cerr << "Cloud before filtering: " << std::endl;
std::cerr << *cloud << std::endl;
// 创建滤波器，对每个点分析的临近点的个数设置为50 ，并将标准差的倍数设置为1 这意味着如果一
//个点的距离超出了平均距离一个标准差以上，则该点被标记为离群点，并将它移除，存储起来
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
sor.setInputCloud(cloud); //设置待滤波的点云
sor.setMeanK(50); //设置在进行统计时考虑查询点临近点数
sor.setStddevMulThresh(1.0); //设置判断是否为离群点的阀值
sor.filter(*cloud_filtered); //滤波并存储
std::cerr << "Cloud after filtering: " << std::endl;
std::cerr << *cloud_filtered << std::endl;
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ>("sun1_inliers.pcd", *cloud_filtered, false);
sor.setNegative(true);
sor.filter(*cloud_filtered);
writer.write<pcl::PointXYZ>("sun1_outliers.pcd", *cloud_filtered, false);
return (0);
}