ASTRA+ROS：基于前端视觉里程计的RGBD深度摄像机稠密重构

前端视觉里程计能给出一个短时间内的轨迹和时间，该程序适用于局部环境的稠密重构。算法使用《视觉SLAM十四讲》中源码，硬件为奥比中光的Astra mini摄像头。下面将对开发流程进行流程罗列，并在后面的详情中解析源码。

1. 安装Astra驱动及ROS应用开发包（购买产品会有提供安装说明文件，运行可以发布ROS图像话题）

2. 编写ROS节点用于接收摄像头图像
3. 将图像信息加载到前端视觉里程计算法中计算位姿，并根据位姿信息构建稠密点云模型

引用，函数声明及全局变量

//文件读取，时间
#include <fstream>
#include <boost/timer.hpp>
//ros头文件
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
//接受同步图片话题
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>


//opencv
#include <opencv2/highgui/highgui.hpp>

//图片数据类型
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>


//pcl和myslam，sophous
#include "myslam/config.h"
#include "myslam/visual_odometry.h"

#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 


//using namespace cv;
using namespace std;
using namespace message_filters;
using namespace sensor_msgs;

//声明点云提取函数
void addPointCloud(myslam::Frame::Ptr frame,SE3 Twc
    ,myslam::Camera::Ptr camera
    ,pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud
);
//声明回调函数
void callback(const ImageConstPtr& image_color_msg, const ImageConstPtr& image_depth_msg);

//创建点云对象
typedef pcl::PointXYZRGB PointT; 
typedef pcl::PointCloud<PointT> PointCloud;
PointCloud::Ptr pointCloud( new PointCloud );
//循环指数
int keyPCloudFrame=10;
int keyOverFrame=0;

PART1：主程序订阅ROS图像话题，并传入回调函数中进行计算

int main(int argc, char **argv)
{
  //初始化ros
  ros::init(argc, argv, "astra_image_listener");
  ros::NodeHandle nh;
  //初始化cv窗口
  cv::namedWindow("color", 0);  
  cv::moveWindow("color",20,20);   

  //订阅话题
  message_filters::Subscriber<Image> image_color_sub(nh,"/camera/rgb/image_raw", 1);// bgr8
  message_filters::Subscriber<Image> image_depth_sub(nh,"/camera/depth/image", 1);// 16UC1
  
  //声明同步消息类型
  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  //定义同步消息
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_color_sub, image_depth_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));
  
  //循环订阅
  ros::Rate rate(10.0);
  
  //进入循环
  while (ros::ok())
  {
    ros::spinOnce();//回调一次
    rate.sleep();
    //若图片存储到20次，则结束循环,生成点云模型文件
    if(keyOverFrame==20)
      ros::shutdown();
  }
  //生成点云文件
  pointCloud->is_dense = false;
  cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
  pcl::io::savePCDFileBinary("map.pcd", *pointCloud );

  return 0;
}

PART2：回调函数进行VO位姿计算，并提取点云

void callback(const ImageConstPtr& image_color_msg, const ImageConstPtr& image_depth_msg)
{
  
  //加载配置文件
  static const std::string& filename="/home/liang/catkin_ws/src/astra_vo_model/config/default.yaml";
  myslam::Config::setParameterFile ( filename );
  //生成vo对象
  static myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );
  //生成摄像机模型，包含摄像机的内参
  static myslam::Camera::Ptr camera ( new myslam::Camera );
  
  
  try
  {
    //获取图片的rgb信息
    Mat color=cv_bridge::toCvShare(image_color_msg, "bgr8")->image;
    //获取图片像素对应的深度信息
    Mat depth=cv_bridge::toCvShare(image_depth_msg, "16UC1")->image;
    
    //信息为空,则重新载入下一张图片
    if ( color.data==nullptr || depth.data==nullptr )
    {
      //ros::shutdown();
      return;
    }
    //创建新的一帧
    myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
    
    //往新的一帧中添加数据
    pFrame->camera_ = camera;
    pFrame->color_ = color;
    pFrame->depth_ = depth;
    pFrame->time_stamp_ = image_color_msg->header.stamp.toSec();
    
    //计算VO耗费时间
    boost::timer timer;
    vo->addFrame ( pFrame );
    cout<<"VO costs time: "<<timer.elapsed() <<endl;

    if ( vo->state_ == myslam::VisualOdometry::LOST )
    {
      ros::shutdown();
      return;
    }
    SE3 Twc = pFrame->T_c_w_.inverse();
    
    //VO计算完成，往点云对象中添加数据
    if(keyPCloudFrame==10)
    {
	boost::timer timer2;
	addPointCloud(pFrame,Twc
	    ,camera,
	    pointCloud);
	cout<<"extract pointCloud costs time: "<<timer2.elapsed() <<endl;
	cout<<"**************extract pointCloud from color and depth******************"<<keyOverFrame<<endl;
	keyPCloudFrame=0;
	//记录20秒结束
	if(keyOverFrame==20)
	{
	  ros::shutdown();
	  return;
	} 
	keyOverFrame++;
    }
    //每秒记录一次点云
    keyPCloudFrame++;
    
    
    //cv::imshow("view", cv_bridge::toCvShare(msg, "16UC1")->image);
    Mat img_show = color.clone();
    
    for ( auto& pt:vo->map_->map_points_ )
    {
	myslam::MapPoint::Ptr p = pt.second;
	Vector2d pixel = pFrame->camera_->world2pixel ( p->pos_, pFrame->T_c_w_ );
	cv::circle ( img_show, cv::Point2f ( pixel ( 0,0 ),pixel ( 1,0 ) ), 5, cv::Scalar ( 0,255,0 ), 2 );
    }
    cv::imshow ( "image", img_show );
    cv::waitKey ( 1 );

  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'TYPE_16UC1'", image_color_msg->encoding.c_str());
  }
}

PART3：点云提取函数

//存储关键帧点云信息
void addPointCloud(myslam::Frame::Ptr frame,SE3 Twc
    ,myslam::Camera::Ptr camera
    ,pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud
)
{
  cout<<"关键帧转换点云中: "<<endl;
  cv::Mat color=frame->color_;
  cv::Mat depth=frame->depth_;
  typedef pcl::PointXYZRGB PointT;

  //转换变换矩阵
  Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
  T.rotate(Twc.unit_quaternion());
  T.pretranslate(Twc.translation());
  //遍历像素
  for ( int v=0; v<color.rows; v++ )
  {
      for ( int u=0; u<color.cols; u++ )
	{
	    unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
	    if ( d==0 ) continue; // 为0表示没有测量到
	    Eigen::Vector3d point; 
	    point[2] = double(d)/camera->depth_scale_; 
	    point[0] = (u-camera->cx_)*point[2]/camera->fx_;
	    point[1] = (v-camera->cy_)*point[2]/camera->fy_; 
	    Eigen::Vector3d pointWorld = T*point;
	    
	    PointT p ;
	    p.x = pointWorld[0];
	    p.y = pointWorld[1];
	    p.z = pointWorld[2];
	    p.b = color.data[ v*color.step+u*color.channels() ];
	    p.g = color.data[ v*color.step+u*color.channels()+1 ];
	    p.r = color.data[ v*color.step+u*color.channels()+2 ];
	    pointCloud->points.push_back( p );
	}
  }     
}

VO部分算法在《视觉SLAM十四讲》，这里不再给出，只会有部分代码的解析。