Ceres入門---Ceres優化三維軌跡

1.前言

本人一直致力於定位研究，接觸各種定位算法，最終需要輸出比較完美的定位軌跡。一直以來濾波算法被運用於軌跡融合，但是研究表明，基於最小二乘的非線性優化方案，有更好的表現。本文基於谷歌開發的Ceres Solver非線性最小二乘優化包，進行軌跡優化。

2.場景介紹

爲了演示的方便，我們拋棄了高度，做了一個二維軌跡仿真。在一條10米的路上，一個機器人從（-5，0，0）出發，沿着（1，0，0）以0.1m/s的速度走向（5，0，0），我們能得到的數據是每秒的速度和位置，但是都是有誤差的。

3.最小二乘理論推導

3.1方程建立

這時就需要建立最小二乘誤差方程。如下：
${\min_{P_i,P_j} \sum_i^{N-1}||V_i*\Delta t-(P_i-P_{j})||}$
其中${\Delta t}$爲${P_i}$到${P_j}$的時間。${P_i}$爲${i}$時刻位置，${P_j}$爲${j}$時刻的位置。

3.2計算雅可比方程

上式也是殘差方程：${residual=V_i*\Delta t-(P_i-P_{j})}$
殘差的維度爲三維。
由於我們將位置當作優化參數，所以參數有兩個，既有兩參數塊，一個爲${P_i}$,一個爲${P_j}$。
所以有
${jacobians[0]=\frac{\partial residual}{\partial P_i}=\begin{bmatrix} -1 & 0& 0 \\ 0 & -1 &0 \\ 0 &0 & -1 \end{bmatrix}}$
維度爲：${3*3}$
${jacobians[1]=\frac{\partial residual}{\partial P_j}=\begin{bmatrix} 1 & 0& 0 \\ 0 & 1 &0 \\ 0 &0 & 1 \end{bmatrix}}$
維度爲：${3*3}$

4.代碼

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include <eigen3/Eigen/Dense>
#include "ros/ros.h"
#include <geometry_msgs/Vector3Stamped.h>
#include<nav_msgs/Path.h>
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<Eigen::Matrix<double, 3, 1>> residual(residuals);
        residual=Deltadis-(Pi-Pj);
        // cout<<"residual:"<<residual;
        if(jacobians)
        {
            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> 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<nav_msgs::Path>("trajectory1",1,true);
    path_pub_n=n.advertise<nav_msgs::Path>("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<N;i++)
    {
        path.header.stamp=ros::Time::now();

        // T((i/25)%2)=T((i/25)%2)+pow(-1,i/50)*1;
        T.setIdentity();
        T=T/10;
        double a=rng.gaussian (p_sigma);
        Vs[i]=T+Eigen::Vector3d(a/10,a/10,0);;

        Pose[i]=T*i;
        Pose[i](0)=Pose[i](0)-5;
        Pose[i]=Pose[i]+Eigen::Vector3d(a*2,a*5-a*a,0);
       
        geometry_msgs::PoseStamped thie_Pose;
        thie_Pose.pose.position.x=Pose[i](0);
        thie_Pose.pose.position.y=Pose[i](1);
        thie_Pose.pose.position.z=Pose[i](2);
            
        thie_Pose.pose.orientation.x=1;
        thie_Pose.pose.orientation.y=1;
        thie_Pose.pose.orientation.z=1;
        thie_Pose.pose.orientation.w=1;

        thie_Pose.header.stamp=ros::Time::now();
        thie_Pose.header.frame_id="path";
            
        path.poses.push_back(thie_Pose);

        path_pub.publish(path);
    }

    double pose_s[N][3];
    ceres::Problem problem;
    for(int i=1;i<N;i++)
    {
        pose_s[i][0]=Pose[i](0);
        pose_s[i][1]=Pose[i](1);
        pose_s[i][2]=Pose[i](2);
        
    }
    for(int i=1;i<N;i++)
    {
        
        Eigen::Vector3d deldis=Vs[i-1];
        problem.AddParameterBlock(pose_s[i-1],SIZE_POSE);
        problem.AddParameterBlock(pose_s[i],SIZE_POSE);
        DeltaDis* deldisf=new DeltaDis(deldis);
        problem.AddResidualBlock(deldisf, nullptr, pose_s[i-1],pose_s[i]);
    }

    
    SolveOptimizationProblem(&problem);

    for(int i=0;i<N;i++)
    {
        geometry_msgs::PoseStamped this_Pose;
        this_Pose.pose.position.x=pose_s[i][0];
        this_Pose.pose.position.y=pose_s[i][1];
        this_Pose.pose.position.z=pose_s[i][2];
            
        this_Pose.pose.orientation.x=1;
        this_Pose.pose.orientation.y=1;
        this_Pose.pose.orientation.z=1;
        this_Pose.pose.orientation.w=1;

        this_Pose.header.stamp=ros::Time::now();
        this_Pose.header.frame_id="n_path";
            
        path1.poses.push_back(this_Pose);

        path_pub_n.publish(path1);
    }
    int a;
    cin>>a;

    return 0;
}

5.結果與結論

綠色的爲仿真軌跡，紅色的爲優化軌跡。可以看到優化結果相當的好。