Fixing ORBSLAM3 TcwMat Empty Error With RTAB-Map

Hey guys! Are you struggling with the infamous TcwMat empty error when using ORBSLAM3 with RTAB-Map in Windows, especially in Stereo-Inertial mode without ROS? You're definitely not alone! This error can be a real head-scratcher, but don't worry, we're going to dive deep into the issue and explore some potential solutions. Let's get this fixed together!

Understanding the Issue

First off, let's break down what's happening. You're likely encountering this error because the transformation matrix TcwMat, which represents the transformation from the camera frame to the world frame, is not being properly initialized or computed within ORBSLAM3. This usually manifests as a [0 x 0] matrix, as you've observed in your logs. This is a critical problem because RTAB-Map relies on this transformation to perform accurate odometry and mapping.

When you see the error message [FATAL] OdometryORBSLAM3.cpp:465:: rtabmap::OdometryORBSLAM3::computeTransform() Condition (TcwMat.cols == 4 && TcwMat.rows == 4) not met! it means the computeTransform() function within the OdometryORBSLAM3 class is expecting a 4x4 matrix, but it's receiving an empty one. The output "not IMU meas" and Tcw is empty at the start is normal, so we have to fix it.

The fact that ORBSLAM3 works fine on its own but fails within RTAB-Map suggests that the integration between the two might be the source of the problem. Issues often arise from how data is passed between the systems, especially the synchronization of IMU and image data. It's like trying to fit puzzle pieces together, and if they're not perfectly aligned, you'll run into problems.

Key Areas to Investigate

Before we jump into solutions, let's pinpoint the most likely culprits:

1. Data Synchronization: The timestamps between your IMU and image data are crucial. If these timestamps aren't properly aligned, ORBSLAM3 might not be able to correctly fuse the inertial measurements with the visual information, leading to an empty TcwMat.
2. IMU Initialization: ORBSLAM3 needs a sufficient amount of IMU data to initialize its inertial state. If the initial IMU data is noisy or insufficient, it can prevent the system from properly estimating the initial pose.
3. Parameter Settings: Incorrect parameters within RTAB-Map or ORBSLAM3 can also cause this issue. For example, the OdomORBSLAM/Inertial parameter needs to be correctly set to true when using IMU data.
4. Vocabulary Loading: A failure in loading the ORB vocabulary file can also lead to issues, although this is less likely given your logs show the vocabulary loading successfully.

Troubleshooting Steps

Okay, let's get our hands dirty and try to fix this! Here’s a step-by-step guide to troubleshooting:

1. Verify Data Timestamps

This is the most critical step, guys. Ensure your IMU and image timestamps are synchronized. Here's how you can do it:

• Double-Check Timestamp Format: Your code shows that you're dividing the IMU timestamp by 1e9. Make sure this conversion is correct and consistent with the image timestamps. Any mismatch in units (e.g., nanoseconds vs. seconds) will cause problems.

• Log Timestamps: Add print statements to your processIMU function to log the IMU timestamps (imus_to_process[i].first) and the image timestamp (imgstamp). This will help you visualize if the IMU data is being processed for the correct image frames.

  int32_t processIMU(double imgstamp){     std::vector<std::pair<double, rtabmap::IMU> > imus_to_process;     {
      auto iterEnd =imus_buffer_.lower_bound(imgstamp);
      if (iterEnd != imus_buffer_.end()){
          iterEnd++;
      }
  
      for(std::map<double, rtabmap::IMU>::iterator iter=imus_buffer_.begin(); iter!=iterEnd;)
      {
          imus_to_process.push_back(*iter);
          imus_buffer_.erase(iter++);
      }
  }
  std::cout<<