把建图系统分为了三个节点,如下所示。
第一个节点作为驱动节点,采集摄像头传感器的数据。
第二个节点利用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的转角。