Cause: The project needs to use the symmetry of the point cloud, and the code was not found on the Internet, so I wrote one myself after searching for relevant information.
Implementation: Based on the rotation matrix, please see for details: (14 messages) Math Notes (3) Mirror Matrix_tkokof1's Blog-CSDN Blog_Mirror Transformation Matrix
Where D is the distance from the plane to the origin. A, B, C, D are the coefficients for determining the plane in the three-dimensional space. Note: After RANSAC fits the plane, it seems that the normal vector is obtained. I don’t know if my point cloud is special or what. Does anyone know?
Core function: transformPointCloud acts as a rotating point cloud, cloud_filtered is the input point cloud, transformedCloud outputs the rotated point cloud, n is the rotation matrix, and the following are n and the rotation function.
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);The following is the source code:
Because my point cloud file contains the ground, I first use ransac segmentation to remove the ground. My program provides two symmetry methods, one is symmetric about the plane fitted by ransac, and the other is symmetric about any given plane. The latter result is the result of symmetry about any plane. If you want to use the ransac fitting to get a plane image, just uncomment the relevant part of the code. I have tried both, and they both work. Point cloud files are not conveniently provided. The blogger is a half-way monk, and the coding ability is not good. If you have any questions or improve the code, please leave a message in the comment area.
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/project_inliers.h>
#include <math.h>
#include <pcl/common/transforms.h>
using namespace std;
typedef pcl::PointXYZ PointType;
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>);
// Fill in the cloud data
pcl::PCDReader reader;//PCD文件读取对象
// Replace the path below with the path where you saved your file
reader.read<pcl::PointXYZ>("XXX.pcd", *cloud);//“”内内容修改成你想分割的点云的文件名
//------------------------------------------PCL分割框架--------------------------------------------------------
//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 可选择配置，设置模型系数需要优化
seg.setOptimizeCoefficients(true);
// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
seg.setModelType(pcl::SACMODEL_PLANE);//设置模型类型
seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型
seg.setMaxIterations(500);//最大迭代次数
seg.setDistanceThreshold(0.005);//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
seg.setInputCloud(cloud);//输入所需要分割的点云对象
//引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
seg.segment(*inliers, *coefficient);
//---------以下为关于ransac拟合平面对称的旋转矩阵相关参数------------
/* float a= coefficient->values[0];
float b= coefficient->values[1];
float c= coefficient->values[2];
float d= coefficient->values[3];
cout << a << endl;
cout << b << endl;
cout << c << endl;
cout << d << endl;
float d1 = d;
if (d < 0)
{
d1 = -d;
}
float e = sqrt(a*a + b * b + c * c);
float r = d1 / e ;
cout << "f" << endl;*/
//----------------------------------------------------
// extract ground
// 从点云中抽取分割的处在平面上的点集
pcl::ExtractIndices<pcl::PointXYZ> extractor;//点提取对象
extractor.setInputCloud(cloud);
extractor.setIndices(inliers);
extractor.setNegative(true);
extractor.filter(*cloud_filtered);
// --------------点云关于ransac拟合出来的平面对称------------------
/*pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Matrix4f n;//旋转矩阵
n << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,
-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,
-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,
0, 0, 0, 1;
cout << n << endl;
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);
//------------------------------------------------------------------
*/
//---------------点云关于任意平面对称---------------------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
float A = 1, B = 2, C = 3, D = -4;//A,B,C,D为平面Ax+By+Cz=D
/*if(D<0)
{
D = -D;
cout << D << endl;
}*/
float e = sqrt(A * A + B * B + C * C);//为了后续将平面法向量转化为单位向量
float a = A / e, b = B / e, c = C / e;
float r = D / e;
Eigen::Matrix4f n;//旋转矩阵n
n << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,
-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,
-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,
0, 0, 0, 1;
cout << n << endl;
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);
//------------------创建对称面以便后续可视化对称面-----------------------
pcl::ModelCoefficients plane_coeff;
plane_coeff.values.resize(4); // We need 4 values
plane_coeff.values[0] = A;
plane_coeff.values[1] = B;
plane_coeff.values[2] = C;
plane_coeff.values[3] = D;
//----------------------------------------------------------------------
//--------------------------------------可视化--------------------------
pcl::visualization::PCLVisualizer viewer;
pcl::visualization::PointCloudColorHandlerCustom<PointType> color(cloud_filtered, 255, 0, 0);
viewer.addPointCloud(cloud_filtered, color, "cloud");
pcl::visualization::PointCloudColorHandlerCustom<PointType> color2(transformedCloud, 0, 100, 0);
viewer.addPointCloud(transformedCloud, color2, "transform");
viewer.addCoordinateSystem();
viewer.addPlane(plane_coeff);//添加对称面
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
}
return 0;
}The following is the running result:
