空间点云与某空间点的距离解算_范数方法
目标
计算空间点云与某特定空间点的全部距离;
方法
点云坐标相当于三维空间向量,两三维点间的向量模即为两点间距离,也相当于3维空间二范数。
假设两点的向量为
A
^
\hat {A}
A^,则待求解为
∣
∣
A
^
∣
∣
2
||\hat{A}||_2
∣∣A^∣∣2,matlab里面的二范数方法为norm(A,2)。
- 首先,读取空间点云;
- 然后,设置特定空间点;
- 接着,使用MATLAB,vecnorm方法,计算每一列的2-范数,获取每一列的欧式距离。
matlab代码
%% Program for normTest to calculate range
radarEndPoints = [7.044000000000001,-6.996000000000000,-0.140000000000000;
5.962500000000000,-7.263999999999999,-0.147500000000000];
radarPostion = (radarEndPoints(1,:) +radarEndPoints(2,:) )/2;
terrainPCL = load('normTestPCL.txt');
for i = 1:length(terrainPCL(:,1))
range(i) = norm(radarPostion-terrainPCL(i,1:3),2);
end
normMatric = radarPostion-terrainPCL(:,1:3);
rangeMatrix = vecnorm(normMatric');
range和rangeMatrix方法比较
eigen求二范数代码
要点是注意eigen中建立的向量是colmajor还是rowmajor,默认是colmajor,为了方便初始化,这里选用了rowmajor形式。具体的在visual studio中新建SquarNorm项目,然后添加main.cpp,键入以下代码:
#include "../Common/common.h"
using namespace Eigen;
using namespace std;
int main()
{
RowVectorXd m_point1(1,3),m_point2(1, 3), m_length(1, 3);
m_point1 << 7.044, -6.996, -0.140;
m_point2 << 5.963, -7.264, -0.148;
m_length = (m_point1 - m_point2).transpose();
cout << "SquareNorm:\n" << m_length.transpose().squaredNorm() << endl;
return 0;
}
运行结果: