sac_model_cylinder.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id$
00028  *
00029  */
00030 
00033 #include <stdlib.h>
00034 #include <door_handle_detector/sample_consensus/sac_model_cylinder.h>
00035 #include <door_handle_detector/geometry/point.h>
00036 #include <door_handle_detector/geometry/distances.h>
00037 
00038 #include <cminpack.h>
00039 
00040 namespace sample_consensus
00041 {
00043 
00048   void
00049     SACModelCylinder::getSamples (int &iterations, std::vector<int> &samples)
00050   {
00051     samples.resize (4);
00052     double trand = indices_.size () / (RAND_MAX + 1.0);
00053 
00054     // Get a random number between 1 and max_indices
00055     int idx = (int)(rand () * trand);
00056     // Get the index
00057     samples[0] = indices_.at (idx);
00058 
00059     // Get a second point which is different than the first
00060     do
00061     {
00062       idx = (int)(rand () * trand);
00063       samples[1] = indices_.at (idx);
00064       iterations++;
00065     } while (samples[1] == samples[0]);
00066     iterations--;
00067 
00068     double Dx1, Dy1, Dz1, Dx2, Dy2, Dz2, Dy1Dy2;
00069     // Compute the segment values (in 3d) between XY
00070     Dx1 = cloud_->points[samples[1]].x - cloud_->points[samples[0]].x;
00071     Dy1 = cloud_->points[samples[1]].y - cloud_->points[samples[0]].y;
00072     Dz1 = cloud_->points[samples[1]].z - cloud_->points[samples[0]].z;
00073 
00074     int iter = 0;
00075     do
00076     {
00077       // Get the third point, different from the first two
00078       do
00079       {
00080         idx = (int)(rand () * trand);
00081         samples[2] = indices_.at (idx);
00082         iterations++;
00083       } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) );
00084       iterations--;
00085 
00086       // Compute the segment values (in 3d) between XZ
00087       Dx2 = cloud_->points[samples[2]].x - cloud_->points[samples[0]].x;
00088       Dy2 = cloud_->points[samples[2]].y - cloud_->points[samples[0]].y;
00089       Dz2 = cloud_->points[samples[2]].z - cloud_->points[samples[0]].z;
00090 
00091       Dy1Dy2 = Dy1 / Dy2;
00092       iter++;
00093 
00094       if (iter > MAX_ITERATIONS_COLLINEAR )
00095       {
00096         ROS_WARN ("[SACModelCylinder::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR);
00097         break;
00098       }
00099       iterations++;
00100     }
00101     // Use Zoli's method for collinearity check
00102     while (((Dx1 / Dx2) == Dy1Dy2) && (Dy1Dy2 == (Dz1 / Dz2)));
00103     iterations--;
00104 
00105     // Need to improve this: we need 4 points, 3 non-collinear always, and the 4th should not be in the same plane as the other 3
00106     // otherwise we can encounter degenerate cases
00107     do
00108     {
00109       samples[3] = (int)(rand () * trand);
00110       iterations++;
00111     } while ( (samples[3] == samples[2]) || (samples[3] == samples[1]) || (samples[3] == samples[0]) );
00112     iterations--;
00113 
00114     return;
00115   }
00116 
00118 
00125   void
00126     SACModelCylinder::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
00127   {
00128     int nr_p = 0;
00129     inliers.resize (indices_.size ());
00130 
00131     // Model coefficients: [point_on_axis axis_direction radius]
00132     // Iterate through the 3d points and calculate the distances from them to the cylinder
00133     for (unsigned int i = 0; i < indices_.size (); i++)
00134     {
00135       // Aproximate the distance from the point to the cylinder as the difference between
00136       //dist(point,cylinder_axis) and cylinder radius
00137       // NOTE: need to revise this.
00138       if (fabs (
00139                 cloud_geometry::distances::pointToLineDistance (cloud_->points.at (indices_[i]), model_coefficients) - model_coefficients[6]
00140                ) < threshold)
00141       {
00142         // Returns the indices of the points whose distances are smaller than the threshold
00143         inliers[nr_p] = indices_[i];
00144         nr_p++;
00145       }
00146     }
00147     inliers.resize (nr_p);
00148     return;
00149   }
00150 
00152 
00156   void
00157     SACModelCylinder::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
00158   {
00159     distances.resize (indices_.size ());
00160 
00161     // Iterate through the 3d points and calculate the distances from them to the cylinder
00162     for (unsigned int i = 0; i < indices_.size (); i++)
00163       // Aproximate the distance from the point to the cylinder as the difference between
00164       //dist(point,cylinder_axis) and cylinder radius
00165       // NOTE: need to revise this.
00166       distances[i] = fabs (cloud_geometry::distances::pointToLineDistance (cloud_->points.at (indices_[i]), model_coefficients) - model_coefficients[6]);
00167     return;
00168   }
00169 
00171 
00177   void
00178     SACModelCylinder::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
00179                                      sensor_msgs::PointCloud &projected_points)
00180   {
00181     std::cerr << "[SACModelCylinder::projecPoints] Not implemented yet." << std::endl;
00182     projected_points = *cloud_;
00183   }
00184 
00186 
00191   void
00192     SACModelCylinder::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
00193   {
00194     std::cerr << "[SACModelCylinder::projecPointsInPlace] Not implemented yet." << std::endl;
00195   }
00196 
00198 
00202   bool
00203     SACModelCylinder::computeModelCoefficients (const std::vector<int> &samples)
00204   {
00205     model_coefficients_.resize (7);
00206 
00207     // Save the nx/ny/nz channel indices the first time we run this
00208     if (nx_idx_ == -1)
00209     {
00210       nx_idx_ = cloud_geometry::getChannelIndex (*cloud_, "nx");
00211       if (nx_idx_ == -1) return (false);
00212     }
00213     if (ny_idx_ == -1)
00214     {
00215       ny_idx_ = cloud_geometry::getChannelIndex (*cloud_, "ny");
00216       if (ny_idx_ == -1) return (false);
00217     }
00218     if (nz_idx_ == -1)
00219     {
00220       nz_idx_ = cloud_geometry::getChannelIndex (*cloud_, "nz");
00221       if (nz_idx_ == -1) return (false);
00222     }
00223 
00224     geometry_msgs::Point32 u, v, w;
00225 
00226     u.x = cloud_->channels[nx_idx_].values.at (samples.at (0));
00227     u.y = cloud_->channels[ny_idx_].values.at (samples.at (0));
00228     u.z = cloud_->channels[nz_idx_].values.at (samples.at (0));
00229 
00230     v.x = cloud_->channels[nx_idx_].values.at (samples.at (1));
00231     v.y = cloud_->channels[ny_idx_].values.at (samples.at (1));
00232     v.z = cloud_->channels[nz_idx_].values.at (samples.at (1));
00233 
00234     w.x = (u.x + cloud_->points.at (samples.at (0)).x) - cloud_->points.at (samples.at (1)).x;
00235     w.y = (u.y + cloud_->points.at (samples.at (0)).y) - cloud_->points.at (samples.at (1)).y;
00236     w.z = (u.z + cloud_->points.at (samples.at (0)).z) - cloud_->points.at (samples.at (1)).z;
00237 
00238     double a = cloud_geometry::dot (u, u);
00239     double b = cloud_geometry::dot (u, v);
00240     double c = cloud_geometry::dot (v, v);
00241     double d = cloud_geometry::dot (u, w);
00242     double e = cloud_geometry::dot (v, w);
00243     double denominator = a*c - b*b;
00244     double sc, tc;
00245     // Compute the line parameters of the two closest points
00246     if (denominator < 1e-8)          // The lines are almost parallel
00247     {
00248       sc = 0.0;
00249       tc = (b > c ? d / b : e / c);  // Use the largest denominator
00250     }
00251     else
00252     {
00253       sc = (b*e - c*d) / denominator;
00254       tc = (a*e - b*d) / denominator;
00255     }
00256 
00257     // point_on_axis, axis_direction
00258     model_coefficients_[0] = cloud_->points.at (samples.at (0)).x + cloud_->channels[nx_idx_].values.at (samples.at (0)) + (sc * u.x);
00259     model_coefficients_[1] = cloud_->points.at (samples.at (0)).y + cloud_->channels[ny_idx_].values.at (samples.at (0)) + (sc * u.y);
00260     model_coefficients_[2] = cloud_->points.at (samples.at (0)).z + cloud_->channels[nz_idx_].values.at (samples.at (0)) + (sc * u.z);
00261     model_coefficients_[3] = cloud_->points.at (samples.at (1)).x + (tc * v.x) - model_coefficients_[0];
00262     model_coefficients_[4] = cloud_->points.at (samples.at (1)).y + (tc * v.y) - model_coefficients_[1];
00263     model_coefficients_[5] = cloud_->points.at (samples.at (1)).z + (tc * v.z) - model_coefficients_[2];
00264 
00265     double norm = sqrt (
00266                         (model_coefficients_[3] * model_coefficients_[3]) +
00267                         (model_coefficients_[4] * model_coefficients_[4]) +
00268                         (model_coefficients_[5] * model_coefficients_[5])
00269                       );
00270     model_coefficients_[3] /= norm;
00271     model_coefficients_[4] /= norm;
00272     model_coefficients_[5] /= norm;
00273 
00274     // cylinder radius
00275     model_coefficients_[6] = cloud_geometry::distances::pointToLineDistance (cloud_->points.at (samples.at (0)), model_coefficients_);
00276 
00277     return (true);
00278   }
00279 
00281 
00286   void
00287     SACModelCylinder::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
00288   {
00289     if (inliers.size () == 0)
00290     {
00291       ROS_ERROR ("[SACModelCylinder::RefitModel] Cannot re-fit 0 inliers!");
00292       refit_coefficients = model_coefficients_;
00293       return;
00294     }
00295     if (model_coefficients_.size () == 0)
00296     {
00297       ROS_WARN ("[SACModelCylinder::RefitModel] Initial model coefficients have not been estimated yet - proceeding without an initial solution!");
00298       best_inliers_ = indices_;
00299     }
00300 
00301     tmp_inliers_ = &inliers;
00302 
00303     int m = inliers.size ();
00304 
00305     double *fvec = new double[m];
00306 
00307     int n = 7;      // 7 unknowns
00308     int iwa[n];
00309 
00310     int lwa = m * n + 5 * n + m;
00311     double *wa = new double[lwa];
00312 
00313     // Set the initial solution
00314     double x[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
00315     if ((int)model_coefficients_.size () == n)
00316       for (int d = 0; d < n; d++)
00317         x[d] = model_coefficients_.at (d);
00318 
00319     // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
00320     double tol = sqrt (dpmpar (1));
00321 
00322     // Optimize using forward-difference approximation LM
00323     int info = lmdif1 (&sample_consensus::SACModelCylinder::functionToOptimize, this, m, n, x, fvec, tol, iwa, wa, lwa);
00324 
00325     // Compute the L2 norm of the residuals
00326     ROS_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g",
00327                info, enorm (m, fvec), model_coefficients_.at (0), model_coefficients_.at (1), model_coefficients_.at (2), model_coefficients_.at (3),
00328                model_coefficients_.at (4), model_coefficients_.at (5), model_coefficients_.at (6), x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
00329 
00330     refit_coefficients.resize (n);
00331     for (int d = 0; d < n; d++)
00332       refit_coefficients[d] = x[d];
00333 
00334     free (wa); free (fvec);
00335   }
00336 
00338 
00346   int
00347     SACModelCylinder::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
00348   {
00349     SACModelCylinder *model = (SACModelCylinder*)p;
00350 
00351     std::vector<double> line_coefficients (6);
00352     for (unsigned int d = 0; d < 6; d++)
00353       line_coefficients[d] = x[d];
00354 
00355     for (int i = 0; i < m; i++)
00356       // dist = f - r
00357       fvec[i] = cloud_geometry::distances::pointToLineDistance (model->cloud_->points[model->tmp_inliers_->at (i)], line_coefficients) - x[6];
00358 
00359     return (0);
00360   }
00361 
00363 
00367   bool
00368     SACModelCylinder::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
00369   {
00370     for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
00371       // Aproximate the distance from the point to the cylinder as the difference between
00372       //dist(point,cylinder_axis) and cylinder radius
00373       // NOTE: need to revise this.
00374       if (fabs (
00375                 cloud_geometry::distances::pointToLineDistance (cloud_->points.at (*it), model_coefficients_) - model_coefficients_[6]
00376                ) > threshold)
00377         return (false);
00378 
00379     return (true);
00380   }
00381 }


door_handle_detector
Author(s): Radu Bogdan Rusu, Marius
autogenerated on Wed Dec 11 2013 14:17:01