ORB_随笔_内存溢出

ORB

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

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

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

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

示例代码

#include 
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
#include
#include
#include

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include

#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 ("RGB/Image", 10); 
	   pub_depth= nh.advertise ("Depth/Image", 10); 
	   pub_tcw= nh.advertise ("CameraPose", 10); 
	   pub_camerapath= nh.advertise ("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 rgb_sub(nh, "/camera/rgb/image_raw", 10);//message_filters是ROS消息过滤器，sensor_msgs是数据类型
    message_filters::Subscriber depth_sub(nh, "/camera/depth/image", 10);
    typedef message_filters::sync_policies::ApproximateTime sync_pol;
    message_filters::Synchronizer 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(0,0),RWC.at(0,1),RWC.at(0,2),
							      RWC.at(1,0),RWC.at(1,1),RWC.at(1,2),
							      RWC.at(2,0),RWC.at(2,1),RWC.at(2,2)); // tf::Matrix3x3 旋转矩阵
				  tf::Vector3 V(tWC.at(0), tWC.at(1), tWC.at(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: "<(0)<<"   "<(1)<<"    "<(2)<header.stamp;
				  header.seq = msgRGB->header.seq;
				  header.frame_id="camera";
				  //cout<<"depth type: "<< depth. type()< 
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的转角。
欢迎分享，转载请注明来源：内存溢出
原文地址:https://54852.com/zaji/4751768.html

ORB

发表评论

评论列表（0条）