search

luxonis / depthai-ros

Official ROS Driver for DepthAI Sensors.
MIT License
239 stars 173 forks source link

Camera factory calibration interpretation #554

Open abaeyens-imod opened 3 days ago

abaeyens-imod commented 3 days ago

Check if issue already exists Here's the same issue on the Luxonis forum.

Describe the bug Applying the factory calibration published in the CameraInfo message does not deliver sensible results. Recalibrating the camera using standard tools resulted in a very different calibration which does work, but preferably we would like to use what gets published on the camera_info topics.

Minimal Reproducible Example Here's an MRE, including a bag file. It uses a marker detection library to generate a set of 3D points from the right camera image and then projects these back onto image space for all three cameras.

Key code used for projecting 3D points to image coordinates using the Luxonis calibration:

void projectMarkersToImageCoordinates(
    const std::vector<stag_ros::STagMarker> markers, 
    const sensor_msgs::CameraInfo & camera_info,
    const cv::Point3d & camera_position,
    std::vector<cv::Point2d> & uvs) {
  // Ensure output is empty
  uvs.clear();
  // Nothing to project => exit
  if (markers.empty()) return;
  // Prepare input
  std::vector<cv::Point3d> object_points;
  for (const stag_ros::STagMarker& m : markers) {
    object_points.push_back(cv::Point3d(
      m.pose.position.x - camera_position.x,
      m.pose.position.y - camera_position.y,
      m.pose.position.z - camera_position.z));
  }
  cv::Mat r(3, 3, CV_64F);
  for (int i = 0; i < 9; ++i) r.at<double>(i) = camera_info.R[i];
  cv::Mat rvec;
  cv::Rodrigues(r, rvec);
  cv::Mat tvec = cv::Mat::zeros(cv::Size(3, 1), CV_64F);
  cv::Mat k(3, 3, CV_64F);
  for (int i = 0; i < 9; ++i) k.at<double>(i) = camera_info.K[i];
  // Assume distortion model follows OpenCV format
  cv::Mat d(camera_info.D.size(), 1, CV_64F);
  for (int i = 0; i < camera_info.D.size(); ++i) d.at<double>(i) = camera_info.D[i];
  // The actual 3D => 2D projection
  cv::projectPoints(object_points, rvec, tvec, k, d, uvs);
}

Expected behavior The centers of the red circles align with the fiducial marker centers in all three images in the screenshot below.

Observed behavior The centers align in the image of the right camera, indicating that the projections of the made node (see the linked MRE code) match those of the marker detection library. This is good. However, the centers don't align in the images of the left and the rgb camera, indicating a severe calibration (interpretation) issue.

Screenshots image

Pipeline Graph Standard pipeline RGBStereo.

Attach system log

Versions: package version
ros-noetic-depthai 2.24.0-2focal.20240308.133037
ros-noetic-depthai-ros 2.9.0-1focal.20240402.150922
ros-noetic-depthai-bridge 2.9.0-1focal.20240308.134342
ros-noetic-depthai-ros-msgs 2.9.0-1focal.20240125.202559
ros-noetic-depthai-ros-driver 2.9.0-1focal.20240402.145653

Additional context Thanks for taking your time to look into this.

Shivam7Sharma commented 2 days ago

Did you use the Luxonis ROS package for calibrating? You can also get the calibration data from the depthai SDK using the calibrate.py file. It will save a file of calibration data if used correctly.

abaeyens-imod commented 1 day ago

Did you use the Luxonis ROS package for calibrating?

This is with the factory calibrations published on the *camera_info topics. I'll edit the initial post to clarify this.

Shivam7Sharma commented 1 day ago

I am not associated with Luxonis. I am just trying to help you. Are you using a FFC board? If you calibrate using the file I suggested, then that code will flash the device memory, I think, and the ROS messages will show the updated calibration data.