有关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);