bounding_box_spectral.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/bounding_box_spectral.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 BoundingBoxSpectral::BoundingBoxSpectral(double bbox_radius, SpectralAnalysis& spectral_information)
00043 {
00044   result_size_ = 12;
00045   result_size_defined_ = true;
00046 
00047   neighborhood_radius_ = bbox_radius;
00048   neighborhood_radius_defined_ = true;
00049 
00050   eig_vecs_min_ = NULL;
00051   eig_vecs_mid_ = NULL;
00052   eig_vecs_max_ = NULL;
00053   spectral_information_ = &spectral_information;
00054 }
00055 
00056 // --------------------------------------------------------------
00057 /* See function definition */
00058 // --------------------------------------------------------------
00059 void BoundingBoxSpectral::clearShared()
00060 {
00061   spectral_information_->clearSpectral();
00062 }
00063 
00064 // --------------------------------------------------------------
00065 /* See function definition */
00066 // --------------------------------------------------------------
00067 std::string BoundingBoxSpectral::getName() const
00068 {
00069   ostringstream oss;
00070   oss << "BoundingBoxSpectral_radius" << neighborhood_radius_ << "_spectralRadius" << spectral_information_->getRadius();
00071   return oss.str();
00072 }
00073 
00074 
00075 // --------------------------------------------------------------
00076 /* See function definition */
00077 // --------------------------------------------------------------
00078 int BoundingBoxSpectral::precompute(const sensor_msgs::PointCloud& data,
00079                                     cloud_kdtree::KdTree& data_kdtree,
00080                                     const std::vector<const geometry_msgs::Point32*>& interest_pts)
00081 {
00082   // Compute spectral information if not already done
00083   if (spectral_information_->isSpectralComputed() == false)
00084   {
00085     if (spectral_information_->analyzeInterestPoints(data, data_kdtree, interest_pts) < 0)
00086     {
00087       return -1;
00088     }
00089   }
00090 
00091   // Retrieve necessary spectral information this class needs to compute features
00092   eig_vecs_min_ = &(spectral_information_->getNormals());
00093   eig_vecs_mid_ = &(spectral_information_->getMiddleEigenVectors());
00094   eig_vecs_max_ = &(spectral_information_->getTangents());
00095 
00096   // verify the eigenvectors are for the interest points
00097   if (eig_vecs_min_->size() != interest_pts.size())
00098   {
00099     ROS_ERROR("BoundingBoxSpectral::precompute() inconsistent number of points and spectral info");
00100     eig_vecs_min_ = NULL;
00101     eig_vecs_mid_ = NULL;
00102     eig_vecs_max_ = NULL;
00103     return -1;
00104   }
00105 
00106   return 0;
00107 }
00108 
00109 // --------------------------------------------------------------
00110 /* See function definition */
00111 // --------------------------------------------------------------
00112 int BoundingBoxSpectral::precompute(const sensor_msgs::PointCloud& data,
00113                                     cloud_kdtree::KdTree& data_kdtree,
00114                                     const std::vector<const std::vector<int>*>& interest_region_indices)
00115 {
00116   // Compute spectral information if not already done
00117   if (spectral_information_->isSpectralComputed() == false)
00118   {
00119     if (spectral_information_->analyzeInterestRegions(data, data_kdtree, interest_region_indices) < 0)
00120     {
00121       return -1;
00122     }
00123   }
00124 
00125   // Retrieve necessary spectral information this class needs to compute features
00126   eig_vecs_min_ = &(spectral_information_->getNormals());
00127   eig_vecs_mid_ = &(spectral_information_->getMiddleEigenVectors());
00128   eig_vecs_max_ = &(spectral_information_->getTangents());
00129 
00130   // Verify the eigenvectors are for the interest regions
00131   if (eig_vecs_min_->size() != interest_region_indices.size())
00132   {
00133     ROS_ERROR("BoundingBoxSpectral::precompute() inconsistent number of regions and spectral info");
00134     eig_vecs_min_ = NULL;
00135     eig_vecs_mid_ = NULL;
00136     eig_vecs_max_ = NULL;
00137     return -1;
00138   }
00139 
00140   return 0;
00141 }
00142 
00143 // --------------------------------------------------------------
00144 /* See function definition
00145  * Invariant: interest_sample_idx is in bounds of field containers.
00146  * Invariant: Eigenvectors are unit */
00147 // --------------------------------------------------------------
00148 void BoundingBoxSpectral::computeNeighborhoodFeature(const sensor_msgs::PointCloud& data,
00149                                                      const vector<int>& neighbor_indices,
00150                                                      const unsigned int interest_sample_idx,
00151                                                      std::vector<float>& result) const
00152 {
00153   // Verify the principle components were extracted
00154   const Eigen3::Vector3d* eig_vec_max = (*eig_vecs_max_)[interest_sample_idx];
00155   const Eigen3::Vector3d* eig_vec_mid = (*eig_vecs_mid_)[interest_sample_idx];
00156   const Eigen3::Vector3d* eig_vec_min = (*eig_vecs_min_)[interest_sample_idx];
00157  
00158 //   if (eig_vec_max->size() == 3 && eig_vec_mid->size() == 3 && eig_vec_min->size() == 3)
00159 //   {
00160 //     ROS_INFO("eig_vec_max: %f %f %f", (*eig_vec_max)[0], (*eig_vec_max)[1], (*eig_vec_max)[2]);
00161 //     ROS_INFO("eig_vec_mid: %f %f %f", (*eig_vec_mid)[0], (*eig_vec_mid)[1], (*eig_vec_mid)[2]);
00162 //     ROS_INFO("eig_vec_min: %f %f %f", (*eig_vec_min)[0], (*eig_vec_min)[1], (*eig_vec_min)[2]);
00163 //     cerr << (*eig_vec_max)[0] << (*eig_vec_max)[1] << (*eig_vec_max)[2] << endl;
00164 //     cerr << (*eig_vec_mid)[0] << (*eig_vec_mid)[1] << (*eig_vec_mid)[2] << endl;
00165 //     float test =  eig_vec_min->x();
00166 //     cerr << test << endl;//<< (*eig_vec_min)[1] << (*eig_vec_min)[2] << endl;
00167 //   }
00168 
00169   if (eig_vec_max == NULL)
00170   {
00171     ROS_DEBUG("BoundingBoxSpectral::computeNeighborhoodFeature() No spectral information for sample %u", interest_sample_idx);
00172     return;
00173   }
00174 
00175   result.resize(result_size_);
00176   const unsigned int nbr_neighbors = neighbor_indices.size();
00177 
00178   // --------------------------
00179   // Check for special case when no points in the bounding box as will initialize
00180   // the min/max extremas using the first point below
00181   if (nbr_neighbors == 0)
00182   {
00183     ROS_INFO("BoundingBoxSpectral::computeNeighborhoodFeature() No points to form bounding box");
00184     for (size_t i = 0 ; i < result_size_ ; i++)
00185     {
00186       result[i] = 0.0;
00187     }
00188     return;
00189   }
00190 
00191   // Initialize extrema values of the first point's scalar projections
00192   // onto the principle components
00193   const geometry_msgs::Point32& first_sensor_point = data.points.at(neighbor_indices[0]);
00194   Eigen3::Vector3d curr_pt;
00195   curr_pt[0] = first_sensor_point.x;
00196   curr_pt[1] = first_sensor_point.y;
00197   curr_pt[2] = first_sensor_point.z;
00198   float min_v1 = curr_pt.dot(*eig_vec_max);
00199   float min_v2 = curr_pt.dot(*eig_vec_mid);
00200   float min_v3 = curr_pt.dot(*eig_vec_min);
00201   float max_v1 = min_v1;
00202   float max_v2 = min_v2;
00203   float max_v3 = min_v3;
00204 
00205   // Loop over remaining points in region and update projection extremas
00206   for (unsigned int i = 1 ; i < nbr_neighbors ; i++)
00207   {
00208     const geometry_msgs::Point32& curr_sensor_pt = data.points.at(neighbor_indices[i]);
00209     curr_pt[0] = curr_sensor_pt.x;
00210     curr_pt[1] = curr_sensor_pt.y;
00211     curr_pt[2] = curr_sensor_pt.z;
00212 
00213     // extrema along biggest eigenvector
00214     float curr_projection = curr_pt.dot(*eig_vec_max);
00215     if (curr_projection < min_v1)
00216     {
00217       min_v1 = curr_projection;
00218     }
00219     if (curr_projection > max_v1)
00220     {
00221       max_v1 = curr_projection;
00222     }
00223     // extrema along middle eigenvector
00224     curr_projection = curr_pt.dot(*eig_vec_mid);
00225     if (curr_projection < min_v2)
00226     {
00227       min_v2 = curr_projection;
00228     }
00229     if (curr_projection > max_v2)
00230     {
00231       max_v2 = curr_projection;
00232     }
00233     // extrema along smallest eigenvector
00234     curr_projection = curr_pt.dot(*eig_vec_min);
00235     if (curr_projection < min_v3)
00236     {
00237       min_v3 = curr_projection;
00238     }
00239     if (curr_projection > max_v3)
00240     {
00241       max_v3 = curr_projection;
00242     }
00243   }
00244 
00245   // --------------------------
00246   result[0] = max_v1 - min_v1;
00247   result[1] = max_v2 - min_v2;
00248   result[2] = max_v3 - min_v3;
00249   result[3] = eig_vec_max->x();
00250   result[4] = eig_vec_max->y();
00251   result[5] = eig_vec_max->z();
00252   result[6] = eig_vec_mid->x();
00253   result[7] = eig_vec_mid->y();
00254   result[8] = eig_vec_mid->z();
00255   result[9] = eig_vec_min->x();
00256   result[10] = eig_vec_min->y();
00257   result[11] = eig_vec_min->z();
00258   //cerr << "eig_vec: " << result[3] << " " << result[4] << " " << result[5] << " " << result[6] << " " << result[7] << " " << 
00259   //result[8] << " " << result[9] << " " << result[10] << " " << result[11] << endl;
00260 }


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