激光SLAM理论与实践-第五期 第五次作业（高斯牛顿法优化）

原创 已于 2022-05-29 13:47:43 修改
· 1k 阅读 · 7 ·
版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#计算机视觉 #矩阵 #自动驾驶

于 2021-01-19 15:39:57 首次发布

所有章节：

1、激光SLAM理论与实践-第五期 第一次作业（矩阵坐标变换）
2、激光SLAM理论与实践-第五期 第二次作业（里程计标定）
3、激光SLAM理论与实践-第五期 第三次作业（去运动畸变）
4、激光SLAM理论与实践-第五期 第四次作业（-帧间匹配算法，imls-icp和csm）
5、激光SLAM理论与实践-第五期 第五次作业（高斯牛顿法优化）
6、激光SLAM理论与实践-第五期 第六次作业 (g2o优化方法)
7、激光SLAM理论与实践-第五期 第七次作业 （mapping）

2、课程PPt和源码

https://download.youkuaiyun.com/download/weixin_44023934/85491811
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

#include <map.h>
#include "gaussian_newton_method.h"

const double GN_PI = 3.1415926;

//进行角度正则化．
double GN_NormalizationAngle(double angle)
{
    if(angle > GN_PI)
        angle -= 2*GN_PI;
    else if(angle < -GN_PI)
        angle += 2*GN_PI;

    return angle;
}

Eigen::Matrix3d GN_V2T(Eigen::Vector3d vec)
{
    Eigen::Matrix3d T;
    T  << cos(vec(2)),-sin(vec(2)),vec(0),
            sin(vec(2)), cos(vec(2)),vec(1),
            0,           0,     1;
    return T;
}

//对某一个点进行转换．
Eigen::Vector2d GN_TransPoint(Eigen::Vector2d pt,Eigen::Matrix3d T)
{
    Eigen::Vector3d tmp_pt(pt(0),pt(1),1);
    tmp_pt = T * tmp_pt;
    return Eigen::Vector2d(tmp_pt(0),tmp_pt(1));
}

//用激光雷达数据创建势场．
map_t* CreateMapFromLaserPoints(Eigen::Vector3d map_origin_pt,
                                std::vector<Eigen::Vector2d> laser_pts,
                                double resolution)
{
    map_t* map = map_alloc();

    map->origin_x = map_origin_pt(0);
    map->origin_y = map_origin_pt(1);
    map->resolution = resolution;
    //固定大小的地图，必要时可以扩大．
    map->size_x = 10000;
    map->size_y = 10000;
    //栅格地图
    map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
    //高斯平滑的sigma－－固定死，是不是后面可以换成随机的
    map->likelihood_sigma = 0.5;
    //上一帧的坐标 转成 矩阵
    Eigen::Matrix3d Trans = GN_V2T(map_origin_pt);
    //设置障碍物         上一帧激光点云
    for(int i = 0; i < laser_pts.size();i++)
    {   //将点经过 Trans 转换  变成世界坐标
        Eigen::Vector2d tmp_pt = GN_TransPoint(laser_pts[i],Trans);  
        int cell_x,cell_y;
        //世界坐标转到地图坐标
        cell_x = MAP_GXWX(map,tmp_pt(0));
        cell_y = MAP_GYWY(map,tmp_pt(1));
        map->cells[MAP_INDEX(map,cell_x,cell_y)].occ_state = CELL_STATUS_OCC;
      
    }

    //进行障碍物的膨胀--最大距离固定死．
    map_update_cspace(map,0.5);

    return map;
}


/**
 * @brief InterpMapValueWithDerivatives
 * 在地图上的进行插值，得到coords处的势场值和对应的关于位置的梯度．
 * 返回值为Eigen::Vector3d ans
 * ans(0)表示市场值
 * ans(1:2)表示梯度
 * @param map
 * @param coords
 * @return
 */
//通过查找，找到相邻位置的分值（似然概率）
//将附近的分值插值，附近的点分别为
//P1 =0,0  P2 =1,0
//P4 =0,1  P3 =1,1  
//待求的点可以理解为 x，y = （0~1,0~1）
Eigen::Vector3d InterpMapValueWithDerivatives(map_t* map,Eigen::Vector2d& coords)
{
 
    Eigen::Vector3d ans;
    //TODO
    //世界坐标转地图
    // double cell_x = MAP_GXWX_DOUBLE(map,coords(0));
    // double cell_y = MAP_GXWX_DOUBLE(map,coords(1));
    // std::cout<<"cell_x,cell_y : "<< cell_x<<", " << cell_y <<std::endl;
    // //X0,Y0 坐标 取整数
    // int x_0  = MAP_GXWX(map,coords(0));
    // int y_0 = MAP_GYWY(map,coords(1));
   
    int x_0  = floor((coords(0) - map->origin_x) / map->resolution+ 0.5 ) + map->size_x / 2;
    int y_0 = floor((coords(1) - map->origin_y) / map->resolution+ 0.5  ) + map->size_y / 2;   
    //由x1-x0 = 1,所以u = x-x0,v = y-y0; 
    double u = (coords(0) - map->origin_x) / map->resolution + 0.5 +double(map->size_x / 2)- (double)x_0 ;
     double v  = (coords(1) - map->origin_y) / map->resolution + 0.5  + double(map->size_y / 2)- (double)y_0;
    // double cell_x ;
    // double cell_y;
    // int x_0,y_0;
    // cell_x = MAP_GXWX_DOUBLE(map, coords(0) );
    // cell_y =MAP_GXWX_DOUBLE(map, coords(1) );
    // x_0 =  MAP_GYWY(map, coords(0));
    // y_0  = MAP_GYWY(map, coords(1));
    // std::cout<<" x_0,y_0 : "<<  x_0<<", " << y_0 <<std::endl;
    //附近坐标分值 顺序别搞错 顺时针
    double P1 ,  P2, P3, P4;
     P1 = map->cells[MAP_INDEX(map,x_0, y_0)].score;
     P2 = map->cells[MAP_INDEX(map,x_0+1 , y_0)].score;
     P3 = map->cells[MAP_INDEX(map,x_0+1, y_0+1 )].score;
     P4 = map->cells[MAP_INDEX(map,x_0 , y_0+1 )].score;
    //M(ST)
    ans(0) = v*(u*P3+(1-u)*P4)+(1-v)*(u*P2+(1-u)*P1);
    //STdx
    ans(1) = (v*(P3-P4) + (1-v)*(P2-P1))/map->resolution ;
    //STdy
    ans(2) =( u*(P3-P2) + (1-u)*(P4 - P1) )/map->resolution  ; 
//     if(1)
// {   
//      // std::cout<<"y_0:"<< y_0 <<";  x_0:"<< x_0 <<";   u:"<< u <<";   v:"<< v <<std::endl;
//       std::cout<<"ans0:"<< ans(0) <<";  ans1:"<< ans(1) <<";  ans2:"<< ans(2) <<std::endl;
//       std::cout<<"P1:"<< P1 <<";  P2:"<< P2<<";  P3:"<< P3 <<";  P4:"<< P4 <<std::endl;
//       std::cout<< std::endl;
// }
    //END OF TODO

    return ans;
}


/**
 * @brief ComputeCompleteHessianAndb
 * 计算H*dx = b中的H和b
 * @param map
 * @param now_pose  机器人的坐标
 * @param laser_pts  在激光雷达的坐标系
 * @param H
 * @param b
 */
double ComputeHessianAndb(map_t* map, Eigen::Vector3d now_pose,
                        std::vector<Eigen::Vector2d>& laser_pts,
                        Eigen::Matrix3d& H, Eigen::Vector3d& b)
{
    H = Eigen::Matrix3d::Zero();
    b = Eigen::Vector3d::Zero();
    //TODO
    Eigen::Matrix3d T;
    //  T << cos(now_pose(2)),-sin(now_pose(2)),0,
    //      sin(now_pose(2)),cos(now_pose(2)),0,
    //      0,0,1;
    T << cos(now_pose(2)),-sin(now_pose(2)),now_pose(0),
         sin(now_pose(2)),cos(now_pose(2)),now_pose(1),
          0,0,1;
          {
              //用现成矩阵变换结果很差
    // T = GN_V2T(now_pose);
  
          }
   
    Eigen::Vector3d laser_pose ;
    Eigen::Vector3d ST3d; 
    Eigen::Vector2d ST2d; 
    Eigen::Vector3d ans; 
    Eigen::Vector2d coords ;
    Eigen::Vector2d dMst ;
    Eigen::Matrix<double,2,3> dST; 
    
    
    double error  = 0.0;
    for(int i=0; i<laser_pts.size();i++)
    {
        {
            // ST = GN_TransPoint(laser_pts[i],  T);
        }
        laser_pose << laser_pts[i](0),laser_pts[i](1),1;
        //M(S(T)) 计算
        ST3d  =   T * laser_pose ;
        ST2d  << ST3d(0),ST3d(1);
        ans = InterpMapValueWithDerivatives( map,ST2d);
        //dst
        dST << 1,0,-sin(now_pose(2))*laser_pts[i](0)-cos(now_pose(2))*laser_pts[i](1),
               0,1, cos(now_pose(2))*laser_pts[i](0)-sin(now_pose(2))*laser_pts[i](1);
        //J
        //  std::cout<<"dST1 :"<< dST(0,2) <<",    dST2 :"<< dST(1,2) <<std::endl;
        //   std::cout<<"dST1 :"<< -sin(now_pose(2))*laser_pts[i](0)-cos(now_pose(2))*laser_pts[i](1) <<
        //     ",   dST2 :"<< cos(now_pose(2))*laser_pts[i](0)-sin(now_pose(2))*laser_pts[i](1) <<std::endl;
        //  std::cout<< std::endl;
        dMst <<  ans(1), ans(2);
        double J1 = ans(1) ;  double J2 =    ans(2);
       {
        //    double J3 =   ans(1)*double((-sin(now_pose(2))*laser_pts[i](0)-cos(now_pose(2))*laser_pts[i](1))+
                    //    (cos(now_pose(2))*laser_pts[i](0)-sin(now_pose(2))*laser_pts[i](1))*ans(2)); 
       }
       double J3 =   ans(1)*dST(0,2)+dST(1,2)*ans(2); 
        H(0,0) += J1*J1;       H(0,1) += J1*J2;     H(0,2) += J3*J1;
        H(1,0) += H(0,1);      H(1,1) += J2*J2;     H(1,2) += J3*J2;
        H(2,0) += H(0,2) ;     H(2,1) += H(1,2) ;   H(2,2) +=J3*J3;
       
        double er = 1-ans(0);
        Eigen::Vector3d  ver;
         ver  << J1*er,J2*er,J3*er; 
         b += ver;
         error += er*er;
    }
    //   std::cout<<" H :"<< H <<std::endl;
    //     std::cout<<" b :"<< b  <<std::endl;
    //      std::cout<< std::endl;
    //END OF TODO
    //    std::cout<<"error : "<< error <<std::endl;
    return  error*0.5 ;
}


/**
 * @brief GaussianNewtonOptimization
 * 进行高斯牛顿优化．
 * @param map
 * @param init_pose
 * @param laser_pts
 */
void GaussianNewtonOptimization(map_t*map,Eigen::Vector3d& init_pose,std::vector<Eigen::Vector2d>& laser_pts)
{
    int maxIteration = 20;
    double lasrterror =0; 
    Eigen::Vector3d now_pose = init_pose;
    bool flat = true;
    Eigen::Vector3d DT ;
    static Eigen::Vector3d DT_sum ;
    for(int i = 0; i < maxIteration;i++)
    {
        //TODO
      Eigen::Matrix3d H ;
      Eigen::Vector3d b ;
      double error = ComputeHessianAndb( map,  now_pose, laser_pts, H, b);    
      Eigen::Vector3d dT = H.ldlt().solve(b) ;
      if( lasrterror > 0 && error > lasrterror)//分值越大，函数结果越小
      {
        //   DT_sum += DT;
        //  std::cout<<"error > lasrterror!"<<std::endl;
        //  std::cout<<"迭代第"<< i << " 次结束 ,总变化量  DT ： "<< DT.transpose()  
        //           << " 总量  SUM_DT ： "<<  DT_sum.transpose()  << std::endl;
         break;
      }
      if(std::isnan(dT[0]))
      {
          std::cout<<"result is nan!"<<std::endl;
          break;
      }
       lasrterror = error;
       DT += dT;
       now_pose += dT;
        //END OF TODO
        if(error <10e-5)
        break;
        
    }
     
    init_pose = now_pose;

}

所有章节：

1、激光SLAM理论与实践-第五期 第一次作业（矩阵坐标变换）
2、激光SLAM理论与实践-第五期 第二次作业（里程计标定）
3、激光SLAM理论与实践-第五期 第三次作业（去运动畸变）
4、激光SLAM理论与实践-第五期 第四次作业（-帧间匹配算法，imls-icp和csm）
5、激光SLAM理论与实践-第五期 第五次作业（高斯牛顿法优化）
6、激光SLAM理论与实践-第五期 第六次作业 (g2o优化方法)
7、激光SLAM理论与实践-第五期 第七次作业 （mapping）

2、课程PPt和源码

https://download.youkuaiyun.com/download/weixin_44023934/85491811

评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值