ceres做slam全局优化的一些体会

本文介绍了一种基于ceres的SLAM全局BA优化方法,通过优化位姿和地图点来减少累积误差,并展示了实际代码实现。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

之前讲到了如何用ceres做相邻两帧的ba优化,是用重投影误差来做的,对于连续的数据流,无论你在前端采用什么样的代数算法pnp或者icp其实都只能算出一个粗略的解,博主亲自做实验来比较代数解和非线性优化解的区别,发现无论怎样,即使我ba给的初值相当垃圾,优化出来的结果一样好于代数解。那么对于长时间的slam问题来说,如何保证可以减少随时间产生的累计误差,一种主流的做法是把我所以看到的关键帧中的mappoint找出来,来和我的观测到的像素坐标做一个重投影误差,那么这就构建了一个关于如何求解误差梯度的问题了,所幸研究者发现至少在slam问题中,其hessian矩阵具有稀疏性。我们在这里比较简单的说明一下。

我们假设在第i个位姿上看到了第j个mappoint,那么这个单个误差项对于整个误差函数到底有多大的贡献?我们记对于此时的单个重投影误差为fij,可以知道,此时误差项是二维的,而变量是几维呢,我们这里不考虑旋转矩阵要用李代数来表示,不然讲解会变得很麻烦,而且我们不打算用g2o,所以在ceres里,李代数还是很好处理的,只要用罗德里格斯公式转换一下便可以了,再交给ceres自动求导呗。所以简单来说,如果我们同时想要优化位姿和mappoint,我们可以看成是2维的,注意,这个说法很不准确,但是博主为了方便理解,简单可以看成第ij项的误差雅克比为误差对位姿i的李代数偏导,和误差对路标j的偏导,很容易发现其中的稀疏性,那就是我的整体雅克比维数是位姿总和n+路标总和m,那么再第i项和j项分别有这么一个贡献。

那么如何用ceres求解?其实很简单,残差我们还是按照老样子定义,只不过再初始化参数时,要初始化所以变量,及所以路标和位姿,那么对于每一个误差项,我们通过指针的索引来制定要优化的位姿和路标点在放入addresidualblocks里面。

废话不多说,来看代码吧,提示,由于我在做后端优化时使用的别人的数据集,所以我没有写一个完整的多线程的slam,其实是我根本不会多线程。。。。我把localba的数据拿出来在放入globalba里面的,看上去有些笨拙,但是对于我这样的入门来说来说可以接受的。

#include<iostream>
#include<fstream>
#include<stdio.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/opencv.hpp>
#include<Eigen/Geometry>
#include<Eigen/Core>
#include<Eigen/SVD>
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include<pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h> 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <octomap/octomap.h>  
#include <octomap/ColorOcTree.h>  
#include<ceres/ceres.h>
#include<ceres/rotation.h>
#include <boost/concept_check.hpp>

using namespace std;
using namespace cv;

struct cost_function_define
{
  //构造函数初始化并传入参数
  cost_function_define(Point3d p1,Point2d p2):_p1(p1),_p2(p2){}
  template<typename T>
  bool operator()(const T* const cere_r,T* residual)const
  {
    //将空间点转为类型T,可以调用旋转函数转化
    T p_1[3];
    T p_2[3];
    p_1[0]=T(_p1.x);
    p_1[1]=T(_p1.y);
    p_1[2]=T(_p1.z);
    //调用选择矩阵,将一图的空间点映射到第二图
    ceres::AngleAxisRotatePoint(cere_r,p_1,p_2);
    //将空间点变为像素坐标
    p_2[0]=p_2[0]+cere_r[3];
    p_2[1]=p_2[1]+cere_r[4];
    p_2[2]=p_2[2]+cere_r[5];
    const T x=p_2[0]/p_2[2];
    const T y=p_2[1]/p_2[2];
    const T u=x*518.0+325.5;
    const T v=y*519.0+253.5;
    //求出第二图的像素坐标
    const T u1=T(_p2.x);
    const T v1=T(_p2.y);
    //定义残差
    residual[0]=u-u1;
    residual[1]=v-v1;
    return true;
  }
   Point3d _p1;
   Point2d _p2;
};

int main()
{
  Point2d transform(const Point2d& p,const Mat& K);
  void writefile(const Mat& mat,const char* filename);
  Mat K=(Mat_<double>(3,3)<<518.0,0,325.5,0,519.0,253.5,0,0,1);
  //存放照片编号
  vector<int>number;
  //存放照片顺序
  vector<int>queue;
  ifstream fin("./number.txt");
  for(int i=0;i<32;i++)
  {
    int a=0;
    fin>>a;
    number.push_back(a);
    queue.push_back(i);
  }
  ifstream fin_1("./data.txt");
  vector<Mat>poses;
  for(int i=0;i<32;i++)
  {
    double data[12]={0};
    fin_1>>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]>>data[7]>>data[8]>>data[9]>>data[10]>>data[11];
    Mat currentpose=(Mat_<double>(3,4)<<data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11]);
    poses.push_back(currentpose);
  }
  //分别存放三维坐标和像素坐标和照片对应顺序
  vector<Point3d> loaclopt;
  vector<Point2d> localnum;
  vector<int> localqueue;
  for(int i=0;i<31;i++)
    for(int j=i+1;j<32;j++)
    {
      char last_c[30];
      char color[30];
      char last_d[30];
      char depth[30];
      sprintf(last_c,"%s%d%s","./rgb_png/",number[i],".png");
      sprintf(color,"%s%d%s","./rgb_png/",number[j],".png");
      sprintf(last_d,"%s%d%s","./depth_png/",number[i],".png");
      sprintf(depth,"%s%d%s","./depth_png/",number[j],".png");
      Mat last_co=imread(last_c);
      Mat colornow=imread(color);
      Mat last_dep=imread(last_d,-1);
      vector<KeyPoint> keypoints1,keypoints2;
      Ptr<FeatureDetector> detector=ORB::create();
      detector->detect(last_co,keypoints1);
      detector->detect(colornow,keypoints2);
      Mat descriptor1,descriptor2;
      Ptr<DescriptorExtractor> descriptor=ORB::create();
      descriptor->compute(last_co,keypoints1,descriptor1);
      descriptor->compute(colornow,keypoints2,descriptor2);
      vector<DMatch> matches;
      Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
      matcher->match(descriptor1,descriptor2,matches);
      double mindis=1000;
      for(int k=0;k<matches.size();k++)
      {
	if(matches[k].distance<mindis)
	{
	  mindis=matches[k].distance;
	}
      }
      vector<DMatch> goodmatch;
      for(int k=0;k<matches.size();k++)
      {
	if(matches[k].distance<=min(4*mindis,20.0))
	{
	  goodmatch.push_back(matches[k]);
	}
      }
      for(int k=0;k<goodmatch.size();k++)
      {
	Point2d pixel1=keypoints1[goodmatch[k].queryIdx].pt;
	Point2d pixel2=keypoints2[goodmatch[k].trainIdx].pt;
	Point2d pixel_cam1=transform(pixel1,K);
	ushort d=last_dep.ptr<unsigned short>(int(pixel1.y))[int(pixel1.x)];
	if(d==0)
	{
	  continue;
	}
        localnum.push_back(pixel2);
	localqueue.push_back(queue[j]);
	Point3d camera_3d=Point3d(pixel_cam1.x*double(d)/1000.0,pixel_cam1.y*double(d)/1000.0,double(d)/1000.0);
	Mat worldpoint=(Mat_<double>(3,3)<<poses[i].at<double>(0,0),poses[i].at<double>(0,1),poses[i].at<double>(0,2),
	                                                                  poses[i].at<double>(1,0),poses[i].at<double>(1,1),poses[i].at<double>(1,2),
                                                                          poses[i].at<double>(2,0),poses[i].at<double>(2,1),poses[i].at<double>(2,2)
	)*(Mat_<double>(3,1)<<camera_3d.x,camera_3d.y,camera_3d.z)+(Mat_<double>(3,1)<<poses[i].at<double>(0,3),poses[i].at<double>(1,3),poses[i].at<double>(2,3));
	Point3d world_3d=Point3d(worldpoint.at<double>(0,0),worldpoint.at<double>(1,0),worldpoint.at<double>(2,0));
	loaclopt.push_back(world_3d);
      }
    }
  cout<<"共有观测点"<<loaclopt.size()<<endl;
  vector<double> lastdata;
  for(int i=1;i<32;i++)
  {
    Mat reserve=(Mat_<double>(3,3)<<poses[i].at<double>(0,0),poses[i].at<double>(1,0),poses[i].at<double>(2,0),
	                                                                  poses[i].at<double>(0,1),poses[i].at<double>(1,1),poses[i].at<double>(2,1),
                                                                          poses[i].at<double>(0,2),poses[i].at<double>(1,2),poses[i].at<double>(2,2));
    Mat reserve_t=-reserve*(Mat_<double>(3,1)<<poses[i].at<double>(0,3),poses[i].at<double>(1,3),poses[i].at<double>(2,3));
    Mat res;
    Rodrigues(reserve,res);
    for(int j=0;j<3;j++)
    {
      lastdata.push_back(res.at<double>(j,0));
    }
    for(int j=0;j<3;j++)
    {
      lastdata.push_back(reserve_t.at<double>(j,0));
    }
  }
  double dataest[186]={0};
  for(int i=0;i<186;i++)
  {
    dataest[i]=lastdata[i];
  }
  double* camera=dataest;
  //定义求解问题
  ceres::Problem problem;
  for(int i=0;i<localnum.size();i++)
  {
    //残差的维度,变量的维度,使用自动求导,定义一个求解残差的构造函数
    ceres::CostFunction* costfunction=new ceres::AutoDiffCostFunction<cost_function_define,2,6>(new cost_function_define(loaclopt[i],localnum[i]));
    ceres::LossFunction* lossfunction=new ceres::HuberLoss(1.0);
    //添加每一次观测的残差
    double* cere_r=camera+6*(localqueue[i]-1);
    problem.AddResidualBlock(costfunction,lossfunction,cere_r);
  }
  //配置求解器
  ceres::Solver::Options option;
  //选择迭代方式
  option.linear_solver_type=ceres::SPARSE_SCHUR;
  //输出迭代信息到屏幕
  option.minimizer_progress_to_stdout=true;
  //显示优化信息
  ceres::Solver::Summary summary;
  //开始求解
  ceres::Solve(option,&problem,&summary);
  //显示优化信息
  cout<<summary.BriefReport()<<endl;
  
  Mat worldaxis=(Mat_<double>(3,4)<<1,0,0,0,0,1,0,0,0,0,1,0);
  writefile(worldaxis,"./local.txt");
  for(int i=0;i<31;i++)
  {
    //将转角和位移变成变化矩阵并存入
    Mat initial_r=(Mat_<double>(3,1)<<camera[6*i],camera[6*i+1],camera[6*i+2]);
    Mat initial_R;
    Rodrigues(initial_r,initial_R);
    Mat initial_t=(Mat_<double>(3,1)<<camera[6*i+3],camera[6*i+4],camera[6*i+5]);
    Mat final_R=initial_R.t();
    Mat final_t=-final_R*initial_t;
    Mat combine=(Mat_<double>(3,4)<<final_R.at<double>(0,0),final_R.at<double>(0,1),final_R.at<double>(0,2),final_t.at<double>(0,0),
	                                         final_R.at<double>(1,0),final_R.at<double>(1,1),final_R.at<double>(1,2),final_t.at<double>(1,0),
	                                         final_R.at<double>(2,0),final_R.at<double>(2,1),final_R.at<double>(2,2),final_t.at<double>(2,0));
    writefile(combine,"./local.txt");
  }
  //写数据到点云
  ifstream fin_2("./local.txt");
  vector<Mat>pose;
  for(int i=0;i<32;i++)
  {
    double data[12]={0};
    fin_2>>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]>>data[7]>>data[8]>>data[9]>>data[10]>>data[11];
    Mat currentpose=(Mat_<double>(3,4)<<data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11]);
    pose.push_back(currentpose);
  }
  cout<<"正在存放点云"<<endl;
  //定义点云指针存放关键帧的点云
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  for(int i=0;i<32;i++)
  {
    //创建点云,用于统计滤波
    //pcl::PointCloud<pcl::PointXYZRGB>::Ptr currentpoint(new pcl::PointCloud<pcl::PointXYZRGB>);
    char colorname[30];
    char depthname[30];
    sprintf(colorname,"%s%d%s","./rgb_png/",number[i],".png");
    sprintf(depthname,"%s%d%s","./depth_png/",number[i],".png");
    //存放关键帧
    Mat keyframe,depthkeyframe;
    keyframe=imread(colorname);
    depthkeyframe=imread(depthname,-1);
    for(int v=0;v<keyframe.rows;v++)
      for(int u=0;u<keyframe.cols;u++)
      {
	ushort d=depthkeyframe.ptr<unsigned short>(v)[u];
	if(d==0||d>7000.0||d<400.0)
	{
	  continue;
	}
	//还原三维点
	Point3d camera_point;
	camera_point.z=double(d)/1000.0;
	camera_point.x=(u-K.at<double>(0,2))/K.at<double>(0,0)*camera_point.z;
	camera_point.y=(v-K.at<double>(1,2))/K.at<double>(1,1)*camera_point.z;
	Mat worldpoint=(Mat_<double>(3,3)<<pose[i].at<double>(0,0),pose[i].at<double>(0,1),pose[i].at<double>(0,2),
	                                                                  pose[i].at<double>(1,0),pose[i].at<double>(1,1),pose[i].at<double>(1,2),
                                                                          pose[i].at<double>(2,0),pose[i].at<double>(2,1),pose[i].at<double>(2,2)
	)*(Mat_<double>(3,1)<<camera_point.x,camera_point.y,camera_point.z)+(Mat_<double>(3,1)<<pose[i].at<double>(0,3),pose[i].at<double>(1,3),pose[i].at<double>(2,3));
	pcl::PointXYZRGB p_3d;
	p_3d.x=worldpoint.at<double>(0,0);
	p_3d.y=worldpoint.at<double>(1,0);
	p_3d.z=worldpoint.at<double>(2,0);
	p_3d.b=keyframe.data[v*keyframe.step+u*keyframe.channels()];
	p_3d.g=keyframe.data[v*keyframe.step+u*keyframe.channels()+1];
	p_3d.r=keyframe.data[v*keyframe.step+u*keyframe.channels()+2];
	pointcloud->points.push_back(p_3d);
      }
      
      //统计滤波
     // pcl::PointCloud<pcl::PointXYZRGB>::Ptr tem(new pcl::PointCloud<pcl::PointXYZRGB>);
     // pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> statistical_filter;
      //设置聚类,阀值
     // statistical_filter.setMeanK(50);
     // statistical_filter.setStddevMulThresh(1.0);
     // statistical_filter.setInputCloud(currentpoint);
     // statistical_filter.filter(*tem);
     // (*pointcloud)+=*tem;
  }
  cout<<"点云读取完成"<<endl;
  pointcloud->is_dense=false;
  cout<<pointcloud->size()<<endl;
  //体素滤波
  pcl::VoxelGrid<pcl::PointXYZRGB> voxel_filter;
  //设置最小方格
  voxel_filter.setLeafSize(0.01,0.01,0.01);
  //定义一个点云指针用于保存滤波后的点
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempoint(new pcl::PointCloud<pcl::PointXYZRGB>);
  voxel_filter.setInputCloud(pointcloud);
  //滤波
  voxel_filter.filter(*tempoint);
  tempoint->swap(*pointcloud);
  cout<<pointcloud->size()<<endl;
  //保存
  pcl::io::savePCDFileBinary("globalmap.pcd",*pointcloud);
  return 0;
}

Point2d transform(const Point2d& p,const Mat& K)
{
  return Point2d(
    (p.x-K.at<double>(0,2))/K.at<double>(0,0),
    (p.y-K.at<double>(1,2))/K.at<double>(1,1)
  );
}

    //将位置写入txt便于读取
void writefile(const Mat& mat,const char* filename)
{
  ofstream fout(filename,ios_base::app);
  for(int i=0;i<mat.rows;i++)
  {
    for(int k=0;k<mat.cols;k++)
    {
      fout<<mat.at<double>(i,k)<<" ";
    }
    fout<<endl;
  }
  fout.close();
}

来看一下结果把,误差还是缩小了一些的

总结:这个程序没有把mappoint考虑进来优化,所以结果一定不是最优解,但是由于博主是想用rgbd做稠密地图所以没有优化路标,接下来博主将会继续探索如何更好的做好后端的ba。

<think>好的,我现在需要帮助用户了解SLAM后端优化的方法或解决方案。首先,我得回顾一下用户提供的引用内容,确保自己正确理解相关概念。用户引用了四个资料,分别涉及SLAM后端优化的不同方面,比如图优化、因子图、滤波方法以及开源工具等。 首先,用户的问题是关于SLAM后端优化的技术和实现方法。根据引用内容,SLAM后端优化主要分为滤波方法(如卡尔曼滤波)和非线性优化方法(如图优化、因子图优化)。我需要先整理这些方法的基本原理、发展历程以及各自的优缺点。 然后,引用中提到图优化和因子图的区别,尤其是iSAM的增量式处理。这部分需要详细说明,因为这是现代SLAM中常用的技术。用户可能想知道不同优化方法的应用场景和性能差异,比如为什么非线性优化逐渐取代了滤波方法,以及增量式处理的优势。 另外,用户提供的引用里提到了开源工具,如g2o、Ceres Solver等,这些是实现后端优化常用的库。应该将这些工具与对应的优化方法结合起来解释,帮助用户理解实际应用中如何选择和使用这些工具。 接下来,我需要确保回答结构清晰,分点列出不同的优化方法,每种方法的原理、特点、应用场景以及相关工具。同时,用户要求生成相关问题,这部分需要基于回答内容提出几个有针对性的问题,比如比较不同方法的优缺点,或者具体工具的使用方法。 需要注意的是,回答中引用的内容要在适当的位置添加引用标识,比如在提到滤波方法时引用[3],图优化时引用[2]和[4],因子图时引用[4]等。同时,要使用中文,数学公式用$$...$$格式,行内公式用$...$,并检查LaTeX语法是否正确。 最后,用户可能对SLAM后端优化的实际应用案例感兴趣,但引用中没有具体例子,可能需要简要提及相关开源项目如ORB-SLAM,但主要依据提供的引用内容。确保回答真实可靠,参考用户提供的资料,避免引入外部知识。 总结步骤: 1. 概述SLAM后端优化的作用和目标。 2. 分点介绍滤波方法(如卡尔曼滤波)及其优缺点,引用[3]。 3. 详细讲解非线性优化方法,包括图优化和因子图优化,引用[2][4]。 4. 比较滤波与非线性的区别,引用[3][4]。 5. 提到实现工具如g2o、Ceres,引用[1]。 6. 生成相关问题,比如优化方法对比、工具选择、增量优化等。 在写作过程中,要确保行内公式正确,例如位姿表示为$T_{ij}$,误差函数用$e_{ij}$,并正确使用数学符号。同时,检查引用标记的位置是否自然,比如在提到图优化时添加[^2][^4],因子图时[^4]等。</think>### SLAM后端优化技术及实现方法 #### 一、后端优化的核心目标 SLAM后端优化的核心是通过融合多源观测数据(如激光雷达、视觉特征点),对系统状态(机器人位姿、环境特征点位置)进行全局一致性优化,消除前端估计的累积误差[^1][^3]。其数学模型可表示为: $$ X^* = \arg\min_X \sum_{i,j} \rho\left( \| e_{ij}(x_i, x_j) \|^2_{\Sigma_{ij}} \right) $$ 其中$X$为待优化变量,$e_{ij}$为约束误差函数,$\rho$为鲁棒核函数。 #### 二、主流优化方法 1. **滤波方法** - **原理**:基于贝叶斯滤波框架(如扩展卡尔曼滤波EKF),通过递推方式更新状态均值和协方差矩阵 - **特点**: - 仅维护当前状态估计,计算复杂度$O(n^2)$[^3] - 典型应用:FastSLAM(基于粒子滤波) - 局限性:无法处理闭环检测等全局优化需求 2. **非线性优化方法** **(1) 图优化(Graph Optimization)** - 将SLAM问题建模为因子图(Factor Graph)[^2][^4] - 节点:机器人位姿$T_i \in SE(3)$、路标点$p_j \in \mathbb{R}^3$ - 边:里程计约束$T_{i,i+1}$、观测约束$z_{ij}$ - 优化目标:最小化所有约束的加权残差平方和 ```math \min_{T,p} \sum \| \log(T_{ij}^{-1}T_i^{-1}T_j) \|^2_{\Sigma_{odom}} + \sum \| \pi(T_i,p_j) - z_{ij} \|^2_{\Sigma_{obs}} ``` - 实现工具:g2o[^1]、Ceres Solver **(2) 增量式优化(iSAM)** - 通过QR分解实现增量更新,避免重复计算[^4] - 关键步骤: - 变量排序优化 - 部分矩阵分解重用 - 仅更新受新观测影响的局部变量 #### 三、技术对比 | 方法 | 计算效率 | 全局优化 | 内存消耗 | 适用场景 | |------------|----------|----------|----------|------------------| | EKF | 低 | 不支持 | 低 | 小规模场景 | | 批处理BA | 高 | 支持 | 高 | 离线建图 | | 因子图优化 | 中 | 支持 | 中 | 在线SLAM | | iSAM2 | 高 | 支持 | 低 | 大规模动态环境 | #### 四、实现要点 1. **稀疏性利用**:通过舒尔补(Schur Complement)将路标点边缘化,降低Hessian矩阵维度[^2] 2. **鲁棒核函数**:使用Huber或Cauchy核抑制异常值影响 3. **先验信息融合**:在因子图中加入GPS/IMU等先验约束 4. **并行计算**:利用CUDA加速雅可比矩阵计算[^1]
评论 10
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值