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
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 ();
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));
246 rec_mode_ = ObjRecRANSAC::SAMPLE_OPP;
252 rec_mode_ = ObjRecRANSAC::TEST_HYPOTHESES;
258 rec_mode_ = ObjRecRANSAC::FULL_RECOGNITION;
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 )
345 sampled_oriented_point_pairs_.clear ();
346 accepted_hypotheses_.clear ();
347 transform_space_.clear ();
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,
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]);
404 aux::diff3 (
b1,
a1, x1); aux::normalize3 (x1);
405 aux::diff3 (
b2,
a2, x2); aux::normalize3 (x2);
407 aux::projectOnPlane3 (
a1_n, x1,
tmp1); aux::normalize3 (
tmp1);
408 aux::projectOnPlane3 (
b1_n, x1,
tmp2); aux::normalize3 (
tmp2);
411 aux::projectOnPlane3 (
a2_n, x2,
tmp1); aux::normalize3 (
tmp1);
412 aux::projectOnPlane3 (
b2_n, x2,
tmp2); aux::normalize3 (
tmp2);
415 aux::cross3 (x1,
y1, z1);
416 aux::cross3 (x2,
y2, z2);
442 float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
443 aux::normalize3 (
cl);
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];
446 signature[1] = std::acos (aux::clamp (aux::dot3 (n2,
cl), -1.0f, 1.0f));
447 signature[2] = std::acos (aux::clamp (aux::dot3 (n1,n2), -1.0f, 1.0f));
Iterator class for point clouds with or without given indices.
Stores some information about the model.
That's a very specialized and simple octree class.
Hypothesis * create(const SimpleOctree< Hypothesis, HypothesisCreator, float >::Node *) const
virtual ~HypothesisCreator()
OrientedPointPair(const float *p1, const float *n1, const float *p2, const float *n2)
virtual ~OrientedPointPair()
This is an output item of the ObjRecRANSAC::recognize() method.
Output(const std::string &object_name, const float rigid_transform[12], float match_confidence, void *user_data)
This is a RANSAC-based 3D object recognition method.
bool do_icp_hypotheses_refinement_
bool ignore_coplanar_opps_
RigidTransformSpace & getRigidTransformSpace()
void getAcceptedHypotheses(std::vector< Hypothesis > &out) const
This function is useful for testing purposes.
void setSceneBoundsEnlargementFactor(float value)
const pcl::recognition::ModelLibrary::HashTable & getHashTable() const
Returns the hash table in the model library.
void ignoreCoplanarPointPairsOff()
Default is on.
float scene_bounds_enlargement_factor_
RigidTransformSpace transform_space_
void enterTestModeSampleOPP()
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
const std::list< ObjRecRANSAC::OrientedPointPair > & getSampledOrientedPointPairs() const
This function is useful for testing purposes.
float getPairWidth() const
const ORROctree & getSceneOctree() const
void clear()
Removes all models from the model library and releases some memory dynamically allocated by this inst...
void sampleOrientedPointPairs(int num_iterations, const std::vector< ORROctree::Node * > &full_scene_leaves, std::list< OrientedPointPair > &output) const
std::list< OrientedPointPair > sampled_oriented_point_pairs_
PointCloudIn::Ptr scene_octree_points_
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...
const std::vector< Hypothesis > & getAcceptedHypotheses() const
This function is useful for testing purposes.
std::vector< Hypothesis > accepted_hypotheses_
float intersection_fraction_
ModelLibrary model_library_
int computeNumberOfIterations(double success_probability) const
void filterGraphOfCloseHypotheses(ORRGraph< Hypothesis > &graph, std::vector< Hypothesis > &out) const
void setMaxCoplanarityAngleDegrees(float max_coplanarity_angle_degrees)
This is a threshold.
void buildGraphOfCloseHypotheses(HypothesisOctree &hypotheses, ORRGraph< Hypothesis > &graph) 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 icpHypothesesRefinementOn()
const ModelLibrary & getModelLibrary() const
void testHypothesis(Hypothesis *hypothesis, int &match, int &penalty) const
float relative_num_of_illegal_pts_
float max_coplanarity_angle_
TrimmedICP< pcl::PointXYZ, float > trimmed_icp_
float frac_of_points_for_icp_refinement_
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).
void enterTestModeTestHypotheses()
ORROctreeZProjection scene_octree_proj_
int generateHypotheses(const std::list< OrientedPointPair > &pairs, std::list< HypothesisBase > &out) const
float rotation_discretization_
void ignoreCoplanarPointPairsOn()
Default is on.
void buildGraphOfConflictingHypotheses(const BVHH &bvh, ORRGraph< Hypothesis * > &graph) const
void icpHypothesesRefinementOff()
ObjRecRANSAC(float pair_width, float voxel_size)
Constructor with some important parameters which can not be changed once an instance of that class is...
Recognition_Mode rec_mode_
float position_discretization_
void filterGraphOfConflictingHypotheses(ORRGraph< Hypothesis * > &graph, std::list< ObjRecRANSAC::Output > &recognized_objects) const
const ModelLibrary::Model * getModel(const std::string &name) 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.