How to get linear velocity from the linear acceleration using Euler integration in ORB_SLAM3?

edit

Hi I would like to estimate linear velocity using Euler integration from the IMU data (linear acceleration) in ORB_SLAM3 so can publish the odometry message. I modified the stereo-inertial node from the ORB_SLAM3 package so can get the linear velocity. Im using ROS2 humble and C++.

So I modified SyncWithImu() function to estimating velocity using Euler integration from IMU linear acceleration data. But I got some errors. Here is the code

#include "monocular-slam-node.hpp"
#include <opencv2/core/core.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
//#include "nav_msgs/msg/odometry.hpp"

using std::placeholders::_1;

MonocularSlamNode::MonocularSlamNode(ORB_SLAM3::System* pSLAM)
    : Node("ORB_SLAM3_ROS2")
{
    m_SLAM = pSLAM;
    m_image_subscriber = this->create_subscription<ImageMsg>("camera",10, std::bind(&MonocularSlamNode::GrabImage, this, std::placeholders::_1));
    m_imu_subscriber = this->create_subscription<ImuMsg>("imu",10, std::bind(&MonocularSlamNode::GrabImu, this, std::placeholders::_1));
    syncThread_ = new std::thread(&MonocularSlamNode::SyncWithImu, this);
    odometry_publisher = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    //m_velocity_pub = this->create_publisher<geometry_msgs::Twist>("lin_velocity", 10);
}

MonocularSlamNode::~MonocularSlamNode()
{
    // Delete sync thread
    syncThread_->join();
    delete syncThread_;

    // Stop all threads
    m_SLAM->Shutdown();

    // Save camera trajectory
    m_SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
}

void MonocularSlamNode::GrabImu(const ImuMsg::SharedPtr msg)
{
    bufImuMutex_.lock();
    imuBuf_.push(msg);
    bufImuMutex_.unlock();
}

void MonocularSlamNode::GrabImage(const ImageMsg::SharedPtr msg)
{
    bufImgMutex_.lock();

    if (!imgBuf_.empty())
        imgBuf_.pop();
    imgBuf_.push(msg);

    bufImgMutex_.unlock();
}
cv::Mat MonocularSlamNode::GetImage(const ImageMsg::SharedPtr msg)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptr;

    try
    {
        cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception &e)
    {
        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
    }

    if (cv_ptr->image.type() == 0)
    {
        return cv_ptr->image.clone();
    }
    else
    {
        std::cerr << "Error image type" << std::endl;
        return cv_ptr->image.clone();
    }
}

void MonocularSlamNode::SyncWithImu()
{
    const double maxTimeDiff = 0.01;

    // Initialize variables for velocity estimation
    cv::Point3f velocity(0, 0, 0);  // Initial velocity (x, y, z)
    double prevTime = 0;           // Previous IMU timestamp

    while (1)
    {
        cv::Mat im;
        double tIm = 0;
        if (!imgBuf_.empty() && !imuBuf_.empty())
        {
            tIm = Utility::StampToSec(imgBuf_.front()->header.stamp);

            bufImgMutex_.lock();
            while (imgBuf_.size() > 1)
            {
                imgBuf_.pop();
                tIm = Utility::StampToSec(imgBuf_.front()->header.stamp);
            }
            bufImgMutex_.unlock();

            if (tIm > Utility::StampToSec(imuBuf_.back()->header.stamp))
                continue;

            bufImgMutex_.lock();
            im = GetImage(imgBuf_.front());
            imgBuf_.pop();
            bufImgMutex_.unlock();

            vector<ORB_SLAM3::IMU::Point> vImuMeas;
            bufImuMutex_.lock();
            if (!imuBuf_.empty())
            {
                // Load imu measurements from buffer
                vImuMeas.clear();
                while (!imuBuf_.empty() && Utility::StampToSec(imuBuf_.front()->header.stamp) <= tIm)
                {
                    double t = Utility::StampToSec(imuBuf_.front()->header.stamp);
                    cv::Point3f acc(imuBuf_.front()->linear_acceleration.x, imuBuf_.front()->linear_acceleration.y, imuBuf_.front()->linear_acceleration.z);
                    cv::Point3f gyr(imuBuf_.front()->angular_velocity.x, imuBuf_.front()->angular_velocity.y, imuBuf_.front()->angular_velocity.z);
                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
                    imuBuf_.pop();
                }
            }
            bufImuMutex_.unlock();

            if (bClahe_)
            {
                clahe_->apply(im, im);
            }

            cv::remap(im, im, M1_, M2_, cv::INTER_LINEAR);

            // Estimate velocity using Euler integration
            if (!vImuMeas.empty())
            {
                // Initialize variables for velocity estimation
                cv::Point3f linearAcc(0, 0, 0);  // Accumulated linear acceleration
                double deltaTime = 0;           // Time difference between consecutive IMU measurements

                // Loop through IMU measurements and perform Euler integration for velocity estimation
                for (size_t i = 1; i < vImuMeas.size(); ++i)
                {
                    // Calculate time difference between consecutive IMU measurements
                    deltaTime = vImuMeas[i].t - vImuMeas[i - 1].t;

                    // Integrate ...