00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
00070 for (unsigned int i = 0; i < indices_.size (); i++)
00071 {
00072
00073
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
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
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
00105
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
00114 for (unsigned int i = 0; i < indices_.size (); i++)
00115
00116
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 }