depth_conversions.h
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 #ifndef DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
00035 #define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
00036 
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/point_cloud2_iterator.h>
00039 #include <image_geometry/pinhole_camera_model.h>
00040 #include <depth_image_proc/depth_traits.h>
00041 
00042 #include <limits>
00043 
00044 namespace depth_image_proc {
00045 
00046 typedef sensor_msgs::PointCloud2 PointCloud;
00047 
00048 // Handles float or uint16 depths
00049 template<typename T>
00050 void convert(
00051     const sensor_msgs::ImageConstPtr& depth_msg,
00052     PointCloud::Ptr& cloud_msg,
00053     const image_geometry::PinholeCameraModel& model,
00054     double range_max = 0.0)
00055 {
00056   // Use correct principal point from calibration
00057   float center_x = model.cx();
00058   float center_y = model.cy();
00059 
00060   // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00061   double unit_scaling = DepthTraits<T>::toMeters( T(1) );
00062   float constant_x = unit_scaling / model.fx();
00063   float constant_y = unit_scaling / model.fy();
00064   float bad_point = std::numeric_limits<float>::quiet_NaN();
00065 
00066   sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
00067   sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
00068   sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
00069   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00070   int row_step = depth_msg->step / sizeof(T);
00071   for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
00072   {
00073     for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
00074     {
00075       T depth = depth_row[u];
00076 
00077       // Missing points denoted by NaNs
00078       if (!DepthTraits<T>::valid(depth))
00079       {
00080         if (range_max != 0.0)
00081         {
00082           depth = DepthTraits<T>::fromMeters(range_max);
00083         }
00084         else
00085         {
00086           *iter_x = *iter_y = *iter_z = bad_point;
00087           continue;
00088         }
00089       }
00090 
00091       // Fill in XYZ
00092       *iter_x = (u - center_x) * depth * constant_x;
00093       *iter_y = (v - center_y) * depth * constant_y;
00094       *iter_z = DepthTraits<T>::toMeters(depth);
00095     }
00096   }
00097 }
00098 
00099 } // namespace depth_image_proc
00100 
00101 #endif


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:42