ceres-solver拟合椭球

最新推荐文章于 2022-11-02 11:15:11 发布

Storming2011

最新推荐文章于 2022-11-02 11:15:11 发布

阅读量2k

点赞数

1. 库编译及引入。windows下用VS build ceres solver 库,

http://blog.csdn.net/h349117102/article/details/12680261;

http://blog.csdn.net/woaik110/article/details/45970031;

下面是椭球方程的公式。

我们要求解的就是。为了演示，我通过程序生成了一个单位球上面的一系列坐标，也就是上面的abc均为1，偏移量均为0。为了验证ceres-solver是否可以完成这个工作。

首先我们编写下面的测试代码。

下面是椭球方程的公式。

首先我们编写下面的测试代码。

#include "glog/logging.h"
#include "ceres/ceres.h"
#include <vector>
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
struct EllipsoidResidual
{
/*
* x, y, z 分别为观测值
*/
EllipsoidResidual(double x, double y, double z):_x(x), _y(y), _z(z){}
/*
*pEllipsoidParameters：-2分别为a、b、c,3-5分别为x0、y0、z0
*/
template <typename T> bool operator () (const T * const pEllipsoidParameters, T * residual) const
{
residual[0] = T(1.0) - T(_x - pEllipsoidParameters[3])*T(_x - pEllipsoidParameters[3])/T(pEllipsoidParameters[0]*pEllipsoidParameters[0]) -
T(_y - pEllipsoidParameters[4])*T(_y - pEllipsoidParameters[4])/T(pEllipsoidParameters[1]*pEllipsoidParameters[1]) -
T(_z - pEllipsoidParameters[5])*T(_z - pEllipsoidParameters[5])/T(pEllipsoidParameters[2]*pEllipsoidParameters[2]);
return true;
}
private:
const double _x;
const double _y;
const double _z;
};
struct Point3D
{
double x;
double y;
double z;
};
/*
* 用于解算椭球参数的类
* m_EllipsoidParameters：椭球参数的初始值
* m_bInitParameters:椭球参数是否已经进行初始化
* m_data：观测值
* m_options：解算方式选项，可以进行设置
* m_summary：解算的报告信息
*
* 使用方式：
* （）设置椭球参数的初始值（m_EllipsoidParameters），并标记m_bInitParameters为true
* （）设置观测值m_data(个数大于)
* （）调用SolveParameters()函数
* （）得到结果m_EllipsoidParameters与解算报告信息m_summary
*/
struct EllipsoidFittingSolver
{
EllipsoidFittingSolver()
{
m_options;
m_options.max_num_iterations = 25;
m_options.linear_solver_type = ceres::DENSE_QR;
m_options.minimizer_progress_to_stdout = true;
m_bInitParameters = false;
}
bool SolveParameters()
{
if (m_data.size() < 6 || !m_bInitParameters)
{
return false;
}
Problem problem;
const int nObservations = m_data.size();
for (int i = 0; i < nObservations; ++i)
{
problem.AddResidualBlock(new AutoDiffCostFunction<EllipsoidResidual, 1, 6>(
new EllipsoidResidual(m_data[i].x, m_data[i].y, m_data[i].z)),
NULL,
m_EllipsoidParameters);
}
Solve(m_options, &problem, &m_summary);
return true;
}
bool m_bInitParameters;
double m_EllipsoidParameters[6];
std::vector<Point3D> m_data;
Solver::Options m_options;
Solver::Summary m_summary;
};
double* readFile(const char* szFilename,int& nCount)
{
if(!szFilename)return NULL;
FILE* fid=fopen(szFilename,"rt");
if(!fid)return NULL;
nCount=0;
fscanf(fid,"%d",&nCount);//读第一行数据，是行数，赋给nCount
int nSize=sizeof(double)*3*nCount;//根据行数分配大小，要取列数据所以是*nCount
double* dbData=(double*)malloc(nSize);
if(!dbData)return NULL;
memset(dbData,0,nSize);//将申请的内存空间初始化为
double dbInvalid=0.;
for(int i=0;i<nCount;i++)
{
double* dbTmp=dbData+3*i;
fscanf(fid,"%lf %lf %lf",dbTmp,dbTmp+1,dbTmp+2);
}
fclose(fid);
return dbData;
}
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
EllipsoidFittingSolver efs;
double dInit[6] = {0.5,0.5,0.5,0.5,0.5,0.5};
memcpy(efs.m_EllipsoidParameters, dInit, sizeof(double)*6);
efs.m_bInitParameters = true;
const char* pszFilename = "testdata2.txt";
fprintf(stderr,"ReadFile begins.\n");
int nCount=-1;
double* dbSrcData= readFile(pszFilename, nCount);
if(nCount<0)
{
fprintf(stderr,"ReadFile Error!\n");
return -1;
}
else
fprintf(stderr,"ReadFile Completed.!\n");
for (int i=0; i<nCount; i++)
{
Point3D pt;
pt.x = dbSrcData[i*3+0];
pt.y = dbSrcData[i*3+1];
pt.z = dbSrcData[i*3+2];
efs.m_data.push_back(pt);
}
free(dbSrcData);
if(efs.SolveParameters())
{
fprintf(stdout,"Solver Success!\n");
fprintf(stdout,"a=%lf b=%lf c=%lf\n",efs.m_EllipsoidParameters[0],efs.m_EllipsoidParameters[1],efs.m_EllipsoidParameters[2]);
fprintf(stdout,"x0=%lf y0=%lf z0=%lf\n",efs.m_EllipsoidParameters[3],efs.m_EllipsoidParameters[4],efs.m_EllipsoidParameters[5]);
fprintf(stdout,"%s\n",efs.m_summary.FullReport().c_str());
}
else
fprintf(stdout,"Solver Failed!\n");
return 0;