Compare cam and IMU tracking results

[Zoltan3057]

Hi,

I tried to compare cam and imu tracking results, However, translation results always seem different.

        // In dmvio::IMUInitializerLogic::addPose functions Add Following
        // get transform DSO IMU
        std::shared_ptr<PoseTransformation> dso_to_imu = coarseIMUOptimizer->getUpdatedTransform();
        Eigen::Matrix4d T_cam_imu = dso_to_imu->getT_cam_imu().matrix();

        // Cam Tracking Result
        Eigen::Matrix4d T_camp_camc = shell.camToTrackingRef.matrix();
        Eigen::Matrix<double,3,3> R2 = T_camp_camc.block(0,0,3,3);

        // IMU Integration Result
        Eigen::Matrix4d T_imup_imuc = Eigen::Matrix4d::Identity();
        T_imup_imuc.block(0,0,3,3) = imuMeasurements.deltaRij().matrix();
        T_imup_imuc.block(0,3,3,1) = imuMeasurements.deltaPij().matrix();
        Eigen::Matrix4d T_camp_camc_measure = T_cam_imu * T_imup_imuc * T_cam_imu.inverse();

        // Compare two Results
        Eigen::Matrix<double,3,3> R1 = T_camp_camc_measure.block(0,0,3,3);
        // two angles are same 
        std::cout << std::fixed << shell.timestamp << " " << R1.eulerAngles(0, 1, 2).transpose() << std::endl;
        std::cout << std::fixed << shell.timestamp << " " << R2.eulerAngles(0, 1, 2).transpose() << std::endl;
        // two translations are always different
        std::cout << std::fixed << shell.timestamp << " " << T_camp_camc_measure.block(0,3,3,1).transpose() << std::endl;
        std::cout << std::fixed << shell.timestamp << " " <<T_camp_camc.block(0,3,3,1).transpose() << std::endl;

thanks in advance.

[lukasvst]

I quickly tried the dataset you linked in the earlier version of your post. It seems like there's a problem with your IMU data or calibration, as the visual-only version seems to work fine and the CoarseIMUInitError is way larger than it should be.

My best guess would be that time calibration is somehow off or maybe that the T_cam_imu is wrong. How have you recorded your dataset and which camera is used?

Unfortunately I cannot provide much support for custom datasets unless there is a sign that there is a bug with DM-VIO.

[Zoltan3057]

Hi, thanks for the reply. Previous Data is recorded by L515 realsense. I happened to set the imu random walk parameters wrongly，which causes the big error in CoarseIMUInitError. GTSAM uses noise model for imu factor.

double NoiseModelFactor::error(const Values& c) const {
  if (active(c)) {
    const Vector b = unwhitenedError(c);
    check(noiseModel_, b.size());
    if (noiseModel_)
      return 0.5 * noiseModel_->distance(b); // wrong imu parameter will cause this distance way higher than usual
    else
      return 0.5 * b.squaredNorm();
  } else {
    return 0.0;
  }
}

I will check T_cam_imu and enable time calibration later to see if those are the reasons for bad performance when imu is enabled. Cheers.