点到直线距离最小二乘解释
推倒部分
形象描述是C到AB距离最短,也就是CD最短用数学语言描述是
m
i
n
∣
∣
(
B
−
A
)
λ
+
A
−
C
∣
∣
min||(B-A) \lambda + A - C ||
min∣∣(B−A)λ+A−C∣∣ 其中
D
=
(
B
−
A
)
λ
+
A
D = (B-A) \lambda + A
D=(B−A)λ+A,其实本质是一个最小二乘问题。因此带入得到
λ
=
(
(
B
−
A
)
T
(
B
−
A
)
)
−
1
(
B
−
A
)
T
(
C
−
A
)
\lambda = ((B-A)^T(B-A))^{-1}(B-A)^T(C-A)
λ=((B−A)T(B−A))−1(B−A)T(C−A)
因此带入得到
C
D
CD
CD
D
C
=
C
−
[
λ
(
B
−
A
)
+
A
]
DC = C - [\lambda (B-A)+A]
DC=C−[λ(B−A)+A]
实现代码
#include "bits/stdc++.h"
#include "eigen3/Eigen/Core"
// 求点到直线距离
using namespace std;
// 定义求解的模板类
// 目的求解C点到AB两点的距离
template<typename point>
class solve
{
private:
point mPA;
point mPB;
point mPC;
public:
// 求解的部分
point getMinVec(bool isNeedCheck = false){
point subBA = mPB - mPA;
double inv = 1./(subBA.transpose()*subBA);
double temp = (subBA.transpose()*(mPC-mPA));
double lambda = inv * temp;
point res = mPC - subBA * lambda - mPA;
// 验证 点积为0,是垂直的状态
if(isNeedCheck){
cout << "check " << (res.transpose() * subBA).transpose() << endl;
}
return res;
}
solve(point _A, point _B, point _C):
mPA(_A), mPB(_B), mPC(_C){
}
~solve(){}
};
int main(int argc, char ** argv){
// 例子1 求解两个2维度的点构成的线 与另一个点的距离
Eigen::Matrix<double,2,1> iA(1,1);
Eigen::Matrix<double,2,1> iB(2,2);
Eigen::Matrix<double,2,1> iC(0,1);
solve<Eigen::Matrix<double,2,1>> cal(iA, iB, iC);
Eigen::Vector2d res = cal.getMinVec();
cout << "2 维度向量:" << res.transpose() << endl;
cout << "2 模长:" << res.norm() << endl;
// 例子2 3维度的
Eigen::Matrix<double, 3, 1> iA3(1, 1, 1);
Eigen::Matrix<double, 3, 1> iB3(2, 2, 2);
Eigen::Matrix<double, 3, 1> iC3(0, 1, 0);
solve<Eigen::Matrix<double,3,1>> cal3(iA3, iB3, iC3);
Eigen::Vector3d res3 = cal3.getMinVec(true);
cout << "3 维度向量:" << res3.transpose() << endl;
cout << "3 模长:" << res3.norm() << endl;
return 0;
}