match3pcs.hpp
Go to the documentation of this file.
1 //
2 // Created by Sandra Alfaro on 30/05/18.
3 //
4 
5 #include <vector>
6 #include <atomic>
7 #include <chrono>
8 #include "gr/utils/shared.h"
9 #include "gr/utils/sampling.h"
10 #include "gr/utils/logger.h"
11 #include "match3pcs.h"
12 
13 namespace gr {
14 
15  template <typename PointType,
16  typename TransformVisitor,
17  typename PairFilteringFunctor,
18  template < class, class > class PFO>
21  const gr::Utils::Logger &logger)
23  {
24  }
25 
26  template <typename PointType,
27  typename TransformVisitor,
28  typename PairFilteringFunctor,
29  template < class, class > class PFO>
31 
32  template <typename PointType,
33  typename TransformVisitor,
34  typename PairFilteringFunctor,
35  template < class, class > class PFO>
37 
38  //Find base in P (random triangle)
39  if(!initBase(base)) return false;
40 
41  // Computes distance between points.
42  const Scalar d1 = (MatchBaseType::base_3D_[0]->pos()- MatchBaseType::base_3D_[1]->pos()).norm();
43  const Scalar d2 = (MatchBaseType::base_3D_[0]->pos()- MatchBaseType::base_3D_[2]->pos()).norm();
44  const Scalar d3 = (MatchBaseType::base_3D_[1]->pos()- MatchBaseType::base_3D_[2]->pos()).norm();
45 
46  /*
47  // Compute normal angles.
48  const Scalar normal_angle_AB;
49  const Scalar normal_angle_AC;
50  const Scalar normal_angle_BC;
51  */
52 
54 
55  // Find all 3pcs in Q
56  for (int i=0; i<MatchBaseType::sampled_Q_3D_.size(); ++i) {
58  for (int j=i+1; j<MatchBaseType::sampled_Q_3D_.size(); ++j) {
60  const Scalar dAB = (b.pos() - a.pos()).norm();
62  for (int k=j+1; k<MatchBaseType::sampled_Q_3D_.size(); ++k) {
64  const Scalar dAC = (c.pos() - a.pos()).norm();
65  const Scalar dBC = (c.pos() - b.pos()).norm();
68 
70  }
71  }
72  }
73 
74  //TODO add filter points
75  /* //Change the normal_angle
76  // Compute normal angles.
77  const Scalar normal_angle1 = (base_3D_[0]->normal() - base_3D_[1]->normal()).norm();
78  const Scalar normal_angle2 = (base_3D_[2]->normal() - base_3D_[3]->normal()).norm();
79 
80  PointFilterFunctor fun(myOptions_, myBase_3D_);
81  std::pair<bool,bool> res = fun(p,q, pair_normals_angle, base_point1,base_point2);
82  if (res.first)
83  pairs->emplace_back(i, j);
84  if (res.second)
85  pairs->emplace_back(j, i);
86  */
87  return congruent_set.size()!=0;
88  }
89 
90 
91  template <typename PointType,
92  typename TransformVisitor,
93  typename PairFilteringFunctor,
94  template < class, class > class PFO>
96  {
98  return false;
99 
103 
104  return true;
105  }
106 }
CongruentSetExplorationBase< Traits, PointType, TransformVisitor, PairFilteringFunctor, OptExts... >::Scalar ComputeTransformation(const InputRange1 &P, const InputRange2 &Q, Eigen::Ref< typename CongruentSetExplorationBase< Traits, PointType, TransformVisitor, PairFilteringFunctor, OptExts... >::MatrixType > transformation, const Sampler< PointType > &sampler, TransformVisitor &v)
Definition: congruentSetExplorationBase.hpp:61