Revision history [back]

I solved the problem. The data available in the disparity image is not the RGB values. Those are encoded in TYPE_32FC1 so inorder to reconstruct the image we have to use some colormap done in stereo_view node in the image_view package. just subscribe the topic where the disparity image is available.

void imageCallback(const stereo_msgs::DisparityImageConstPtr& disp) {

Mat_ Vec3b disparity_color_;

float min_disparity = disp->min_disparity;

float max_disparity = disp->max_disparity;


float multiplier = 255.0f / (max_disparity - min_disparity);

assert(disp->image.encoding == sensor_msgs::image_encodings::TYPE_32FC1);

const cv::Mat_<float> dmat(disp->image.height, disp->image.width,

                           (float*)&disp->image.data[0], disp->image.step);

disparity_color_.create(disp->image.height, disp->image.width);


for (int row = 0; row < disparity_color_.rows; ++row) {

  const float* d = dmat[row];

  for (int col = 0; col < disparity_color_.cols; ++col) {

    int index = (d[col] - min_disparity) * multiplier + 0.5;

    index = std::min(255, std::max(0, index));

    // Fill as BGR

    disparity_color_(row, col)[2] = colormap[3*index + 0];

    disparity_color_(row, col)[1] = colormap[3*index + 1];

    disparity_color_(row, col)[0] = colormap[3*index + 2];
  }
}

imshow( "view", disparity_color_ ); }

I solved the problem. The data available in the disparity image is not the RGB values. Those are encoded in TYPE_32FC1 so inorder to reconstruct the image we have to use some colormap done in stereo_view node in the image_view package. just subscribe the topic where the disparity image is available.

void imageCallback(const stereo_msgs::DisparityImageConstPtr& disp)
{
 
disp) {
  Mat_ Vec3b disparity_color_;
 
disparity_color_;
  float min_disparity = disp->min_disparity;
 
disp->min_disparity;
  float max_disparity = disp->max_disparity;
  float multiplier = 255.0f / (max_disparity - min_disparity);
  assert(disp->image.encoding == sensor_msgs::image_encodings::TYPE_32FC1);
  const cv::Mat_<float> dmat(disp->image.height, disp->image.width,
                             (float*)&disp->image.data[0], disp->image.step);
  disparity_color_.create(disp->image.height, disp->image.width);

  for (int row = 0; row < disparity_color_.rows; ++row) {
    const float* d = dmat[row];
    for (int col = 0; col < disparity_color_.cols; ++col) {
      int index = (d[col] - min_disparity) * multiplier + 0.5;
      index = std::min(255, std::max(0, index));
      // Fill as BGR
      disparity_color_(row, col)[2] = colormap[3*index + 0];
      disparity_color_(row, col)[1] = colormap[3*index + 1];
      disparity_color_(row, col)[0] = colormap[3*index + 2];
   }
 }
 
 imshow( "view", disparity_color_ );
}
 }