Ceres入门---Ceres优化三维轨迹

1.前言

本人一直致力于定位研究，接触各种定位算法，最终需要输出比较完美的定位轨迹。一直以来滤波算法被运用于轨迹融合，但是研究表明，基于最小二乘的非线性优化方案，有更好的表现。本文基于谷歌开发的Ceres Solver非线性最小二乘优化包，进行轨迹优化。

2.场景介绍

为了演示的方便，我们抛弃了高度，做了一个二维轨迹仿真。在一条10米的路上，一个机器人从（-5，0，0）出发，沿着（1，0，0）以0.1m/s的速度走向（5，0，0），我们能得到的数据是每秒的速度和位置，但是都是有误差的。

3.最小二乘理论推导

3.1方程建立

这时就需要建立最小二乘误差方程。如下：
$$\underset{P_i,P_j}{\min }\sum _i^{N-1}||V_i\ast \Delta t-(P_i-P_j)||$$
其中$\Delta t$为$P_i$到$P_j$的时间。$P_i$为$i$时刻位置，$P_j$为$j$时刻的位置。

3.2计算雅可比方程

上式也是残差方程：$$residual=V_i\ast \Delta t-(P_i-P_j)$$
残差的维度为三维。
由于我们将位置当作优化参数，所以参数有两个，既有两参数块，一个为$P_i$,一个为$P_j$。
所以有
$$jacobians[0]=\frac{\partial residual}{\partial P_i}=\left[\begin{array}{ccc}-1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$$
维度为：$3\ast 3$
$$jacobians[1]=\frac{\partial residual}{\partial P_j}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
维度为：$3\ast 3$

4.代码

#include 
#include 
#include 
#include 
#include 
#include "ros/ros.h"
#include 
#include
using namespace std;
#define C_PI (double)3.141592653589793
nav_msgs::Path path;
nav_msgs::Path path1;
ros::Publisher path_pub;
ros::Publisher path_pub_n;
int N=101;
// 代价函数的计算模型
class DeltaDis:public ceres::SizedCostFunction<3,3,3>
{
    public:
    DeltaDis()=delete;
    DeltaDis(Eigen::Vector3d deltadis)
    {
        Deltadis=deltadis;
    }
    virtual ~DeltaDis(){}
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
       
        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
        

        Eigen::Map> residual(residuals);
        residual=Deltadis-(Pi-Pj);
        // cout<<"residual:"<> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();
                jacobian_pose_i(0,0)=-1.0;
                jacobian_pose_i(2,2)=-1;
                jacobian_pose_i(1,1)=-1;
            }
            if (jacobians[1])
            {
                Eigen::Map> jacobian_pose_j(jacobians[1]);
                jacobian_pose_j.setZero();
                jacobian_pose_j(0,0)=1;
                jacobian_pose_j(2,2)=1;
                jacobian_pose_j(1,1)=1;
            }
        }
    }
    private:
    Eigen::Vector3d Deltadis;
};
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
  CHECK(problem != NULL);

  ceres::Solver::Options options;
  options.max_num_iterations = 100;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);

  std::cout << summary.FullReport() << '\n';

  return summary.IsSolutionUsable();
}


enum SIZE_PARAMETERIZATION
{
    SIZE_POSE = 3,
};
int main ( int argc, char** argv )
{
    ros::init(argc, argv, "mp_node");
	ros::NodeHandle n;
    path_pub=n.advertise("trajectory1",1,true);
    path_pub_n=n.advertise("trajectory2",1,true);

    double Pi[12][SIZE_POSE];
    double Pj[12][SIZE_POSE];
    
    path.header.frame_id="map";
    path1.header.frame_id="map";
  
    Eigen::Vector3d Pose[N];


    Eigen::Vector3d T;
    T.setZero();

    double p_sigma=0.1;               // 噪声Sigma值
    cv::RNG rng;  
    Eigen::Vector3d  Vs[N];                      // OpenCV随机数产生器
    for(int i=0;i>a;

    return 0;
}

5.结果与结论

绿色的为仿真轨迹，红色的为优化轨迹。可以看到优化结果相当的好。