DepthImageToLaserScan.cpp
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 #include <depthimage_to_laserscan/DepthImageToLaserScan.h>
00035 
00036 using namespace depthimage_to_laserscan;
00037   
00038 DepthImageToLaserScan::DepthImageToLaserScan(){
00039 }
00040 
00041 DepthImageToLaserScan::~DepthImageToLaserScan(){
00042 }
00043 
00044 double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{
00045   return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0));
00046 }
00047 
00048 double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const{
00049   double dot_product = ray1.x*ray2.x + ray1.y*ray2.y + ray1.z*ray2.z;
00050   double magnitude1 = magnitude_of_ray(ray1);
00051   double magnitude2 = magnitude_of_ray(ray2);;
00052   return acos(dot_product / (magnitude1 * magnitude2));
00053 }
00054 
00055 bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{  
00056   // Check for NaNs and Infs, a real number within our limits is more desirable than these.
00057   bool new_finite = std::isfinite(new_value);
00058   bool old_finite = std::isfinite(old_value);
00059   
00060   // Infs are preferable over NaNs (more information)
00061   if(!new_finite && !old_finite){ // Both are not NaN or Inf.
00062     if(!isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
00063       return true;
00064     }
00065     return false; // Do not replace old_value
00066   }
00067   
00068   // If not in range, don't bother
00069   bool range_check = range_min <= new_value && new_value <= range_max;
00070   if(!range_check){
00071     return false;
00072   }
00073   
00074   if(!old_finite){ // New value is in range and finite, use it.
00075     return true;
00076   }
00077   
00078   // Finally, if they are both numerical and new_value is closer than old_value, use new_value.
00079   bool shorter_check = new_value < old_value;
00080   return shorter_check;
00081 }
00082 
00083 sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
00084               const sensor_msgs::CameraInfoConstPtr& info_msg){
00085   // Set camera model
00086   cam_model_.fromCameraInfo(info_msg);
00087   
00088   // Calculate angle_min and angle_max by measuring angles between the left ray, right ray, and optical center ray
00089   cv::Point2d raw_pixel_left(0, cam_model_.cy());
00090   cv::Point2d rect_pixel_left = cam_model_.rectifyPoint(raw_pixel_left);
00091   cv::Point3d left_ray = cam_model_.projectPixelTo3dRay(rect_pixel_left);
00092   
00093   cv::Point2d raw_pixel_right(depth_msg->width-1, cam_model_.cy());
00094   cv::Point2d rect_pixel_right = cam_model_.rectifyPoint(raw_pixel_right);
00095   cv::Point3d right_ray = cam_model_.projectPixelTo3dRay(rect_pixel_right);
00096   
00097   cv::Point2d raw_pixel_center(cam_model_.cx(), cam_model_.cy());
00098   cv::Point2d rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center);
00099   cv::Point3d center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center);
00100   
00101   double angle_max = angle_between_rays(left_ray, center_ray);
00102   double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image
00103   
00104   // Fill in laserscan message
00105   sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
00106   scan_msg->header = depth_msg->header;
00107   if(output_frame_id_.length() > 0){
00108     scan_msg->header.frame_id = output_frame_id_;
00109   }
00110   scan_msg->angle_min = angle_min;
00111   scan_msg->angle_max = angle_max;
00112   scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / depth_msg->width;
00113   scan_msg->time_increment = 0.0;
00114   scan_msg->scan_time = scan_time_;
00115   scan_msg->range_min = range_min_;
00116   scan_msg->range_max = range_max_;
00117   
00118   // Check scan_height vs image_height
00119   if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){
00120     std::stringstream ss;
00121     ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height.";
00122     throw std::runtime_error(ss.str());
00123   }
00124 
00125   // Calculate and fill the ranges
00126   uint32_t ranges_size = depth_msg->width;
00127   scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());
00128   
00129   if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
00130   {
00131     convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
00132   }
00133   else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
00134   {
00135     convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
00136   }
00137   else
00138   {
00139     std::stringstream ss;
00140     ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
00141     throw std::runtime_error(ss.str());
00142   }
00143   
00144   return scan_msg;
00145 }
00146 
00147 void DepthImageToLaserScan::set_scan_time(const float scan_time){
00148   scan_time_ = scan_time;
00149 }
00150 
00151 void DepthImageToLaserScan::set_range_limits(const float range_min, const float range_max){
00152   range_min_ = range_min;
00153   range_max_ = range_max;
00154 }
00155 
00156 void DepthImageToLaserScan::set_scan_height(const int scan_height){
00157   scan_height_ = scan_height;
00158 }
00159 
00160 void DepthImageToLaserScan::set_output_frame(const std::string output_frame_id){
00161   output_frame_id_ = output_frame_id;
00162 }


depthimage_to_laserscan
Author(s): Chad Rockey
autogenerated on Tue Jan 7 2014 11:09:01