laser_interval_calc.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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_interval_calc.h>
00038 #include <ros/console.h>
00039 #include <vector>
00040 #include <algorithm>
00041 
00042 using namespace laser_cb_detector;
00043 using namespace std;
00044 
00045 bool LaserIntervalCalc::computeInterval(const calibration_msgs::DenseLaserSnapshot& snapshot,
00046                                         const calibration_msgs::CalibrationPattern& features,
00047                                         calibration_msgs::Interval& result)
00048 {
00049   const unsigned int N = features.image_points.size();
00050 
00051   vector<ros::Time> min_times, max_times;
00052   min_times.resize(N);
00053   max_times.resize(N);
00054 
00055   if (N == 0)
00056   {
00057     result.start = snapshot.header.stamp;
00058     result.end   = snapshot.header.stamp;
00059     return true;
00060   }
00061 
00062   // Find the min and max time bounds for each image point. Store this in vectors min_times and max_times
00063   for (unsigned int i=0; i<N; i++)
00064   {
00065     // Figure out which scan happened first
00066     int x_rounded = (int) features.image_points[i].x;
00067     int y_rounded = (int) features.image_points[i].y;
00068 
00069     // Don't need an x axis range check, since we don't dereference anything based on this index
00070 
00071     // Error checking on Y axis
00072     if (features.image_points[i].y <= 0.0 || y_rounded >= (int) snapshot.num_scans-1)
00073     {
00074       ROS_ERROR("Image point #%u (%.2f, %.2f) is outside of Y range (0.00, %u)", i,
00075                 features.image_points[i].x, features.image_points[i].y, snapshot.num_scans-1);
00076       return false;
00077     }
00078 
00079     ros::Time min_scan_start = min( snapshot.scan_start[y_rounded],
00080                                     snapshot.scan_start[y_rounded+1] );
00081     ros::Time max_scan_start = max( snapshot.scan_start[y_rounded],
00082                                     snapshot.scan_start[y_rounded+1] );
00083 
00084     min_times[i] = min_scan_start + ros::Duration(snapshot.time_increment * x_rounded);
00085     max_times[i] = max_scan_start + ros::Duration(snapshot.time_increment * (x_rounded+1));
00086   }
00087 
00088   // Compute the min and max times over both vectors
00089   ros::Time min_time = min_times[0];
00090   ros::Time max_time = max_times[0];
00091 
00092   for (unsigned int i=0; i<N; i++)
00093   {
00094     min_time = min (min_time, min_times[i]);
00095     max_time = max (max_time, max_times[i]);
00096   }
00097 
00098   result.start = min_time;
00099   result.end   = max_time;
00100 
00101   return true;
00102 }


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