00001 /* 00002 * Copyright (c) 2009 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_oriented_plane.cpp 10961 2009-02-11 00:20:50Z tfoote $ 00028 * 00029 */ 00030 00033 #include <door_handle_detector/sample_consensus/sac_model_oriented_plane.h> 00034 #include <door_handle_detector/geometry/angles.h> 00035 #include <door_handle_detector/geometry/point.h> 00036 #include <door_handle_detector/geometry/nearest.h> 00037 00038 namespace sample_consensus 00039 { 00041 00048 void 00049 SACModelOrientedPlane::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers) 00050 { 00051 int nr_p = 0; 00052 00053 // Obtain the plane normal 00054 geometry_msgs::Point32 n; 00055 n.x = model_coefficients.at (0); 00056 n.y = model_coefficients.at (1); 00057 n.z = model_coefficients.at (2); 00058 00059 double angle_error = cloud_geometry::angles::getAngle3D (axis_, n); 00060 00061 // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis 00062 if ( (angle_error > eps_angle_) && ( (M_PI - angle_error) > eps_angle_ ) ) 00063 { 00064 inliers.resize (0); 00065 return; 00066 } 00067 00068 inliers.resize (indices_.size ()); 00069 // Iterate through the 3d points and calculate the distances from them to the plane 00070 for (unsigned int i = 0; i < indices_.size (); i++) 00071 { 00072 // Calculate the distance from the point to the plane normal as the dot product 00073 // D = (P-A).N/|N| 00074 if (fabs (model_coefficients.at (0) * cloud_->points.at (indices_.at (i)).x + 00075 model_coefficients.at (1) * cloud_->points.at (indices_.at (i)).y + 00076 model_coefficients.at (2) * cloud_->points.at (indices_.at (i)).z + 00077 model_coefficients.at (3)) < threshold) 00078 { 00079 // Returns the indices of the points whose distances are smaller than the threshold 00080 inliers[nr_p] = indices_[i]; 00081 nr_p++; 00082 } 00083 } 00084 inliers.resize (nr_p); 00085 return; 00086 } 00087 00089 00093 void 00094 SACModelOrientedPlane::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances) 00095 { 00096 // Obtain the plane normal 00097 geometry_msgs::Point32 n; 00098 n.x = model_coefficients.at (0); 00099 n.y = model_coefficients.at (1); 00100 n.z = model_coefficients.at (2); 00101 00102 double angle_error = cloud_geometry::angles::getAngle3D (axis_, n); 00103 00104 // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis 00105 // TODO: check this 00106 if ( (angle_error > eps_angle_) && ( (M_PI - angle_error) > eps_angle_ ) ) 00107 { 00108 distances.resize (0); 00109 return; 00110 } 00111 00112 distances.resize (indices_.size ()); 00113 // Iterate through the 3d points and calculate the distances from them to the plane 00114 for (unsigned int i = 0; i < indices_.size (); i++) 00115 // Calculate the distance from the point to the plane normal as the dot product 00116 // D = (P-A).N/|N| 00117 distances[i] = fabs (model_coefficients.at (0) * cloud_->points.at (indices_[i]).x + 00118 model_coefficients.at (1) * cloud_->points.at (indices_[i]).y + 00119 model_coefficients.at (2) * cloud_->points.at (indices_[i]).z + 00120 model_coefficients.at (3)); 00121 return; 00122 } 00123 00124 }