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; //用定义好的类创建一个对象 点MM.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_camerapathsize_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 threadsSLAM.Shutdown();// Save camera trajectorySLAM.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_ptrRGBtry{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);
stf::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的转角。

ORB_SLAM2代码阅读及总结使用（二）相关推荐

ORB_SLAM2代码阅读(5)——Bundle Adjustment
ORB_SLAM2代码阅读(5)--Bundle Adjustment 1. 说明 2. Bundle Adjustment(BA)的物理意义 3. BA的数学表达 4. BA的求解方法 4.1 最速 ...
ORB_SLAM2代码阅读(4)——LoopClosing线程
ORB_SLAM2代码阅读(4)--LoopClosing线程 1.说明 2.简介 3.检测回环 4.计算Sim3 4.1 为什么在进行回环检测的时候需要计算相似变换矩阵,而不是等距变换? 4.2 累 ...
ORB_SLAM2代码阅读(2)——tracking线程
ORB_SLAM2代码阅读(2)--Tracking线程 1. 说明 2. 简介 2.1 Tracking 流程 2.2 Tracking 线程的二三四 2.2.1 Tracking 线程的二种模式 ...
ORB_SLAM2代码阅读(3)——LocalMapping线程
ORB_SLAM2代码阅读(3)--LocalMapping线程 1.说明 2.简介 3.处理关键帧 4. 地图点剔除 5. 创建新的地图点 6.相邻搜索 6.剔除冗余关键帧 1.说明本文介绍ORB ...
ORB_SLAM2代码阅读(1)——系统入口
ORB_SLAM2代码阅读(1)--系统简介 1.说明 2.简介 3.stereo_kitti.cc 4.SLAM系统文件(System.cc) 4.1 构造函数System() 4.2 TrackS ...
工程代码_Egret开发笔记（二）基础工程代码阅读
代码目录结构在Egret Wing中打开上一节中我们创建的项目工程,查看代码目录结构,Forward在如下图中标记了各个目录的及关键文件的用途. 代码阅读理解接下来我们从web入口一步一步阅读初始 ...
深度学习项目代码阅读建议
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|机器学习实验室犹豫很久要不要把读代码这个事情专门挑出来写 ...
《代码阅读方法与实践之读书笔记之一》
<代码阅读方法与实践之读书笔记之一> 阅读代码是程序员的基本技能,同时也是软件开发.维护.演进.审查和重用过程中不可或缺的组成部分.<代码阅读方法与实践之读书笔记之一>这本书围 ...
[置顶] Linux协议栈代码阅读笔记（一）
Linux协议栈代码阅读笔记(一) (基于linux-2.6.21.7) (一)用户态通过诸如下面的C库函数访问协议栈服务 int socket(int domain, int type, int p ...

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

ORB_SLAM2代码阅读及总结使用（二）相关推荐

最新文章

热门文章