41 #include <pcl/recognition/ransac_based/hypothesis.h>
42 #include <pcl/recognition/ransac_based/model_library.h>
43 #include <pcl/recognition/ransac_based/rigid_transform_space.h>
44 #include <pcl/recognition/ransac_based/orr_octree.h>
45 #include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
46 #include <pcl/recognition/ransac_based/simple_octree.h>
47 #include <pcl/recognition/ransac_based/trimmed_icp.h>
48 #include <pcl/recognition/ransac_based/orr_graph.h>
49 #include <pcl/recognition/ransac_based/auxiliary.h>
50 #include <pcl/recognition/ransac_based/bvh.h>
51 #include <pcl/pcl_exports.h>
52 #include <pcl/point_cloud.h>
58 #define OBJ_REC_RANSAC_VERBOSE
100 Output (
const std::string& object_name,
const float rigid_transform[12],
float match_confidence,
void* user_data) :
101 object_name_ (object_name),
102 match_confidence_ (match_confidence),
103 user_data_ (user_data)
105 memcpy(this->rigid_transform_, rigid_transform, 12*
sizeof (
float));
111 float rigid_transform_[12];
120 : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2)
127 const float *p1_, *n1_, *
p2_, *n2_;
155 this->clearTestData ();
162 model_library_.removeAllModels ();
163 scene_octree_.clear ();
164 scene_octree_proj_.clear ();
165 sampled_oriented_point_pairs_.clear ();
166 transform_space_.clear ();
167 scene_octree_points_.reset ();
177 max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS;
178 model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees);
184 scene_bounds_enlargement_factor_ = value;
191 ignore_coplanar_opps_ =
true;
192 model_library_.ignoreCoplanarPointPairsOn ();
199 ignore_coplanar_opps_ =
false;
200 model_library_.ignoreCoplanarPointPairsOff ();
206 do_icp_hypotheses_refinement_ =
true;
212 do_icp_hypotheses_refinement_ =
false;
229 return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data));
241 recognize (
const PointCloudIn& scene,
const PointCloudN& normals, std::list<ObjRecRANSAC::Output>& recognized_objects,
double success_probability = 0.99);
263 inline const std::list<ObjRecRANSAC::OrientedPointPair>&
266 return (sampled_oriented_point_pairs_);
271 inline const std::vector<Hypothesis>&
274 return (accepted_hypotheses_);
282 out = accepted_hypotheses_;
289 return (model_library_.getHashTable ());
295 return (model_library_);
301 return (model_library_.getModel (name));
307 return (scene_octree_);
313 return (transform_space_);
332 const double p_obj = 0.25f;
334 const double p = p_obj*relative_obj_size_;
336 if ( 1.0 - p <= 0.0 )
339 return static_cast<int> (std::log (1.0-success_probability)/std::log (1.0-p) + 1.0);
345 sampled_oriented_point_pairs_.clear ();
346 accepted_hypotheses_.clear ();
347 transform_space_.clear ();
351 sampleOrientedPointPairs (
int num_iterations,
const std::vector<ORROctree::Node*>& full_scene_leaves, std::list<OrientedPointPair>& output)
const;
354 generateHypotheses (
const std::list<OrientedPointPair>& pairs, std::list<HypothesisBase>& out)
const;
359 groupHypotheses(std::list<HypothesisBase>& hypotheses,
int num_hypotheses,
RigidTransformSpace& transform_space,
360 HypothesisOctree& grouped_hypotheses)
const;
363 testHypothesis (
Hypothesis* hypothesis,
int& match,
int& penalty)
const;
366 testHypothesisNormalBased (
Hypothesis* hypothesis,
float& match)
const;
378 filterGraphOfConflictingHypotheses (
ORRGraph<Hypothesis*>& graph, std::list<ObjRecRANSAC::Output>& recognized_objects)
const;
387 const float *a1,
const float *a1_n,
const float *b1,
const float* b1_n,
388 const float *a2,
const float *a2_n,
const float *b2,
const float* b2_n,
389 float* rigid_transform)
const
392 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];
395 o1[0] = 0.5f*(a1[0] + b1[0]);
396 o1[1] = 0.5f*(a1[1] + b1[1]);
397 o1[2] = 0.5f*(a1[2] + b1[2]);
399 o2[0] = 0.5f*(a2[0] + b2[0]);
400 o2[1] = 0.5f*(a2[1] + b2[1]);
401 o2[2] = 0.5f*(a2[2] + b2[2]);
419 invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2];
420 invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2];
421 invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2];
427 rigid_transform[9] = o2[0] - Ro1[0];
428 rigid_transform[10] = o2[1] - Ro1[1];
429 rigid_transform[11] = o2[2] - Ro1[2];
442 float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
445 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];