利用间接平差进行多点拟合

最新推荐文章于 2024-04-24 21:12:34 发布

正义的小伙伴0_O

最新推荐文章于 2024-04-24 21:12:34 发布

阅读量1.1k

点赞数

文章标签： c++

本文链接：https://blog.csdn.net/DDDlc_/article/details/116532185

版权

最近自己在学习多点拟合方法，了解到测量平差理论基础，其本质是利用最小二乘解最优问题。

设拟合前后的两个三维点集 $P=\left \{ \vec{P_{i}} \right \}$ 、 $X=\left \{ \vec{X_{i}} \right \}$ ,则 $P=R*X+T$ 。

${ \left[ \begin{array}{ccc} X0 \\ Y0 \\ Z0 \\ \end{array} \right ]} =$ ${ \left[ \begin{array}{ccc} a1 & a2 & a3\\ a4 & a5 & a6\\ a7 & a8 & a9 \end{array} \right ]}*$ ${ \left[ \begin{array}{ccc} X1 \\ Y1 \\ Z1 \\ \end{array} \right ]}+$ ${ \left[ \begin{array}{ccc} t1 \\ t2 \\ t3 \\ \end{array} \right ]}$

首先将方程展开，

$\begin{cases}X0=a1X1+a2Y1+a3Z1+t1\\ Y0=a4X1+a5Y1+a6Z1+t2\\ Z0=a7X1+a8Y1+a9Z1+t3 \end{cases}$

利用泰勒展开式将方程一阶展开：

$\begin{cases}X0=a10X1+a20Y1+a30Z1+t10+X1da1+Y1da2+Z1da3+dt1 \\Y0=a40X1+a50Y1+a60Z1+t20+X1da4+Y1da5+Z1da6+dt2 \\Z0=a70X1+a80Y1+a90Z1+t30+X1da7+Y1da8+Z1da9+dt3 \end{cases}$

整理后得

$B=\begin{bmatrix} X1 &Y1 & Z1 &0 &0 &0 &0 &0 &0 &1 &0 & 0\\ 0 &0 &0 &X1 &Y1 & Z1 &0 &0 &0 &0 &1 & 0\\ 0 &0 &0&0 &0 &0 &X1 &Y1 & Z1 &0 &0 & 1\\ \end{bmatrix}$ $Defaultvalue = \begin{bmatrix} a10\\ a20\\ a30\\ a40\\ a50 \\a60 \\a70 \\a80 \\a90 \\t10 \\t20 \\ t30 \end{bmatrix}$ $Delta_x= \begin{bmatrix} da1\\ da2\\ da3\\ da4\\ da5 \\da6 \\da7 \\da8 \\da9 \\dt1 \\dt2 \\ dt3 \end{bmatrix}$

$P=B*Defaultvalue+B*Delta_x$

当输入为点集时，P矩阵大小为3n*1。B矩阵大小为3n*12，因为展开式有3个轴*n个向量。

根据平差理论基础 $V=B\hat{x}-l$

又因为 $V=B* Delta_x -\left ( P - B * Defaultvalue \right )$

所以我们得到过程量l、矩阵B，通过优化 $\hat{x}$ ，使 $\hat{x}=\left ( B^{T}PB \right )^{-1}B^{T}Pl$ ，将 $\hat{x}$ 加到Defaultvalue中，更新使得 $\hat{x}=X0+\hat{x}$ ，通过设定阈值或迭代次数来终止迭代，得到最终的优化结果。

练手题目：

两个三维点集 $P=\left \{ \vec{P_{i}} \right \}$ 、 $X=\left \{ \vec{X_{i}} \right \}$ ，两个集合点的数量相同，都是（大于等于4），且几何不相关（不共面），并且点是一一对应的，即 $\vec{p_{i}}$ 对应 $\vec{x_{i}}$ 。现在需要用C++设计一个最小二乘算法，使得两个点集彼此收敛到对应点距离和最小，这里的距离采用欧式距离。具体算法可参考文献1中的间接平差。测试数据自己造就可以：造两个相似的点集（ $N\geq 4$ ），两个点集相差一个线性变换（ $\vec{p_{i}}=R\vec{x_{i}}+T$ ），然后通过最小二乘拟合得到矩阵R和平移向量l 。

代码如下：

#include<iostream>
#include<iomanip>
#include <fstream>
#include <Eigen/Dense>
#include <string>
#include <vector>
using namespace std;
int NumsofPoint = 6;//拟合点个数
//生成数据函数（预设值）
void Generat_Defaultvalue() {
	Eigen::MatrixXf Matrix_A(3, 3);
	Eigen::MatrixXf Matrix_T(3, 5);
	Eigen::MatrixXf X1(3, 5);
	Eigen::MatrixXf Matrix_result(3, 5);
	Eigen::MatrixXf X0(3, 5);

	Matrix_A << 1, 0, 1,
		0, 1, 1,
		1, 0, 0;
	Matrix_T << 3, 3, 3, 3, 3,
		1, 1, 1, 1, 1,
		2, 2, 2, 2, 2;
	X1 << 1, 1, 1, 5, 1,
		2, 3, 0, 4, 5,
		3, 2, 2, 1, 2;
	X0 << 7, 6, 6, 9, 6,
		6, 6, 3, 6, 8,
		3, 3, 3, 7, 3;

	Matrix_result = Matrix_A*X1 + Matrix_T;
	std::cout << "Generating point coordinates:" << std::endl << Matrix_result << std::endl;
	getchar();
}
//利用间接平差做多点拟合(预设值）
int IndirectAdjustment_Defaultvalue() {
	//IndirectAdjustment
	int Iterations = 5;
	Eigen::MatrixXf B(15, 12);//整理后的矩阵
	Eigen::MatrixXf X1(3, 5);//原始点
	Eigen::MatrixXf X0(3, 5);//匹配点
	Eigen::MatrixXf XYZ_cal(15, 1);//匹配点格式调整
	Eigen::MatrixXf P(5, 5);//单位阵
	Eigen::MatrixXf l(3, 1);//中间过程
	Eigen::MatrixXf SE(12, 1);//预设值a10~t30
	Eigen::MatrixXf Finalresult(12, 1);//最终优化的结果
	Eigen::MatrixXf Delta_x(12, 1);//累加值
	P = Eigen::MatrixXf::Identity(15, 15);
	SE = Eigen::MatrixXf::Ones(12, 1);
	X1 << 1, 1, 1, 5, 1,
		2, 3, 0, 4, 5,
		3, 2, 2, 1, 2;//手动赋值
	X0 << 7, 6, 6, 9, 6,
		6, 6, 3, 6, 8,
		3, 3, 3, 7, 3;//手动赋值
	XYZ_cal << 7, 6, 3, 6, 6, 3, 6, 3, 3, 9, 6, 7, 6, 8, 3;
	//calculate
	B << 1, 2, 3, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 1,
		1, 3, 2, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 1, 3, 2, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 1, 3, 2, 0, 0, 1,
		1, 0, 2, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 1, 0, 2, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 1, 0, 2, 0, 0, 1,
		5, 4, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 5, 4, 1, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 5, 4, 1, 0, 0, 1,
		1, 5, 2, 0, 0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 1, 5, 2, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 1, 5, 2, 0, 0, 1;//15*12
	//迭代求最优解
	while (Iterations--) {
		l = XYZ_cal - B*SE;//15*1-15*12~12*1
		Delta_x = (B.transpose()*P*B).inverse()*B.transpose()*P*l;//(12*15~15*15~15*12)~12*15~15*15~15*1
		SE = Delta_x + SE;//12*1+12*1
	}
	//控制格式，小于阈值的数归0
	float threshold = 0.005;
	for (int i = 0; i < 12; i++) {
		if (SE(i) < threshold)
			SE(i) = 0;
	}
	std::cout << std::setprecision(1) << SE << std::endl;
	getchar();
	//system("pause");
	return 0;
}
//读取数据函数(3*NumsofPoint)
Eigen::MatrixXf Readtxt(string inputpath) {
	Eigen::MatrixXf X(3, NumsofPoint);//原始点
	fstream in;
	in.open(inputpath, ios::in);//打开一个file
	if (!in.is_open()) {
		cout << "Can not find txt " << endl;
		system("pause");
	}
	string buff;
	int i = 0;//行数i
	int x = 0, y = 0, k = 0;
	while (getline(in, buff)) {
		vector<double> nums;
		char *s_input = (char *)buff.c_str();
		const char * split = ",";
		char *p = strtok(s_input, split);
		double a;
		while (p != NULL) {
			a = atof(p);
			nums.push_back(a);
			p = strtok(NULL, split);
		}
		for (k = 0; k < NumsofPoint; k++) {
			X(i, k) = nums[k];
		}
		i++;
	}
	//std::cout << X << std::endl;
	in.close();
	return X;
}
//读取A矩阵函数(3*3)
Eigen::MatrixXf ReadAtxt(string inputpath) {
	Eigen::MatrixXf X(3, 3);//原始点
	fstream in;
	in.open(inputpath, ios::in);//打开一个file
	if (!in.is_open()) {
		cout << "Can not find txt " << endl;
		system("pause");
	}
	string buff;
	int i = 0;//行数i
	int x = 0, y = 0, k = 0;
	while (getline(in, buff)) {
		vector<double> nums;
		char *s_input = (char *)buff.c_str();
		const char * split = ",";
		char *p = strtok(s_input, split);
		double a;
		while (p != NULL) {
			a = atof(p);
			nums.push_back(a);
			p = strtok(NULL, split);
		}
		for (k = 0; k < 3; k++) {
			X(i, k) = nums[k];
		}
		i++;
	}
	//std::cout << X << std::endl;
	in.close();
	return X;
}
//利用间接平差做多点拟合（从文件中读取版）
void IndirectAdjustment_Readvalue() {
	//读取点
	Eigen::MatrixXf X0(3, NumsofPoint);//原始点
	Eigen::MatrixXf X1(3, NumsofPoint);//匹配点
	Eigen::MatrixXf XYZ_cal(NumsofPoint*3, 1);//匹配点格式调整
	X0 = Readtxt("E:\\C++_workspace\\MultiPointFitting\\X0.txt");//原始点);
	X1 = Readtxt("E:\\C++_workspace\\MultiPointFitting\\X1.txt");//原始点);
	//调整X0格式为行向量(按照X方向逐一赋值
	//	for (size_t t = 0, k = 0; t < 3 * NumsofPoint; t++) {
	//		XYZ_cal(t, 0) = X0(k, t%NumsofPoint);
	//		//t为XYZ_cal中列坐标行数，k为X0中坐标行数
	//		if (t % NumsofPoint == NumsofPoint - 1) {
	//			k++;
	//		}
	//	}
	//调整X0格式为行向量(按照Y方向逐一赋值
	for (size_t t = 0; t < NumsofPoint; t++) {
		for (size_t k = 0; k < 3; k++) {
			XYZ_cal(3 * t + k, 0) = X0(k, t);
		}
	}
	//构造B矩阵
	Eigen::MatrixXf B = Eigen::MatrixXf::Zero(3 * NumsofPoint, 12);
	for (size_t i = 0; i < NumsofPoint; i++) {
		for (size_t k = 0; k < 3; k++) {//j为第n个点的行数
			int j = k + i * 3;
			B(j, (j - 3 * i) * 3) = X1(0, i);
			B(j, (j - 3 * i) * 3 + 1) = X1(1, i);
			B(j, (j - 3 * i) * 3 + 2) = X1(2, i);
			B(j, (j - 3 * i) + 9) = 1;
			//std::cout << j<< " " << (j - 3 * i) * 3 << std::endl;
		}
	}
	Eigen::MatrixXf Defaultvalue(12, 1);//预设值a10~t30
	Eigen::MatrixXf Finalresult(12, 1);//最终优化的结果
	Eigen::MatrixXf Delta_x(12, 1);//累加值
	Eigen::MatrixXf P(5, 5);//单位阵
	Eigen::MatrixXf l(3, 1);//中间过程
	P = Eigen::MatrixXf::Identity(NumsofPoint*3, NumsofPoint*3);
	Defaultvalue = Eigen::MatrixXf::Ones(12, 1);
	int Iterations = 5;//迭代次数
	while (Iterations--) {
		l = XYZ_cal - B*Defaultvalue;//15*1-15*12~12*1
		Delta_x = (B.transpose()*P*B).inverse()*B.transpose()*P*l;//(12*15~15*15~15*12)~12*15~15*15~15*1
		Defaultvalue = Delta_x + Defaultvalue;//12*1+12*1
	}
	//控制格式，小于阈值的数归0
	float threshold = 0.005;
	for (int i = 0; i < 12; i++) {
		if (Defaultvalue(i) < threshold)
			Defaultvalue(i) = 0;
	}
	Finalresult = Defaultvalue;
	std::cout << std::setprecision(1) << Finalresult << std::endl;
	//system("pause");
	getchar();
}
//生成参考数据（从文件中读取版）
void Generat_Readvalue() {
	Eigen::MatrixXf X1(3, NumsofPoint);//匹配点
	X1 = Readtxt("E:\\C++_workspace\\MultiPointFitting\\X1.txt");
	Eigen::MatrixXf A(3, 3);
	/*
	A example:3*3
	A << 1, 0, 1,
	0, 1, 1,
	1, 0, 0;
	*/
	A = ReadAtxt("E:\\C++_workspace\\MultiPointFitting\\A.txt");
	Eigen::MatrixXf T(3, NumsofPoint);
	/*
	T example:3*NumsofPoint
	T << 3, 3, 3, 3, 3,
	1, 1, 1, 1, 1,
	2, 2, 2, 2, 2;
	*/
	T = Readtxt("E:\\C++_workspace\\MultiPointFitting\\T.txt");
	/*
	X1 << 1, 1, 1, 5, 1,
	2, 3, 0, 4, 5,
	3, 2, 2, 1, 2;
	X0 << 7, 6, 6, 9, 6,
	6, 6, 3, 6, 8,
	3, 3, 3, 7, 3;
	*/
	Eigen::MatrixXf Matrix_result(3, NumsofPoint);
	Matrix_result = A*X1 + T;//3*3~3*NumsofPoint+3*NumsofPoint
	std::cout << "Generating point coordinates:" << std::endl << Matrix_result << std::endl;
	getchar();
}
void main() {
	//Generat_Readvalue();
	IndirectAdjustment_Readvalue();
}

参考文献：

[1] 误差理论与测量平差基础武汉大学测绘学院测量平差学科组编著

[2] 测量平差之间接平差 http://blog.csdn.net/fourierFeng/article/details/47272167

正义的小伙伴0_O

关注

0
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
利用间接平差进行多点拟合

利用间接平差做多点拟合最近自己在学习多点拟合方法，了解到测量平差理论基础，其本质是利用最小二乘解最优问题。设拟合前后点集为X0、X1，则X0=R*X1+T。首先将方程展开，利用拉格朗日算子将方程一阶展开：整理后得B = X100 Y100 Z100 0X100 Y1 00Z1 000X1 00Y1 00Z1 1000 100 01TX0Y0Z0 = B * Defaultvalue +...
复制链接

扫一扫