box_fit2_algo.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Zoltan-Csaba Marton <marton@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <pcl_cloud_algos/box_fit2_algo.h>
00031 
00032 // Sample Consensus
00033 //#include <point_cloud_mapping/sample_consensus/ransac.h>
00034 #include <pcl/sample_consensus/ransac.h>
00035 #include <pcl/sample_consensus/impl/ransac.hpp>
00036 // For computeCentroid
00037 //#include <point_cloud_mapping/geometry/nearest.h>
00038 
00039 //template class pcl::RandomSampleConsensus<pcl::Normal>;
00040 
00041 using namespace std;
00042 using namespace pcl_cloud_algos;
00043 using namespace pcl;
00044 //using namespace sample_consensus;
00045 
00046 void RobustBoxEstimation::getMinAndMax(Eigen::VectorXf model_coefficients, boost::shared_ptr<SACModelOrientation<PointXYZINormal> > model, std::vector<int> &min_max_indices, std::vector<float> &min_max_distances)
00047 {
00048 
00049   boost::shared_ptr<vector<int> > inliers = model->getIndices();
00050   boost::shared_ptr<vector<int> > indices = model->getIndices();
00051 
00052   // Initialize result vectors
00053   min_max_indices.resize (6);
00054   min_max_distances.resize (6);
00055   min_max_distances[0] = min_max_distances[2] = min_max_distances[4] = +DBL_MAX;
00056   min_max_distances[1] = min_max_distances[3] = min_max_distances[5] = -DBL_MAX;
00057 
00058   // The 3 coordinate axes are nm, nc and axis_
00059   Eigen::Vector3f nm;
00060   nm[0] = model_coefficients[0];
00061   nm[1] = model_coefficients[1];
00062   nm[2] = model_coefficients[2];
00063   //Eigen::Vector3f nm = Eigen::Vector3d::Map(&(*model_coefficients)[0]).cast<float> ();
00064   Eigen::Vector3f nc = model->axis_.cross (nm);
00065 
00066   // Find minimum and maximum distances from origin along the three axes
00067   for (std::vector<int>::iterator it = inliers->begin (); it != inliers->end (); it++)
00068   //for (unsigned i = 0; i < inliers.size (); i++)
00069   {
00070     // @NOTE inliers is a list of indices of the indices_ array!
00071     Eigen::Vector3f point (cloud_->points[(*indices)[*it]].x, cloud_->points[(*indices)[*it]].y, cloud_->points[(*indices)[*it]].z);
00072     //Eigen::Vector3f point (cloud_->points[indices_[*it]].x - center.x, cloud_->points[indices_[*it]].y - center.y, cloud_->points[indices_[*it]].z - center.z);
00073     //Eigen::Vector3f point (cloud_->points[indices_[inliers[i]]].x, cloud_->points[indices_[inliers[i]]].y, cloud_->points[indices_[inliers[i]]].z);
00074     double dists[3];
00075     dists[0] = nm.dot(point);
00076     dists[1] = nc.dot(point);
00077     dists[2] = model->axis_.dot(point);
00078     for (int d=0; d<3; d++)
00079     {
00080       if (min_max_distances[2*d+0] > dists[d]) { min_max_distances[2*d+0] = dists[d]; min_max_indices[2*d+0] = *it; }
00081       if (min_max_distances[2*d+1] < dists[d]) { min_max_distances[2*d+1] = dists[d]; min_max_indices[2*d+1] = *it; }
00082     }
00083   }
00084 
00085 }
00086 std::vector<std::string> RobustBoxEstimation::requires ()
00087 {
00088   std::vector<std::string> requires;
00089   // requires 3D coordinates
00090   requires.push_back("x");
00091   requires.push_back("y");
00092   requires.push_back("z");
00093   // requires normals
00094   requires.push_back("nx");
00095   requires.push_back("ny");
00096   requires.push_back("nz");
00097   return requires;
00098 }
00099 
00100 void RobustBoxEstimation::pre ()
00101 {
00102   BoxEstimation::pre ();
00103   nh_.param("eps_angle", eps_angle_, eps_angle_);
00104   nh_.param("success_probability", success_probability_, success_probability_);
00105 }
00106 
00108 
00111 bool RobustBoxEstimation::find_model(boost::shared_ptr<const pcl::PointCloud <pcl::PointXYZINormal> > cloud, std::vector<double> &coeff)
00112 {
00113   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Looking for box in a cluster of %u points", (unsigned)cloud->points.size ());
00114 
00115   // Compute center point
00116   //cloud_geometry::nearest::computeCentroid (*cloud, box_centroid_);
00117 
00118   // TODO template SAC model and method on input point type and get rid of this
00119 //  PointCloud<Normal> nrmls ;
00120 //  nrmls.header = cloud->header;
00121 //  nrmls.points.resize(cloud->points.size());
00122 //  for(size_t i = 0 ; i < cloud->points.size(); i++)
00123 //  {
00124 //    nrmls.points[i].normal[0] = cloud->points[i].normal[0];
00125 //    nrmls.points[i].normal[1] = cloud->points[i].normal[1];
00126 //    nrmls.points[i].normal[2] = cloud->points[i].normal[2];
00127 //  }
00128 
00129   // Create model - @NOTE: the model needs/uses only pcl::Normal but this way it is simpler to use
00130   SACModelOrientation<PointXYZINormal>::Ptr model (new SACModelOrientation<PointXYZINormal> (cloud));
00131   model->axis_[0] = 0 ;
00132   model->axis_[1] = 0 ;
00133   model->axis_[2] = 1 ;
00134   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Axis is (%g,%g,%g) and maximum angular difference %g",
00135       model->axis_[0], model->axis_[1], model->axis_[2], eps_angle_);
00136 
00137   // Check probability of success and decide on method
00138   Eigen::VectorXf refined;
00139   vector<int> inliers;
00141   if (success_probability_ > 0 && success_probability_ < 1)
00142   {
00143     if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Using RANSAC with stop probability of %g and model refinement", success_probability_);
00144 
00145     // Fit model using RANSAC
00146     RandomSampleConsensus<PointXYZINormal>::Ptr sac (new RandomSampleConsensus<PointXYZINormal> (model, eps_angle_));
00147     sac->setProbability (success_probability_);
00148     if (!sac->computeModel ())
00149     {
00150       if (verbosity_level_ > -2) ROS_ERROR ("[RobustBoxEstimation] No model found using the angular threshold of %g!", eps_angle_);
00151       return false;
00152     }
00153 
00154     // Get inliers and refine result
00155     sac->getInliers(inliers);
00156     if (verbosity_level_ > 1) cerr << "number of inliers: " << inliers.size () << endl;
00157     // Exhaustive search for best model
00158     std::vector<int> best_sample;
00159     std::vector<int> best_inliers;
00160     Eigen::VectorXf model_coefficients;
00161     for (unsigned i = 0; i < cloud->points.size (); i++)
00162     {
00163       std::vector<int> selection (1);
00164       selection[0] = i;
00165       model->computeModelCoefficients (selection, model_coefficients);
00166 
00167       model->selectWithinDistance (model_coefficients, eps_angle_, inliers);
00168       if (best_inliers.size () < inliers.size ())
00169       {
00170         best_inliers = inliers;
00171         best_sample = selection;
00172       }
00173     }
00174 
00175     // Check if successful and save results
00176     if (best_inliers.size () > 0)
00177     {
00178       model->computeModelCoefficients (best_sample, refined);
00179       //model->getModelCoefficients (refined);
00181       inliers = best_inliers;
00182       //model->setBestModel (best_sample);
00183       //model->setBestInliers (best_inliers);
00184       // refine results: needs inliers to be set!
00185       // sac->refineCoefficients(refined);
00186     }
00188     //model->computeModelCoefficients(model->getBestModel ());
00189     //Eigen::VectorXf original;
00190     //model->getModelCoefficients (original);
00191     //if (verbosity_level_ > 1) cerr << "original direction: " << original[0] << " " << original[1] << " " << original[2] << ", found at point nr " << original[3] << endl;
00192     //sac->refineCoefficients(refined);
00193    // if (verbosity_level_ > 1) cerr << "refitted direction: " << refined.at (0) << " " << refined.at (1) << " " << refined.at (2) << ", initiated from point nr " << refined.at (3) << endl;
00194    // if (refined[3] == -1)
00195    //   refined = original;
00196   }
00197   else
00198   {
00199     if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Using exhaustive search in %ld points", cloud->points.size ());
00200 
00201     // Exhaustive search for best model
00202     std::vector<int> best_sample;
00203     std::vector<int> best_inliers;
00204     Eigen::VectorXf model_coefficients;
00205     for (unsigned i = 0; i < cloud->points.size (); i++)
00206     {
00207       std::vector<int> selection (1);
00208       selection[0] = i;
00209       model->computeModelCoefficients (selection, model_coefficients);
00210 
00211       model->selectWithinDistance (model_coefficients, eps_angle_, inliers);
00212       if (best_inliers.size () < inliers.size ())
00213       {
00214         best_inliers = inliers;
00215         best_sample = selection;
00216       }
00217     }
00218 
00219     // Check if successful and save results
00220     if (best_inliers.size () > 0)
00221     {
00222       model->computeModelCoefficients (best_sample, refined);
00223       //model->getModelCoefficients (refined);
00225       inliers = best_inliers;
00226       //model->setBestModel (best_sample);
00227       //model->setBestInliers (best_inliers);
00228       // refine results: needs inliers to be set!
00229       // sac->refineCoefficients(refined);
00230     }
00231     else
00232     {
00233       if (verbosity_level_ > -2) ROS_ERROR ("[RobustBoxEstimation] No model found using the angular threshold of %g!", eps_angle_);
00234       return false;
00235     }
00236   }
00237 
00238   // Save fixed axis
00239   coeff[12+0] = model->axis_[0];
00240   coeff[12+1] = model->axis_[1];
00241   coeff[12+2] = model->axis_[2];
00242 
00243   // Save complementary axis (cross product)
00244   coeff[9+0] = model->axis_[1]*refined[2] - model->axis_[2]*refined[1];
00245   coeff[9+1] = model->axis_[2]*refined[0] - model->axis_[0]*refined[2];
00246   coeff[9+2] = model->axis_[0]*refined[1] - model->axis_[1]*refined[0];
00247 
00248   // Save principle axis (corrected)
00249   refined[0] = - (model->axis_[1]*coeff[9+2] - model->axis_[2]*coeff[9+1]);
00250   refined[1] = - (model->axis_[2]*coeff[9+0] - model->axis_[0]*coeff[9+2]);
00251   refined[2] = - (model->axis_[0]*coeff[9+1] - model->axis_[1]*coeff[9+0]);
00252 
00253   ROS_INFO("refined[0]: %f", refined[0]);
00254   ROS_INFO("refined[1]: %f", refined[1]);
00255   ROS_INFO("refined[2]: %f", refined[2]);
00256   coeff[6+0] = refined[0];
00257   coeff[6+1] = refined[1];
00258   coeff[6+2] = refined[2];
00259 
00260   /*// Save complementary axis (AGIAN, JUST TO MAKE SURE)
00261   coeff[9+0] = model->axis_[1]*refined[2] - model->axis_[2]*refined[1];
00262   coeff[9+1] = model->axis_[2]*refined[0] - model->axis_[0]*refined[2];
00263   coeff[9+2] = model->axis_[0]*refined[1] - model->axis_[1]*refined[0];*/
00264 
00265   // Compute minimum and maximum along each dimension for the whole cluster
00266   vector<int> min_max_indices;
00267   vector<float> min_max_distances;
00268   //boost::shared_ptr<vector<int> > indices (new vector<int>);
00269   //indices = model->getIndices();
00270 
00271   //model->getMinAndMax (&refined, &inliers, min_max_indices, min_max_distances);
00272   //getMinAndMax (&refined, model->getIndices (), min_max_indices, min_max_distances);
00273   getMinAndMax (refined, model, min_max_indices, min_max_distances);
00274   //vector<int> min_max_indices = model->getMinAndMaxIndices (refined);
00275 
00276   //cerr << min_max_distances.at (1) << " " << min_max_distances.at (0) << endl;
00277   //cerr << min_max_distances.at (3) << " " << min_max_distances.at (2) << endl;
00278   //cerr << min_max_distances.at (5) << " " << min_max_distances.at (4) << endl;
00279 
00280   // Save dimensions
00281   coeff[3+0] = min_max_distances.at (1) - min_max_distances.at (0);
00282   coeff[3+1] = min_max_distances.at (3) - min_max_distances.at (2);
00283   coeff[3+2] = min_max_distances.at (5) - min_max_distances.at (4);
00284 
00285   // Distance of box's geometric center relative to origin along orientation axes
00286   double dist[3];
00287   dist[0] = min_max_distances[0] + coeff[3+0] / 2;
00288   dist[1] = min_max_distances[2] + coeff[3+1] / 2;
00289   dist[2] = min_max_distances[4] + coeff[3+2] / 2;
00290 
00291   // Compute position of the box's geometric center in XYZ
00292   coeff[0] = dist[0]*coeff[6+0] + dist[1]*coeff[9+0] + dist[2]*coeff[12+0];
00293   coeff[1] = dist[0]*coeff[6+1] + dist[1]*coeff[9+1] + dist[2]*coeff[12+1];
00294   coeff[2] = dist[0]*coeff[6+2] + dist[1]*coeff[9+2] + dist[2]*coeff[12+2];
00295   //coeff[0] = box_centroid_.x + dist[0]*coeff[6+0] + dist[1]*coeff[9+0] + dist[2]*coeff[12+0];
00296   //coeff[1] = box_centroid_.y + dist[0]*coeff[6+1] + dist[1]*coeff[9+1] + dist[2]*coeff[12+1];
00297   //coeff[2] = box_centroid_.z + dist[0]*coeff[6+2] + dist[1]*coeff[9+2] + dist[2]*coeff[12+2];
00298   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Cluster center x: %g, y: %g, z: %g", coeff[0], coeff[1], coeff[2]);
00299 
00300   // Print info
00301   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Dimensions x: %g, y: %g, z: %g",
00302       coeff[3+0], coeff[3+1], coeff[3+2]);
00303   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Direction vectors: \n\t%g %g %g \n\t%g %g %g \n\t%g %g %g",
00304       coeff[3+3], coeff[3+4], coeff[3+5],
00305       coeff[3+6], coeff[3+7], coeff[3+8],
00306       coeff[3+9], coeff[3+10],coeff[3+11]);
00307 
00308   return true;
00309 }
00310 
00311 #ifdef CREATE_NODE
00312 int main (int argc, char* argv[])
00313 {
00314   return standalone_node <RobustBoxEstimation> (argc, argv);
00315 }
00316 #endif
00317 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:48