tabletop_object_detector: tabletop_object_detector: iterative_distance

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 
00035 // Author(s): Marius Muja, Matei Ciocarlie and Romain Thibaux
00036 
00037 #include "tabletop_object_detector/model_fitter.h"
00038 
00039 #include <math.h>
00040 #include <distance_field/propagation_distance_field.h>
00041 
00042 #include "tabletop_object_detector/marker_generator.h"
00043 
00044 namespace tabletop_object_detector {
00045 
00047 class IterativeTranslationFitter : public DistanceFieldFitter
00048 {
00049  private:
00050 
00052   template <class PointCloudType>
00053     geometry_msgs::Point32 centerOfSupport(const PointCloudType& cloud);
00054 
00056   template <class PointCloudType>
00057     double getFitScoreAndGradient(const PointCloudType& cloud, 
00058                                   const geometry_msgs::Point32& location, 
00059                                   geometry_msgs::Point32& vector,
00060                                   double &maxDist);
00061  public:
00063   IterativeTranslationFitter() : DistanceFieldFitter() {}
00065   ~IterativeTranslationFitter() {}
00066 
00068   template <class PointCloudType>
00069     ModelFitInfo fitPointCloud(const PointCloudType& cloud);
00070 };
00071 
00072 //------------------------- Implementation follows ----------------------------------------
00073 
00077 template <class PointCloudType>
00078 geometry_msgs::Point32 IterativeTranslationFitter::centerOfSupport(const PointCloudType& cloud) 
00079 {
00080   geometry_msgs::Point32 center;
00081   center.x = center.y = center.z = 0;
00082   if (cloud.points.empty())
00083   {
00084     return center;
00085   }
00086   for (unsigned int i=0; i<cloud.points.size(); ++i) 
00087   {
00088     center.x += cloud.points[i].x;
00089     center.y += cloud.points[i].y;
00090   }
00091   center.x /= cloud.points.size();
00092   center.y /= cloud.points.size();
00093   return center;
00094 }
00095 
00096 
00097 template <class PointCloudType>
00098 double IterativeTranslationFitter::getFitScoreAndGradient(const PointCloudType& cloud, 
00099                                                           const geometry_msgs::Point32& location, 
00100                                                           geometry_msgs::Point32& vector,
00101                                                           double &max_dist)
00102 {
00103   double score = 0;
00104   max_dist = 0;
00105   
00106   vector.x = 0;
00107   vector.y = 0;
00108   vector.z = 0;
00109   int cnt = 0;
00110   
00111   for (size_t i=0;i<cloud.points.size();i++) 
00112   {
00113     double wx = cloud.points[i].x-location.x;
00114     double wy = cloud.points[i].y-location.y;
00115     double wz = cloud.points[i].z-location.z;
00116     
00117     int x, y, z;
00118     double val = truncate_value_;
00119     if (distance_voxel_grid_->worldToGrid(wx,wy,wz,x,y,z)) 
00120     {
00121       distance_field::PropDistanceFieldVoxel& voxel = distance_voxel_grid_->getCell(x,y,z);
00122       double cx, cy, cz;
00123       if (voxel.closest_point_[0] != distance_field::PropDistanceFieldVoxel::UNINITIALIZED) 
00124       {
00125         distance_voxel_grid_->gridToWorld(voxel.closest_point_[0],
00126                                           voxel.closest_point_[1],
00127                                           voxel.closest_point_[2],
00128                                           cx,cy,cz);
00129         val = distance_voxel_grid_->getDistanceFromCell(x,y,z);
00130         vector.x += (cx-wx);
00131         vector.y += (cy-wy);
00132         vector.z += (cz-wz);
00133         cnt++;
00134         if (val>truncate_value_) 
00135         {
00136           val = truncate_value_;
00137         }
00138       }
00139       else
00140       {
00141       }
00142     }
00143     else
00144     {
00145     }    
00146     max_dist = std::max(max_dist,val);
00147     //score += val*val;
00148     score += val;
00149   }
00150   score /= (cloud.points.size());
00151   if (cnt!=0) 
00152   {
00153     vector.x /=  cnt;
00154     vector.y /=  cnt;
00155     vector.z /=  cnt;
00156   }
00157   
00158   return score;
00159 }
00160 
00171 template <class PointCloudType>
00172 ModelFitInfo IterativeTranslationFitter::fitPointCloud(const PointCloudType& cloud)
00173 {
00174   if (cloud.points.empty()) 
00175   {
00176     ROS_ERROR("Attempt to fit model to empty point cloud");
00177     geometry_msgs::Pose bogus_pose;
00178     return ModelFitInfo(model_id_, bogus_pose, -1.0);
00179   }
00180   
00181   // compute center of point cloud
00182   geometry_msgs::Point32 center = centerOfSupport<PointCloudType>(cloud);
00183 
00184   geometry_msgs::Point32 location = center;
00185   geometry_msgs::Point32 vector;
00186   double max_dist;
00187   geometry_msgs::Pose pose;
00188     
00189   double score = getFitScoreAndGradient<PointCloudType>(cloud, location, vector, max_dist);
00190   double old_score = score + 1;
00191  
00192   double EPS = 1.0e-6;
00193   int max_iterations = 100;
00194   int iter = 0;
00195   while (score < old_score - EPS && iter < max_iterations) 
00196   {
00197     old_score = score;
00198     location.x -= vector.x;
00199     location.y -= vector.y;
00200     // see above comment on search along z
00201     // location.z -= vector.z;
00202     score = getFitScoreAndGradient<PointCloudType>(cloud, location, vector, max_dist);
00203     iter++;
00204   }
00205 
00206   if (iter == max_iterations) 
00207   {
00208     ROS_WARN("Maximum iterations reached in model fitter");
00209   }
00210 
00211   pose.position.x = location.x;
00212   pose.position.y = location.y;
00213   pose.position.z = location.z;
00214   pose.orientation.x = 0;
00215   pose.orientation.y = 0;
00216   pose.orientation.z = 0;
00217   pose.orientation.w = 1;
00218 
00219   return ModelFitInfo(model_id_, pose, old_score);
00220 }
00221 
00222 } //namespace
iterative_distance_fitter.h
