normalset.hpp
Go to the documentation of this file.
1 // Copyright 2014 Nicolas Mellado
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // -------------------------------------------------------------------------- //
16 //
17 // Authors: Nicolas Mellado
18 //
19 // An implementation of the Super 4-points Congruent Sets (Super 4PCS)
20 // algorithm presented in:
21 //
22 // Super 4PCS: Fast Global Pointcloud Registration via Smart Indexing
23 // Nicolas Mellado, Dror Aiger, Niloy J. Mitra
24 // Symposium on Geometry Processing 2014.
25 //
26 // Data acquisition in large-scale scenes regularly involves accumulating
27 // information across multiple scans. A common approach is to locally align scan
28 // pairs using Iterative Closest Point (ICP) algorithm (or its variants), but
29 // requires static scenes and small motion between scan pairs. This prevents
30 // accumulating data across multiple scan sessions and/or different acquisition
31 // modalities (e.g., stereo, depth scans). Alternatively, one can use a global
32 // registration algorithm allowing scans to be in arbitrary initial poses. The
33 // state-of-the-art global registration algorithm, 4PCS, however has a quadratic
34 // time complexity in the number of data points. This vastly limits its
35 // applicability to acquisition of large environments. We present Super 4PCS for
36 // global pointcloud registration that is optimal, i.e., runs in linear time (in
37 // the number of data points) and is also output sensitive in the complexity of
38 // the alignment problem based on the (unknown) overlap across scan pairs.
39 // Technically, we map the algorithm as an 'instance problem' and solve it
40 // efficiently using a smart indexing data organization. The algorithm is
41 // simple, memory-efficient, and fast. We demonstrate that Super 4PCS results in
42 // significant speedup over alternative approaches and allows unstructured
43 // efficient acquisition of scenes at scales previously not possible. Complete
44 // source code and datasets are available for research use at
45 // http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/.
46 
47 
48 #ifndef _OPENGR_ACCELERATORS_INDEXED_NORMAL_SET_HPP_
49 #define _OPENGR_ACCELERATORS_INDEXED_NORMAL_SET_HPP_
50 
51 #include <math.h>
52 #include <set>
53 #include <Eigen/Geometry>
54 #include <gr/accelerators/utils.h>
55 
56 namespace gr{
57 
58 template <class Point, int dim, int _ngSize, typename Scalar>
60  for(unsigned int i = 0; i != _grid.size(); i++)
61  delete(_grid[i]);
62 }
63 
64 /*!
65  \return Cell id corresponding to p in the euclidean grid
66  \warning p must be normalized between 0 and 1
67  */
68 template <class Point, int dim, int _ngSize, typename Scalar>
69 int
71  const Point& p) const
72 {
73  // Unroll the loop over the different dimensions at compile time
75 }
76 
77 /*!
78  \return Cell id corresponding to p in the euclidean grid
79  \warning p must be normalized between 0 and 1
80  */
81 template <class Point, int dim, int _ngSize, typename Scalar>
82 int
84  const Point& n) const
85 {
86  // Unroll the loop over the different dimension at compile time
88 }
89 
90 
91 template <class Point, int dim, int _ngSize, typename Scalar>
92 int
94  const Point& pCoord) const
95 {
96  // Unroll the loop over the different dimensions at compile time
98 }
99 
100 
101 template <class Point, int dim, int _ngSize, typename Scalar>
102 int
104  const Point& nCoord) const
105 {
106  // Unroll the loop over the different dimension at compile time
108 }
109 
110 
111 template <class Point, int dim, int _ngSize, typename Scalar>
112 bool
114  const Point& p,
115  const Point& n,
116  unsigned int id)
117 {
118  const int pId = indexPos(p);
119  if (pId == -1) return false;
120 
121  const int nId = indexNormal(n);
122  if (nId == -1) return false;
123 
126  neiArray arr;
127  neiFun.get<dim>( pId, _egSize, arr );
128 
129  for (auto& gid : arr){
130  if (gid != -1) {
131  if (_grid[gid] == NULL) _grid[gid] = new AngularGrid;
132  (_grid[gid])->at(nId).push_back(id);
133  }
134  }
135 // if (_grid[pId] == NULL) _grid[pId] = new AngularGrid;
136 // (_grid[pId])->at(nId).push_back(id);
137 
138 
139  return true;
140 }
141 
142 
143 template <class Point, int dim, int _ngSize, typename Scalar>
144 void
146  const Point& p,
147  std::vector<unsigned int>&nei)
148 {
150  if ( grid == NULL ) return;
151 
152  for(typename AngularGrid::const_iterator it = grid->cbegin();
153  it != grid->cend(); it++){
154  const std::vector<unsigned int>& lnei = *it;
155  nei.insert( nei.end(), lnei.begin(), lnei.end() );
156  }
157 }
158 
159 
160 template <class Point, int dim, int _ngSize, typename Scalar>
161 void
163  const Point& p,
164  const Point& n,
165  std::vector<unsigned int>&nei)
166 {
168  if ( grid == NULL ) return;
169 
170  const std::vector<unsigned int>& lnei = grid->at(indexNormal(n));
171  nei.insert( nei.end(), lnei.begin(), lnei.end() );
172 }
173 
174 
175 template <class Point, int dim, int _ngSize, typename Scalar>
176 void
178  const Point& p,
179  const Point& n,
181  std::vector<unsigned int>&nei,
182  bool tryReverse)
183 {
184  // FIXME_REFACTORING
185  //F for(const auto&lnei : (*grid))
186  //F nei.insert( nei.end(), lnei.begin(), lnei.end() );
187  //F }
188  //F return;
189 
191  if ( grid == NULL ) return;
192  {
193  // END_FIXME_REFACTORING
194 
195  ////////////// TESTS
196 // gr::Utils::OneRingNeighborhood neiFun;
197 // static const int dddim = 3;
198 // using neiArray = typename gr::Utils::OneRingNeighborhood::NeighborhoodType<dddim>::type;
199 // neiArray arr;
200 // neiFun.get<dddim>( 3, 4, arr );
201 // for (const auto&e : arr) std::cout << e << " ";
202 // std::cout << "\n";
203 
204  ////////////// TESTS END
205 
206  const Scalar alpha = std::acos(cosAlpha);
207  const Scalar perimeter = Scalar(2) * M_PI * std::atan(alpha);
208  const unsigned int nbSample = 2*std::ceil(perimeter*Scalar(_ngSize) /Scalar(2.));
209  const Scalar angleStep = Scalar(2) * M_PI / Scalar(nbSample);
210 
211  const Scalar sinAlpha = std::sin(alpha);
212 
214  q.setFromTwoVectors(Point(0.,0.,1.), n);
215 
216  std::set<unsigned int> colored;
217 // for (AngularGrid* grid: angularGrids(p)) {//_grid){
218 // for (AngularGrid* grid: _grid){
219 // if (grid == NULL) continue;
220 
221  // Do the rendering independently of the content
222  for(unsigned int a = 0; a != nbSample; a++){
224  const Point dir = ( q * Point(sinAlpha*std::cos(theta),
225  sinAlpha*std::sin(theta),
226  cosAlpha ) ).normalized();
227  int id = indexNormal( dir );
228  if(grid->at(id).size() != 0){
229  colored.insert(id);
230  }
231 
232  if (tryReverse){
233  id = indexNormal( -dir );
234  if(grid->at(id).size() != 0){
235  colored.insert(id);
236  }
237  }
238  }
239 
240  for( std::set<unsigned int>::const_iterator it = colored.cbegin();
241  it != colored.cend(); it++){
242  const std::vector<unsigned int>& lnei = grid->at(*it);
243  nei.insert( nei.end(), lnei.begin(), lnei.end() );
244  }
245  } //F
246 }
247 
248 } // namespace Super4CS
249 
250 
251 #endif // _INDEXED_NORMAL_SET_HPP_
#define M_PI
Definition: utils.h:57
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