ORB_SLAM3
ORB_SLAM3 copied to clipboard
UZ-SLAMLab
How to publish pose topic on ROS2 with monocular -inertial node?
Hi
I checked the post for the mono-inertial node of ros1 with [https://github.com/pinkfloydfan/ORB_SLAM3/blob/83d41a1e924c6b9e45ce0e2aca7398107c3bb111/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc#L206] . Now try to adapt for ros2 humble and also to publish the pose. Here is my function where it should happen
...
void ImageGrabber::SyncWithImu()
{
while (rclcpp::ok())
{
cv::Mat im;
double tIm = 0;
if (!img0Buf.empty()&&!mpImuGb->imuBuf.empty())
{
tIm = img0Buf.front()->header.stamp.sec + img0Buf.front()->header.stamp.nanosec * 1e-9;
if (tIm > mpImuGb->imuBuf.back()->header.stamp.sec + mpImuGb->imuBuf.back()->header.stamp.nanosec * 1e-9)
continue;
{
this->mBufMutex.lock();
im = GetImage(img0Buf.front());
img0Buf.pop();
this->mBufMutex.unlock();
}
vector<ORB_SLAM3::IMU::Point> vImuMeas;
mpImuGb->mBufMutex.lock();
if(!mpImuGb->imuBuf.empty())
{
// Load imu measurements from buffer
vImuMeas.clear();
while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.sec + mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9 <= tIm)
{
double t = mpImuGb->imuBuf.front()->header.stamp.sec + mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9;
cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
mpImuGb->imuBuf.pop();
}
}
mpImuGb->mBufMutex.unlock();
if(mbClahe)
mClahe->apply(im,im);
cv::Mat T_, R_, t_ ;
//Sophus::SE3f T_;
//stores return variable of TrackMonocular
//cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
T_= mpSLAM->TrackMonocular(im,tIm,vImuMeas);
if (!(T_.empty()))
{
RCLCPP_INFO(logger_, "Enering publisher execution block");
cv::Size s = T_.size();
if ((s.height >= 3) && (s.width >= 3))
{
R_ = T_.rowRange(0,3).colRange(0,3).t();
t_ = -R_*T_.rowRange(0,3).col(3);
std::vector<float> q = ORB_SLAM3::Converter::toQuaternion(R_);
float scale_factor=1.0;
tf2::Transform tf_transform;
tf_transform.setOrigin(tf2::Vector3(t_.at<float>(0, 0) * scale_factor, t_.at<float>(0, 1) * scale_factor, t_.at<float>(0, 2) * scale_factor));
tf_transform.setRotation(tf2::Quaternion(q[0], q[1], q[2], q[3]));
//RCLCPP_INFO(this->get_logger(), "Mono_Inertial node started");
RCLCPP_INFO(logger_, "Transformation matrix R_: %s", cv::format("%f %f %f\n%f %f %f\n%f %f %f",
R_.at<float>(0, 0), R_.at<float>(0, 1), R_.at<float>(0, 2),
R_.at<float>(1, 0), R_.at<float>(1, 1), R_.at<float>(1, 2),
R_.at<float>(2, 0), R_.at<float>(2, 1), R_.at<float>(2, 2)).c_str());
}
}
}
//RCLCPP_INFO(logger_, "After publisher execution block");
std::chrono::milliseconds tSleep(1);
std::this_thread::sleep_for(tSleep);
}
}
...
The code has one problem as need to convert Cv::Mat type to Sophus::SE3. So I would like to publish the pose of the ORB_SLAM3 using camera and IMU. The problem is in this line
T_= mpSLAM->TrackMonocular(im,tIm,vImuMeas);
And I got this error:
`error: no match for ‘operator=’ (operand types are ‘cv::Mat’ and ‘Sophus::SE3f’ {aka ‘Sophus::SE3’}) 257 | T_= mpSLAM->TrackMonocular(im,tIm,vImuMeas);`
So how is the way to implement CvMatToSE3 function , mean this conversion? Or do I need to implement the CvMatToSE3 function in the Converter class (inside Converter.h in ~ORB_SLAM3/include/Converter.h or a separate header/cpp file)?
Try this:
Sophus::SE3f T_ = SLAM_->TrackMonocular(im, tIm, vImuMeas);
cv::Mat T_cv = cv::Mat(4, 4, CV_32F, T_.matrix().data());
it worked for me.
Hi,
@astronaut71 do you have the code on transforming the ros monocular inertial implementation for orbslam3 to ros2 humble? I'm also working on the same thing since most of the ros2 orbslam3 wrapper doesn't have it implemented. Can I get your code as a reference to what I currently have?
Hi
@astronaut71 I am also working on implementing the mono-inertial node for ros2 using this wrapper https://github.com/zang09/ORB_SLAM3_ROS2/tree/main/src But it is also missing mono-inertial node? Did you get it to work? if so, do you mind sharing the code?
@astronaut71 @y2d2 @Unknown9190 Did any of you get the mono_inertial node to work? I have been stuck on this for weeks. If you can share the code it would be much appreciated
Hi .You check my repo https://github.com/astronaut71/orb_slam3_ros2. I tried bu not working
I tried many version of this node nut none working. maybe you can fix it
#include <iostream>
#include <queue>
#include <thread>
#include <mutex>
#include "rclcpp/rclcpp.hpp"
#include <cv_bridge/cv_bridge.h>
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include "utility.hpp"
#include <System.h>
#include "ImuTypes.h"
#include" Converter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Transform.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
using namespace std;
class ImuGrabber
{
public:
ImuGrabber(const rclcpp::Logger& logger)
: logger_(logger) {
// Constructor implementation
}
void GrabImu(const sensor_msgs::msg::Imu::SharedPtr imu_msg);
queue<sensor_msgs::msg::Imu::SharedPtr> imuBuf;
std::mutex mBufMutex;
private:
rclcpp::Logger logger_;
};
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bClahe, rclcpp::Logger logger)
: mpSLAM(pSLAM), mpImuGb(pImuGb), mbClahe(bClahe), logger_(logger) {}
void GrabImage(const sensor_msgs::msg::Image::SharedPtr img_msg);
cv::Mat GetImage(const sensor_msgs::msg::Image::SharedPtr img_msg);
void SyncWithImu();
void SetPub(rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub);
queue<sensor_msgs::msg::Image::SharedPtr> img0Buf;
std::mutex mBufMutex;
ORB_SLAM3::System* mpSLAM;
ImuGrabber *mpImuGb;
bool mbClahe;
cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(4, 4));
private:
rclcpp::Logger logger_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr orb_pub_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
class MonoInertial : public rclcpp::Node
{
public:
MonoInertial(const std::string& vocabulary_path, const std::string& settings_path, bool do_equalize)
: Node("Mono_Inertial"),
SLAM_(nullptr),
imugb_(nullptr),
igb_(nullptr),
logger_(get_logger()),
pose_pub_(nullptr),
sub_imu_(nullptr),
sub_img0_(nullptr)
{
RCLCPP_INFO(logger_, "Mono_Inertial node started");
SLAM_ = new ORB_SLAM3::System(vocabulary_path, settings_path, ORB_SLAM3::System::IMU_MONOCULAR, true);
imugb_ = new ImuGrabber(logger_);
igb_ = new ImageGrabber(SLAM_, imugb_, do_equalize, logger_);
RCLCPP_INFO(logger_, "Creating publisher for /orb_pose topic...");
pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/orb_pose", rclcpp::SystemDefaultsQoS());
RCLCPP_INFO(logger_, "Publisher for /orb_pose topic created.");
sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
"/anafi/drone/imu", rclcpp::SystemDefaultsQoS(), [&](const sensor_msgs::msg::Imu::SharedPtr imu_msg) { imugb_->GrabImu(imu_msg); });
sub_img0_ = create_subscription<sensor_msgs::msg::Image>(
"/anafi/camera/image", rclcpp::SystemDefaultsQoS(), [&](const sensor_msgs::msg::Image::SharedPtr img_msg) { igb_->GrabImage(img_msg); });
igb_->SetPub(pose_pub_);
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
sync_thread_ = std::thread(&ImageGrabber::SyncWithImu, igb_);
}
private:
ORB_SLAM3::System* SLAM_;
ImuGrabber* imugb_;
ImageGrabber* igb_;
rclcpp::Logger logger_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img0_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::thread sync_thread_;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
bool do_equalize = false;
if (argc < 3 || argc > 4)
{
RCLCPP_ERROR(rclcpp::get_logger("Mono_Inertial"), "Usage: ros2 run ORB_SLAM3 Mono_Inertial path_to_vocabulary path_to_settings [do_equalize]");
rclcpp::shutdown();
return 1;
}
if (argc == 4)
{
std::string sbEqual(argv[3]);
if (sbEqual == "true")
do_equalize = true;
}
auto node = std::make_shared<MonoInertial>(argv[1], argv[2], do_equalize);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::SharedPtr img_msg)
{
mBufMutex.lock();
if (!img0Buf.empty())
img0Buf.pop();
img0Buf.push(img_msg);
mBufMutex.unlock();
//RCLCPP_INFO(logger_, "ImageGrabber: New image data grabbed.");
}
cv::Mat ImageGrabber::GetImage(const sensor_msgs::msg::Image::SharedPtr img_msg)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what());
return cv::Mat();
}
cv::Mat gray_img;
cv::cvtColor(cv_ptr->image, gray_img, cv::COLOR_RGB2GRAY);
if (mbClahe)
{
cv::Mat processed_img;
mClahe->apply(gray_img, processed_img);
return processed_img;
}
else
{
return gray_img;
}
}
void ImageGrabber::SyncWithImu()
{
// Define a rate control to limit loop frequency
rclcpp::Rate loop_rate(30); // Adjust the frequency as needed
while (rclcpp::ok())
{
// Fetch IMU data with lock and minimal work within lock
std::vector<ORB_SLAM3::IMU::Point> vImuMeas;
{
std::lock_guard<std::mutex> lock(mpImuGb->mBufMutex);
while (!mpImuGb->imuBuf.empty())
{
auto imu_msg = mpImuGb->imuBuf.front();
double t = imu_msg->header.stamp.nanosec * 0.17;
//double t = imu_msg->header.stamp.nanosec * 0.17;
cv::Point3f gyr(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
cv::Point3f acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(gyr, acc,t));
mpImuGb->imuBuf.pop();
}
}
// Process image with minimal work within lock
cv::Mat im;
double tIm = 0;
{
std::lock_guard<std::mutex> lock(mBufMutex);
if (!img0Buf.empty())
{
tIm = img0Buf.front()->header.stamp.sec * 1e-9*0.45;
//tIm = img0Buf.front()->header.stamp.sec*1e2 + img0Buf.front()->header.stamp.nanosec * 1e-9;
im = GetImage(img0Buf.front());
img0Buf.pop();
}
}
if (!vImuMeas.empty() && !im.empty())
{
// Perform image processing outside the lock
if (mbClahe)
mClahe->apply(im, im);
//std::lock_guard<std::mutex> lock(mBufMutex);
auto image_msg = img0Buf.front();
//im = GetImage(img0Buf.front());
img0Buf.pop();
// Perform SLAM operations
Sophus::SE3f Tcw_se3 = mpSLAM->TrackMonocular(im, Utility::StampToSec(image_msg->header.stamp), vImuMeas);
// Publish pose data
Eigen::Matrix4f Tcw_eigen = Tcw_se3.matrix().cast<float>();
Eigen::Matrix3f R_eigen = Tcw_eigen.block<3, 3>(0, 0);
Eigen::Vector3f t_eigen = Tcw_eigen.block<3, 1>(0, 3);
geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
pose_msg.header.frame_id = "/world";
pose_msg.pose.position.x = t_eigen(0);
pose_msg.pose.position.y = t_eigen(1);
pose_msg.pose.position.z = t_eigen(2);
Eigen::Quaterniond quat(R_eigen.cast<double>());
pose_msg.pose.orientation.x = quat.x();
pose_msg.pose.orientation.y = quat.y();
pose_msg.pose.orientation.z = quat.z();
pose_msg.pose.orientation.w = quat.w();
orb_pub_->publish(pose_msg);
}
// Control loop frequency
loop_rate.sleep();
}
}
void ImuGrabber::GrabImu(const sensor_msgs::msg::Imu::SharedPtr imu_msg)
{
mBufMutex.lock();
imuBuf.push(imu_msg);
mBufMutex.unlock();
}
void ImageGrabber::SetPub(rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub)
{
orb_pub_ = pub;
}
hi @Nikolaiarn I posted my repo and some of the nodes I tried. Please let me know if you able to fix it
@astronaut71, thanks for the reply. I will let you know if I get t to work.
I believe I have mono-inertial functionality working - it runs but I have no calibration for my IMU to confirm and haven't run any rosbags yet. I will have real-world tests by end of this week anyway when I'm back at the robot. The topic for the imu is currently just /imu and can be remapped
https://github.com/TheRealBeef/ORB_SLAM3_ROS2
I'm currently running with the following command - note I've done "bad" and edited TUM-VI.yaml to match my current webcam. I'll revert that before I call the repo completed.
ros2 run orbslam3 mono-inertial src/orbslam3_ros2/vocabulary/ORBvoc.txt src/orbslam3_ros2/config/monocular-inertial/TUM-VI.yaml --ros-args --remap /camera:=/camera1/image_raw
I will be adding the publisher for frame/map this week as well
unsubscribe
------------------ 原始邮件 ------------------ 发件人: "UZ-SLAMLab/ORB_SLAM3" @.>; 发送时间: 2024年5月4日(星期六) 晚上7:24 @.>; @.***>; 主题: Re: [UZ-SLAMLab/ORB_SLAM3] How to publish pose topic on ROS2 with monocular -inertial node? (Issue #793)
I believe I have mono-inertial working - it runs but I have no calibration for my IMU to confirm and haven't run any rosbags yet. I will have real-world tests by end of this week anyway when I'm back at the robot
https://github.com/TheRealBeef/ORB_SLAM3_ROS2
I'm currently running with the following command - note I've done "bad" and edited TUM-VI.yaml to match my current webcam. I'll revert that before I call the repo completed.
ros2 run orbslam3 mono-inertial src/orbslam3_ros2/vocabulary/ORBvoc.txt src/orbslam3_ros2/config/monocular-inertial/TUM-VI.yaml --ros-args --remap /camera:=/camera1/image_raw
— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you are subscribed to this thread.Message ID: @.***>
