ceres库学习

有关ceres库进行平差–记录学习

可以借鉴https://blog.csdn.net/cqrtxwd/article/details/78956227.

1.构建仿函数，重载()运算符，在重载中定义误差函数；有固定的格式要求，可以用类也可以用结构体，常用类，可以写一个专门存放仿函数的cost.h文件，定义在同一个命名空间下。
下面展示一些 内联代码片。

// 通过类定义仿函数CostFunctor
class CostFunctor{
public:
//声明构造函数,括号中放置传入的参数或对象，利用初始化列表的方式进行赋值
	CostFunctor(Type1 *a(对象),Type2 &b（传递）){}
	//重载运算符,d1,d2,residuals是传递仿函数的维度，d1d2是输入维度，residuals是输出维度
	template<typename T> bool operator(T d1,T d2,T *residuals(残差模块)){
		//一些特殊的数据类型都需要用T的形式，可以用using关键字，例如
		using Vector3JetT=Eigen::Matrix<T,3,1>;
		using QuaternionJetT = Eigen::Quaternion<T>;
    	using Matrix3JetT = Eigen::Matrix<T, 3, 3>;
    	//...进行误差函数的定义
    	//给定残差
    	residuals[0]=...;
    	residuals[1]=...;
    
		return true;
    }//重载运算符

private:
	Type1 *a；
	Type2 &b；
};// CostFunctor
	

2.进行平差，在adjustment()中进行
（1）首先对problem_私有成员变量实例化，在.h文件中声明私有成员变量
内联代码片。

std::shared_ptr<ceres::Problem> problem_;
std::vector<ceres::ResidualBlockId> blocks_;

在对应的.cpp文件中实例化

problem_ = std::make_shared<ceres::Problem>();
//对内存进行清理
block_.clear();

（2）添加观测方程,构建最小二乘

 ceres::CostFunction *cost = new ceres::AutoDiffCostFunction(自动求导)<CostFunctor, 3, 3, 3>(
            new ControlPointCostFunctor(a, b));
 //构建最小二乘，向problem类传递残差模块，代价函数，损失函数，函数输入（在重载运算符中的输入数据）
 auto block = problem_->AddResidualBlock(
            cost, new ceres::ScaledLoss(nullptr, （权）, ceres::TAKE_OWNERSHIP),d1.data(), d2.data());      
 block_.push(block);
 

（3）配置求解器

 ceres::Solver::Options solve_op;
    ceres::Solver::Options op;
    op.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    op.num_threads = Eigen::nbThreads();

    ceres::Solver::Summary summary;
    ceres::Solve(op, problem_.get(), &summary);

3.计算协方差矩阵等精度信息，在evaluate()中进行。
（1）求单位权中误差，在给定的参数位置求解problem，给出当前位置的cost,梯度，以及雅可比矩阵。

 //单位权中误差
 double sigma_0;
 //声明对象
 ceres::Problem::EvaluateOptions eval_op;
 //添加残差块，若只统计一个就直接赋值，若多个则需要用insert，需要注明插入的起点，以及插入的序列的起点和终点
 eval_op.residual_blocks = blocks1;
 eval_op.residual_blocks.insert(end(eval_op.residual_blocks), begin(blocks2), end(blocks2));
 eval_op.apply_loss_function = false;//必须定义

 double cost;
 std::vector<double> residuals;
 problem_->Evaluate(eval_op, &cost, &residuals, nullptr, nullptr);
 cost *= 2.0;                                                             // ceres solver 计算的是 1/2 f(x)^2
 sigma_0 = std::sqrt(cost / (residuals.size() - corrections_.size() * 3)); //中误差=sqrt（VTPV/(r-t)）

//若要统计残差，就不需要cost
problem_->Evaluate(eval_op, nullptr, &residuals, nullptr, nullptr);
//resiuals 中的记录残差的格式为 dx,dy,dz
for (int i = 0; i < .size(); ++i) {
 errors[i] = residuals[3 * i + 0] * residuals[3 * i + 0] + residuals[3 * i + 1] * residuals[3 * i + 1] +
 residuals[3 * i + 2] * residuals[3 * i + 2];
 errors[i] = sqrt(errors[i]) * config_.sigma;
}
//统计均方根误差
rmse(errors);

（2）计算协方差

ceres::Covariance::Options options;
options.apply_loss_function = false;
ceres::Covariance covariance(options);
std::vector<std::pair<const double *, const double *>> covariance_blocks;
//给covariance_blocks赋值
covariance_blocks.push_back(std::make_pair(c.second.data(), c.second.data()));
//计算协方差
covariance.Compute(covariance_blocks, problem_.get());
//建立协因数阵
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> cov_t = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();
//获取协因数阵
covariance.GetCovarianceBlock(correction.second.dx.data(), correction.second.dx.data(), cov_t.data());
cov_t = sigma_0 * sigma_0 * cov_t;//乘以单位权中误差的平方
//协因数阵的主对角线上的元素为观测值的协因数
sigma_x=std::sqrt(cov_t(0,0);
sigma_y=std::sqrt(cov_t(1,1);
sigma_z=std::sqrt(cov_t(2,2);