orientation_generic.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage
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 
00035 #include <ias_descriptors_3d/generic/orientation_generic.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 OrientationGeneric::OrientationGeneric()
00043 {
00044   result_size_ = 1;
00045   result_size_defined_ = true;
00046 }
00047 
00048 OrientationGeneric::~OrientationGeneric()
00049 {
00050 }
00051 
00052 // --------------------------------------------------------------
00053 /* See function definition */
00054 // --------------------------------------------------------------
00055 void OrientationGeneric::doComputation(const sensor_msgs::PointCloud& data,
00056                                        cloud_kdtree::KdTree& data_kdtree,
00057                                        const std::vector<const geometry_msgs::Point32*>& interest_pts,
00058                                        std::vector<std::vector<float> >& results)
00059 {
00060   // ----------------------------------------
00061   // Compute orientation feature for each interest point
00062   int nbr_interest_pts = interest_pts.size();
00063 #pragma omp parallel for
00064   for (int i = 0 ; i < nbr_interest_pts ; i++)
00065   {
00066     computeOrientation(i, results[static_cast<size_t> (i)]);
00067   }
00068 }
00069 
00070 // --------------------------------------------------------------
00071 /* See function definition */
00072 // --------------------------------------------------------------
00073 void OrientationGeneric::doComputation(const sensor_msgs::PointCloud& data,
00074                                        cloud_kdtree::KdTree& data_kdtree,
00075                                        const std::vector<const std::vector<int>*>& interest_region_indices,
00076                                        std::vector<std::vector<float> >& results)
00077 {
00078   // ----------------------------------------
00079   // Compute orientation feature for each interest region
00080   int nbr_interest_regions = interest_region_indices.size();
00081 #pragma omp parallel for
00082   for (int i = 0 ; i < nbr_interest_regions ; i++)
00083   {
00084     computeOrientation(i, results[static_cast<size_t> (i)]);
00085   }
00086 }
00087 
00088 // --------------------------------------------------------------
00089 /* See function definition.
00090  * Invariant: interest_sample_idx is within bounds */
00091 // --------------------------------------------------------------
00092 inline void OrientationGeneric::computeOrientation(const unsigned int interest_sample_idx,
00093                                                    std::vector<float>& result) const
00094 {
00095   // Retrieve local direction for current interest point/region
00096   const Eigen3::Vector3d* curr_local_direction = (*local_directions_)[interest_sample_idx];
00097 
00098   // NULL indicates could not extract local direction
00099   if (curr_local_direction != NULL)
00100   {
00101     // Invariant: local and reference directions are both unit length
00102     float cos_theta = curr_local_direction->dot(reference_direction_);
00103     if (cos_theta < 0.0)
00104     {
00105       cos_theta = curr_local_direction->dot(reference_direction_flipped_);
00106     }
00107 
00108     result.resize(result_size_);
00109     result[0] = cos_theta;
00110   }
00111 }
00112 


ias_descriptors_3d
Author(s): Daniel Munoz/ dmunoz@willowgarage.com, Dejan Pangercic (patched version)
autogenerated on Mon Oct 6 2014 08:48:26