Go to the documentation of this file.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
00036 #include <door_handle_detector/sample_consensus/sac_model_oriented_line.h>
00037 #include <door_handle_detector/geometry/angles.h>
00038 #include <door_handle_detector/geometry/point.h>
00039 #include <door_handle_detector/geometry/nearest.h>
00040
00041 namespace sample_consensus
00042 {
00044
00051 void
00052 SACModelOrientedLine::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
00053 {
00054 double sqr_threshold = threshold * threshold;
00055
00056
00057 geometry_msgs::Point32 p3, p4;
00058 p3.x = model_coefficients.at (3) - model_coefficients.at (0);
00059 p3.y = model_coefficients.at (4) - model_coefficients.at (1);
00060 p3.z = model_coefficients.at (5) - model_coefficients.at (2);
00061
00062 double angle_error = cloud_geometry::angles::getAngle3D (axis_, p3);
00063
00064
00065 if (angle_error > eps_angle_)
00066 {
00067 inliers.resize (0);
00068 return;
00069 }
00070
00071 int nr_p = 0;
00072 inliers.resize (indices_.size ());
00073
00074 for (unsigned int i = 0; i < indices_.size (); i++)
00075 {
00076
00077
00078
00079
00080
00081
00082
00083 p4.x = model_coefficients.at (3) - cloud_->points.at (indices_.at (i)).x;
00084 p4.y = model_coefficients.at (4) - cloud_->points.at (indices_.at (i)).y;
00085 p4.z = model_coefficients.at (5) - cloud_->points.at (indices_.at (i)).z;
00086
00087
00088
00089
00090 geometry_msgs::Point32 c = cloud_geometry::cross (p4, p3);
00091 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);
00092
00093 if (sqr_distance < sqr_threshold)
00094 {
00095
00096 inliers[nr_p] = indices_[i];
00097 nr_p++;
00098 }
00099 }
00100 inliers.resize (nr_p);
00101 return;
00102 }
00103
00105
00109 void
00110 SACModelOrientedLine::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
00111 {
00112
00113 geometry_msgs::Point32 p3, p4;
00114 p3.x = model_coefficients.at (3) - model_coefficients.at (0);
00115 p3.y = model_coefficients.at (4) - model_coefficients.at (1);
00116 p3.z = model_coefficients.at (5) - model_coefficients.at (2);
00117
00118 double angle_error = cloud_geometry::angles::getAngle3D (axis_, p3);
00119
00120
00121 if (angle_error > eps_angle_)
00122 {
00123 distances.resize (0);
00124 return;
00125 }
00126
00127 distances.resize (indices_.size ());
00128
00129 for (unsigned int i = 0; i < indices_.size (); i++)
00130 {
00131
00132
00133 p4.x = model_coefficients.at (3) - cloud_->points.at (indices_.at (i)).x;
00134 p4.y = model_coefficients.at (4) - cloud_->points.at (indices_.at (i)).y;
00135 p4.z = model_coefficients.at (5) - cloud_->points.at (indices_.at (i)).z;
00136
00137 geometry_msgs::Point32 c = cloud_geometry::cross (p4, p3);
00138 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);
00139 }
00140 return;
00141 }
00142
00143 }