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 y = (v - center_y) * depth * constant_y;
00200             double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
00201             
00202             // Calculate actual distance
00203             r = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0));
00204           }
00205           
00206           // Determine if this point should be used.
00207           if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
00208             scan_msg->ranges[index] = r;
00209           }
00210         }
00211       }
00212     }
00213     
00214     image_geometry::PinholeCameraModel cam_model_; 
00215     
00216     float scan_time_; 
00217     float range_min_; 
00218     float range_max_; 
00219     int scan_height_; 
00220     std::string output_frame_id_; 
00221   };
00222   
00223   
00224 }; // depthimage_to_laserscan
00225 
00226 #endif


depthimage_to_laserscan
Author(s): Chad Rockey
autogenerated on Sun Oct 5 2014 23:25:32