joint_image_interpolator.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 <opencv2/imgproc/imgproc.hpp>
00038 
00039 #include <laser_joint_processor/joint_image_interpolator.h>
00040 #include <ros/console.h>
00041 
00042 using namespace laser_joint_processor;
00043 using namespace std;
00044 
00045 bool JointImageInterpolator::interp(const std::vector <geometry_msgs::Point>& points,
00046                                     cv::Mat_<cv::Vec2f> image, std::vector<float>& positions, std::vector<float>& velocities)
00047 {
00048   const unsigned int N = points.size();
00049 
00050   // Allocate Maps
00051   cv::Mat_<float> map_x_mat(N, 1);
00052   cv::Mat_<float> map_y_mat(N, 1);
00053 
00054   // Set up maps
00055   for (unsigned int i=0; i<N; i++)
00056   {
00057     map_x_mat(i) = points[i].x;
00058     map_y_mat(i) = points[i].y;
00059   }
00060 
00061   // Allocate Destination Image
00062   cv::Mat_<cv::Vec2f> dest_mat(N, 1);
00063 
00064   // Perform the OpenCV interpolation
00065   cv::remap(image, dest_mat, map_x_mat, map_y_mat, cv::INTER_LINEAR, cv::BORDER_WRAP);
00066 
00067   // Copy results into output vectors
00068   positions.resize(N);
00069   velocities.resize(N);
00070   for (unsigned int i=0; i<N; i++)
00071   {
00072     positions[i]  = dest_mat(i)[0];
00073     velocities[i] = dest_mat(i)[1];
00074   }
00075 
00076   return true;
00077 }
00078 
00079 
00080 bool laser_joint_processor::interpSnapshot(const std::vector <geometry_msgs::Point>& points,
00081                                            const calibration_msgs::DenseLaserSnapshot& snapshot,
00082                                            std::vector<float>& angles,
00083                                            std::vector<float>& ranges)
00084 {
00085   const unsigned int N = points.size();
00086 
00087   // Check to make sure points are in range
00088   for (unsigned int i=0; i<N; i++)
00089   {
00090     if (points[i].x < 0  ||
00091         points[i].x > snapshot.readings_per_scan-1 ||
00092         points[i].y < 0 ||
00093         points[i].y > snapshot.num_scans-1 )
00094     {
00095       return false;
00096     }
00097   }
00098 
00099   // Set up input image
00100   cv::Mat_<float> range_image(snapshot.num_scans, snapshot.readings_per_scan, const_cast<float*>(&snapshot.ranges[0]));
00101 
00102   // Allocate Maps
00103   cv::Mat_<float> map_x_mat(N, 1);
00104   cv::Mat_<float> map_y_mat(N, 1);
00105 
00106   // Set up maps
00107   for (unsigned int i=0; i<N; i++)
00108   {
00109     map_x_mat(i) = points[i].x;
00110     map_y_mat(i) = points[i].y;
00111   } 
00112 
00113   // Allocate Destination Image
00114   ranges.resize(N);
00115   cv::Mat_<float> ranges_mat(N, 1, &ranges[0]);
00116 
00117   // Perform the OpenCV interpolation
00118   cv::remap(range_image, ranges_mat, map_x_mat, map_y_mat, cv::INTER_LINEAR, cv::BORDER_WRAP);
00119 
00120   // Do angle interp manually
00121   angles.resize(N);
00122   for (unsigned int i=0; i<N; i++)
00123     angles[i] = snapshot.angle_min +  points[i].x*snapshot.angle_increment;
00124 
00125   return true;
00126 }
00127 
00128 
00129 
00130 
00131 


laser_joint_processor
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:32