processor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <ros/assert.h>
00035 #include "stereo_image_proc/processor.h"
00036 #include <sensor_msgs/image_encodings.h>
00037 #include <cmath>
00038 #include <limits>
00039 
00040 namespace stereo_image_proc {
00041 
00042 bool StereoProcessor::process(const sensor_msgs::ImageConstPtr& left_raw,
00043                               const sensor_msgs::ImageConstPtr& right_raw,
00044                               const image_geometry::StereoCameraModel& model,
00045                               StereoImageSet& output, int flags) const
00046 {
00047   // Do monocular processing on left and right images
00048   int left_flags = flags & LEFT_ALL;
00049   int right_flags = flags & RIGHT_ALL;
00050   if (flags & STEREO_ALL) {
00051     // Need the rectified images for stereo processing
00052     left_flags |= LEFT_RECT;
00053     right_flags |= RIGHT_RECT;
00054   }
00055   if (flags & (POINT_CLOUD | POINT_CLOUD2)) {
00056     flags |= DISPARITY;
00057     // Need the color channels for the point cloud
00058     left_flags |= LEFT_RECT_COLOR;
00059   }
00060   if (!mono_processor_.process(left_raw, model.left(), output.left, left_flags))
00061     return false;
00062   if (!mono_processor_.process(right_raw, model.right(), output.right, right_flags >> 4))
00063     return false;
00064 
00065   // Do block matching to produce the disparity image
00066   if (flags & DISPARITY) {
00067     processDisparity(output.left.rect, output.right.rect, model, output.disparity);
00068   }
00069 
00070   // Project disparity image to 3d point cloud
00071   if (flags & POINT_CLOUD) {
00072     processPoints(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points);
00073   }
00074 
00075   // Project disparity image to 3d point cloud
00076   if (flags & POINT_CLOUD2) {
00077     processPoints2(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points2);
00078   }
00079 
00080   return true;
00081 }
00082 
00083 void StereoProcessor::processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
00084                                        const image_geometry::StereoCameraModel& model,
00085                                        stereo_msgs::DisparityImage& disparity) const
00086 {
00087   // Fixed-point disparity is 16 times the true value: d = d_fp / 16.0 = x_l - x_r.
00088   static const int DPP = 16; // disparities per pixel
00089   static const double inv_dpp = 1.0 / DPP;
00090 
00091   // Block matcher produces 16-bit signed (fixed point) disparity image
00092   if (current_stereo_algorithm_ == BM)
00093 #if CV_MAJOR_VERSION == 3
00094     block_matcher_->compute(left_rect, right_rect, disparity16_);
00095   else
00096     sg_block_matcher_->compute(left_rect, right_rect, disparity16_);
00097 #else
00098     block_matcher_(left_rect, right_rect, disparity16_);
00099   else
00100     sg_block_matcher_(left_rect, right_rect, disparity16_);
00101 #endif
00102 
00103   // Fill in DisparityImage image data, converting to 32-bit float
00104   sensor_msgs::Image& dimage = disparity.image;
00105   dimage.height = disparity16_.rows;
00106   dimage.width = disparity16_.cols;
00107   dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00108   dimage.step = dimage.width * sizeof(float);
00109   dimage.data.resize(dimage.step * dimage.height);
00110   cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00111   // We convert from fixed-point to float disparity and also adjust for any x-offset between
00112   // the principal points: d = d_fp*inv_dpp - (cx_l - cx_r)
00113   disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx()));
00114   ROS_ASSERT(dmat.data == &dimage.data[0]);
00116 
00117   // Stereo parameters
00118   disparity.f = model.right().fx();
00119   disparity.T = model.baseline();
00120 
00122 
00123   // Disparity search range
00124   disparity.min_disparity = getMinDisparity();
00125   disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1;
00126   disparity.delta_d = inv_dpp;
00127 }
00128 
00129 inline bool isValidPoint(const cv::Vec3f& pt)
00130 {
00131   // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
00132   // and zero disparities (point mapped to infinity).
00133   return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
00134 }
00135 
00136 void StereoProcessor::processPoints(const stereo_msgs::DisparityImage& disparity,
00137                                     const cv::Mat& color, const std::string& encoding,
00138                                     const image_geometry::StereoCameraModel& model,
00139                                     sensor_msgs::PointCloud& points) const
00140 {
00141   // Calculate dense point cloud
00142   const sensor_msgs::Image& dimage = disparity.image;
00143   const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00144   model.projectDisparityImageTo3d(dmat, dense_points_, true);
00145 
00146   // Fill in sparse point cloud message
00147   points.points.resize(0);
00148   points.channels.resize(3);
00149   points.channels[0].name = "rgb";
00150   points.channels[0].values.resize(0);
00151   points.channels[1].name = "u";
00152   points.channels[1].values.resize(0);
00153   points.channels[2].name = "v";
00154   points.channels[2].values.resize(0);
00155   
00156   for (int32_t u = 0; u < dense_points_.rows; ++u) {
00157     for (int32_t v = 0; v < dense_points_.cols; ++v) {
00158       if (isValidPoint(dense_points_(u,v))) {
00159         // x,y,z
00160         geometry_msgs::Point32 pt;
00161         pt.x = dense_points_(u,v)[0];
00162         pt.y = dense_points_(u,v)[1];
00163         pt.z = dense_points_(u,v)[2];
00164         points.points.push_back(pt);
00165         // u,v
00166         points.channels[1].values.push_back(u);
00167         points.channels[2].values.push_back(v);
00168       }
00169     }
00170   }
00171 
00172   // Fill in color
00173   namespace enc = sensor_msgs::image_encodings;
00174   points.channels[0].values.reserve(points.points.size());
00175   if (encoding == enc::MONO8) {
00176     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00177       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00178         if (isValidPoint(dense_points_(u,v))) {
00179           uint8_t g = color.at<uint8_t>(u,v);
00180           int32_t rgb = (g << 16) | (g << 8) | g;
00181           points.channels[0].values.push_back(*(float*)(&rgb));
00182         }
00183       }
00184     }
00185   }
00186   else if (encoding == enc::RGB8) {
00187     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00188       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00189         if (isValidPoint(dense_points_(u,v))) {
00190           const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
00191           int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00192           points.channels[0].values.push_back(*(float*)(&rgb_packed));
00193         }
00194       }
00195     }
00196   }
00197   else if (encoding == enc::BGR8) {
00198     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00199       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00200         if (isValidPoint(dense_points_(u,v))) {
00201           const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
00202           int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00203           points.channels[0].values.push_back(*(float*)(&rgb_packed));
00204         }
00205       }
00206     }
00207   }
00208   else {
00209     ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
00210   }
00211 }
00212 
00213 void StereoProcessor::processPoints2(const stereo_msgs::DisparityImage& disparity,
00214                                      const cv::Mat& color, const std::string& encoding,
00215                                      const image_geometry::StereoCameraModel& model,
00216                                      sensor_msgs::PointCloud2& points) const
00217 {
00218   // Calculate dense point cloud
00219   const sensor_msgs::Image& dimage = disparity.image;
00220   const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00221   model.projectDisparityImageTo3d(dmat, dense_points_, true);
00222 
00223   // Fill in sparse point cloud message
00224   points.height = dense_points_.rows;
00225   points.width  = dense_points_.cols;
00226   points.fields.resize (4);
00227   points.fields[0].name = "x";
00228   points.fields[0].offset = 0;
00229   points.fields[0].count = 1;
00230   points.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00231   points.fields[1].name = "y";
00232   points.fields[1].offset = 4;
00233   points.fields[1].count = 1;
00234   points.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00235   points.fields[2].name = "z";
00236   points.fields[2].offset = 8;
00237   points.fields[2].count = 1;
00238   points.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00239   points.fields[3].name = "rgb";
00240   points.fields[3].offset = 12;
00241   points.fields[3].count = 1;
00242   points.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00243   //points.is_bigendian = false; ???
00244   points.point_step = 16;
00245   points.row_step = points.point_step * points.width;
00246   points.data.resize (points.row_step * points.height);
00247   points.is_dense = false; // there may be invalid points
00248  
00249   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00250   int i = 0;
00251   for (int32_t u = 0; u < dense_points_.rows; ++u) {
00252     for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00253       if (isValidPoint(dense_points_(u,v))) {
00254         // x,y,z,rgba
00255         memcpy (&points.data[i * points.point_step + 0], &dense_points_(u,v)[0], sizeof (float));
00256         memcpy (&points.data[i * points.point_step + 4], &dense_points_(u,v)[1], sizeof (float));
00257         memcpy (&points.data[i * points.point_step + 8], &dense_points_(u,v)[2], sizeof (float));
00258       }
00259       else {
00260         memcpy (&points.data[i * points.point_step + 0], &bad_point, sizeof (float));
00261         memcpy (&points.data[i * points.point_step + 4], &bad_point, sizeof (float));
00262         memcpy (&points.data[i * points.point_step + 8], &bad_point, sizeof (float));
00263       }
00264     }
00265   }
00266 
00267   // Fill in color
00268   namespace enc = sensor_msgs::image_encodings;
00269   i = 0;
00270   if (encoding == enc::MONO8) {
00271     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00272       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00273         if (isValidPoint(dense_points_(u,v))) {
00274           uint8_t g = color.at<uint8_t>(u,v);
00275           int32_t rgb = (g << 16) | (g << 8) | g;
00276           memcpy (&points.data[i * points.point_step + 12], &rgb, sizeof (int32_t));
00277         }
00278         else {
00279           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00280         }
00281       }
00282     }
00283   }
00284   else if (encoding == enc::RGB8) {
00285     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00286       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00287         if (isValidPoint(dense_points_(u,v))) {
00288           const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
00289           int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00290           memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
00291         }
00292         else {
00293           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00294         }
00295       }
00296     }
00297   }
00298   else if (encoding == enc::BGR8) {
00299     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00300       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00301         if (isValidPoint(dense_points_(u,v))) {
00302           const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
00303           int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00304           memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
00305         }
00306         else {
00307           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00308         }
00309       }
00310     }
00311   }
00312   else {
00313     ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
00314   }
00315 }
00316 
00317 } //namespace stereo_image_proc


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed May 1 2019 02:51:55