标题
一、代码常用
0、常用小技巧
#include <cmath>//#define M_PI 3.14159265358979323846
M_PI
1、代码运行时间
#include <ctime>
clock_t start=clock();
clock_t end=clock();
cout<<"用时："<<(double)(end-start)<<"ms"<<endl;
2、获取点云XYZ方向的最大值与最小值
#include <pcl/common/common.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZ minPt, maxPt;//把最大/最小用点来存
pcl::getMinMax3D (*cloud, minPt, maxPt);
3、Eigen矩阵的读与写
- 读
#include <fstream>
Matrix4f T;
std::ofstream fout;
fout.open("mat.txt",std::ios::app);//在文件末尾追加写入
fout << T << std::endl;//每次写完一个矩阵以后换行
fout.close();
- 写
#include <fstream>
std::vector<Matrix4f> T0;//创建储存多个矩阵的vector对象
std::ifstream fin;
fin.open("mat.txt");
//下面for的终止条件中的6表示txt文件中矩阵的个数
for (int n = 0; n < 6; n++){
Matrix4f tempT;
for (int i = 0; i < 4; i++){
for (int j = 0; j < 4; j++){
fin >> tempT(i, j);
}
}//for循环遍历依次读取
T0.push_back(tempMat);
}
fin.close();
4、面面交线和点到线的垂点
//计算两面之间的交线
void line_between_plane(Eigen::Vector3f Ps,Eigen::Vector3f U,Eigen::Vector3f Pt,Eigen::Vector3f V,Eigen::Vector3f &Pw,Eigen::Vector3f &nw)
{
//计算交线方向
nw=U.cross(V)/(U.cross(V).norm());
//判断使用哪个公式
if(abs(nw(0))>=abs(nw(1))&&abs(nw(0))>=abs(nw(2)))
{
//计算x=0时候的点
Pw(0)=0;
Pw(1)=(V(2)*Ps.dot(U)-U(2)*Pt.dot(V))/(U(1)*V(2)-V(1)*U(2));
Pw(2)=(V(1)*Ps.dot(U)-U(1)*Pt.dot(V))/(U(2)*V(1)-V(2)*U(1));
}
else if(abs(nw(1))>=abs(nw(0))&&abs(nw(1))>=abs(nw(2)))
{
//计算y=0时候的点
Pw(0)=(V(2)*Ps.dot(U)-U(2)*Pt.dot(V))/(U(0)*V(2)-V(0)*U(2));
Pw(1)=0;
Pw(2)=(V(0)*Ps.dot(U)-U(0)*Pt.dot(V))/(U(2)*V(0)-V(2)*U(0));
}
else
{
//计算z=0时候的点
Pw(0)=(V(1)*Ps.dot(U)-U(1)*Pt.dot(V))/(U(0)*V(1)-V(0)*U(1));
Pw(1)=(V(0)*Ps.dot(U)-U(0)*Pt.dot(V))/(U(1)*V(0)-V(1)*U(0));
Pw(2)=0;
}
}
//计算点到直线的垂点
void point_from_line(Eigen::Vector3f P0,Eigen::Vector3f W,Eigen::Vector3f P,Eigen::Vector3f &T)
{
float t=(W.dot(P-P0))/(W.dot(W));
T=P0+t*W;
}
5、计算向量之间夹角
#include <iostream>
#include <pcl/common/common.h>
using namespace std;
int main(int argc, char** argv)
{
Eigen::Vector3f v1(2, 4, 7), v2(7, 8, 9);
pcl::getAngle3D(v1, v2, false);//false: 计算的夹角结果为弧度制，true：计算的夹角结果为角度制
double radian_angle = atan2(v1.cross(v2).norm(), v1.transpose() * v2); //计算结果取值范围为[0,PI]，即用弧度制表示夹角
cout << "自己写的代码计算结果：" << radian_angle << endl;
cout << "调用库函数实现的结果：" << pcl::getAngle3D(v1, v2, false) << endl;
return 0;
}
6、点云PCA求特征值和特征向量
#include <pcl/common/centroid.h>
//协方差矩阵
Eigen::Vector4f i_centroid;
pcl::compute3DCentroid(*i_cloud, i_centroid); // 计算质心，质心为齐次坐标（c0,c1,c2,1）
Eigen::Matrix3f i_covariance_matrix;
pcl::computeCovarianceMatrixNormalized(*i_cloud, i_centroid, i_covariance_matrix);//计算协方差矩阵
//计算矩阵特征值、特征向量
#include <pcl/common/eigen.h>
//1、只能计算对称半正定矩阵
Eigen::Vector3f eigenvalue;
Eigen::Matrix3f eigenvector;//向量竖着看
pcl::eigen33<Eigen::Matrix3f, Eigen::Vector3f>(i_covariance_matrix, eigenvector, eigenvalue);
或
//2、可以计算所有矩阵，但精度没有第一种高
Eigen::EigenSolver<Eigen::Matrix3f> es(i_covariance_matrix);
cout << es.pseudoEigenvalueMatrix() << endl;
cout << es.pseudoEigenvectors() << endl;
7、点云向平面投影
/*
plane_coeff ：平面一般式系数
p_0 ：输入点
p_p ：投影点
*/
void pointProjection(Eigen::Vector4f& plane_coeff, Eigen::Vector3f& p_0, Eigen::Vector3f& p_p)
{
float a = plane_coeff(0), b = plane_coeff(1), c = plane_coeff(2), d = plane_coeff(3);
p_p(0) = ((b * b + c * c) * p_0(0) - a * (b * p_0(1) + c * p_0(2) + d)) / (a * a + b * b + c * c);
p_p(1) = ((a * a + c * c) * p_0(1) - b * (a * p_0(0) + c * p_0(2) + d)) / (a * a + b * b + c * c);
p_p(2) = ((a * a + b * b) * p_0(2) - c * (a * p_0(0) + b * p_0(1) + d)) / (a * a + b * b + c * c);
}
8、给点云添加高斯噪声
// ---------------------------生成高斯分布的点云数据---------------------------------------
//点云添加噪声就是模拟采集点云数据时产生的误差，结果就是点云中点位置的微小变动。
pcl::PointCloud<pcl::PointXYZ>::Ptr gauss_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::common::CloudGenerator<pcl::PointXYZ, pcl::common::NormalGenerator<float> > generator; //高斯噪声产生器
uint32_t seed = static_cast<uint32_t> (time(NULL)); //生成随机种子
pcl::common::NormalGenerator<float>::Parameters x_params(xmean, xstddev, seed++);
generator.setParametersForX(x_params); //根据参数添加x方向的噪声
pcl::common::NormalGenerator<float>::Parameters y_params(ymean, ystddev, seed++);
generator.setParametersForY(y_params); //根据参数添加y方向的噪声
pcl::common::NormalGenerator<float>::Parameters z_params(zmean, zstddev, seed++);
generator.setParametersForZ(z_params); //根据参数添加z方向的噪声
generator.fill((*cloud).width, (*cloud).height, *gauss_cloud); //产生等数据量的随机噪声
// ---------------------------添加高斯分布的随机噪声--------------------------------------
for (size_t i = 0; i < cloud->points.size(); ++i)
{
gauss_cloud->points[i].x += cloud->points[i].x;
gauss_cloud->points[i].y += cloud->points[i].y;
gauss_cloud->points[i].z += cloud->points[i].z;
}
9、C++读写取txt文件中的离散点
//---------------读取txt文件-------------------//
void CreateCloudFromTxt(const string& file_path, pcl::PointCloud<PointT>::Ptr cloud)
{
std::ifstream file(file_path.c_str());//c_str()：生成一个const char*指针，指向以空字符终止的数组。
std::string line;
PointT point;
//float nx, ny, nz;
while (getline(file, line)) {
std::stringstream ss(line);
ss >> point.x;
ss >> point.y;
ss >> point.z;
//ss >> nx;
//ss >> ny;
//ss >> nz;
cloud->push_back(point);
}
file.close();
}
//---------------写入txt文件-------------------//
ofstream outdata;
outdata.open("outdata.txt", ofstream::app);
for (int i_point = 0; i_point < fittingPoints.size(); i_point++)
{
outdata << fittingPoints[i_point](0) << " " << fittingPoints[i_point](1) << endl;
}
outdata.close();
10、Qt界面鼠标选点关键部分
网上有很多控制台实现的代码，但是对于Qt应用的实现较少，遂记录之。
不同之处在于Qt要在类中实现，一开始回调函数的作用域定义为文件内，结果出错。
后来将回调函数放在了窗口类中作为静态成员函数，
回调传参设为this指针，这下直接能用了，能用就行，没有深究。
//向view注册屏幕选择事件
pcl::PointCloud<PointT>::Ptr clicked_points_3d_(new pcl::PointCloud<PointT>);
//回调函数（参数由用户设置）
void _PointCloudUI::pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
_PointCloudUI* data = (_PointCloudUI*)args; //获取参数指针
if (event.getPointIndex() == -1) //鼠标事件异常，退出。
return;
PointT current_point;
event.getPoint(current_point.x, current_point.y, current_point.z); //从事件中获取当前点坐标
data->clicked_points_3d->push_back(current_point); //存下这个点
// 设置屏幕渲染属性，红色显示选择的点
pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
data->viewer->removePointCloud("clicked_points");
data->viewer->addPointCloud(data->clicked_points_3d, red, "clicked_points");
data->viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "clicked_points");
//在文本框显示
QString PointPosition = QString("(%1, %2, %3)").arg(current_point.x).arg(current_point.y).arg(current_point.z);
data->ui.textBrowser_message->append(QString("点坐标：" + PointPosition));
}
二、经典算法
1、3次b样条曲线实现
实质就是需要两步：生成控制点、根据控制点拟合插值。
b样条生成过程中是针对每个维度来讲的，因此可以实现二维、三维的拟合插值。
//通过离散点集反求控制点，使用LU的追加法
vector<VectorXXf> BSplineInterpolate(vector<VectorXXf> _discretePoints)
{
vector<VectorXXf> _controlPoints;
//初始化相关参数 AX=b
int N = _discretePoints.size();
Eigen::MatrixXf A = Eigen::MatrixXf::Zero(N + 2, N + 2);
Eigen::VectorXf p_x = Eigen::VectorXf::Zero(N + 2);
Eigen::VectorXf p_y = Eigen::VectorXf::Zero(N + 2);
Eigen::VectorXf b_x = Eigen::VectorXf::Zero(N + 2);
Eigen::VectorXf b_y = Eigen::VectorXf::Zero(N + 2);
for (int row = 0; row < N + 2; row++)
{
//初始化A
//第0行
if (row == 0)
{
A(0, 0) = 6;
A(0, 1) = -6;
}
//第1行-第N行
else if (row > 0 && row < N + 1)
{
A(row, row) = 4;
A(row, row - 1) = 1;
A(row, row + 1) = 1;
}
//第N+1行
else
{
A(N + 1, N) = 6;
A(N + 1, N + 1) = -6;
}
//初始化b
if (row > 0 && row < N + 1)
{
b_x(row) = 6 * _discretePoints[row - 1](0);
b_y(row) = 6 * _discretePoints[row - 1](1);
}
}
//方法一：追赶法 A分解为L、U
Eigen::MatrixXf T = A;
for (int row = 1; row < N + 2; row++)
{
T(row, row - 1) = T(row, row - 1) / T(row - 1, row - 1);
T(row, row) = T(row, row) - T(row - 1, row) * T(row, row - 1);
}
Eigen::MatrixXf L = Eigen::MatrixXf::Identity(N + 2, N + 2);//单位矩阵
for (int row = 1; row < N + 2; row++)
{
L(row, row - 1) = T(row, row - 1);
}
Eigen::MatrixXf U = Eigen::MatrixXf::Zero(N + 2, N + 2);
U(N + 1, N + 1) = T(N + 1, N + 1);
for (int row = 0; row < N + 1; row++)
{
U(row, row) = T(row, row);//对角线对应T
U(row, row + 1) = T(row, row + 1);
}
Eigen::VectorXf X = L.inverse() * b_x;
p_x = U.inverse() * X;
Eigen::VectorXf Y = L.inverse() * b_y;
p_y = U.inverse() * Y;
//方法二：x=A逆b
//p_x = A.inverse() * b_x;
//p_y = A.inverse() * b_y;
//将控制点存如容器
for (int i_point = 0; i_point < N + 2; i_point++)
{
VectorXXf point_temp(p_x(i_point), p_y(i_point));
_controlPoints.push_back(point_temp);
}
return _controlPoints;
}
/*B样条曲线拟合
@return 返回拟合得到的曲线的插值点
@discretePoints 输入的离散点，至少4个点
@closed 是否拟合闭合曲线，true表示闭合，false不闭合
@stride 拟合精度
*/
vector<VectorXXf> BSplineFit(vector<VectorXXf> _discretePoints, bool _closed, double _stride)
{
vector<VectorXXf> _fittingPoints;
//遍历所有离散点 由于封闭和不封闭的区别，这里遍历前后都少几个。
for (int i = 0; i < (_closed ? _discretePoints.size() : _discretePoints.size() - 3); i++) {
//构造基函数系数矩阵和输入离散点的乘积
VectorXXf xy[4];
xy[0] = (_discretePoints[i] + 4 * _discretePoints[(i + 1) % _discretePoints.size()] + _discretePoints[(i + 2) % _discretePoints.size()]) / 6;
xy[1] = -(_discretePoints[i] - _discretePoints[(i + 2) % _discretePoints.size()]) / 2;
xy[2] = (_discretePoints[i] - 2 * _discretePoints[(i + 1) % _discretePoints.size()] + _discretePoints[(i + 2) % _discretePoints.size()]) / 2;
xy[3] = -(_discretePoints[i] - 3 * _discretePoints[(i + 1) % _discretePoints.size()] + 3 * _discretePoints[(i + 2) % _discretePoints.size()] - _discretePoints[(i + 3) % _discretePoints.size()]) / 6;
//完成最终的P(t)公式计算，根据拟合精度计算每个插值点的值totalPoints
for (double t = 0; t <= 1; t += _stride) {
VectorXXf totalPoints = VectorXXf(0, 0);
for (int j = 0; j < 4; j++) {
totalPoints += xy[j] * pow(t, j);
}
_fittingPoints.push_back(totalPoints);//存储计算的插值
}
}
return _fittingPoints;
}
三、cc常用
1、两个点之间的距离
Tools->point picking
2、移除当前视角被遮挡的点云
Display->Toggle Centered Perspective：切换到视角状态
Plugins->Hidden Point Removal：移除被遮挡点
3、合并两块点云（并集）
选中两块点云Edit->Merge
4、计算两点云最短距离点之间的集合（找交集）
选中两块点云Tools->Distances->Closest Point Set
2180
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
