stereo_wall_detection: sac_model_plane.cpp Source 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: sac_model_plane.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
00028  *
00029  */
00030 
00033 #include <stereo_wall_detection/sample_consensus/sac_model_plane.h>
00034 #include <stereo_wall_detection/geometry/nearest.h>
00035 
00036 namespace sample_consensus
00037 {
00039 
00044   void
00045     SACModelPlane::getSamples (int &iterations, std::vector<int> &samples)
00046   {
00047     samples.resize (3);
00048     double trand = indices_.size () / (RAND_MAX + 1.0);
00049 
00050     // Get a random number between 1 and max_indices
00051     int idx = (int)(rand () * trand);
00052     // Get the index
00053     samples[0] = indices_.at (idx);
00054 
00055     // Get a second point which is different than the first
00056     do
00057     {
00058       idx = (int)(rand () * trand);
00059       samples[1] = indices_.at (idx);
00060       iterations++;
00061     } while (samples[1] == samples[0]);
00062     iterations--;
00063 
00064     double Dx1, Dy1, Dz1, Dx2, Dy2, Dz2, Dy1Dy2;
00065     // Compute the segment values (in 3d) between XY
00066     Dx1 = cloud_->points[samples[1]].x - cloud_->points[samples[0]].x;
00067     Dy1 = cloud_->points[samples[1]].y - cloud_->points[samples[0]].y;
00068     Dz1 = cloud_->points[samples[1]].z - cloud_->points[samples[0]].z;
00069 
00070     int iter = 0;
00071     do
00072     {
00073       // Get the third point, different from the first two
00074       do
00075       {
00076         idx = (int)(rand () * trand);
00077         samples[2] = indices_.at (idx);
00078         iterations++;
00079       } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) );
00080       iterations--;
00081 
00082       // Compute the segment values (in 3d) between XZ
00083       Dx2 = cloud_->points[samples[2]].x - cloud_->points[samples[0]].x;
00084       Dy2 = cloud_->points[samples[2]].y - cloud_->points[samples[0]].y;
00085       Dz2 = cloud_->points[samples[2]].z - cloud_->points[samples[0]].z;
00086 
00087       Dy1Dy2 = Dy1 / Dy2;
00088       iter++;
00089 
00090       if (iter > MAX_ITERATIONS_COLLINEAR )
00091       {
00092         ROS_WARN ("[SACModelPlane::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR);
00093         break;
00094       }
00095       iterations++;
00096     }
00097     // Use Zoli's method for collinearity check
00098     while (((Dx1 / Dx2) == Dy1Dy2) && (Dy1Dy2 == (Dz1 / Dz2)));
00099     iterations--;
00100 
00101     return;
00102   }
00103 
00105 
00113   void
00114     SACModelPlane::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
00115   {
00116     int nr_p = 0;
00117     inliers.resize (indices_.size ());
00118 
00119     // Iterate through the 3d points and calculate the distances from them to the plane
00120     for (unsigned int i = 0; i < indices_.size (); i++)
00121     {
00122       // Calculate the distance from the point to the plane normal as the dot product
00123       // D = (P-A).N/|N|
00124       if (fabs (model_coefficients.at (0) * cloud_->points.at (indices_.at (i)).x +
00125                 model_coefficients.at (1) * cloud_->points.at (indices_.at (i)).y +
00126                 model_coefficients.at (2) * cloud_->points.at (indices_.at (i)).z +
00127                 model_coefficients.at (3)) < threshold)
00128       {
00129         // Returns the indices of the points whose distances are smaller than the threshold
00130         inliers[nr_p] = indices_[i];
00131         nr_p++;
00132       }
00133     }
00134     inliers.resize (nr_p);
00135     return;
00136   }
00137 
00139 
00143   void
00144     SACModelPlane::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
00145   {
00146     distances.resize (indices_.size ());
00147 
00148     // Iterate through the 3d points and calculate the distances from them to the plane
00149     for (unsigned int i = 0; i < indices_.size (); i++)
00150       // Calculate the distance from the point to the plane normal as the dot product
00151       // D = (P-A).N/|N|
00152       distances[i] = fabs (model_coefficients.at (0) * cloud_->points.at (indices_[i]).x +
00153                            model_coefficients.at (1) * cloud_->points.at (indices_[i]).y +
00154                            model_coefficients.at (2) * cloud_->points.at (indices_[i]).z +
00155                            model_coefficients.at (3));
00156     return;
00157   }
00158 
00160 
00165   void
00166     SACModelPlane::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
00167                                   sensor_msgs::PointCloud &projected_points)
00168   {
00169     // Allocate enough space
00170     projected_points.points.resize (inliers.size ());
00171     projected_points.set_channels_size (cloud_->get_channels_size ());
00172 
00173     // Create the channels
00174     for (unsigned int d = 0; d < projected_points.get_channels_size (); d++)
00175     {
00176       projected_points.channels[d].name = cloud_->channels[d].name;
00177       projected_points.channels[d].values.resize (inliers.size ());
00178     }
00179 
00180     // Get the plane normal
00181     // Calculate the 2-norm: norm (x) = sqrt (sum (abs (v)^2))
00182     //double n_norm = sqrt (model_coefficients.at (0) * model_coefficients.at (0) +
00183     //                      model_coefficients.at (1) * model_coefficients.at (1) +
00184     //                      model_coefficients.at (2) * model_coefficients.at (2));
00185     //model_coefficients.at (0) /= n_norm;
00186     //model_coefficients.at (1) /= n_norm;
00187     //model_coefficients.at (2) /= n_norm;
00188     //model_coefficients.at (3) /= n_norm;
00189 
00190     // Iterate through the 3d points and calculate the distances from them to the plane
00191     for (unsigned int i = 0; i < inliers.size (); i++)
00192     {
00193       // Calculate the distance from the point to the plane
00194       double distance_to_plane = model_coefficients.at (0) * cloud_->points.at (inliers.at (i)).x +
00195                                  model_coefficients.at (1) * cloud_->points.at (inliers.at (i)).y +
00196                                  model_coefficients.at (2) * cloud_->points.at (inliers.at (i)).z +
00197                                  model_coefficients.at (3) * 1;
00198       // Calculate the projection of the point on the plane
00199       projected_points.points[i].x = cloud_->points.at (inliers.at (i)).x - distance_to_plane * model_coefficients.at (0);
00200       projected_points.points[i].y = cloud_->points.at (inliers.at (i)).y - distance_to_plane * model_coefficients.at (1);
00201       projected_points.points[i].z = cloud_->points.at (inliers.at (i)).z - distance_to_plane * model_coefficients.at (2);
00202       // Copy the other attributes
00203       for (unsigned int d = 0; d < projected_points.get_channels_size (); d++)
00204         projected_points.channels[d].values[i] = cloud_->channels[d].values[inliers.at (i)];
00205     }
00206   }
00207 
00209 
00213   void
00214     SACModelPlane::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
00215   {
00216     // Get the plane normal
00217     // Calculate the 2-norm: norm (x) = sqrt (sum (abs (v)^2))
00218     //double n_norm = sqrt (model_coefficients.at (0) * model_coefficients.at (0) +
00219     //                      model_coefficients.at (1) * model_coefficients.at (1) +
00220     //                      model_coefficients.at (2) * model_coefficients.at (2));
00221     //model_coefficients.at (0) /= n_norm;
00222     //model_coefficients.at (1) /= n_norm;
00223     //model_coefficients.at (2) /= n_norm;
00224     //model_coefficients.at (3) /= n_norm;
00225 
00226     // Iterate through the 3d points and calculate the distances from them to the plane
00227     for (unsigned int i = 0; i < inliers.size (); i++)
00228     {
00229       // Calculate the distance from the point to the plane
00230       double distance_to_plane = model_coefficients.at (0) * cloud_->points.at (inliers.at (i)).x +
00231                                  model_coefficients.at (1) * cloud_->points.at (inliers.at (i)).y +
00232                                  model_coefficients.at (2) * cloud_->points.at (inliers.at (i)).z +
00233                                  model_coefficients.at (3) * 1;
00234       // Calculate the projection of the point on the plane
00235       cloud_->points.at (inliers.at (i)).x = cloud_->points.at (inliers.at (i)).x - distance_to_plane * model_coefficients.at (0);
00236       cloud_->points.at (inliers.at (i)).y = cloud_->points.at (inliers.at (i)).y - distance_to_plane * model_coefficients.at (1);
00237       cloud_->points.at (inliers.at (i)).z = cloud_->points.at (inliers.at (i)).z - distance_to_plane * model_coefficients.at (2);
00238     }
00239   }
00240 
00242 
00247   bool
00248     SACModelPlane::computeModelCoefficients (const std::vector<int> &samples)
00249   {
00250     model_coefficients_.resize (4);
00251     double Dx1, Dy1, Dz1, Dx2, Dy2, Dz2, Dy1Dy2;
00252     // Compute the segment values (in 3d) between XY
00253     Dx1 = cloud_->points.at (samples.at (1)).x - cloud_->points.at (samples.at (0)).x;
00254     Dy1 = cloud_->points.at (samples.at (1)).y - cloud_->points.at (samples.at (0)).y;
00255     Dz1 = cloud_->points.at (samples.at (1)).z - cloud_->points.at (samples.at (0)).z;
00256 
00257     // Compute the segment values (in 3d) between XZ
00258     Dx2 = cloud_->points.at (samples.at (2)).x - cloud_->points.at (samples.at (0)).x;
00259     Dy2 = cloud_->points.at (samples.at (2)).y - cloud_->points.at (samples.at (0)).y;
00260     Dz2 = cloud_->points.at (samples.at (2)).z - cloud_->points.at (samples.at (0)).z;
00261 
00262     Dy1Dy2 = Dy1 / Dy2;
00263     if (((Dx1 / Dx2) == Dy1Dy2) && (Dy1Dy2 == (Dz1 / Dz2)))     // Check for collinearity
00264       return (false);
00265 
00266     // Compute the plane coefficients from the 3 given points in a straightforward manner
00267     // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
00268     model_coefficients_[0] = (cloud_->points.at (samples.at (1)).y - cloud_->points.at (samples.at (0)).y) *
00269                              (cloud_->points.at (samples.at (2)).z - cloud_->points.at (samples.at (0)).z) -
00270                              (cloud_->points.at (samples.at (1)).z - cloud_->points.at (samples.at (0)).z) *
00271                              (cloud_->points.at (samples.at (2)).y - cloud_->points.at (samples.at (0)).y);
00272 
00273     model_coefficients_[1] = (cloud_->points.at (samples.at (1)).z - cloud_->points.at (samples.at (0)).z) *
00274                              (cloud_->points.at (samples.at (2)).x - cloud_->points.at (samples.at (0)).x) -
00275                              (cloud_->points.at (samples.at (1)).x - cloud_->points.at (samples.at (0)).x) *
00276                              (cloud_->points.at (samples.at (2)).z - cloud_->points.at (samples.at (0)).z);
00277 
00278     model_coefficients_[2] = (cloud_->points.at (samples.at (1)).x - cloud_->points.at (samples.at (0)).x) *
00279                              (cloud_->points.at (samples.at (2)).y - cloud_->points.at (samples.at (0)).y) -
00280                              (cloud_->points.at (samples.at (1)).y - cloud_->points.at (samples.at (0)).y) *
00281                              (cloud_->points.at (samples.at (2)).x - cloud_->points.at (samples.at (0)).x);
00282     // calculate the 2-norm: norm (x) = sqrt (sum (abs (v)^2))
00283     // nx ny nz (aka: ax + by + cz ...
00284     double n_norm = sqrt (model_coefficients_[0] * model_coefficients_[0] +
00285                           model_coefficients_[1] * model_coefficients_[1] +
00286                           model_coefficients_[2] * model_coefficients_[2]);
00287     model_coefficients_[0] /= n_norm;
00288     model_coefficients_[1] /= n_norm;
00289     model_coefficients_[2] /= n_norm;
00290 
00291     // ... + d = 0
00292     model_coefficients_[3] = -1 * (model_coefficients_[0] * cloud_->points.at (samples.at (0)).x +
00293                                    model_coefficients_[1] * cloud_->points.at (samples.at (0)).y +
00294                                    model_coefficients_[2] * cloud_->points.at (samples.at (0)).z);
00295 
00296     return (true);
00297   }
00298 
00300 
00305   void
00306     SACModelPlane::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
00307   {
00308     if (inliers.size () == 0)
00309     {
00310       ROS_ERROR ("[SACModelPlane::RefitModel] Cannot re-fit 0 inliers!");
00311       refit_coefficients = model_coefficients_;
00312       return;
00313     }
00314 
00315     Eigen::Vector4d plane_coefficients;
00316     double curvature;
00317 
00318     // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
00319     cloud_geometry::nearest::computePointNormal (*cloud_, inliers, plane_coefficients, curvature);
00320 
00321     refit_coefficients.resize (4);
00322     for (int d = 0; d < 4; d++)
00323       refit_coefficients[d] = plane_coefficients (d);
00324   }
00325 
00327 
00331   bool
00332     SACModelPlane::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
00333   {
00334     for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
00335       if (fabs (model_coefficients_.at (0) * cloud_->points.at (*it).x +
00336                 model_coefficients_.at (1) * cloud_->points.at (*it).y +
00337                 model_coefficients_.at (2) * cloud_->points.at (*it).z +
00338                 model_coefficients_.at (3)) > threshold)
00339         return (false);
00340 
00341     return (true);
00342   }
00343 }