39 #ifndef PCL_RECOGNITION_OBJ_REC_RANSAC_H_
40 #define PCL_RECOGNITION_OBJ_REC_RANSAC_H_
42 #include <pcl/recognition/ransac_based/hypothesis.h>
43 #include <pcl/recognition/ransac_based/model_library.h>
44 #include <pcl/recognition/ransac_based/rigid_transform_space.h>
45 #include <pcl/recognition/ransac_based/orr_octree.h>
46 #include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
47 #include <pcl/recognition/ransac_based/simple_octree.h>
48 #include <pcl/recognition/ransac_based/trimmed_icp.h>
49 #include <pcl/recognition/ransac_based/orr_graph.h>
50 #include <pcl/recognition/ransac_based/auxiliary.h>
51 #include <pcl/recognition/ransac_based/bvh.h>
52 #include <pcl/registration/transformation_estimation_svd.h>
53 #include <pcl/correspondence.h>
54 #include <pcl/pcl_exports.h>
55 #include <pcl/point_cloud.h>
61 #define OBJ_REC_RANSAC_VERBOSE
103 Output (
const std::string& object_name,
const float rigid_transform[12],
float match_confidence,
void* user_data) :
104 object_name_ (object_name),
105 match_confidence_ (match_confidence),
106 user_data_ (user_data)
108 memcpy(this->rigid_transform_, rigid_transform, 12*
sizeof (
float));
114 float rigid_transform_[12];
123 : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2)
130 const float *p1_, *n1_, *
p2_, *n2_;
158 this->clearTestData ();
165 model_library_.removeAllModels ();
166 scene_octree_.clear ();
167 scene_octree_proj_.clear ();
168 sampled_oriented_point_pairs_.clear ();
169 transform_space_.clear ();
170 scene_octree_points_.reset ();
180 max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS;
181 model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees);
187 scene_bounds_enlargement_factor_ = value;
194 ignore_coplanar_opps_ =
true;
195 model_library_.ignoreCoplanarPointPairsOn ();
202 ignore_coplanar_opps_ =
false;
203 model_library_.ignoreCoplanarPointPairsOff ();
209 do_icp_hypotheses_refinement_ =
true;
215 do_icp_hypotheses_refinement_ =
false;
232 return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data));
244 recognize (
const PointCloudIn& scene,
const PointCloudN& normals, std::list<ObjRecRANSAC::Output>& recognized_objects,
double success_probability = 0.99);
266 inline const std::list<ObjRecRANSAC::OrientedPointPair>&
269 return (sampled_oriented_point_pairs_);
274 inline const std::vector<Hypothesis>&
277 return (accepted_hypotheses_);
285 out = accepted_hypotheses_;
292 return (model_library_.getHashTable ());
298 return (model_library_);
304 return (model_library_.getModel (name));
310 return (scene_octree_);
316 return (transform_space_);
335 const double p_obj = 0.25f;
337 const double p = p_obj*relative_obj_size_;
339 if ( 1.0 - p <= 0.0 )
342 return static_cast<int> (log (1.0-success_probability)/log (1.0-p) + 1.0);
348 sampled_oriented_point_pairs_.clear ();
349 accepted_hypotheses_.clear ();
350 transform_space_.clear ();
354 sampleOrientedPointPairs (
int num_iterations,
const std::vector<ORROctree::Node*>& full_scene_leaves, std::list<OrientedPointPair>& output)
const;
357 generateHypotheses (
const std::list<OrientedPointPair>& pairs, std::list<HypothesisBase>& out)
const;
362 groupHypotheses(std::list<HypothesisBase>& hypotheses,
int num_hypotheses,
RigidTransformSpace& transform_space,
363 HypothesisOctree& grouped_hypotheses)
const;
366 testHypothesis (
Hypothesis* hypothesis,
int& match,
int& penalty)
const;
369 testHypothesisNormalBased (
Hypothesis* hypothesis,
float& match)
const;
381 filterGraphOfConflictingHypotheses (
ORRGraph<Hypothesis*>& graph, std::list<ObjRecRANSAC::Output>& recognized_objects)
const;
390 const float *a1,
const float *a1_n,
const float *b1,
const float* b1_n,
391 const float *a2,
const float *a2_n,
const float *b2,
const float* b2_n,
392 float* rigid_transform)
const
395 float o1[3], o2[3], x1[3], x2[3], y1[3], y2[3], z1[3], z2[3], tmp1[3], tmp2[3], Ro1[3], invFrame1[3][3];
398 o1[0] = 0.5f*(a1[0] + b1[0]);
399 o1[1] = 0.5f*(a1[1] + b1[1]);
400 o1[2] = 0.5f*(a1[2] + b1[2]);
402 o2[0] = 0.5f*(a2[0] + b2[0]);
403 o2[1] = 0.5f*(a2[1] + b2[1]);
404 o2[2] = 0.5f*(a2[2] + b2[2]);
422 invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2];
423 invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2];
424 invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2];
430 rigid_transform[9] = o2[0] - Ro1[0];
431 rigid_transform[10] = o2[1] - Ro1[1];
432 rigid_transform[11] = o2[2] - Ro1[2];
445 float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
448 signature[0] = std::acos (
aux::clamp (
aux::dot3 (n1,cl), -1.0f, 1.0f)); cl[0] = -cl[0]; cl[1] = -cl[1]; cl[2] = -cl[2];
484 #endif // PCL_RECOGNITION_OBJ_REC_RANSAC_H_