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 <geometry_msgs/PoseStamped.h> #include <tf/tf.h> #include <tf/transform_datatypes.h> #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.

[qiuliangcheng]

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.

but i still get the warning:
[ WARN] [1681531910.010008887]: TF to MSG: Quaternion Not Properly Normalized
and i see the pose,it x,y,z just 0.00001 ,i dont't konw how to slove the problem,could you help me?

[CQnancy]

运行良好，但将不可避免地在前 5 分钟内抛出运行时错误，并显示以下消息：

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

我很好奇是否想到了任何明显的原因，如果没有，我会仔细阅读文档并尝试解决它。同样，这一切都是在 ROS_mono.cc 中完成的，而不是上面的 ROS_stereo.cc 中完成的。

你好，mkestrada2，
我也在 ROS_mono.cc 中实现了这个，运行大约一分钟后我得到了与你相同的错误......
Segmentation fault (core dumped)
你解决过这个问题吗？和记忆力有关系吗？
感谢您的帮助！

我解决了删除 GrabImage 函数中的 ros::spin() 问题。我没有能力准确解释原因，但分段错误可能是由多个 ros::spin 引起的，订阅者试图读取尚未发布的数据。此外，据我了解，你不需要加入出版商。

但我仍然收到警告： [警告] [1681531910.010008887]：TF到MSG：四元数未正确标准化 ，我看到了姿势，它的x，y，z只是0.00001，我不知道如何解决这个问题，你能吗帮我？