xtion pro live 单目视觉半直接法(SVO)实践

svo的下载编译可以查看教程：https://github.com/uzh-rpg/rpg_svo/wiki
跑完下作者提供的数据集后，我就想用自己手边的摄像头来看下实际效果。
但要用自己的摄像头首先得对摄像头进行标定，获得相机的参数，这里我采用的是
ATAN模型，标定的是xtion_pro_live 的rgb摄像头。
(相机标定：https://github.com/uzh-rpg/rpg_svo/wiki/Camera-Calibration)

要标定它的话，首先我们要下载PTAM，用ptam的cameracalibrator来标定。
下载ptam包，编译之前安装依赖项：

sudo apt-get install freeglut3-dev
sudo apt-get install libblas-dev
sudo apt-get install liblapacke-dev

成功编译后，进入cameracalibrator.launch修改输入topic ：

<launch>
   <node name="cameracalibrator" pkg="ptam" type="cameracalibrator" clear_params="true" output="screen">
        <remap from="image" to="/mono_image" />
        <remap from="pose" to="pose"/>
    <rosparam file="$(find ptam)/PtamFixParams.yaml"/>
   </node>
</launch>

这里你会有疑问输入图像/mono_image从哪里来呢？
ptam的cameracalibrator需要的图像是单通道的灰度图像，而xtion_pro_live提供的/camera/rgb/image_raw是3通道的rgb图像，所以需要转换格式：/camera/rgb/image_raw –>/mono_image。所以/mono_image就是转换后的单通道灰度图像。
那怎么进行转换呢？不用担心，这里有转换程序：

    //rgb图像转换为灰度图像的程序  
    #include <ros/ros.h>   
    #include <opencv2/opencv.hpp>  
    #include <opencv/cv.h>  
    #include <opencv/highgui.h>  
    #include <image_transport/image_transport.h>  
    #include <cv_bridge/cv_bridge.h>  
    #include <sensor_msgs/image_encodings.h>  
    #include "ros/ros.h"  
    #include "std_msgs/String.h"  

    ros::Publisher image_pub ;  

    void chatterCallback(const sensor_msgs::ImageConstPtr& msg)  
    {  
        cv_bridge::CvImagePtr  cv_ptr;  
        cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);   
        cv::Mat image_gray;  
        cv::cvtColor(cv_ptr->image, image_gray,CV_BGR2GRAY);//灰度化  

        cv_bridge::CvImage  cvi;  
        sensor_msgs::Image  ros_img;  
        ros::Time time