How to rectify downsampled stereo images using binning?

edit

Hi there!

This is my first post! (So far I found all the answers I needed, which is awesome!)

I am writing my own node for rectifying stereo images. The reason I do this is to be able to rectify/undistort only every n-th frame and also to downsample the images right at the beginning, both to reduce computational cost.

So far, I manage to rectify images at the full resolution and then downsample them. I suspect that first downsampling and then rectifying them would be faster, which I was trying to get to work. Looking through the code of PinholeCameraModel ros.org/doc/api/image_geometry/html/c++/pinhole__camera__model_8cpp_source.html, it seemed to me that when I would set the binning_x and binning_y fields in the camera_info to let's say 2, the PinholeCameraModel would compute the rectification map for downsampled images. Unfortunately, this gave weirdly warped images. What am I missing? What else needs to be set?

Thanks in advance!

Here is my callback function:

void process(const sensor_msgs::ImageConstPtr& l_image_msg, const sensor_msgs::ImageConstPtr& r_image_msg, const sensor_msgs::CameraInfoConstPtr& l_info_msg, const sensor_msgs::CameraInfoConstPtr& r_info_msg){
if (count_ == 0) // The count_ stuff is there not to process every frame but to skip some in between
{
  // Prepare images
  int32_t l_step, r_step;
  cv_bridge::CvImageConstPtr l_cv_ptr, r_cv_ptr;
  l_cv_ptr = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8);
  l_step = l_cv_ptr->image.step[0];
  r_cv_ptr = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8);
  r_step = r_cv_ptr->image.step[0];

  ROS_ASSERT(l_step == r_step);
  ROS_ASSERT(l_image_msg->width == r_image_msg->width);
  ROS_ASSERT(l_image_msg->height == r_image_msg->height);

  cv_bridge::CvImagePtr l_image_rect(new cv_bridge::CvImage(l_cv_ptr->header, l_cv_ptr->encoding));
  cv_bridge::CvImagePtr r_image_rect(new cv_bridge::CvImage(r_cv_ptr->header, r_cv_ptr->encoding));

  // Downsample if desired
  if (config_.downsample)
  {
    // Copy cam info to be able to modify
    sensor_msgs::CameraInfo l_info_msg_new(*l_info_msg);
    sensor_msgs::CameraInfo r_info_msg_new(*r_info_msg);

    // Modify cam info
    l_info_msg_new.binning_x = 2;
    l_info_msg_new.binning_y = 2;
    r_info_msg_new.binning_x = 2;
    r_info_msg_new.binning_y = 2;

    // Update camera models
    l_model_.fromCameraInfo(l_info_msg_new);
    r_model_.fromCameraInfo(r_info_msg_new);

    // Downsample
    cv_bridge::CvImagePtr l_image_ds(new cv_bridge::CvImage(l_cv_ptr->header, l_cv_ptr->encoding));
    cv_bridge::CvImagePtr r_image_ds(new cv_bridge::CvImage(r_cv_ptr->header, r_cv_ptr->encoding));
    cv::resize(l_cv_ptr->image, l_image_ds->image, cv::Size(0, 0), 0.5, 0.5, CV_INTER_LINEAR);
    cv::resize(r_cv_ptr->image, r_image_ds->image, cv::Size(0, 0), 0.5, 0.5, CV_INTER_LINEAR);

    // Rectify
    l_model_.rectifyImage(l_image_ds->image, l_image_rect->image, CV_INTER_LINEAR); 
    r_model_.rectifyImage(r_image_ds->image, r_image_rect->image, CV_INTER_LINEAR); 
  }
  else
  {
    // Update camera models
    l_model_.fromCameraInfo(l_info_msg);
    r_model_.fromCameraInfo(r_info_msg);

    // Rectify
    l_model_.rectifyImage(l_cv_ptr->image, l_image_rect->image, CV_INTER_LINEAR); 
    r_model_.rectifyImage(r_cv_ptr->image, r_image_rect->image, CV_INTER_LINEAR); 
  }

  // Publish
  left_pub_->publish(l_image_rect->toImageMsg());
  right_pub_->publish(r_image_rect->toImageMsg());

  ++count_;
}
else if (count_ >= config_.frames_to_skip) // >= because config_.frames_to_skip can change
  count_ = 0;
else
  ++count_;}

Best Tim