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
00031
00032
00035 #include <door_handle_detector/sample_consensus/sac_model_parallel_lines.h>
00036 #include <door_handle_detector/geometry/point.h>
00037 #include <door_handle_detector/geometry/nearest.h>
00038
00039 using namespace std;
00040
00041 namespace sample_consensus
00042 {
00043
00045
00051 double
00052 SACModelParallelLines::pointToLineSquareDistance (const geometry_msgs::Point32 &line_point1, const geometry_msgs::Point32 &line_point2, const geometry_msgs::Point32 &point)
00053 {
00054 geometry_msgs::Point32 v12, v1p;
00055 v12.x = line_point2.x - line_point1.x;
00056 v12.y = line_point2.y - line_point1.y;
00057 v12.z = line_point2.z - line_point1.z;
00058 v1p.x = point.x - line_point1.x;
00059 v1p.y = point.y - line_point1.y;
00060 v1p.z = point.z - line_point1.z;
00061 geometry_msgs::Point32 c = cloud_geometry::cross (v12, v1p);
00062 double sqr_distance = (c.x * c.x + c.y * c.y + c.z * c.z) / (v12.x * v12.x + v12.y * v12.y + v12.z * v12.z);
00063 return (sqr_distance);
00064 }
00065
00066
00068
00075 void
00076 SACModelParallelLines::closestLine (const std::vector<int> &indices, const std::vector<double> &model_coefficients,
00077 std::vector<int> *closest_line, std::vector<double> *closest_dist)
00078 {
00079 int end = indices.size ();
00080 geometry_msgs::Point32 d1, d2, l1, c1, c2;
00081 l1.x = model_coefficients[3] - model_coefficients[0];
00082 l1.y = model_coefficients[4] - model_coefficients[1];
00083 l1.z = model_coefficients[5] - model_coefficients[2];
00084 double l_sqr_length = (l1.x * l1.x + l1.y * l1.y + l1.z * l1.z);
00085 double sqr_distance1, sqr_distance2;
00086 for (int i = 0; i < end; i++)
00087 {
00088
00089 d1.x = cloud_->points[indices[i]].x - model_coefficients[0];
00090 d1.y = cloud_->points[indices[i]].y - model_coefficients[1];
00091 d1.z = cloud_->points[indices[i]].z - model_coefficients[2];
00092 d2.x = cloud_->points[indices[i]].x - model_coefficients[6];
00093 d2.y = cloud_->points[indices[i]].y - model_coefficients[7];
00094 d2.z = cloud_->points[indices[i]].z - model_coefficients[8];
00095
00096 c1 = cloud_geometry::cross (l1, d1);
00097 sqr_distance1 = (c1.x * c1.x + c1.y * c1.y + c1.z * c1.z);
00098 c2 = cloud_geometry::cross (l1, d2);
00099 sqr_distance2 = (c2.x * c2.x + c2.y * c2.y + c2.z * c2.z);
00100
00101 if (sqr_distance1 < sqr_distance2)
00102 {
00103 (*closest_line)[i] = 0;
00104 (*closest_dist)[i] = sqrt (sqr_distance1 / l_sqr_length);
00105 }
00106 else
00107 {
00108 (*closest_line)[i] = 1;
00109 (*closest_dist)[i] = sqrt (sqr_distance2 / l_sqr_length);
00110 }
00111 }
00112 }
00113
00114 void
00115 SACModelParallelLines::closestLine (const std::set<int> &indices, const std::vector<double> &model_coefficients,
00116 std::vector<int> *closest_line, std::vector<double> *closest_dist)
00117 {
00118 std::set<int>::iterator end = indices.end ();
00119 geometry_msgs::Point32 d1, d2, l1, c1, c2;
00120 l1.x = model_coefficients[3] - model_coefficients[0];
00121 l1.y = model_coefficients[4] - model_coefficients[1];
00122 l1.z = model_coefficients[5] - model_coefficients[2];
00123 double l_sqr_length = (l1.x * l1.x + l1.y * l1.y + l1.z * l1.z);
00124 double sqr_distance1, sqr_distance2;
00125 int i = 0;
00126 for (std::set<int>::iterator it = indices.begin (); it != end; ++it)
00127 {
00128
00129 d1.x = cloud_->points[*it].x - model_coefficients[0];
00130 d1.y = cloud_->points[*it].y - model_coefficients[1];
00131 d1.z = cloud_->points[*it].z - model_coefficients[2];
00132 d2.x = cloud_->points[*it].x - model_coefficients[6];
00133 d2.y = cloud_->points[*it].y - model_coefficients[7];
00134 d2.z = cloud_->points[*it].z - model_coefficients[8];
00135
00136 c1 = cloud_geometry::cross (l1, d1);
00137 sqr_distance1 = (c1.x * c1.x + c1.y * c1.y + c1.z * c1.z);
00138 c2 = cloud_geometry::cross (l1, d2);
00139 sqr_distance2 = (c2.x * c2.x + c2.y * c2.y + c2.z * c2.z);
00140
00141 if (sqr_distance1 < sqr_distance2)
00142 {
00143 (*closest_line)[i] = 0;
00144 (*closest_dist)[i] = sqrt (sqr_distance1 / l_sqr_length);
00145 }
00146 else
00147 {
00148 (*closest_line)[i] = 1;
00149 (*closest_dist)[i] = sqrt (sqr_distance2 / l_sqr_length);
00150 }
00151 ++i;
00152 }
00153 }
00154
00156
00161 void
00162 SACModelParallelLines::getSamples (int &iterations, std::vector<int> &samples)
00163 {
00164 std::vector<int> random_idx (3);
00165 double trand = indices_.size () / (RAND_MAX + 1.0);
00166
00167
00168 int idx = (int)(rand()*trand);
00169
00170 random_idx[0] = indices_[idx];
00171
00172
00173 int iter = 0;
00174 int total_points = 1;
00175
00176 double sqr_min_line_sep_m = min_line_sep_m_ * min_line_sep_m_;
00177 double sqr_max_line_sep_m = max_line_sep_m_ * max_line_sep_m_;
00178 double sqr_distance;
00179 int r0 = 0, r1 = 1, r2 = 2;
00180
00181 while (total_points < 3)
00182 {
00183 iter++;
00184
00185 if (iter > MAX_ITERATIONS_UNIQUE)
00186 {
00187 ROS_WARN ("[SACModelParallelLines::getSamples] WARNING: Could not select 3 unique, non-colinear points in %d iterations!", MAX_ITERATIONS_UNIQUE);
00188 break;
00189 }
00190
00191 idx = (int)(rand ()*trand);
00192 random_idx[total_points] = indices_[idx];
00193
00194 if ( (random_idx[total_points] == random_idx[0]) || ((total_points==2) && random_idx[total_points] == random_idx[1]) )
00195 {
00196 continue;
00197 }
00198
00199
00200 if (total_points == 2)
00201 {
00202
00203 r1 = 0;
00204 for (r0 = 0; r0 < 3; r0++)
00205 {
00206 r1 = (r0+1)%3;
00207 r2 = (r1+1)%3;
00208 sqr_distance = pointToLineSquareDistance (cloud_->points[random_idx[r0]], cloud_->points[random_idx[r1]], cloud_->points[random_idx[r2]]);
00209
00210 if (sqr_distance == 0.0 || sqr_distance < sqr_min_line_sep_m || sqr_distance > sqr_max_line_sep_m)
00211 continue;
00212
00213 break;
00214 }
00215 if (r0 == 3)
00216 continue;
00217 }
00218
00219
00220 total_points++;
00221 }
00222
00223 iterations += iter-1;
00224
00225 samples.resize (3);
00226 samples[0] = random_idx[r0];
00227 samples[1] = random_idx[r1];
00228 samples[2] = random_idx[r2];
00229 return;
00230 }
00231
00233
00240 void
00241 SACModelParallelLines::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
00242 {
00243 int nr_p = 0;
00244 inliers.resize (indices_.size ());
00245
00246
00247 std::vector<int> closest_line (indices_.size ());
00248 std::vector<double> closest_dist (indices_.size ());
00249 closestLine (indices_, model_coefficients, &closest_line, &closest_dist);
00250
00251
00252 for (unsigned int i = 0; i < closest_dist.size (); i++)
00253 {
00254 if (closest_dist[i] < threshold)
00255 {
00256 inliers[nr_p] = indices_[i];
00257 nr_p++;
00258 }
00259 }
00260 inliers.resize (nr_p);
00261 return;
00262 }
00263
00265
00269 void
00270 SACModelParallelLines::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
00271 {
00272 distances.resize (indices_.size ());
00273
00274 std::vector<int> closest_line (indices_.size ());
00275 closestLine (indices_, model_coefficients, &closest_line, &distances);
00276
00277 return;
00278 }
00279
00281
00286 void
00287 SACModelParallelLines::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
00288 sensor_msgs::PointCloud &projected_points)
00289 {
00290
00291 projected_points.points.resize (inliers.size ());
00292
00293
00294 projected_points.set_channels_size (cloud_->get_channels_size ());
00295 for (unsigned int d = 0; d < projected_points.get_channels_size (); d++)
00296 {
00297 projected_points.channels[d].name = cloud_->channels[d].name;
00298 projected_points.channels[d].values.resize (inliers.size ());
00299 }
00300
00301
00302 std::vector<int> closest_line (inliers.size ());
00303 std::vector<double> closest_dist (inliers.size ());
00304 closestLine (inliers, model_coefficients, &closest_line, &closest_dist);
00305
00306 geometry_msgs::Point32 l1;
00307 l1.x = model_coefficients[3] - model_coefficients[0];
00308 l1.y = model_coefficients[4] - model_coefficients[1];
00309 l1.z = model_coefficients[5] - model_coefficients[2];
00310 double l_sqr_length = (l1.x * l1.x + l1.y * l1.y + l1.z * l1.z);
00311
00312
00313 for (unsigned int i = 0; i < inliers.size (); i++)
00314 {
00315 double mx,my,mz;
00316 if (closest_line[i] == 0)
00317 {
00318 mx = model_coefficients[0];
00319 my = model_coefficients[1];
00320 mz = model_coefficients[2];
00321 }
00322 else
00323 {
00324 mx = model_coefficients[6];
00325 my = model_coefficients[7];
00326 mz = model_coefficients[8];
00327 }
00328 double k = (
00329 ( cloud_->points[inliers[i]].x * l1.x + cloud_->points[inliers[i]].y * l1.y + cloud_->points[inliers[i]].z * l1.z ) -
00330 ( mx * l1.x + my * l1.y + mz * l1.z )
00331 ) / l_sqr_length;
00332
00333 projected_points.points[i].x = mx + k * l1.x;
00334 projected_points.points[i].y = my + k * l1.y;
00335 projected_points.points[i].z = mz + k * l1.z;
00336
00337
00338 for (unsigned int d = 0; d < projected_points.get_channels_size (); d++)
00339 projected_points.channels[d].values[i] = cloud_->channels[d].values[inliers[i]];
00340 }
00341 }
00342
00344
00348 void
00349 SACModelParallelLines::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
00350 {
00351
00352
00353 std::vector<int> closest_line (inliers.size ());
00354 std::vector<double> closest_dist (inliers.size ());
00355 closestLine (inliers, model_coefficients, &closest_line, &closest_dist);
00356
00357 geometry_msgs::Point32 l1;
00358 l1.x = model_coefficients[3] - model_coefficients[0];
00359 l1.y = model_coefficients[4] - model_coefficients[1];
00360 l1.z = model_coefficients[5] - model_coefficients[2];
00361 double l_sqr_length = (l1.x * l1.x + l1.y * l1.y + l1.z * l1.z);
00362
00363
00364 for (unsigned int i = 0; i < inliers.size(); i++)
00365 {
00366 double mx,my,mz;
00367 if (closest_line[i] == 0)
00368 {
00369 mx = model_coefficients[0];
00370 my = model_coefficients[1];
00371 mz = model_coefficients[2];
00372 }
00373 else
00374 {
00375 mx = model_coefficients[6];
00376 my = model_coefficients[7];
00377 mz = model_coefficients[8];
00378 }
00379 double k = (
00380 ( cloud_->points[inliers[i]].x * l1.x + cloud_->points[inliers[i]].y * l1.y + cloud_->points[inliers[i]].z * l1.z ) -
00381 ( mx * l1.x + my * l1.y + mz * l1.z )
00382 ) / l_sqr_length;
00383
00384 cloud_->points[inliers[i]].x = mx + k * l1.x;
00385 cloud_->points[inliers[i]].y = my + k * l1.y;
00386 cloud_->points[inliers[i]].z = mz + k * l1.z;
00387 }
00388 }
00389
00390
00391
00393
00396 bool
00397 SACModelParallelLines::computeModelCoefficients (const std::vector<int> &samples)
00398 {
00399 model_coefficients_.resize (9);
00400 model_coefficients_[0] = cloud_->points[samples[0]].x;
00401 model_coefficients_[1] = cloud_->points[samples[0]].y;
00402 model_coefficients_[2] = cloud_->points[samples[0]].z;
00403 model_coefficients_[3] = cloud_->points[samples[1]].x;
00404 model_coefficients_[4] = cloud_->points[samples[1]].y;
00405 model_coefficients_[5] = cloud_->points[samples[1]].z;
00406 model_coefficients_[6] = cloud_->points[samples[2]].x;
00407 model_coefficients_[7] = cloud_->points[samples[2]].y;
00408 model_coefficients_[8] = cloud_->points[samples[2]].z;
00409
00410 return (true);
00411 }
00412
00413
00415
00420 void
00421 SACModelParallelLines::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
00422 {
00423 if (inliers.size () == 0)
00424 {
00425 ROS_ERROR ("[SACModelParallelLines::RefitModel] Cannot re-fit 0 inliers!");
00426 refit_coefficients = model_coefficients_;
00427 return;
00428 }
00429
00430
00431 std::vector<int> closest_line (inliers.size ());
00432 std::vector<double> closest_dist (inliers.size ());
00433 closestLine (inliers, model_coefficients_, &closest_line, &closest_dist);
00434
00435 refit_coefficients.resize (9);
00436
00437
00438 geometry_msgs::Point32 centroid1, centroid2, centroid;
00439 std::vector<int> inliers1, inliers2;
00440 int end = inliers.size ();
00441 for (int i=0; i < end; ++i)
00442 {
00443 (closest_line[i] == 0) ? inliers1.push_back (inliers[i]) : inliers2.push_back (inliers[i]);
00444 }
00445 cloud_geometry::nearest::computeCentroid (*cloud_, inliers1, centroid1);
00446 cloud_geometry::nearest::computeCentroid (*cloud_, inliers2, centroid2);
00447
00448
00449 sensor_msgs::PointCloud zero_cloud;
00450 zero_cloud.points.resize (inliers.size ());
00451 geometry_msgs::Point32 tpoint;
00452 for (unsigned int i = 0; i < inliers1.size (); i++)
00453 {
00454 tpoint.x = cloud_->points[inliers1[i]].x - centroid1.x;
00455 tpoint.y = cloud_->points[inliers1[i]].y - centroid1.y;
00456 tpoint.z = cloud_->points[inliers1[i]].z - centroid1.z;
00457 zero_cloud.points.push_back (tpoint);
00458 }
00459 for (unsigned int i = 0; i < inliers2.size (); i++)
00460 {
00461 tpoint.x = cloud_->points[inliers2[i]].x - centroid2.x;
00462 tpoint.y = cloud_->points[inliers2[i]].y - centroid2.y;
00463 tpoint.z = cloud_->points[inliers2[i]].z - centroid2.z;
00464 zero_cloud.points.push_back (tpoint);
00465 }
00466
00467
00468 Eigen::Matrix3d covariance_matrix;
00469 std::vector<int> zero_inliers (zero_cloud.points.size ());
00470 for (unsigned int i = 0; i < zero_cloud.points.size (); i++)
00471 {
00472 zero_inliers[i] = i;
00473 }
00474 cloud_geometry::nearest::computeCovarianceMatrix (zero_cloud, zero_inliers, covariance_matrix, centroid);
00475
00476
00477
00478
00479 refit_coefficients[0] = centroid1.x;
00480 refit_coefficients[1] = centroid1.y;
00481 refit_coefficients[2] = centroid1.z;
00482
00483 refit_coefficients[6] = centroid2.x;
00484 refit_coefficients[7] = centroid2.y;
00485 refit_coefficients[8] = centroid2.z;
00486
00487
00488 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
00489 Eigen::Vector3d eigen_values = ei_symm.eigenvalues ();
00490 Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();
00491
00492 refit_coefficients[3] = eigen_vectors(0, 2) + refit_coefficients[0];
00493 refit_coefficients[4] = eigen_vectors(1, 2) + refit_coefficients[1];
00494 refit_coefficients[5] = eigen_vectors(2, 2) + refit_coefficients[2];
00495 }
00496
00498
00502 bool
00503 SACModelParallelLines::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
00504 {
00505 std::vector<int> closest_line (indices.size ());
00506 std::vector<double> closest_dist (indices.size ());
00507 closestLine (indices, model_coefficients_, &closest_line, &closest_dist);
00508
00509 for (unsigned int i = 0; i < closest_dist.size (); ++i)
00510 {
00511 if (closest_dist[i] > threshold)
00512 return (false);
00513 }
00514 return (true);
00515 }
00516 }
00517