sac_model_line.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: sac_model_line.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
00028  *
00029  */
00030 
00036 #include <Eigen/Core>
00037 #include <Eigen/QR>
00038 #include <Eigen/Eigenvalues>
00039 #include <sac_model_line.h>
00040 
00041 #include <ros/console.h>
00042 
00043 namespace sample_consensus
00044 {
00046 
00051   void
00052     SACModelLine::getSamples (int &iterations, std::vector<int> &samples)
00053   {
00054     samples.resize (2);
00055     double trand = indices_.size () / (RAND_MAX + 1.0);
00056 
00057     // Get a random number between 1 and max_indices
00058     int idx = (int)(rand () * trand);
00059     // Get the index
00060     samples[0] = indices_.at (idx);
00061 
00062     // Get a second point which is different than the first
00063     int iter = 0;
00064     do
00065     {
00066       idx = (int)(rand () * trand);
00067       samples[1] = indices_.at (idx);
00068       iter++;
00069 
00070       if (iter > MAX_ITERATIONS_UNIQUE)
00071       {
00072         ROS_WARN ("[SACModelLine::getSamples] WARNING: Could not select 2 unique points in %d iterations!", MAX_ITERATIONS_UNIQUE);
00073         break;
00074       }
00075       iterations++;
00076     } while (samples[1] == samples[0]);
00077     iterations--;
00078     return;
00079   }
00080 
00082 
00089   void
00090     SACModelLine::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
00091   {
00092     double sqr_threshold = threshold * threshold;
00093 
00094     int nr_p = 0;
00095     inliers.resize (indices_.size ());
00096 
00097     // Obtain the line direction
00098     pcl::PointXYZ p3, p4;
00099     p3.x = model_coefficients.at (3) - model_coefficients.at (0);
00100     p3.y = model_coefficients.at (4) - model_coefficients.at (1);
00101     p3.z = model_coefficients.at (5) - model_coefficients.at (2);
00102 
00103     // Iterate through the 3d points and calculate the distances from them to the plane
00104     for (unsigned int i = 0; i < indices_.size (); i++)
00105     {
00106       // Calculate the distance from the point to the line
00107       // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
00108       // P1, P2 = line points, P0 = query point
00109       // P1P2 = <x2-x1, y2-y1, z2-z1> = <x3, y3, z3>
00110       // P1P0 = < x-x1,  y-y1, z-z1 > = <x4, y4, z4>
00111       // P1P2 x P1P0 = < y3*z4 - z3*y4, -(x3*z4 - x4*z3), x3*y4 - x4*y3 >
00112       //             = < (y2-y1)*(z-z1) - (z2-z1)*(y-y1), -[(x2-x1)*(z-z1) - (x-x1)*(z2-z1)], (x2-x1)*(y-y1) - (x-x1)*(y2-y1) >
00113       p4.x = model_coefficients.at (3) - cloud_->points.at (indices_.at (i)).x;
00114       p4.y = model_coefficients.at (4) - cloud_->points.at (indices_.at (i)).y;
00115       p4.z = model_coefficients.at (5) - cloud_->points.at (indices_.at (i)).z;
00116 
00117       // P1P2 = sqrt (x3^2 + y3^2 + z3^2)
00118       // a = sqrt [(y3*z4 - z3*y4)^2 + (x3*z4 - x4*z3)^2 + (x3*y4 - x4*y3)^2]
00119       //double distance = SQR_NORM (cANN::cross (p4, p3)) / SQR_NORM (p3);
00120       pcl::PointXYZ c = cross (p4, p3);
00121       double sqr_distance = (c.x * c.x + c.y * c.y + c.z * c.z) / (p3.x * p3.x + p3.y * p3.y + p3.z * p3.z);
00122 
00123       if (sqr_distance < sqr_threshold)
00124       {
00125         // Returns the indices of the points whose squared distances are smaller than the threshold
00126         inliers[nr_p] = indices_[i];
00127         nr_p++;
00128       }
00129     }
00130     inliers.resize (nr_p);
00131     return;
00132   }
00133 
00135 
00139   void
00140     SACModelLine::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
00141   {
00142     distances.resize (indices_.size ());
00143 
00144     // Obtain the line direction
00145     pcl::PointXYZ p3, p4;
00146     p3.x = model_coefficients.at (3) - model_coefficients.at (0);
00147     p3.y = model_coefficients.at (4) - model_coefficients.at (1);
00148     p3.z = model_coefficients.at (5) - model_coefficients.at (2);
00149 
00150     // Iterate through the 3d points and calculate the distances from them to the plane
00151     for (unsigned int i = 0; i < indices_.size (); i++)
00152     {
00153       // Calculate the distance from the point to the line
00154       // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
00155       p4.x = model_coefficients.at (3) - cloud_->points.at (indices_.at (i)).x;
00156       p4.y = model_coefficients.at (4) - cloud_->points.at (indices_.at (i)).y;
00157       p4.z = model_coefficients.at (5) - cloud_->points.at (indices_.at (i)).z;
00158 
00159       pcl::PointXYZ c = cross (p4, p3);
00160       distances[i] = sqrt (c.x * c.x + c.y * c.y + c.z * c.z) / (p3.x * p3.x + p3.y * p3.y + p3.z * p3.z);
00161     }
00162     return;
00163   }
00164 
00166 
00171   void
00172     SACModelLine::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
00173                                  PointCloud &projected_points)
00174   {
00175     // Allocate enough space
00176     projected_points.points.resize (inliers.size ());
00177 
00178     // Compute the line direction (P2 - P1)
00179     pcl::PointXYZ p21;
00180     p21.x = model_coefficients.at (3) - model_coefficients.at (0);
00181     p21.y = model_coefficients.at (4) - model_coefficients.at (1);
00182     p21.z = model_coefficients.at (5) - model_coefficients.at (2);
00183 
00184     // Iterate through the 3d points and calculate the distances from them to the line
00185     for (unsigned int i = 0; i < inliers.size (); i++)
00186     {
00187       // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
00188       double k = ( cloud_->points.at (inliers.at (i)).x * p21.x + cloud_->points.at (inliers.at (i)).y * p21.y + cloud_->points.at (inliers.at (i)).z * p21.z -
00189                    (model_coefficients_[0] * p21.x + model_coefficients_[1] * p21.y + model_coefficients_[2] * p21.z)
00190                  ) / (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z);
00191       // Calculate the projection of the point on the line (pointProj = A + k * B)
00192       projected_points.points[i].x = model_coefficients_.at (0) + k * p21.x;
00193       projected_points.points[i].y = model_coefficients_.at (1) + k * p21.y;
00194       projected_points.points[i].z = model_coefficients_.at (2) + k * p21.z;
00195       // Copy the other attributes
00196     }
00197   }
00198 
00200 
00204   void
00205     SACModelLine::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
00206   {
00207     // Compute the line direction (P2 - P1)
00208     pcl::PointXYZ p21;
00209     p21.x = model_coefficients.at (3) - model_coefficients.at (0);
00210     p21.y = model_coefficients.at (4) - model_coefficients.at (1);
00211     p21.z = model_coefficients.at (5) - model_coefficients.at (2);
00212 
00213     // Iterate through the 3d points and calculate the distances from them to the line
00214     for (unsigned int i = 0; i < inliers.size (); i++)
00215     {
00216       // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
00217       double k = ( cloud_->points.at (inliers.at (i)).x * p21.x + cloud_->points.at (inliers.at (i)).y * p21.y + cloud_->points.at (inliers.at (i)).z * p21.z -
00218                    (model_coefficients_[0] * p21.x + model_coefficients_[1] * p21.y + model_coefficients_[2] * p21.z)
00219                  ) / (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z);
00220       // Calculate the projection of the point on the line (pointProj = A + k * B)
00221       cloud_->points.at (inliers.at (i)).x = model_coefficients_.at (0) + k * p21.x;
00222       cloud_->points.at (inliers.at (i)).y = model_coefficients_.at (1) + k * p21.y;
00223       cloud_->points.at (inliers.at (i)).z = model_coefficients_.at (2) + k * p21.z;
00224     }
00225   }
00226 
00228 
00233   bool
00234     SACModelLine::computeModelCoefficients (const std::vector<int> &samples)
00235   {
00236     model_coefficients_.resize (6);
00237     model_coefficients_[0] = cloud_->points.at (samples.at (0)).x;
00238     model_coefficients_[1] = cloud_->points.at (samples.at (0)).y;
00239     model_coefficients_[2] = cloud_->points.at (samples.at (0)).z;
00240     model_coefficients_[3] = cloud_->points.at (samples.at (1)).x;
00241     model_coefficients_[4] = cloud_->points.at (samples.at (1)).y;
00242     model_coefficients_[5] = cloud_->points.at (samples.at (1)).z;
00243 
00244     return (true);
00245   }
00246 
00248 
00253   void
00254     SACModelLine::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
00255   {
00256     if (inliers.size () == 0)
00257     {
00258       ROS_ERROR ("[SACModelLine::RefitModel] Cannot re-fit 0 inliers!");
00259       refit_coefficients = model_coefficients_;
00260       return;
00261     }
00262 
00263     refit_coefficients.resize (6);
00264 
00265     // Compute the centroid of the samples
00266     pcl::PointXYZ centroid;
00267     // Compute the 3x3 covariance matrix
00268     Eigen::Matrix3d covariance_matrix;
00269     computeCovarianceMatrix (*cloud_, inliers, covariance_matrix, centroid);
00270 
00271     refit_coefficients[0] = centroid.x;
00272     refit_coefficients[1] = centroid.y;
00273     refit_coefficients[2] = centroid.z;
00274 
00275     // Extract the eigenvalues and eigenvectors
00276     //cloud_geometry::eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
00277     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix.cast<float> ());
00278     Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00279     Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00280 
00281     refit_coefficients[3] = eigen_vectors (0, 2) + refit_coefficients[0];
00282     refit_coefficients[4] = eigen_vectors (1, 2) + refit_coefficients[1];
00283     refit_coefficients[5] = eigen_vectors (2, 2) + refit_coefficients[2];
00284   }
00285 
00287 
00291   bool
00292     SACModelLine::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
00293   {
00294     double sqr_threshold = threshold * threshold;
00295     for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
00296     {
00297       pcl::PointXYZ p3, p4;
00298       p3.x = model_coefficients_.at (3) - model_coefficients_.at (0);
00299       p3.y = model_coefficients_.at (4) - model_coefficients_.at (1);
00300       p3.z = model_coefficients_.at (5) - model_coefficients_.at (2);
00301 
00302       p4.x = model_coefficients_.at (3) - cloud_->points.at (*it).x;
00303       p4.y = model_coefficients_.at (4) - cloud_->points.at (*it).y;
00304       p4.z = model_coefficients_.at (5) - cloud_->points.at (*it).z;
00305 
00306       pcl::PointXYZ c = cross (p4, p3);
00307       double sqr_distance = (c.x * c.x + c.y * c.y + c.z * c.z) / (p3.x * p3.x + p3.y * p3.y + p3.z * p3.z);
00308 
00309       if (sqr_distance < sqr_threshold)
00310         return (false);
00311     }
00312     return (true);
00313   }
00314 }


semantic_point_annotator
Author(s): Radu Bogdan Rusu
autogenerated on Mon Dec 2 2013 13:19:40