版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/qq_15642411/article/details/84652889
github :地址 程序下载
测试数据:
16.012 79.963 5083.205 5852.099 527.925
88.56 81.134 5780.02 5906.365 571.549
13.362 -79.37 5210.879 4258.446 461.81
82.24 -80.027 5909.264 4314.283 455.484
测试数据2
-86.15 -68.99 36589.41 25273.32 2195.17
-53.40 82.21 37631.08 31324.51 728.69
-14.78 -76.63 39100.97 24934.98 2386.50
10.46 64.43 40426.54 30319.81 757.31
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>
#include<fstream>
#include<math.h>
using namespace std;
double sumVector(vector<double> x)
{
double sum=0.0;
for(int i=0; i<x.size();++i)
{
sum+=x[i];
}
return sum/x.size();
}
// 代价函数的计算模型
struct Resection
{
Resection ( double X,double Y,double Z,double x, double y,double f ) :_X(X),_Y(Y),_Z(Z), _x ( x ), _y ( y ),_f(f) {}
// 残差的计算
template <typename T>
bool operator() (const T* const camPose, T* residual ) const // 残差
{
T Xs=camPose[0];
T Ys=camPose[1];
T Zs=camPose[2];
T w=camPose[3];
T p=camPose[4];
T k=camPose[5];
T a1=cos(k)*cos(p);
T b1=-sin(k)*cos(w) + sin(p)*sin(w)*cos(k);
T c1=sin(k)*sin(w) + sin(p)*cos(k)*cos(w);
T a2=sin(k)*cos(p);
T b2=sin(k)*sin(p)*sin(w) + cos(k)*cos(w);
T c2=sin(k)*sin(p)*cos(w) - sin(w)*cos(k);
T a3=-sin(p);
T b3=sin(w)*cos(p);
T c3=cos(p)*cos(w);
// R=rotationVectorToMatrix(omega,pho,kappa);
residual[0]=T(_x)-T(_f)*T((a1*(_X-Xs)+b1*(_Y-Ys)+c1*(_Z-Zs))/(a3*(_X-Xs)+b3*(_Y-Ys)+c3*(_Z-Zs)));
residual[1]=T(_y)-T(_f)*T((a2*(_X-Xs)+b2*(_Y-Ys)+c2*(_Z-Zs))/(a3*(_X-Xs)+b3*(_Y-Ys)+c3*(_Z-Zs)));
return true; //千万不要写成return 0,要写成return true
}
private:
const double _x;
const double _y;
const double _f;
const double _X;
const double _Y;
const double _Z;
};
int main ( int argc, char** argv )
{
google::InitGoogleLogging(argv[0]);
//read file
string filename=argv[1];
ifstream fin(filename.c_str());
string line;
vector<double> x;
vector<double> y;
vector<double> X;
vector<double> Y;
vector<double> Z;
while(getline(fin,line))
{
char* pEnd;
double a,b,c,d,e;
a=strtod(line.c_str(),&pEnd);
b=strtod(pEnd,&pEnd);
c=strtod(pEnd,&pEnd);
d=strtod(pEnd,&pEnd);
e=strtod(pEnd,nullptr);
x.push_back(a);
y.push_back(b);
X.push_back(c);
Y.push_back(d);
Z.push_back(e);
}
//初始化参数
double camPose[6]={0};
camPose[0]=sumVector(X);
camPose[1]=sumVector(Y);
camPose[2]=sumVector(Z);
double f = 153.24; //mm
// camPose[2]=50*f;
//构建最小二乘
ceres::Problem problem;
try
{
for(int i=0;i<x.size();++i)
{
ceres::CostFunction *costfunction=new ceres::AutoDiffCostFunction<Resection,2,6>(new Resection(X[i],Y[i],Z[i],x[i]/1000,y[i]/1000,f/1000));
//将残差方程和观测值加入到problem,nullptr表示核函数为无,
problem.AddResidualBlock(costfunction,nullptr,camPose);
}
}
catch(...)
{
cout<<"costFunction error"<<endl;
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 输出到cout
// options.max_num_iterations=25;
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve ( options, &problem, &summary ); // 开始优化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 输出结果
cout<<summary.BriefReport() <<endl;
cout<<"*************结果****************"<<endl;
cout<<"estimated Xs,Ys,Zs,omega,pho,kappa = ";
for ( auto p:camPose) cout<<p<<" ";
cout<<endl;
return 0;
}
- 纠正以上错误,刚仔细看了一下,犯了大错,才发现ceres solver 出来的结果是no convergence(不收敛)
哪里出错了尼?
原来是初始化了初值(不能初始化,只能全0初始化)
这是修改后的代码(要注意)
double camPose[6]={};
// camPose[0]=sumVector(X);
// camPose[1]=sumVector(Y);
// camPose[2]=sumVector(Z);
正确截图: