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_
ModelLibrary::PointCloudN PointCloudN
void mult3x3(const T m[9], const T v[3], T out[3])
out = mat*v.
void projectOnPlane3(const T x[3], const T planeNormal[3], T out[3])
Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'...
const ModelLibrary & getModelLibrary() const
OrientedPointPair(const float *p1, const float *n1, const float *p2, const float *n2)
const pcl::recognition::ModelLibrary::HashTable & getHashTable() const
Returns the hash table in the model library.
That's a very specialized and simple octree class.
const std::list< ObjRecRANSAC::OrientedPointPair > & getSampledOrientedPointPairs() const
This function is useful for testing purposes.
ORROctreeZProjection scene_octree_proj_
void enterTestModeTestHypotheses()
void setMaxCoplanarityAngleDegrees(float max_coplanarity_angle_degrees)
This is a threshold.
Hypothesis * create(const SimpleOctree< Hypothesis, HypothesisCreator, float >::Node *) const
PointCloudIn::Ptr scene_octree_points_
void enterTestModeSampleOPP()
Recognition_Mode rec_mode_
void setSceneBoundsEnlargementFactor(float value)
boost::shared_ptr< PointCloud< PointT > > Ptr
bool do_icp_hypotheses_refinement_
void diff3(const T a[3], const T b[3], T c[3])
c = a - b
float getPairWidth() const
float max_coplanarity_angle_
void clear()
Removes all models from the model library and releases some memory dynamically allocated by this inst...
void icpHypothesesRefinementOn()
SimpleOctree< Hypothesis, HypothesisCreator, float > HypothesisOctree
RigidTransformSpace & getRigidTransformSpace()
float rotation_discretization_
This is a RANSAC-based 3D object recognition method.
void computeRigidTransform(const float *a1, const float *a1_n, const float *b1, const float *b1_n, const float *a2, const float *a2_n, const float *b2, const float *b2_n, float *rigid_transform) const
Computes the rigid transform that maps the line (a1, b1) to (a2, b2).
virtual ~OrientedPointPair()
bool addModel(const PointCloudIn &points, const PointCloudN &normals, const std::string &object_name, void *user_data=NULL)
Add an object model to be recognized.
const ORROctree & getSceneOctree() const
T clamp(T value, T min, T max)
int computeNumberOfIterations(double success_probability) const
Stores some information about the model.
ModelLibrary::PointCloudIn PointCloudIn
void ignoreCoplanarPointPairsOff()
Default is on.
std::vector< Hypothesis > accepted_hypotheses_
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
float scene_bounds_enlargement_factor_
bool ignore_coplanar_opps_
float intersection_fraction_
This is an output item of the ObjRecRANSAC::recognize() method.
This class is an implementation of bounding volume hierarchies.
void normalize3(T v[3])
Normalize v.
std::list< OrientedPointPair > sampled_oriented_point_pairs_
float frac_of_points_for_icp_refinement_
const ModelLibrary::Model * getModel(const std::string &name) const
virtual ~HypothesisCreator()
RigidTransformSpace transform_space_
void getAcceptedHypotheses(std::vector< Hypothesis > &out) const
This function is useful for testing purposes.
ModelLibrary model_library_
void cross3(const T v1[3], const T v2[3], T out[3])
static void compute_oriented_point_pair_signature(const float *p1, const float *n1, const float *p2, const float *n2, float signature[3])
Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles betwe...
Output(const std::string &object_name, const float rigid_transform[12], float match_confidence, void *user_data)
TrimmedICP< pcl::PointXYZ, float > trimmed_icp_
void icpHypothesesRefinementOff()
const std::vector< Hypothesis > & getAcceptedHypotheses() const
This function is useful for testing purposes.
float position_discretization_
float relative_num_of_illegal_pts_
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
void ignoreCoplanarPointPairsOn()
Default is on.