How to convert or implement a CVMat to SE function in ROS2?

edit

I created a node for monocular inertial ORB_SLAM3. Using ROS2 Humble and C++. The code has one problem as need to convert Cv::Mat type to Sophus::SE3. Here is the function where the convert should happened

...

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 &timestamp, 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);
  }
}
...

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)?

Thanks