ORB_SLAM2代码阅读及总结使用（二）

把建图系统分为了三个节点，如下所示。
第一个节点作为驱动节点，采集摄像头传感器的数据。
第二个节点利用ORB_SLAM主要做姿态估计，提供Tcw
第三个节点作为见图节点，收集第一和第二节点的建图节点接受图像数据和位姿数据，进行点云的拼接。

C++知识点补充
C++中使用关键字class来定义类，其基本形式如下

class 类名
{
    public:
    //公共的行为或属性
 
    private:
    //公共的行为或属性
};

（1）public 与 private 为属性/方法限制的关键字，private表示该部分内容是私密的，不能被外部所访问或调用，只能被本类内部访问；而public表示公开的属性和方法，外界可以直接访问或者调用。
（2）public可用于外部的函数调用
（3）结束部分的分号不能省略。

示例代码

#include <iostream>
using namespace std;
class Point
{
    public:
    void setPoint(int x, int y) //实现setPoint函数
    {
        xPos = x;
        yPos = y;
    }
 
    void printPoint() //实现printPoint函数
    {
        cout<< "x = " << xPos << endl;
        cout<< "y = " << yPos << endl;
    }
    private:
       int xPos;
       int yPos;
};
 
int main()
 
{
    Point M; //用定义好的类创建一个对象 点M
    M.setPoint(10, 20); //设置 M点 的x,y值
    M.printPoint(); //输出 M点 的信息
    return 0;
}

在类外定义成员函数通过在类内进行声明，然后在类外通过作用域操作符：：进行实现。
总结：此语法的定义就是将原本需要在class中进行函数的定义，可以在class外实现。

返回类型 类名::成员函数名(参数列表)
{
     //函数体
}

void Point::setPoint(int x, int y) //通过作用域操作符 '::' 实现setPoint函数
{
    xPos = x;
    yPos = y;
}

Nav_msgs/Path 功能包
ORB_SLAM2中的位姿信息由下列函数返回，将其转换为所需的数据类型发布出去

mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec())

//nav_msgs/Path数据类型  
Header header  
geometry_msgs/PoseStamped[] poses  
//类型扩展：  
Header header  
    uint32 seq  
    time stamp  
    string frame_id  
geometry_msgs/PoseStamped[] poses  
    Header header  
        uint32 seq  
        time stamp  
        string frame_id  
    geometry_msgs/Pose pose  
        geometry_msgs/Point position  
            float64 x  
            float64 y  
            float64 z  
        geometry_msgs/Quaternion orientation  
            float64 x  
            float64 y  
            float64 z  
            float64 w

添加ROS节点文件

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>
#include <cv_bridge/cv_bridge.h>

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

class ImageGrabber
{
public:
	ros::NodeHandle nh; //创建句柄
	ros::Publisher  pub_rgb,pub_depth,pub_tcw,pub_camerapath;//创建4个发布节点pub_rgb, pub_depth, pub_tcw,pub_camerapath
	size_t mcounter=0;	 
	nav_msgs::Path  camerapath;
	//nav_msgs/Path这个功能是利用rviz中的类型实现的，只需要发布nav_msgs/Path类型的消息，然后在rviz
	//上订阅该消息就可以显示轨迹路径。而nav_msgs/Path类型的数据很简单，就是一个位姿的集合。
	//navigation功能包中显示规划路径用的就是这个。

    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM),nh("~") //？此处可能是SLAM和ROS的连接函数
    {
	   //创建ROS的发布节点
	   pub_rgb= nh.advertise<sensor_msgs::Image> ("RGB/Image", 10); 
	   pub_depth= nh.advertise<sensor_msgs::Image> ("Depth/Image", 10); 
	   pub_tcw= nh.advertise<geometry_msgs::PoseStamped> ("CameraPose", 10); 
	   pub_camerapath= nh.advertise<nav_msgs::Path> ("Path", 10); 
    }

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); // 此函数会在class外定义

    ORB_SLAM2::System* mpSLAM;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 3)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    ImageGrabber igb(&SLAM);//ImageGrabber类创建一个对象将新创建的SLAM系统导入

    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 10);//message_filters是ROS消息过滤器，sensor_msgs是数据类型
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 10);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB; //利用CV_bridge中的CvImageConstPtr创建一个cv_ptrRGB
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    bool  isKeyFrame =true;
    cv::Mat Tcw;// 创建矩阵Tcw

	//ORB_SLAM2中的位姿信息由下列函数返回
    Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());//存放到cv_ptrRGB->imag
	//mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec())

    if (!Tcw.empty())
	{
				  //cv::Mat Twc =Tcw.inv();
				  //cv::Mat TWC=orbslam->mpTracker->mCurrentFrame.mTcw.inv();  
				  cv::Mat RWC= Tcw.rowRange(0,3).colRange(0,3);  
				  cv::Mat tWC= Tcw.rowRange(0,3).col(3);
s
				  tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
							      RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
							      RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2)); // tf::Matrix3x3 旋转矩阵
				  tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2)); // tf::Vector 保存平移矩阵
				  
				 tf::Quaternion q; //四元数
				  M.getRotation(q); //获取四元数，旋转矩阵中保存的就是四元数，存储到q中
				  
			      tf::Pose tf_pose(q,V); // tf::Pose 位姿（位置+姿态）成员getRotation()或getBasis()用于获取旋转矩阵；getOffset()用于获取平移向量
				  
				   double roll,pitch,yaw;
				   M.getRPY(roll,pitch,yaw); //转换函数tf::Matrix3x3.getRPY()
				  //cout<<"roll: "<<roll<<"  pitch: "<<pitch<<"  yaw: "<<yaw;
				  // cout<<"    t: "<<tWC.at<float>(0)<<"   "<<tWC.at<float>(1)<<"    "<<tWC.at<float>(2)<<endl;
				   
				   if(roll == 0 || pitch==0 || yaw==0)
					return ;
				   // ------
				  
				  std_msgs::Header header ;
				  header.stamp =msgRGB->header.stamp;
				  header.seq = msgRGB->header.seq;
				  header.frame_id="camera";
				  //cout<<"depth type: "<< depth. type()<<endl;
				  
				  sensor_msgs::Image::ConstPtr rgb_msg = msgRGB;
				  sensor_msgs::Image::ConstPtr depth_msg=msgD;
				  
				 geometry_msgs::PoseStamped tcw_msg;
				 tcw_msg.header=header;
				 tf::poseTFToMsg(tf_pose, tcw_msg.pose);
				  
				 camerapath.header =header;
				 camerapath.poses.push_back(tcw_msg);
				  
				                      
				 pub_camerapath.publish(camerapath);  //相机轨迹
				 if( isKeyFrame)
				{
					 pub_tcw.publish(tcw_msg);	//Tcw位姿信息
					 pub_rgb.publish(rgb_msg);
					 pub_depth.publish(depth_msg);
				}
	}
	else
	{
	  cout<<"Twc is empty ..."<<endl;
	}
}

Summery: 在轨迹跟踪中最重要的函数是//mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec())。返回Tcw估计的位姿信息。其中前3row和前3col是旋转矩阵Matrix3x3。平移矩阵是row前3.第3
col。 M.getRPY(roll,pitch,yaw); //转换函数tf::Matrix3x3.getRPY()获取roll，pitch，yaw的转角。