DepthImageToLaserScan.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* 
00031  * Author: Chad Rockey
00032  */
00033 
00034 #ifndef DEPTH_IMAGE_TO_LASERSCAN
00035 #define DEPTH_IMAGE_TO_LASERSCAN
00036 
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/LaserScan.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <image_geometry/pinhole_camera_model.h>
00041 #include <depthimage_to_laserscan/depth_traits.h>
00042 #include <sstream>
00043 #include <limits.h>
00044 #include <math.h>
00045 
00046 namespace depthimage_to_laserscan
00047 { 
00048   class DepthImageToLaserScan
00049   {
00050   public:
00051     DepthImageToLaserScan();
00052     ~DepthImageToLaserScan();
00053 
00066     sensor_msgs::LaserScanPtr convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
00067                                            const sensor_msgs::CameraInfoConstPtr& info_msg);
00068     
00079     void set_scan_time(const float scan_time);
00080     
00091     void set_range_limits(const float range_min, const float range_max);
00092     
00103     void set_scan_height(const int scan_height);
00104     
00114     void set_output_frame(const std::string output_frame_id);
00115 
00116   private:
00126     double magnitude_of_ray(const cv::Point3d& ray) const;
00127 
00139     double angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const;
00140     
00154     bool use_point(const float new_value, const float old_value, const float range_min, const float range_max) const;
00155 
00169     template<typename T>
00170     void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, 
00171                  const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
00172       // Use correct principal point from calibration
00173       float center_x = cam_model.cx();
00174       float center_y = cam_model.cy();
00175 
00176       // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00177       double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
00178       float constant_x = unit_scaling / cam_model.fx();
00179       float constant_y = unit_scaling / cam_model.fy();
00180       
00181       const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00182       int row_step = depth_msg->step / sizeof(T);
00183 
00184       int offset = (int)(cam_model.cy()-scan_height/2);
00185       depth_row += offset*row_step; // Offset to center of image
00186 
00187       for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
00188                 for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row
00189                 {       
00190                   T depth = depth_row[u];
00191                   
00192                   double r = depth; // Assign to pass through NaNs and Infs
00193                   double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
00194                   int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
00195                   
00196                   if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
00197                     // Calculate in XYZ
00198                     double x = (u - center_x) * depth * constant_x;
00199                     double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
00200                     
00201                     // Calculate actual distance
00202                     r = sqrt(pow(x, 2.0) + pow(z, 2.0));
00203                   }
00204           
00205           // Determine if this point should be used.
00206           if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
00207             scan_msg->ranges[index] = r;
00208           }
00209         }
00210       }
00211     }
00212     
00213     image_geometry::PinholeCameraModel cam_model_; 
00214     
00215     float scan_time_; 
00216     float range_min_; 
00217     float range_max_; 
00218     int scan_height_; 
00219     std::string output_frame_id_; 
00220   };
00221   
00222   
00223 }; // depthimage_to_laserscan
00224 
00225 #endif


depthimage_to_laserscan
Author(s): Chad Rockey
autogenerated on Thu Aug 27 2015 12:57:01