Orb_slam2: Get Pose Information from ORBSLAM

[ksivakumar]

Hi, I wish to extract pose information about my Zed stereo camera that is running ORBSLAM2. I see in the MapPoint.h file there is a function GetWorldPos(), but it returns a Mat. In what format is this Mat in and how would I convert this to global pose? I would like to convert this Mat into a PoseStamped ROS message. For context, my goal is to just map the pose in RVIZ and make a map, so I'd like to then publish the PoseStamped message.

Thank you for any help.

[saikrn112]

Hi Sivakumar, if you simply want to publish pose using a stereo node then add these set of lines in the ros_stereo.cpp
in int main() declare your pose publisher pose_pub and in ImageGrabber::GrabStereo method, retrieve the cv::Mat pose information after every frame capture by replacing
mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
with
Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
Now you can use ROS tf library to get the quaternion and position information from the matrix. Here is how I did it (inspired from this), there might be an efficient way of doing it.

geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="map";

cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

tf::Transform new_transform;
new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));

tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
new_transform.setRotation(quaternion);

tf::poseTFToMsg(new_transform, pose.pose);
pose_pub.publish(pose);

Hope this helps!

[ksivakumar]

Great, thank you so much! I will try this solution and follow up if there are any issues.

[ksivakumar]

So for some reason the pose publisher does not show up when I do rostopic list while the code is running. As a result, I'm not able to subscribe to the topic from RVIZ. I put the publisher code all below the line that has the sync registerCallback. Is this correct?

I also tried adding cout statements ending with endl to see if the file was indeed getting to where I was publishing the node. However, I am getting no output from these cout statements I added within the main function. So not sure why file is not reading cout statements nor publishing to the topic.

Also this is what I have for my publisher definition:
ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("orb_pose", 100);

Any help on why I am not able to publish would be much appreciated!

[ksivakumar]

And one more question just about the code, what variable would I need to convert into a ROS message type if I wanted to publish the key points detected by ORB SLAM? I would like to create the map based off these key features!

[saikrn112]

Hey sorry for the late update, can you post your ImageGrabber::GrabStereo method that you modified in ros_stereo.cpp

For extracting key features you can use methods mpSLAM->GetmpMapAllMapPoints() and mpSLAM->GetmpMapReferenceMapPoints() within in ros_stereo.cpp and use PCL to make a map. I haven't done it myself yet.

[ksivakumar]

Thanks for following up! I actually got it working, I had put your code in the main function instead of the GrabStereo method which was why nothing was being called back repeatedly.

And great, I'll try getting that to work then! Will follow up if I have any other questions!

[ksivakumar]

So when I run the ORB_SLAM stereo file using my own stereo camera through ROS topics, I can see points in your GUI but there seems to be no save button or stop button that will let me save the map which I can then use at a later time for localization. Is there a way to save the map as a file which I can pull up later? If not, what file would I need to edit to change the GUI functionalities?

Also, I see that there are these lines in ros_stereo.cpp:

    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");

Where are these text files being saved? I have been exiting the application through Ctrl+c, so not sure if these text files will save then. There is also no stop button on the GUI, so how would I ensure the saving of the map?

Thanks!

[mkestrada2]

Hi, I had a similar interest in returning the pose data with a mono setup. to that end, I was able to adapt and modify the above code for my use case, but I am experiencing a stability issue. I can launch the software and it appears to function as usual. I am also able to read the published orb_pose messages a Simulink program using a ROS subscriber block. The issue is that it will crash if left running. I made all of the modification listed in above posts in ImageGrabber:

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvShare(msg);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    bool test=0;

    cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
    ros::NodeHandle nh;
    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("orb_pose", 5);

    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id ="map";
    test = !Tcw;

    // test if 
    if(test==1){
        tf::Transform new_transform;
        tf::poseTFToMsg(new_transform, pose.pose);
        pose_pub.publish(pose);
    }
    else{
    cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
    cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
    vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

    tf::Transform new_transform;
    new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));

    tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
    new_transform.setRotation(quaternion);

    tf::poseTFToMsg(new_transform, pose.pose);
    pose_pub.publish(pose);
    }
    ros::spin();
}

where:

bool operator ! (const cv::Mat&m) { return m.empty();}

defines the behavior of "!".

this runs fine but will inevitably throw a run-time error within the first ~5 minutes with the following messages:

[warn] [time stamp]: TF to MSG: Quaternion Not Properly Normalized
Segmentation fault (core dumped)

I was curious if any obvious reason for the came to mind, if not, I will peruse documentation and try to solve it. Again, this is all done in ROS_mono.cc, not ROS_stereo.cc as above.

[sume-cn]

Hi Sivakumar, if you simply want to publish pose using a stereo node then add these set of lines in the ros_stereo.cpp
in int main() declare your pose publisher pose_pub and in ImageGrabber::GrabStereo method, retrieve the cv::Mat pose information after every frame capture by replacing
mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
with
Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
Now you can use ROS tf library to get the quaternion and position information from the matrix. Here is how I did it (inspired from this), there might be an efficient way of doing it.

geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="map";

cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

tf::Transform new_transform;
new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));

tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
new_transform.setRotation(quaternion);

tf::poseTFToMsg(new_transform, pose.pose);
pose_pub.publish(pose);

Hope this helps!

thanks, very helpful

additional header files:
`#include

include

include

include"../../../include/Converter.h"`

[chrismath]

I have tried this for stereo vision. But when I published pose are not providing the correct pose. I tried the V1_01 dataset while running this I get a good pose out. As I interfaced my stereo camera the pose I get is corresponding to the motion I make, the change in position is not mapped correctly.

[MaEppl]

Hey,
I also managed to get the actual pose with the described method. Now I would like to get the optimized camera position as a geometry_msgs::TransformStamped message after I used the shutdown() function. I'm not sure about the best way to do this. The SaveTrajectoryTUM() function should already deliver the correct format (x y z qx qy qz qw), so no additional conversion to the geometry_msgs is necessary?
Another idea is to write a script that converts the TUM.txt file into a bag file.

[y-elena]

this runs fine but will inevitably throw a run-time error within the first ~5 minutes with the following messages:

[warn] [time stamp]: TF to MSG: Quaternion Not Properly Normalized
Segmentation fault (core dumped)

I was curious if any obvious reason for the came to mind, if not, I will peruse documentation and try to solve it. Again, this is all done in ROS_mono.cc, not ROS_stereo.cc as above.

Hi mkestrada2,

I implemented this as well in ROS_mono.cc and I am getting the same error as you after about a minute of running...

Segmentation fault (core dumped)

Did you ever solve this? Does it have to do with memory?

Thanks for your help!

[scogliodellareginasssup]

this runs fine but will inevitably throw a run-time error within the first ~5 minutes with the following messages:

[warn] [time stamp]: TF to MSG: Quaternion Not Properly Normalized
Segmentation fault (core dumped)

I was curious if any obvious reason for the came to mind, if not, I will peruse documentation and try to solve it. Again, this is all done in ROS_mono.cc, not ROS_stereo.cc as above.

Hi mkestrada2,

I implemented this as well in ROS_mono.cc and I am getting the same error as you after about a minute of running...

Segmentation fault (core dumped)

Did you ever solve this? Does it have to do with memory?

Thanks for your help!

I solved removing the ros::spin() in the GrabImage function.
I have no competences to explain why exactly but a segmentation fault can be caused by multiple ros::spin with the subscriber trying to read data which have not been published yet.
Moreover, as far as I understood, you don't need to spin in a publisher.