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   block_matcher_(left_rect, right_rect, disparity16_);
00093 
00094   // Fill in DisparityImage image data, converting to 32-bit float
00095   sensor_msgs::Image& dimage = disparity.image;
00096   dimage.height = disparity16_.rows;
00097   dimage.width = disparity16_.cols;
00098   dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00099   dimage.step = dimage.width * sizeof(float);
00100   dimage.data.resize(dimage.step * dimage.height);
00101   cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00102   // We convert from fixed-point to float disparity and also adjust for any x-offset between
00103   // the principal points: d = d_fp*inv_dpp - (cx_l - cx_r)
00104   disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx()));
00105   ROS_ASSERT(dmat.data == &dimage.data[0]);
00107 
00108   // Stereo parameters
00109   disparity.f = model.right().fx();
00110   disparity.T = model.baseline();
00111 
00113 
00114   // Disparity search range
00115   disparity.min_disparity = getMinDisparity();
00116   disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1;
00117   disparity.delta_d = inv_dpp;
00118 }
00119 
00120 inline bool isValidPoint(const cv::Vec3f& pt)
00121 {
00122   // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
00123   // and zero disparities (point mapped to infinity).
00124   return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
00125 }
00126 
00127 void StereoProcessor::processPoints(const stereo_msgs::DisparityImage& disparity,
00128                                     const cv::Mat& color, const std::string& encoding,
00129                                     const image_geometry::StereoCameraModel& model,
00130                                     sensor_msgs::PointCloud& points) const
00131 {
00132   // Calculate dense point cloud
00133   const sensor_msgs::Image& dimage = disparity.image;
00134   const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00135   model.projectDisparityImageTo3d(dmat, dense_points_, true);
00136 
00137   // Fill in sparse point cloud message
00138   points.points.resize(0);
00139   points.channels.resize(3);
00140   points.channels[0].name = "rgb";
00141   points.channels[0].values.resize(0);
00142   points.channels[1].name = "u";
00143   points.channels[1].values.resize(0);
00144   points.channels[2].name = "v";
00145   points.channels[2].values.resize(0);
00146   
00147   for (int32_t u = 0; u < dense_points_.rows; ++u) {
00148     for (int32_t v = 0; v < dense_points_.cols; ++v) {
00149       if (isValidPoint(dense_points_(u,v))) {
00150         // x,y,z
00151         geometry_msgs::Point32 pt;
00152         pt.x = dense_points_(u,v)[0];
00153         pt.y = dense_points_(u,v)[1];
00154         pt.z = dense_points_(u,v)[2];
00155         points.points.push_back(pt);
00156         // u,v
00157         points.channels[1].values.push_back(u);
00158         points.channels[2].values.push_back(v);
00159       }
00160     }
00161   }
00162 
00163   // Fill in color
00164   namespace enc = sensor_msgs::image_encodings;
00165   points.channels[0].values.reserve(points.points.size());
00166   if (encoding == enc::MONO8) {
00167     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00168       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00169         if (isValidPoint(dense_points_(u,v))) {
00170           uint8_t g = color.at<uint8_t>(u,v);
00171           int32_t rgb = (g << 16) | (g << 8) | g;
00172           points.channels[0].values.push_back(*(float*)(&rgb));
00173         }
00174       }
00175     }
00176   }
00177   else if (encoding == enc::RGB8) {
00178     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00179       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00180         if (isValidPoint(dense_points_(u,v))) {
00181           const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
00182           int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00183           points.channels[0].values.push_back(*(float*)(&rgb_packed));
00184         }
00185       }
00186     }
00187   }
00188   else if (encoding == enc::BGR8) {
00189     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00190       for (int32_t v = 0; v < dense_points_.cols; ++v) {
00191         if (isValidPoint(dense_points_(u,v))) {
00192           const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
00193           int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00194           points.channels[0].values.push_back(*(float*)(&rgb_packed));
00195         }
00196       }
00197     }
00198   }
00199   else {
00200     ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
00201   }
00202 }
00203 
00204 void StereoProcessor::processPoints2(const stereo_msgs::DisparityImage& disparity,
00205                                      const cv::Mat& color, const std::string& encoding,
00206                                      const image_geometry::StereoCameraModel& model,
00207                                      sensor_msgs::PointCloud2& points) const
00208 {
00209   // Calculate dense point cloud
00210   const sensor_msgs::Image& dimage = disparity.image;
00211   const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00212   model.projectDisparityImageTo3d(dmat, dense_points_, true);
00213 
00214   // Fill in sparse point cloud message
00215   points.height = dense_points_.rows;
00216   points.width  = dense_points_.cols;
00217   points.fields.resize (4);
00218   points.fields[0].name = "x";
00219   points.fields[0].offset = 0;
00220   points.fields[0].count = 1;
00221   points.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00222   points.fields[1].name = "y";
00223   points.fields[1].offset = 4;
00224   points.fields[1].count = 1;
00225   points.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00226   points.fields[2].name = "z";
00227   points.fields[2].offset = 8;
00228   points.fields[2].count = 1;
00229   points.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00230   points.fields[3].name = "rgb";
00231   points.fields[3].offset = 12;
00232   points.fields[3].count = 1;
00233   points.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00234   //points.is_bigendian = false; ???
00235   points.point_step = 16;
00236   points.row_step = points.point_step * points.width;
00237   points.data.resize (points.row_step * points.height);
00238   points.is_dense = false; // there may be invalid points
00239  
00240   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00241   int i = 0;
00242   for (int32_t u = 0; u < dense_points_.rows; ++u) {
00243     for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00244       if (isValidPoint(dense_points_(u,v))) {
00245         // x,y,z,rgba
00246         memcpy (&points.data[i * points.point_step + 0], &dense_points_(u,v)[0], sizeof (float));
00247         memcpy (&points.data[i * points.point_step + 4], &dense_points_(u,v)[1], sizeof (float));
00248         memcpy (&points.data[i * points.point_step + 8], &dense_points_(u,v)[2], sizeof (float));
00249       }
00250       else {
00251         memcpy (&points.data[i * points.point_step + 0], &bad_point, sizeof (float));
00252         memcpy (&points.data[i * points.point_step + 4], &bad_point, sizeof (float));
00253         memcpy (&points.data[i * points.point_step + 8], &bad_point, sizeof (float));
00254       }
00255     }
00256   }
00257 
00258   // Fill in color
00259   namespace enc = sensor_msgs::image_encodings;
00260   i = 0;
00261   if (encoding == enc::MONO8) {
00262     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00263       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00264         if (isValidPoint(dense_points_(u,v))) {
00265           uint8_t g = color.at<uint8_t>(u,v);
00266           int32_t rgb = (g << 16) | (g << 8) | g;
00267           memcpy (&points.data[i * points.point_step + 12], &rgb, sizeof (int32_t));
00268         }
00269         else {
00270           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00271         }
00272       }
00273     }
00274   }
00275   else if (encoding == enc::RGB8) {
00276     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00277       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00278         if (isValidPoint(dense_points_(u,v))) {
00279           const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
00280           int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00281           memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
00282         }
00283         else {
00284           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00285         }
00286       }
00287     }
00288   }
00289   else if (encoding == enc::BGR8) {
00290     for (int32_t u = 0; u < dense_points_.rows; ++u) {
00291       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
00292         if (isValidPoint(dense_points_(u,v))) {
00293           const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
00294           int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00295           memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
00296         }
00297         else {
00298           memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
00299         }
00300       }
00301     }
00302   }
00303   else {
00304     ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
00305   }
00306 }
00307 
00308 } //namespace stereo_image_proc


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Aug 26 2015 11:57:43