https://download.csdn.net/download/w2492602718/89487609
本文源代码及测试数据已上传CSDN，免费资源，引用请注明。
本文测试数据来自山东科技大学测绘与空间信息学院，特此鸣谢！
本文所有代码均由CSDN用户CV-X.WANG提供，任何个人或者团体，不得进行商用和教学活动，引用或部分引用，均需获得授权。
一、RPC简介
RPC，即Rational Polynomial Coefficient，中文释义：有理多项式系数，是一种用于描述卫星影像几何成像模型的方法，它通过建立影像地理信息的数学模型，保护了卫星相关参数的机密性。
二、模型的建立
为求解RPC参数，在此，我们用到了有理函数模型。在该模型中，我们是将像点坐标（R，C）表示为以相应的物方点空间坐标（X，Y，Z）为自变量的多项式的比值。如下式所示。
n为第n个点；
上式中，（Rn，Cn）表示为影像的像素坐标；
（Xn，Yn，Zn）是经过平移缩放后的正则化坐标，正则化后取值位于[-1,1]；
P1如下图所示，P2,P3,P4只需要将下式中的ai改写为bi、ci、di即可。
RFM 采用正则化坐标是为了提高模型中各系数求解的稳定性并减少计算过程中由于数据数量级差别过大引入的舍入误差，正则化公式为
求解正则化平移参数和正则化比例参数：
其中，m为控制点数。
三、有理函数模型的形式
RFM中的多项式可以取不同的阶数，分母可以相同也可以不相。下表给出了RFM的9种形式、各种形式下待求RPC参数的个数以及求解RPC参数所需的最少地面控制点。
正常情况而言，我们通常是解算78个参数即可。
四、C++求解RPC参数全过程及源码
1.读取sl-BLH.csv文件，提取参数
下图为sl-BLH.csv文件内容
#include"source.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <algorithm>//获取数组的最大值和最小值
#include <cmath>//计算绝对值
#include <limits> // 包含数值类型的最大值和最小值
#include <Eigen/Dense>
// Function to split a string by a delimiter
std::vector<std::string> split(const std::string &s, char delimiter) {
std::vector<std::string> tokens;
std::string token;
std::istringstream tokenStream(s);
while (std::getline(tokenStream, token, delimiter)) {
tokens.push_back(token);
}
return tokens;
}
int main() {
// Open input file
std::ifstream inputFile("sl-BLH.csv");
if (!inputFile.is_open()) {
std::cerr << "Error opening input file." << std::endl;
return 1;
}
// Temporary storage for extracted data
std::vector<Cartesian_Coordinate> extractedDataXYZ; // 存储笛卡尔坐标
std::vector<ImagePts> extractedSL;//存储像点
// Read input file line by line
std::string line;
while (std::getline(inputFile, line)) {
// Split line by ","
std::vector<std::string> tokens = split(line, ',');
// Check if the first column value divided by 100 has a remainder of 0 and the second column value is 0
if (tokens.size() >= 20) { // Ensure there are enough columns
try {
int firstColumn = std::stoi(tokens[0]);
int secondColumn = std::stoi(tokens[1]);
if (firstColumn % 100 == 0 && secondColumn % 100 == 0) {
ImagePts imgpts;
imgpts.C= std::stod(tokens[0]); // 列号
imgpts.R = std::stod(tokens[1]); // 行号
extractedSL.push_back(imgpts);
// Convert BLH to XYZ
Geodetic_Coordinate geodetic;
//BLH 纬度 经度 高程
geodetic.B = std::stod(tokens[17]); // 纬度
geodetic.L = std::stod(tokens[18]); // 经度
geodetic.H = std::stod(tokens[19]); // 高度
Cartesian_Coordinate cartesian;
geodetic_to_cartesian(geodetic, &cartesian);
// Store XYZ in extractedDataXYZ vector
extractedDataXYZ.push_back(cartesian);
}
}
catch (const std::invalid_argument& e) {
std::cerr << "Invalid argument: " << e.what() << std::endl;
}
catch (const std::out_of_range& e) {
std::cerr << "Out of range: " << e.what() << std::endl;
}
}
else {
std::cerr << "Error: Insufficient columns in the line." << std::endl;
}
}
// Close input file
inputFile.close();
2.正则化求X0,Y0,Z0,R0,C0
// 定义变量以保存计算结果
number_Sum sum;
// 计算imgpts.s和imgpts.l内所有数值的和
for (const auto& imgpts : extractedSL) {
sum.CSum += imgpts.C;
sum.RSum += imgpts.R;
}
//计算geodetic.B、L、H（实际上是XYZ的值）内所有数值的和
for (const auto& geodetic : extractedDataXYZ) {
sum.Xsum += geodetic.X;
sum.Ysum += geodetic.Y;
sum.Zsum += geodetic.Z;
}
int num = 8405;
//正则化平移参数
Regularization0 Regularization0;
Regularization0.X0 = sum.Xsum / num;
Regularization0.Y0 = sum.Ysum / num;
Regularization0.Z0 = sum.Zsum / num;
Regularization0.C0 = sum.CSum / num;
Regularization0.R0 = sum.RSum / num;3.求Xs,Ys,Zs,Rs,Cs
//正则化比例系数的最大值、最小值（前提）
MAX_MIN max_min;
//求像点CR列行 的最大最小值
// 初始化最大值和最小值为第一个元素的值
max_min.Cmax = extractedSL[0].C;
max_min.Cmin = extractedSL[0].C;
max_min.Rmax = extractedSL[0].R;
max_min.Rmin = extractedSL[0].R;
// 遍历 extractedSL 向量，更新最大值和最小值
for (const auto& imgpts : extractedSL) {
if (imgpts.C > max_min.Cmax) {
max_min.Cmax = imgpts.C; // 更新最大值
}
if (imgpts.C < max_min.Cmin) {
max_min.Cmin = imgpts.C; // 更新最小值
}
}
for (const auto& imgpts : extractedSL) {
if (imgpts.R > max_min.Rmax) {
max_min.Rmax = imgpts.R; // 更新最大值
}
if (imgpts.R < max_min.Rmin) {
max_min.Rmin = imgpts.R; // 更新最小值
}
}
//求像点XYZ的最大最小值
max_min.Xmax = extractedDataXYZ[0].X;
max_min.Xmin = extractedDataXYZ[0].X;
max_min.Ymax = extractedDataXYZ[0].Y;
max_min.Ymin = extractedDataXYZ[0].Y;
max_min.Zmax = extractedDataXYZ[0].Z;
max_min.Zmin = extractedDataXYZ[0].Z;
// 遍历extractedDataXYZ向量，更新最大值和最小值
for (const auto& XYZ : extractedDataXYZ) {
if (XYZ.X > max_min.Xmax) {
max_min.Xmax = XYZ.X; // 更新最大值
}
if (XYZ.X < max_min.Xmin) {
max_min.Xmin = XYZ.X; // 更新最小值
}
}
for (const auto& XYZ : extractedDataXYZ) {
if (XYZ.Y > max_min.Ymax) {
max_min.Ymax = XYZ.Y; // 更新最大值
}
if (XYZ.Y < max_min.Ymin) {
max_min.Ymin = XYZ.Y; // 更新最小值
}
}
for (const auto& XYZ : extractedDataXYZ) {
if (XYZ.Z > max_min.Zmax) {
max_min.Zmax = XYZ.Z; // 更新最大值
}
if (XYZ.Z < max_min.Zmin) {
max_min.Zmin = XYZ.Z; // 更新最小值
}
}
//正则化比例系数的绝对值（前提）
abs_number abs = calculate_abs(max_min, Regularization0);
Regularization_num Regularization_num_s;
// 初始化 Regularization_num_s 的值为 abs 中的第一个绝对值
Regularization_num_s.Xs = abs.Xabs1;
Regularization_num_s.Ys = abs.Yabs1;
Regularization_num_s.Zs = abs.Zabs1;
Regularization_num_s.Cs = abs.Cabs1;
Regularization_num_s.Rs = abs.Rabs1;
//std::cout << "Xabs1=" << abs.Xabs1 << std::endl << "Xabs2=" << abs.Xabs2 << std::endl;
// 依次比较第二个绝对值，如果更大则更新 Regularization_num_s 的对应值
if (abs.Xabs2 > abs.Xabs1) Regularization_num_s.Xs = abs.Xabs2;
if (abs.Yabs2 > abs.Yabs1) Regularization_num_s.Ys = abs.Yabs2;
if (abs.Zabs2 > abs.Zabs1) Regularization_num_s.Zs = abs.Zabs2;
if (abs.Cabs2 > abs.Cabs1) Regularization_num_s.Cs = abs.Cabs2;
if (abs.Rabs2 > abs.Rabs1) Regularization_num_s.Rs = abs.Rabs2;
4.推导出Xn,Yn,Zn,Rn,Cn
//正则化结果
std::vector<double>Xn;
std::vector<double>Yn;
std::vector<double>Zn;
std::vector<double>Cn;
std::vector<double>Rn;
// 遍历 extractedDataXYZ 中的每个元素
for (const auto& point : extractedDataXYZ) {
// 将每个元素的 X 值与 Regularization0.X0 相加，并将结果添加到新向量 zzh_resultX 中
Xn.push_back((point.X - Regularization0.X0) / Regularization_num_s.Xs);
Yn.push_back((point.Y - Regularization0.Y0) / Regularization_num_s.Ys);
Zn.push_back((point.Z - Regularization0.Z0) / Regularization_num_s.Zs);
}
//遍历extractedSL中的每个元素
for (const auto& sl : extractedSL) {
Cn.push_back((sl.C - Regularization0.C0) / Regularization_num_s.Cs);
Rn.push_back((sl.R - Regularization0.R0) / Regularization_num_s.Rs);
}
//std::cin.get();
// 创建一个列矩阵Rn_matrix，大小为Rn.size()行1列
Eigen::MatrixXd Rn_matrix(Rn.size(), 1);
// 将Rn中的元素逐个写入列矩阵中
for (int i = 0; i < Rn.size(); ++i) {
Rn_matrix(i, 0) = Rn[i];
}
// 创建一个列矩阵Cn_matrix，大小为Cn.size()行1列
Eigen::MatrixXd Cn_matrix(Cn.size(), 1);
// 将Rn中的元素逐个写入列矩阵中
for (int i = 0; i < Cn.size(); ++i) {
Cn_matrix(i, 0) = Cn[i];
}5.利用最小二乘求解RPC参数
（1）公式推导
为了能够采用最小二乘平差原理求解RPC参数，可将式Rn、Cn改写成如下形式:
对上述式子线性化后，可以得到误差方程（为书写方便，此处省略下标n）
由于J和K相互独立，利用上式求解RPC参数时可分开进行。
以J的求解为例，若有m个地面控制点，则可将式上式子中的第一式写成矩阵形式:
根据最小二乘原理，可得法方程
我们可以根据上述式子，求出J，J实际上就是在行方向上的39个参数组成的矩阵。
下面代码为求解J矩阵的全流程
/构建M矩阵///
// Ensure Xn, Yn, and Zn have the same size
size_t dataSize = extractedDataXYZ.size();
if (Xn.size() != dataSize || Yn.size() != dataSize || Zn.size() != dataSize) {
std::cerr << "Error: Size mismatch between Xn, Yn, and Zn vectors." << std::endl;
return 1;
}
// 创建一个矩阵，大小为Xn.size()行39列，实际上是8405行。
Eigen::MatrixXd M_matrix(dataSize, 39);
// 填充矩阵M
for (int i = 0; i < dataSize; ++i)
{
// 第一列为1
M_matrix(i, 0) = 1.0;
// 第二列为Zn中的第i个数值
M_matrix(i, 1) = Zn[i];
// 第三列为Yn中的第i个数值
M_matrix(i, 2) = Yn[i];
// 第四列为Xn中的第i个数值
M_matrix(i, 3) = Xn[i];
//第五列为ZY
M_matrix(i, 4) = Zn[i] * Yn[i];
//6 ZX
M_matrix(i, 5) = Zn[i] * Xn[i];
//7 YX
M_matrix(i, 6) = Yn[i] * Xn[i];
//8 Z2
M_matrix(i, 7) = Zn[i] * Zn[i];
//9 Y2
M_matrix(i, 8) = Yn[i] * Yn[i];
//10 X2
M_matrix(i, 9) = Xn[i] * Xn[i];
//11 ZYX
M_matrix(i, 10) = Zn[i] * Yn[i] * Xn[i];
//12 Z2Y
M_matrix(i, 11) = Zn[i] * Zn[i] * Yn[i];
//13 Z2X
M_matrix(i, 12) = Zn[i] * Zn[i] * Xn[i];
//14 Y2Z
M_matrix(i, 13) = Yn[i] * Yn[i] * Zn[i];
//15 Y2X
M_matrix(i, 14) = Yn[i] * Yn[i] * Xn[i];
//16 ZX2
M_matrix(i, 15) = Zn[i] * Xn[i] * Xn[i];
//17 YX2
M_matrix(i, 16) = Yn[i] * Xn[i] * Xn[i];
//18 Z3
M_matrix(i, 17) = Zn[i] * Zn[i] * Zn[i];
//19 Y3
M_matrix(i, 18) = Yn[i] * Yn[i] * Yn[i];
//20 X3
M_matrix(i, 19) = Xn[i] * Xn[i] * Xn[i];
//21 -Z
M_matrix(i, 20) = -1 * Zn[i];
//22 -Y
M_matrix(i, 21) = -1 * Yn[i];
//23 -X
M_matrix(i, 22) = -1 * Xn[i];
//24 -ZY
M_matrix(i, 23) = -1 * Zn[i] * Yn[i];
//25 -ZX
M_matrix(i, 24) = -1 * Zn[i] * Xn[i];
//26 -YX
M_matrix(i, 25) = -1 * Yn[i] * Xn[i];
//27 -Z2
M_matrix(i, 26) = -1 * Zn[i] * Zn[i];
//28 -Y2
M_matrix(i, 27) = -1 * Yn[i] * Yn[i];
//29 -X2
M_matrix(i, 28) = -1 * Xn[i] * Xn[i];
//30 -ZYX
M_matrix(i, 29) = -1 * Zn[i] * Yn[i] * Xn[i];
//31 -Z2Y
M_matrix(i, 30) = -1 * Zn[i] * Zn[i] * Yn[i];
//32 -Z2X
M_matrix(i, 31) = -1 * Zn[i] * Zn[i] * Xn[i];
//33 -Y2Z
M_matrix(i, 32) = -1 * Yn[i] * Yn[i] * Zn[i];
//34 -Y2X
M_matrix(i, 33) = -1 * Yn[i] * Yn[i] * Xn[i];
//35 -ZX2
M_matrix(i, 34) = -1 * Zn[i] * Xn[i] * Xn[i];
//36 -YX2
M_matrix(i, 35) = -1 * Yn[i] * Xn[i] * Xn[i];
//37 -Z3
M_matrix(i, 36) = -1 * Zn[i] * Zn[i] * Zn[i];
//38 -Y3
M_matrix(i, 37) = -1 * Yn[i] * Yn[i] * Yn[i];
//39 -X3
M_matrix(i, 38) = -1 * Xn[i] * Xn[i] * Xn[i];
}
输出矩阵M
//std::cout << "Matrix M:\n" << M_matrix << std::endl;
//std::cin.get();
// Output matrix M to a text file
std::ofstream outputFile("M_matrix_output.txt");
if (outputFile.is_open()) {
// Write matrix M to the file
outputFile << "Matrix M:\n" << M_matrix << std::endl;
// Close the file
outputFile.close();
std::cout << "Matrix M has been written to matrix_output.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
/构建M矩阵结束///
计算J矩阵求解 an bn
// Calculate the transpose of matrix M and save it as MT
Eigen::MatrixXd MT_matrix = M_matrix.transpose();
std::ofstream outputFile2("MT_matrix.txt");
if (outputFile2.is_open()) {
outputFile2 << "Matrix MT:\n" << MT_matrix << std::endl;
outputFile2.close();
std::cout << "Matrix MT has been written to MT_matrix.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
// Calculate the product M^T * M
Eigen::MatrixXd MTM = MT_matrix * M_matrix;
std::ofstream outputFile3("MTM_matrix.txt");
if (outputFile3.is_open()) {
outputFile3 << "Matrix MTM:\n" << MTM << std::endl;
outputFile3.close();
std::cout << "Matrix MTM has been written to MTM.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
// 检查矩阵是否可逆
double determinant = MTM.determinant();
if (determinant == 0) {
std::cerr << "矩阵不可逆！" << std::endl;
// 退出程序
}
//矩阵不可逆 只能求解伪逆矩阵
// 计算 M^T * M 的伪逆
Eigen::MatrixXd MTM_inverse = MTM.completeOrthogonalDecomposition().pseudoInverse();
std::ofstream outputFile4("MTM_inverse.txt");
if (outputFile4.is_open()) {
outputFile4 << "MTM_inverse:\n" << MTM_inverse << std::endl;
outputFile4.close();
std::cout << "Matrix MTM_inverse has been written to MTM_inverse.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
Calculate the inverse of the product (MTM)^(-1)
//Eigen::MatrixXd MTM_inverse = MTM.inverse();
// Calculate M^T * R
Eigen::MatrixXd MT_R = MT_matrix * Rn_matrix;
std::ofstream outputFile5("MT_R.txt");
if (outputFile5.is_open()) {
outputFile5 << "MT_R_inverse:\n" << MT_R << std::endl;
outputFile5.close();
std::cout << "Matrix MT_R has been written to MT_R.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
// Calculate J = (MTM)^(-1) * (MT_R)
Eigen::MatrixXd J = MTM_inverse * MT_R;
// Output the matrix to a .txt file
std::ofstream outputFile1("J_matrix.txt");
if (outputFile1.is_open()) {
outputFile1 << "Matrix J:\n" << J << std::endl;
outputFile1.close();
std::cout << "Matrix J has been written to J_matrix.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
计算J矩阵求解 an bn结束
同理可求K矩阵，进而获得列方向的39个参数组成的矩阵。
//同理计算K矩阵，求解cn，dn
// Calculate M^T * C
Eigen::MatrixXd MT_C = MT_matrix * Cn_matrix;
std::ofstream outputFile6("MT_C.txt");
if (outputFile6.is_open()) {
outputFile6 << "MT_C_inverse:\n" << MT_C << std::endl;
outputFile6.close();
std::cout << "Matrix MT_C has been written to MT_C.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
// Calculate J = (MTM)^(-1) * (MT_R)
Eigen::MatrixXd K = MTM_inverse * MT_C;
std::ofstream outputFile7("K_matrix.txt");
if (outputFile7.is_open()) {
outputFile7 << "Matrix K:\n" << K << std::endl;
outputFile7.close();
std::cout << "Matrix K has been written to K.txt" << std::endl;
}
else {
std::cerr << "Error opening output file." << std::endl;
return 1;
}
std::cin.get();
return 0;
}本文公式推导部分参考：
胡龙.基于有理函数模型的资源三号卫星影像对地目标定位试验[D].西南交通大学,2016.
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
