semantic_point_annotator: semantic_point_annotator: sac_model

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     geometry_msgs::Point32 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       geometry_msgs::Point32 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     geometry_msgs::Point32 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       geometry_msgs::Point32 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                                  sensor_msgs::PointCloud &projected_points)
00174   {
00175     // Allocate enough space
00176     projected_points.points.resize (inliers.size ());
00177     projected_points.channels.resize (cloud_->channels.size ());
00178 
00179     // Create the channels
00180     for (unsigned int d = 0; d < projected_points.channels.size (); d++)
00181     {
00182       projected_points.channels[d].name = cloud_->channels[d].name;
00183       projected_points.channels[d].values.resize (inliers.size ());
00184     }
00185 
00186     // Compute the line direction (P2 - P1)
00187     geometry_msgs::Point32 p21;
00188     p21.x = model_coefficients.at (3) - model_coefficients.at (0);
00189     p21.y = model_coefficients.at (4) - model_coefficients.at (1);
00190     p21.z = model_coefficients.at (5) - model_coefficients.at (2);
00191 
00192     // Iterate through the 3d points and calculate the distances from them to the line
00193     for (unsigned int i = 0; i < inliers.size (); i++)
00194     {
00195       // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
00196       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 -
00197                    (model_coefficients_[0] * p21.x + model_coefficients_[1] * p21.y + model_coefficients_[2] * p21.z)
00198                  ) / (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z);
00199       // Calculate the projection of the point on the line (pointProj = A + k * B)
00200       projected_points.points[i].x = model_coefficients_.at (0) + k * p21.x;
00201       projected_points.points[i].y = model_coefficients_.at (1) + k * p21.y;
00202       projected_points.points[i].z = model_coefficients_.at (2) + k * p21.z;
00203       // Copy the other attributes
00204       for (unsigned int d = 0; d < projected_points.channels.size (); d++)
00205         projected_points.channels[d].values[i] = cloud_->channels[d].values[inliers.at (i)];
00206     }
00207   }
00208 
00210 
00214   void
00215     SACModelLine::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
00216   {
00217     // Compute the line direction (P2 - P1)
00218     geometry_msgs::Point32 p21;
00219     p21.x = model_coefficients.at (3) - model_coefficients.at (0);
00220     p21.y = model_coefficients.at (4) - model_coefficients.at (1);
00221     p21.z = model_coefficients.at (5) - model_coefficients.at (2);
00222 
00223     // Iterate through the 3d points and calculate the distances from them to the line
00224     for (unsigned int i = 0; i < inliers.size (); i++)
00225     {
00226       // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
00227       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 -
00228                    (model_coefficients_[0] * p21.x + model_coefficients_[1] * p21.y + model_coefficients_[2] * p21.z)
00229                  ) / (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z);
00230       // Calculate the projection of the point on the line (pointProj = A + k * B)
00231       cloud_->points.at (inliers.at (i)).x = model_coefficients_.at (0) + k * p21.x;
00232       cloud_->points.at (inliers.at (i)).y = model_coefficients_.at (1) + k * p21.y;
00233       cloud_->points.at (inliers.at (i)).z = model_coefficients_.at (2) + k * p21.z;
00234     }
00235   }
00236 
00238 
00243   bool
00244     SACModelLine::computeModelCoefficients (const std::vector<int> &samples)
00245   {
00246     model_coefficients_.resize (6);
00247     model_coefficients_[0] = cloud_->points.at (samples.at (0)).x;
00248     model_coefficients_[1] = cloud_->points.at (samples.at (0)).y;
00249     model_coefficients_[2] = cloud_->points.at (samples.at (0)).z;
00250     model_coefficients_[3] = cloud_->points.at (samples.at (1)).x;
00251     model_coefficients_[4] = cloud_->points.at (samples.at (1)).y;
00252     model_coefficients_[5] = cloud_->points.at (samples.at (1)).z;
00253 
00254     return (true);
00255   }
00256 
00258 
00263   void
00264     SACModelLine::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
00265   {
00266     if (inliers.size () == 0)
00267     {
00268       ROS_ERROR ("[SACModelLine::RefitModel] Cannot re-fit 0 inliers!");
00269       refit_coefficients = model_coefficients_;
00270       return;
00271     }
00272 
00273     refit_coefficients.resize (6);
00274 
00275     // Compute the centroid of the samples
00276     geometry_msgs::Point32 centroid;
00277     // Compute the 3x3 covariance matrix
00278     Eigen::Matrix3d covariance_matrix;
00279     computeCovarianceMatrix (*cloud_, inliers, covariance_matrix, centroid);
00280 
00281     refit_coefficients[0] = centroid.x;
00282     refit_coefficients[1] = centroid.y;
00283     refit_coefficients[2] = centroid.z;
00284 
00285     // Extract the eigenvalues and eigenvectors
00286     //cloud_geometry::eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
00287     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix.cast<float> ());
00288     Eigen::Vector3f eigen_values  = ei_symm.eigenvalues ();
00289     Eigen::Matrix3f eigen_vectors = ei_symm.eigenvectors ();
00290 
00291     refit_coefficients[3] = eigen_vectors (0, 2) + refit_coefficients[0];
00292     refit_coefficients[4] = eigen_vectors (1, 2) + refit_coefficients[1];
00293     refit_coefficients[5] = eigen_vectors (2, 2) + refit_coefficients[2];
00294   }
00295 
00297 
00301   bool
00302     SACModelLine::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
00303   {
00304     double sqr_threshold = threshold * threshold;
00305     for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
00306     {
00307       geometry_msgs::Point32 p3, p4;
00308       p3.x = model_coefficients_.at (3) - model_coefficients_.at (0);
00309       p3.y = model_coefficients_.at (4) - model_coefficients_.at (1);
00310       p3.z = model_coefficients_.at (5) - model_coefficients_.at (2);
00311 
00312       p4.x = model_coefficients_.at (3) - cloud_->points.at (*it).x;
00313       p4.y = model_coefficients_.at (4) - cloud_->points.at (*it).y;
00314       p4.z = model_coefficients_.at (5) - cloud_->points.at (*it).z;
00315 
00316       geometry_msgs::Point32 c = cross (p4, p3);
00317       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);
00318 
00319       if (sqr_distance < sqr_threshold)
00320         return (false);
00321     }
00322     return (true);
00323   }
00324 }