Adjustments for QueueSynchronizer and IMU fixes

- Added a 15000 nanoseconds threshold for sync
- Removed Identity check for the IMI FrontendPatams since the IMU is looking backwards in ours.

examples/OakKimeraVIO.cpp

@@ -32,13 +32,14 @@
 #include "kimera-vio/utils/Statistics.h"
 #include "kimera-vio/utils/Timer.h"
 
+bool enableStereo = false;
+
 dai::Pipeline createPipeline(){
       dai::Pipeline pipeline;
       pipeline.setXLinkChunkSize(0);
       // Define sources and outputs
       auto monoLeft = pipeline.create<dai::node::MonoCamera>();
       auto monoRight = pipeline.create<dai::node::MonoCamera>();
-      // auto stereo = pipeline.create<dai::node::StereoDepth>()
       auto imu = pipeline.create<dai::node::IMU>();
 
       // enable ACCELEROMETER_RAW at 500 hz rate
@@ -47,13 +48,15 @@ dai::Pipeline createPipeline(){
       imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
 
       // min number of imu msgs in batch of X, if the host is not blocked and USB bandwidth is available
-      imu->setBatchReportThreshold(1);
+      imu->setBatchReportThreshold(8);
       // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
       // if lower or equal to batchReportThreshold then the sending is always blocking on device
       // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
       imu->setMaxBatchReports(20);
 
       auto xoutImu = pipeline.create<dai::node::XLinkOut>();
+      auto xoutL = pipeline.create<dai::node::XLinkOut>();
+      auto xoutR = pipeline.create<dai::node::XLinkOut>();
       auto xoutRectifL = pipeline.create<dai::node::XLinkOut>();
       auto xoutRectifR = pipeline.create<dai::node::XLinkOut>();
 
@@ -62,8 +65,10 @@ dai::Pipeline createPipeline(){
       // xoutRight->setStreamName("right");
       // xoutDisp->setStreamName("disparity");
       // xoutDepth->setStreamName("depth");
-      xoutRectifL->setStreamName("left");
-      xoutRectifR->setStreamName("right");
+      xoutRectifL->setStreamName("rectLeft");
+      xoutRectifR->setStreamName("rectRight");
+      xoutL->setStreamName("left");
+      xoutR->setStreamName("right");
 
       // Properties
       monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P);
@@ -81,20 +86,28 @@ dai::Pipeline createPipeline(){
 
       // stereo->enableDistortionCorrection(true);
 
-      //  Linking
-      // monoLeft->out.link(stereo->left);
-      // monoRight->out.link(stereo->right);
-
+    if (enableStereo){
+      auto stereo = pipeline.create<dai::node::StereoDepth>();
       // stereo->syncedLeft.link(xoutLeft->input);
       // stereo->syncedRight.link(xoutRight->input);
       // stereo->disparity.link(xoutDisp->input);
+      //  Linking
+      monoLeft->out.link(stereo->left);
+      monoRight->out.link(stereo->right);
 
+      stereo->setRectification(false);
       // if(outputRectified) {
-      // stereo->rectifiedLeft.link(xoutRectifL->input);
-      // stereo->rectifiedRight.link(xoutRectifR->input);
-      monoLeft->out.link(xoutRectifL->input);
-      monoRight->out.link(xoutRectifR->input);
-      // }
+      stereo->syncedLeft.link(xoutL->input);
+      stereo->syncedRight.link(xoutR->input);
+      stereo->rectifiedLeft.link(xoutRectifL->input);
+      stereo->rectifiedRight.link(xoutRectifR->input);
+
+    }
+    else{
+      monoLeft->out.link(xoutL->input);
+      monoRight->out.link(xoutR->input);
+    }
+
       imu->out.link(xoutImu->input);
 
       // if(outputDepth) {

include/kimera-vio/pipeline/QueueSynchronizer.h

@@ -135,15 +135,30 @@ class SimpleQueueSynchronizer : public QueueSynchronizerBase<T> {
             << name_id;
       }
     }
-    CHECK_EQ(timestamp, payload_timestamp)
+
+    // Keeping it less than 15000 ns. a.k.a 0.015 milliseconds
+    CHECK(std::abs(timestamp - payload_timestamp) < 15000)  
+        << "Syncing queue " << queue->queue_id_ << " in module " << name_id
+        << " failed;\n Could not retrieve exact timestamp requested: \n"
+        << " - Requested timestamp:                 " << timestamp << '\n'
+        << " - Actual timestamp:                    " << payload_timestamp << '\n'
+        << " - Requested vs Actual timestamp diff:  " << std::abs(timestamp - payload_timestamp) << " Nano Seconds \n"
+        << (i >= max_iterations
+                ? "Reached max number of sync attempts: " +
+                      std::to_string(max_iterations)
+                : "");
+
+
+    /* CHECK_EQ(timestamp, payload_timestamp)
         << "Syncing queue " << queue->queue_id_ << " in module " << name_id
         << " failed;\n Could not retrieve exact timestamp requested: \n"
         << " - Requested timestamp: " << timestamp << '\n'
         << " - Actual timestamp:    " << payload_timestamp << '\n'
         << (i >= max_iterations
                 ? "Reached max number of sync attempts: " +
                       std::to_string(max_iterations)
-                : "");
+                : ""); 
+    */
     CHECK(*pipeline_payload);
     return true;
   }

params/OAK-D/BackendParams.yaml

@@ -4,7 +4,7 @@
 backend_modality: 0
 
 #INITIALIZATION PARAMETERS
-autoInitialize: 0
+autoInitialize: 1  # 0 -> From GroundTruth; 1 -> From IMU
 roundOnAutoInitialize: 0
 initialPositionSigma: 1e-05
 initialRollPitchSigma: 0.174533 # 10.0/180.0*M_PI
@@ -38,7 +38,7 @@ landmarkDistanceThreshold: 10
 #   * is smaller than this threshold after triangulation, otherwise result is
 #   * flagged as degenerate.
 #   */
-outlierRejection: 3
+outlierRejection: 8
 # ///< threshold to decide whether to re-triangulate
 retriangulationThreshold: 0.001
 

params/OAK-D/LeftCameraParamsS2BNO.yaml

@@ -14,7 +14,7 @@ T_BS:
 
 # Camera specific definitions.
 rate_hz: 30
-resolution: [720, 1280]
+resolution: [1280, 720]
 camera_model: pinhole
 intrinsics: [803.0302124, 808.99627686, 621.92236328, 383.08587646] #fu, fv, cu, cv
 distortion_model: radial-tangential

params/OAK-D/RightCameraParamsS2BNO.yaml

@@ -6,15 +6,15 @@ camera_id: right_cam
 T_BS:
   cols: 4
   rows: 4
-  data: [0.000,  0.000,  1.000,  0.000,  # got from -> ros2 run tf2_ros tf2_echo world left_camera
-         1.000,  0.000,  0.000,  0.000,  # TODO(saching): But is the direction of tf correct ?
-         0.000, -1.000,  0.000, -0.001,
-         0.000,  0.000,  0.000,  1.000 ]
+  data: [ 0.000,  0.000,  1.000,  0.000,  # got from -> ros2 run tf2_ros tf2_echo world left_camera
+         -1.000,  0.000,  0.000,  0.000,  # TODO(saching): But is the direction of tf correct ?
+          0.000, -1.000,  0.000, -0.001,
+          0.000,  0.000,  0.000,  1.000 ]
 
 
 # Camera specific definitions.
 rate_hz: 30
-resolution: [720, 1280]
+resolution: [1280, 720]
 camera_model: pinhole
 intrinsics: [799.40985107, 805.4039917, 643.600341, 352.34115601] #fu, fv, cu, cv
 distortion_model: radial-tangential

scripts/stereoVIOOak.bash

@@ -87,7 +87,8 @@ $BUILD_PATH/stereoVIO_OAKD \
   --log_output="$LOG_OUTPUT" \
   --save_frontend_images=1 \
   --visualize_frontend_images=1 \
-  --output_path="$OUTPUT_PATH"
+  --output_path="$OUTPUT_PATH" \
+  --logbuflevel=1
 
 # If in debug mode, you can run gdb to trace problems.
 #export PARAMS_PATH=../params/Euroc

src/dataprovider/OAKDataProvider.cpp

@@ -83,10 +83,11 @@ void OAKDataProvider::leftImageCallback(std::string name, std::shared_ptr<dai::A
     CHECK(left_frame_callback_) << "Did you forget to register the left image callback to the VIO Pipeline?";
     auto daiDataPtr = std::dynamic_pointer_cast<dai::ImgFrame>(data);
     cv::Mat imageFrame = daiDataPtr->getCvFrame();
+    Timestamp localTimestamp = timestampAtFrame(daiDataPtr->getTimestamp());
     // TODO(saching): Add option to equalize the image from histogram
     left_frame_callback_(
         VIO::make_unique<Frame>(daiDataPtr->getSequenceNum(),
-                                timestampAtFrame(daiDataPtr->getTimestamp()),
+                                localTimestamp,
                                 left_cam_info_,
                                 imageFrame));
 }
@@ -95,10 +96,12 @@ void OAKDataProvider::rightImageCallback(std::string name, std::shared_ptr<dai::
     CHECK(right_frame_callback_) << "Did you forget to register the right image callback to the VIO Pipeline?";
     auto daiDataPtr = std::dynamic_pointer_cast<dai::ImgFrame>(data);
     cv::Mat imageFrame = daiDataPtr->getCvFrame();
+    Timestamp localTimestamp = timestampAtFrame(daiDataPtr->getTimestamp());
+
     // TODO(saching): Add option to equalize the image from histogram
     right_frame_callback_(
         VIO::make_unique<Frame>(daiDataPtr->getSequenceNum(),
-                                timestampAtFrame(daiDataPtr->getTimestamp()),
+                                localTimestamp,
                                 right_cam_info_,
                                 imageFrame));
 }
@@ -130,7 +133,7 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                 dai::IMUReportAccelerometer accel0, accel1;
                 dai::IMUReportGyroscope currGyro;
                 accel0.sequence = -1;
-                LOG(INFO) << "IMU INTERPOLATION: Interpolating LINEAR_INTERPOLATE_ACCEL mode ";
+                LOG(WARNING) << "IMU INTERPOLATION: Interpolating LINEAR_INTERPOLATE_ACCEL mode ";
                 while(accelHist.size()) {
                     if(accel0.sequence == -1) {
                         accel0 = accelHist.front();
@@ -147,18 +150,18 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                         double dt = duration_ms.count();
 
                         if(!gyroHist.size()) {
-                            LOG(INFO) << "IMU INTERPOLATION: Gyro data not found. Dropping accel data points";
+                            LOG(WARNING) << "IMU INTERPOLATION: Gyro data not found. Dropping accel data points";
                         }
                         while(gyroHist.size()) {
                             currGyro = gyroHist.front();
 
-                            /* LOG(INFO) <<
+                            /* LOG(WARNING) <<
                                 "IMU INTERPOLATION: ",
                                 "Accel 0: Seq => " << accel0.sequence << " timeStamp => " << (accel0.timestamp.get() - _steadyBaseTime).count();
-                            LOG(INFO) << "IMU INTERPOLATION: ",
+                            LOG(WARNING) << "IMU INTERPOLATION: ",
                                                      "currGyro 0: Seq => " << currGyro.sequence << "timeStamp => "
                                                                            << (currGyro.timestamp.get() - _steadyBaseTime).count();
-                            LOG(INFO) <<
+                            LOG(WARNING) <<
                                 "IMU INTERPOLATION: ",
                                 "Accel 1: Seq => " << accel1.sequence << " timeStamp => " << (accel1.timestamp.get() - _steadyBaseTime).count(); */
                             if(currGyro.timestamp.get() > accel0.timestamp.get() && currGyro.timestamp.get() <= accel1.timestamp.get()) {
@@ -182,14 +185,14 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                                 }
                             } else {
                                 gyroHist.pop_front();
-                                LOG(INFO) << "IMU INTERPOLATION: Dropping GYRO with old timestamps which are below accel10";
+                                LOG(WARNING) << "IMU INTERPOLATION: Dropping GYRO with old timestamps which are below accel10";
                             }
                         }
                         // gyroHist.push_back(currGyro); // Undecided whether this is necessary
                         accel0 = accel1;
                     }
                 }
-                LOG(INFO) << "IMU INTERPOLATION: Count  ->" << i << " Placing Accel 0 Seq Number :" << accel0.sequence;
+                LOG(WARNING) << "IMU INTERPOLATION: Count  ->" << i << " Placing Accel 0 Seq Number :" << accel0.sequence;
 
                 accelHist.push_back(accel0);
             }
@@ -200,7 +203,7 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                 dai::IMUReportGyroscope gyro0, gyro1;
                 dai::IMUReportAccelerometer currAccel;
                 gyro0.sequence = -1;
-                LOG(INFO) << "IMU INTERPOLATION: Interpolating LINEAR_INTERPOLATE_GYRO mode ";
+                LOG(WARNING) << "IMU INTERPOLATION: Interpolating LINEAR_INTERPOLATE_GYRO mode ";
                 while(gyroHist.size()) {
                     if(gyro0.sequence == -1) {
                         gyro0 = gyroHist.front();
@@ -213,18 +216,18 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                         double dt = duration_ms.count();
 
                         if(!accelHist.size()) {
-                            LOG(INFO) << "IMU INTERPOLATION: Accel data not found. Dropping data";
+                            LOG(WARNING) << "IMU INTERPOLATION: Accel data not found. Dropping data";
                         }
                         while(accelHist.size()) {
                             currAccel = accelHist.front();
                             /*                             
-                            LOG(INFO) << "IMU INTERPOLATION: ",
+                            LOG(WARNING) << "IMU INTERPOLATION: ",
                                                      "gyro 0: Seq => " << gyro0.sequence << std::endl
                                                                        << "       timeStamp => " << (gyro0.timestamp.get() - _steadyBaseTime).count();
-                            LOG(INFO) << "IMU INTERPOLATION: ",
+                            LOG(WARNING) << "IMU INTERPOLATION: ",
                                                      "currAccel 0: Seq => " << currAccel.sequence << std::endl
                                                                             << "       timeStamp => " << (currAccel.timestamp.get() - _steadyBaseTime).count();
-                            LOG(INFO)  << "IMU INTERPOLATION: ",
+                            LOG(WARNING)  << "IMU INTERPOLATION: ",
                                                      "gyro 1: Seq => " << gyro1.sequence << std::endl
                                                                        << "       timeStamp => " << (gyro1.timestamp.get() - _steadyBaseTime).count(); */
                             if(currAccel.timestamp.get() > gyro0.timestamp.get() && currAccel.timestamp.get() <= gyro1.timestamp.get()) {
@@ -246,7 +249,7 @@ void OAKDataProvider::FillImuDataLinearInterpolation(std::vector<dai::IMUPacket>
                                 }
                             } else {
                                 accelHist.pop_front();
-                                LOG(INFO) << "IMU INTERPOLATION: Droppinh ACCEL with old timestamps which are below accel10";
+                                LOG(WARNING) << "IMU INTERPOLATION: Droppinh ACCEL with old timestamps which are below accel10";
                             }
                         }
                         gyro0 = gyro1;

src/frontend/StereoFrame.cpp

@@ -43,7 +43,10 @@ StereoFrame::StereoFrame(const FrameId& id,
   CHECK_EQ(id_, left_frame_.id_);
   CHECK_EQ(id_, right_frame_.id_);
   CHECK_EQ(timestamp_, left_frame_.timestamp_);
-  CHECK_EQ(timestamp_, right_frame_.timestamp_);
+  // CHECK_EQ(timestamp_, right_frame_.timestamp_);
+    // Keeping it less than 15000 ns. a.k.a 0.015 milliseconds
+  CHECK(std::abs(timestamp_ - right_frame_.timestamp_) < 15000);  
+
 }
 
 void StereoFrame::setRectifiedImages(const cv::Mat& left_rectified_img,

src/imu-frontend/ImuFrontendParams.cpp

@@ -47,8 +47,8 @@ bool ImuParams::parseYAML(const std::string& filepath) {
       UtilsOpenCV::poseVectorToGtsamPose3(vector_pose);
 
   // Sanity check: IMU is usually chosen as the body frame.
-  LOG_IF(FATAL, !body_Pose_cam.equals(gtsam::Pose3::Identity()))
-      << "parseImuData: we expected identity body_Pose_cam_: is everything ok?";
+  // LOG_IF(FATAL, !body_Pose_cam.equals(gtsam::Pose3::Identity()))
+  //     << "parseImuData: we expected identity body_Pose_cam_: is everything ok?";
 
   double rate_hz = 0.0;
   yaml_parser.getYamlParam("rate_hz", &rate_hz);