如何用ba进行小规模场景恢复

博主轰轰烈烈的想要妄图利用自家的kinect进行场景的重构失败了，画面惨不忍睹，物体均有明显的位移，思考很久，总结原因有这样几点：1.选用了三维了旋转矩阵，而机器人是做二维移动的，有了误差.2.kinect可能没有进行深度图与色彩图的匹配，这一点我感觉应该不存在的，因为我调用的是opencv里的videocapture类，并且调用了grab和retrieve两个函数，但是，深度和彩色有一些时差（约几到十几个毫秒）。光圈中心未对齐。因为深度毕竟是靠另一个相机获取的，所以深度传感器和彩色传感器参数可能不一致但是不管怎么说，还是失败了。。。博主很无奈啊，于是用tum数据集来做吧。

效果还是很不错的，博主的ba误差控制在了每个特征点在1个像素误差范围内，并且在位移较小时，重构的图没有明显的位移，还是比价不错的，但美中不足的是数据集其实大部分图很粗糙，画质不是很好，这就给匹配带来了挑战，导致大量的误匹配和画面的位移，所以要想做到很不错的场景恢复，博主私以为在没有闭环这块时，最好可以使机器人避免移动到之前的位置，并且确保画面清晰，那问题就不大了，最后上博主的一张图，效果还是不错的，博主选用了11张变化不大的图片，可以看出重位移很少。

在这里，博主的重建还算不错，基于统计滤波和体素滤波。

好了，上段代码把，博主没有给出ceres和求出两帧的代码，直接上如何利用两帧滤波

 //将位姿读取出来便于后面的点云还原
  ifstream fin("./data.txt");
  vector<Mat>poses;
  for(int i=0;i<8;i++)
  {
    double data[12]={0};
    fin>>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);
  }
  //定义点云指针存放关键帧的点云
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  for(int i=0;i<8;i++)
  {
    //创建点云，用于统计滤波
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr currentpoint(new pcl::PointCloud<pcl::PointXYZRGB>);
    char colorname[30];
    char depthname[30];
    sprintf(colorname,"%s%d%s","./color2/",i,".png");
    sprintf(depthname,"%s%d%s","./depth2/image",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)
	{
	  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)<<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_point.x,camera_point.y,camera_point.z)+(Mat_<double>(3,1)<<poses[i].at<double>(0,3),poses[i].at<double>(1,3),poses[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];
	currentpoint->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;
  }
  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("map.pcd",*pointcloud);
   //将点云转换为八叉树
  //读取点云文件
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_octomap(new  pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::io::loadPCDFile<pcl::PointXYZRGB>("./map.pcd",*cloud_octomap);
  //创建带颜色的八叉树,设置分辨率
  octomap::ColorOcTree color_tree(0.01);
  for(int i=0;i<cloud_octomap->size();i++)
  {
    //将点云插入地图
    color_tree.updateNode(octomap::point3d(cloud_octomap->points[i].x,cloud_octomap->points[i].y,cloud_octomap->points[i].z),
         true);
  }
  //设置地图颜色，还原原始色彩
  for(int i=0;i<cloud_octomap->size();i++)
  {
    color_tree.integrateNodeColor(cloud_octomap->points[i].x,cloud_octomap->points[i].y,cloud_octomap->points[i].z,
         cloud_octomap->points[i].r,cloud_octomap->points[i].g,cloud_octomap->points[i].b );
  }
  //更新地图 
  color_tree.updateInnerOccupancy();
  //存储文件，ot格式
  color_tree.write("color_tree.ot");
  return 0;
}

博主很懒又很菜，没有用eigen库来进行变化矩阵的求解。大家将就一下把。

最后把pcl点云做出八叉树地图，这里，博主碰到一个百思不得其解的问题，就是博主的.ot文件如果过大，octovis将崩溃无法读取，不知道有没有朋友了解的？