laser_cb_detector.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008-2012, 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 
00036 
00037 #include <laser_cb_detector/laser_cb_detector.h>
00038 #include <ros/console.h>
00039 #include <cv_bridge/cv_bridge.h>
00040 //#include <highgui.h>
00041 
00042 using namespace std;
00043 using namespace laser_cb_detector;
00044 
00045 LaserCbDetector::LaserCbDetector() : configured_(false) {}
00046 
00047 bool LaserCbDetector::configure(const ConfigGoal& config)
00048 {
00049   config_ = config;
00050   image_cb_detector::ConfigGoal image_cfg;
00051   // TODO: setup message
00052 
00053   image_cfg.num_x = config.num_x;
00054   image_cfg.num_y = config.num_y;
00055   image_cfg.spacing_x = config.spacing_x;
00056   image_cfg.spacing_y = config.spacing_y;
00057 
00058   image_cfg.width_scaling = config.width_scaling;
00059   image_cfg.height_scaling = config.height_scaling;
00060 
00061   image_cfg.subpixel_window = config.subpixel_window;
00062   image_cfg.subpixel_zero_zone = config.subpixel_zero_zone;
00063 
00064   detector_.configure(image_cfg);
00065   return true;
00066 }
00067 
00068 bool LaserCbDetector::detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
00069                              calibration_msgs::CalibrationPattern& result)
00070 {
00071   // ***** Convert the snapshot into an image, based on intensity window in config *****
00072   if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
00073     return false;
00074   cv::Mat image = bridge_.toCvMat();
00075 
00076   if (config_.flip_horizontal)
00077   {
00078     ROS_DEBUG("Flipping image");
00079     cv::flip(image, image, 1);
00080   }
00081   else
00082     ROS_DEBUG("Not flipping image");
00083 
00084   cv_bridge::CvImage cv_image(snapshot.header, "mono8", image);
00085   sensor_msgs::ImagePtr ros_image = cv_image.toImageMsg();
00086   if(detector_.detect(ros_image, result)){
00087     if (config_.flip_horizontal){
00088       for(int i=0; i < result.image_points.size(); i++)
00089         result.image_points[i].x = image.cols - result.image_points[i].x - 1;
00090     }
00091     return true;
00092   }else
00093     return false;
00094 }
00095 
00096 bool LaserCbDetector::getImage(const calibration_msgs::DenseLaserSnapshot& snapshot, sensor_msgs::Image& ros_image)
00097 {
00098   if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
00099   {
00100     ROS_ERROR("Error building cv::Mat from DenseLaserSnapshot's intensity data");
00101     return false;
00102   }
00103   cv::Mat image = bridge_.toCvMat();
00104 
00105   cv_bridge::CvImage(snapshot.header, "mono8", image).toImageMsg(ros_image);
00106 
00107   return true;
00108 }
00109 


laser_cb_detector
Author(s): Vijay Pradeep
autogenerated on Wed Aug 26 2015 10:56:07