Point Cloud Library (PCL)
1.11.0
|
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/registration/transformation_estimation_svd.h>
52 #include <pcl/correspondence.h>
53 #include <pcl/pcl_exports.h>
54 #include <pcl/point_cloud.h>
60 #define OBJ_REC_RANSAC_VERBOSE
102 Output (
const std::string& object_name,
const float rigid_transform[12],
float match_confidence,
void* user_data) :
103 object_name_ (object_name),
104 match_confidence_ (match_confidence),
105 user_data_ (user_data)
107 memcpy(this->rigid_transform_, rigid_transform, 12*
sizeof (
float));
113 float rigid_transform_[12];
122 : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2)
129 const float *p1_, *n1_, *
p2_, *n2_;
157 this->clearTestData ();
164 model_library_.removeAllModels ();
165 scene_octree_.clear ();
166 scene_octree_proj_.clear ();
167 sampled_oriented_point_pairs_.clear ();
168 transform_space_.clear ();
169 scene_octree_points_.reset ();
179 max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS;
180 model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees);
186 scene_bounds_enlargement_factor_ = value;
193 ignore_coplanar_opps_ =
true;
194 model_library_.ignoreCoplanarPointPairsOn ();
201 ignore_coplanar_opps_ =
false;
202 model_library_.ignoreCoplanarPointPairsOff ();
208 do_icp_hypotheses_refinement_ =
true;
214 do_icp_hypotheses_refinement_ =
false;
231 return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data));
265 inline const std::list<ObjRecRANSAC::OrientedPointPair>&
268 return (sampled_oriented_point_pairs_);
273 inline const std::vector<Hypothesis>&
276 return (accepted_hypotheses_);
284 out = accepted_hypotheses_;
291 return (model_library_.getHashTable ());
297 return (model_library_);
303 return (model_library_.getModel (name));
309 return (scene_octree_);
315 return (transform_space_);
334 const double p_obj = 0.25f;
336 const double p = p_obj*relative_obj_size_;
338 if ( 1.0 - p <= 0.0 )
341 return static_cast<int> (std::log (1.0-success_probability)/std::log (1.0-p) + 1.0);
347 sampled_oriented_point_pairs_.clear ();
348 accepted_hypotheses_.clear ();
349 transform_space_.clear ();
353 sampleOrientedPointPairs (
int num_iterations,
const std::vector<ORROctree::Node*>& full_scene_leaves, std::list<OrientedPointPair>& output)
const;
356 generateHypotheses (
const std::list<OrientedPointPair>& pairs, std::list<HypothesisBase>& out)
const;
389 const float *a1,
const float *a1_n,
const float *b1,
const float* b1_n,
390 const float *a2,
const float *a2_n,
const float *b2,
const float* b2_n,
391 float* rigid_transform)
const
394 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];
397 o1[0] = 0.5f*(a1[0] + b1[0]);
398 o1[1] = 0.5f*(a1[1] + b1[1]);
399 o1[2] = 0.5f*(a1[2] + b1[2]);
401 o2[0] = 0.5f*(a2[0] + b2[0]);
402 o2[1] = 0.5f*(a2[1] + b2[1]);
403 o2[2] = 0.5f*(a2[2] + b2[2]);
421 invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2];
422 invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2];
423 invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2];
429 rigid_transform[9] = o2[0] - Ro1[0];
430 rigid_transform[10] = o2[1] - Ro1[1];
431 rigid_transform[11] = o2[2] - Ro1[2];
444 float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
447 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];
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...
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).
float relative_num_of_illegal_pts_
const std::list< ObjRecRANSAC::OrientedPointPair > & getSampledOrientedPointPairs() const
This function is useful for testing purposes.
float rotation_discretization_
float max_coplanarity_angle_
PointCloudIn::Ptr scene_octree_points_
OrientedPointPair(const float *p1, const float *n1, const float *p2, const float *n2)
Hypothesis * create(const SimpleOctree< Hypothesis, HypothesisCreator, float >::Node *) const
void recognize(const PointCloudIn &scene, const PointCloudN &normals, std::list< ObjRecRANSAC::Output > &recognized_objects, double success_probability=0.99)
This method performs the recognition of the models loaded to the model library with the method addMod...
void buildGraphOfConflictingHypotheses(const BVHH &bvh, ORRGraph< Hypothesis * > &graph) const
This is an output item of the ObjRecRANSAC::recognize() method.
std::vector< Hypothesis > accepted_hypotheses_
This is a RANSAC-based 3D object recognition method.
Output(const std::string &object_name, const float rigid_transform[12], float match_confidence, void *user_data)
TrimmedICP< pcl::PointXYZ, float > trimmed_icp_
void setMaxCoplanarityAngleDegrees(float max_coplanarity_angle_degrees)
This is a threshold.
void filterGraphOfConflictingHypotheses(ORRGraph< Hypothesis * > &graph, std::list< ObjRecRANSAC::Output > &recognized_objects) const
void mult3x3(const T m[9], const T v[3], T out[3])
out = mat*v.
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
void setSceneBoundsEnlargementFactor(float value)
virtual ~HypothesisCreator()
float scene_bounds_enlargement_factor_
void icpHypothesesRefinementOn()
float getPairWidth() const
const pcl::recognition::ModelLibrary::HashTable & getHashTable() const
Returns the hash table in the model library.
ModelLibrary model_library_
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'.
void cross3(const T v1[3], const T v2[3], T out[3])
void testHypothesis(Hypothesis *hypothesis, int &match, int &penalty) const
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
T clamp(T value, T min, T max)
std::list< OrientedPointPair > sampled_oriented_point_pairs_
void getAcceptedHypotheses(std::vector< Hypothesis > &out) const
This function is useful for testing purposes.
const ORROctree & getSceneOctree() const
bool addModel(const PointCloudIn &points, const PointCloudN &normals, const std::string &object_name, void *user_data=nullptr)
Add an object model to be recognized.
void filterGraphOfCloseHypotheses(ORRGraph< Hypothesis > &graph, std::vector< Hypothesis > &out) const
bool ignore_coplanar_opps_
int computeNumberOfIterations(double success_probability) const
void sampleOrientedPointPairs(int num_iterations, const std::vector< ORROctree::Node * > &full_scene_leaves, std::list< OrientedPointPair > &output) const
int generateHypotheses(const std::list< OrientedPointPair > &pairs, std::list< HypothesisBase > &out) const
Recognition_Mode rec_mode_
const std::vector< Hypothesis > & getAcceptedHypotheses() const
This function is useful for testing purposes.
float intersection_fraction_
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
float frac_of_points_for_icp_refinement_
void clear()
Removes all models from the model library and releases some memory dynamically allocated by this inst...
void buildGraphOfCloseHypotheses(HypothesisOctree &hypotheses, ORRGraph< Hypothesis > &graph) const
This class is an implementation of bounding volume hierarchies.
That's a very specialized and simple octree class.
RigidTransformSpace & getRigidTransformSpace()
float position_discretization_
Stores some information about the model.
shared_ptr< PointCloud< pcl::PointXYZ > > Ptr
void normalize3(T v[3])
Normalize v.
void ignoreCoplanarPointPairsOff()
Default is on.
void enterTestModeTestHypotheses()
const ModelLibrary & getModelLibrary() const
RigidTransformSpace transform_space_
void diff3(const T a[3], const T b[3], T c[3])
c = a - b
ORROctreeZProjection scene_octree_proj_
pcl::PointCloud< pcl::Normal > PointCloudN
ObjRecRANSAC(float pair_width, float voxel_size)
Constructor with some important parameters which can not be changed once an instance of that class is...
void icpHypothesesRefinementOff()
bool do_icp_hypotheses_refinement_
void ignoreCoplanarPointPairsOn()
Default is on.
void enterTestModeSampleOPP()
const ModelLibrary::Model * getModel(const std::string &name) const
virtual ~OrientedPointPair()
int groupHypotheses(std::list< HypothesisBase > &hypotheses, int num_hypotheses, RigidTransformSpace &transform_space, HypothesisOctree &grouped_hypotheses) const
Groups close hypotheses in 'hypotheses'.
void testHypothesisNormalBased(Hypothesis *hypothesis, float &match) const