normalset.h

// Copyright 2014 Nicolas Mellado
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// -------------------------------------------------------------------------- //
//
// Authors: Nicolas Mellado
//
// An implementation of the Super 4-points Congruent Sets (Super 4PCS)
// algorithm presented in:
//
// Super 4PCS: Fast Global Pointcloud Registration via Smart Indexing
// Nicolas Mellado, Dror Aiger, Niloy J. Mitra
// Symposium on Geometry Processing 2014.
//
// Data acquisition in large-scale scenes regularly involves accumulating
// information across multiple scans. A common approach is to locally align scan
// pairs using Iterative Closest Point (ICP) algorithm (or its variants), but
// requires static scenes and small motion between scan pairs. This prevents
// accumulating data across multiple scan sessions and/or different acquisition
// modalities (e.g., stereo, depth scans). Alternatively, one can use a global
// registration algorithm allowing scans to be in arbitrary initial poses. The
// state-of-the-art global registration algorithm, 4PCS, however has a quadratic
// time complexity in the number of data points. This vastly limits its
// applicability to acquisition of large environments. We present Super 4PCS for
// global pointcloud registration that is optimal, i.e., runs in linear time (in
// the number of data points) and is also output sensitive in the complexity of
// the alignment problem based on the (unknown) overlap across scan pairs.
// Technically, we map the algorithm as an 'instance problem' and solve it
// efficiently using a smart indexing data organization. The algorithm is
// simple, memory-efficient, and fast. We demonstrate that Super 4PCS results in
// significant speedup over alternative approaches and allows unstructured
// efficient acquisition of scenes at scales previously not possible. Complete
// source code and datasets are available for research use at
// http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/.


#ifndef _SUPER4PCS_ACCELERATORS_INDEXED_NORMAL_SET_H_
#define _SUPER4PCS_ACCELERATORS_INDEXED_NORMAL_SET_H_

#include "super4pcs/utils/disablewarnings.h"
#include "super4pcs/accelerators/utils.h"

namespace GlobalRegistration{

template <
  class Point,      
  int dim,          
  int _ngSize,      
  typename _Scalar   
  >
struct IndexedNormalSet{
  typedef std::array< std::vector<unsigned int>,
                      Utils::POW(_ngSize, dim)> AngularGrid;

  enum MOVE_DIR { POSITIVE, NEGATIVE };
  using Scalar = _Scalar;

#ifdef DEBUG
#define VALIDATE_INDICES true
#pragma message "Index validation is enabled in debug mode"
#else
#define VALIDATE_INDICES false
#endif

private:
  static constexpr Scalar _nepsilon = Scalar(1.)/Scalar(_ngSize) + 0.00001;
  std::vector<AngularGrid*> _grid;
  Scalar _epsilon;
  int _egSize;    

  inline int indexPos   ( const Point& p) const;
  inline int indexNormal( const Point& n) const;

  inline Point coordinatesPos   ( const Point& p) const
  { return p/_epsilon;  }
  inline Point coordinatesNormal( const Point& n) const
  {
    static const Point half = Point::Ones()/Scalar(2.);
    return (n/Scalar(2.) + half)/_nepsilon;
  }

  inline int indexCoordinatesPos   ( const Point& pCoord) const;
  inline int indexCoordinatesNormal( const Point& nCoord) const;

  //inline Point indexToPos (int id) const;

public:
  inline IndexedNormalSet(const Scalar epsilon)
  : _epsilon(epsilon) {
    const int gridDepth = -std::log2(epsilon);
    _egSize = std::pow(2,gridDepth);
    _epsilon = 1.f/_egSize;

    _grid = std::vector<AngularGrid*> (std::pow(_egSize, dim), NULL);
  }

  virtual inline ~IndexedNormalSet();

  inline bool addElement(const Point& pos,
                         const Point& normal,
                         unsigned int id);

  inline AngularGrid* angularGrid(const Point& p) {
    const int pId = indexPos(p);
    if (pId == -1) return NULL;
    return _grid[pId];
  }

  inline void getNeighbors( const Point& p,
                            std::vector<unsigned int>&nei);
  inline void getNeighbors( const Point& p,
                            const Point& n,
                            std::vector<unsigned int>&nei);
  inline void getNeighbors( const Point& p,
                            const Point& n,
                            Scalar alpha,
                            std::vector<unsigned int>&nei,
                            bool tryReverse = false);
};

} // namespace Super4PCS

#include "super4pcs/accelerators/normalset.hpp"

#endif // _INDEXED_NORMAL_SET_H_