学习ceres记录

本文主要是介绍学习ceres记录，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

@学习ceres记录

http://ceres-solver.org/nnls_tutorial.html

一.非线性最小二乘

在这里插入图片描述
$\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)$ 是ResidualBlock, $f_i(\cdot)$ 是CostFunction，
大多数情况一群小标量一起出现，例如相机的平移位姿和四元数旋转分量，例如 $\left[x_{i_1},... , x_{i_k}\right]$ 是ParameterBlock，也可以是一元的。
$\rho_i$ 是鲁棒核函数

1.1 例1 non-linear least squares problem

$\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2$
例如最小化：
$\frac{1}{2}(10 -x)^2.$

1.1.1 step1

write a functor that will evaluate this the function: $f (x) = 10 - x$

struct CostFunctor {template <typename T>bool operator()(const T* const x, T* residual) const {residual[0] = 10.0 - x[0];return true;}
};

operator() is a emplated method,允许ceres调用CostFunctor::operator()
T根据需要的类型：
T=double 直接需要数据
T=Jet 需要Jacobians

1.1.2 构建non-linear least squares problem

int main(int argc, char** argv) {google::InitGoogleLogging(argv[0]);// The variable to solve for with its initial value.double initial_x = 5.0;double x = initial_x;// Build the problem.Problem problem;// Set up the only cost function (also known as residual). This uses// auto-differentiation to obtain the derivative (jacobian).CostFunction* cost_function =new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);problem.AddResidualBlock(cost_function, nullptr, &x);// Run the solver!Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = true;Solver::Summary summary;Solve(options, &problem, &summary);std::cout << summary.BriefReport() << "\n";std::cout << "x : " << initial_x<< " -> " << x << "\n";return 0;
}
/*AutoDiffCostFunction将CostFunctor作为输入，
自动对其进行区分，并为其提供一个CostFunction接口。
*/

输出

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time0  4.512500e+01    0.00e+00    9.50e+00   0.00e+00   0.00e+00  1.00e+04       0    5.33e-04    3.46e-031  4.511598e-07    4.51e+01    9.50e-04   9.50e+00   1.00e+00  3.00e+04       1    5.00e-04    4.05e-032  5.012552e-16    4.51e-07    3.17e-08   9.50e-04   1.00e+00  9.00e+04       1    1.60e-05    4.09e-03
Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10

Ceres只在迭代结束时打印出显示，并在检测到收敛时立即终止，这就是为什么这里只看到两次迭代，而不是三次。

2.导数

2.1 Numeric Derivatives(数值导数)

残差调库函数时
用户构建残差，并创建NumericDiffCostFunction例如对于 $f (x) = 10 - x$
相应的factor是

struct NumericDiffCostFunctor {bool operator()(const double* const x, double* residual) const {residual[0] = 10.0 - x[0];return true;}
};

加入到问题中变为

CostFunction* cost_function =new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);

建议使用自动微分

2.2 Analytic Derivatives(解析微分)

在某些时候自动微分并不可靠，计算微分解析时比利用链式法则自动求导更有效。
在这种情况下自己提供your own residual and jacobian computation code
如果知道参数和残差的size(),定义CostFunction和SizedCostFunction的子类，以下是这种子类function的实现。

class QuadraticCostFunction : public ceres::SizedCostFunction<1 /* number of residuals */, 1 /* size of first parameter */> {public:virtual ~QuadraticCostFunction() {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {const double x = parameters[0][0];residuals[0] = 10 - x;// f'(x) = -1. Since there's only 1 parameter and that parameter// has 1 dimension, there is only 1 element to fill in the// jacobians.//// Since the Evaluate function can be called with the jacobians// pointer equal to nullptr, the Evaluate function must check to see// if jacobians need to be computed.//// For this simple problem it is overkill to check if jacobians[0]// is nullptr, but in general when writing more complex// CostFunctions, it is possible that Ceres may only demand the// derivatives w.r.t. a subset of the parameter blocks.// Compute the Jacobian if asked for.if (jacobians != nullptr && jacobians[0] != nullptr) {jacobians[0][0] = -1;}return true;}
};

SimpleCostFunction（实现的子类）::Evaluate提供了一个输入参数数组、一个用于残差的输出数组残差和一个用于 Jacobian 的输出数组 jacobians。 jacobians 数组是可选的，Evaluate 需要检查它是否为非空，如果是，则用残差函数的导数值填充它。在这种情况下，由于残差函数是线性的，雅可比矩阵是常数。
从上面的代码片段可以看出，实现CostFunction对象有点复杂。我们建议，除非您有充分的理由自己管理雅可比计算，否则您可以使用AutoDiffCostFunction或NumericDiffCostFunction来构造剩余块。

2.3 More About Derivatives

3 Powell’s Function

error

static Eigen::Matrix<double,3,3> skew(Eigen::Matrix<double,3,1>& mat_in){                 //  反对称矩阵定义Eigen::Matrix<double,3,3> skew_mat;skew_mat.setZero();skew_mat(0,1) = -mat_in(2);skew_mat(0,2) =  mat_in(1);skew_mat(1,2) = -mat_in(0);skew_mat(1,0) =  mat_in(2);skew_mat(2,0) = -mat_in(1);skew_mat(2,1) =  mat_in(0);return skew_mat;
}class PlaneAnalyticCostFunction  :   public  ceres::SizedCostFunction<1, 4, 3>{
public:Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;Eigen::Vector3d ljm_norm;double s;PlaneAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_): curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),last_point_m(last_point_m_), s(s_){}virtual  bool  Evaluate(double  const  *const  *parameters, double  *residuals, double  **jacobians)const {      //   定义残差模型// 叉乘运算， j,l,m 三个但构成的平行四边面积(摸)和该面的单位法向量(方向)Eigen::Vector3d  ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);ljm_norm.normalize();    //  单位法向量Eigen::Map<const Eigen::Quaterniond>  q_last_curr(parameters[0]);Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);Eigen::Vector3d  lp;      // “从当前阵的当前点” 经过转换矩阵转换到“上一阵的同线束激光点”Eigen::Vector3d  lp_r = q_last_curr *  curr_point ;                        //  for compute jacobian o rotation  L: dp_drlp = q_last_curr *  curr_point  +  t_last_curr;  // 残差函数double  phi1 =  (lp - last_point_j ).dot(ljm_norm);residuals[0]  =   std::fabs(phi1);if(jacobians != NULL){if(jacobians[0] != NULL){phi1 = phi1  /  residuals[0];//  RotationEigen::Matrix3d  skew_lp_r  = skew(lp_r);Eigen::Matrix3d  dp_dr;dp_dr.block<3,3>(0,0) =  -skew_lp_r;Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor>>  J_so3_r(jacobians[0]);J_so3_r.setZero();J_so3_r.block<1,3>(0,0) =  phi1 *  ljm_norm.transpose() *  (dp_dr);Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor>>  J_so3_t(jacobians[1]);J_so3_t.block<1,3>(0,0)  = phi1 * ljm_norm.transpose();                                                                                                                                                                                                                                            }}return  true;}
};

如果不加static

/usr/bin/ld: CMakeFiles/aloam_laser_odometry_node.dir/src/models/loam/aloam_registration.cpp.o: in function `skew(Eigen::Matrix<double, 3, 1, 0, 3, 1>&)':
aloam_registration.cpp:(.text+0x0): multiple definition of `skew(Eigen::Matrix<double, 3, 1, 0, 3, 1>&)'; CMakeFiles/aloam_laser_odometry_node.dir/src/aloam_laser_odometry_node.cpp.o:aloam_laser_odometry_node.cpp:(.text+0x2f0): first defined here
collect2: error: ld returned 1 exit status
make[2]: *** [lidar_localization/CMakeFiles/aloam_laser_odometry_node.dir/build.make:762: /home/quanchao/shenlanxueyuan/sensorfusion4/PA3/devel/lib/lidar_localization/aloam_laser_odometry_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:590: lidar_localization/CMakeFiles/aloam_laser_odometry_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....