Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
00057 bool new_finite = std::isfinite(new_value);
00058 bool old_finite = std::isfinite(old_value);
00059
00060
00061 if(!new_finite && !old_finite){
00062 if(!isnan(new_value)){
00063 return true;
00064 }
00065 return false;
00066 }
00067
00068
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){
00075 return true;
00076 }
00077
00078
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
00086 cam_model_.fromCameraInfo(info_msg);
00087
00088
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);
00103
00104
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
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
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 }