oicp.hpp

#ifndef ICP_OICP_HPP
#define ICP_OICP_HPP

#include <iostream>
#include "jmath/jann.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"

namespace jafar {
  namespace icp {
    
    class OICP {
    private:
      double m_epsilon;
      double m_max_dist;
      int m_max_iter;
      unsigned int m_model_nb_points;
      unsigned int m_scene_nb_points;
      ublas::matrix<double> m_model_points;
      ublas::matrix<double> m_scene_points;
      jann::KD_tree_index< flann::L2<double> > *m_model_index;
      size_t m_K;
      jblas::mat R;
      jblas::vec T;

      inline void barycenter(const jblas::mat&points, jblas::vec& center);
    public: 

      OICP(size_t max_nb_points = 1024, double epsilon = 1e-4, double max_dist = 5., 
           // int K = 20, 
           int max_iter = 100) : 
        m_epsilon(epsilon), m_max_dist(max_dist), m_max_iter(max_iter), m_K(1), 
        m_model_nb_points(0), m_scene_nb_points(0) {}

      OICP(const jblas::mat& model, jblas::mat& scene, 
           double epsilon = 1e-4, double max_dist = 5., 
           // size_t K = 20, 
           int max_iter = 100) : 
        m_epsilon(epsilon), m_max_dist(max_dist), m_max_iter(max_iter), m_K(1),
        m_model_nb_points(model.size1()), m_scene_nb_points(scene.size1()),
        m_model_points(model), m_scene_points(scene)
      {
        JFR_ASSERT(scene.size2() == model.size2(),
                   "points of model and scene must be of same dimension")
      }

      ~OICP()
      {
        delete m_model_index;
      }

      void add_model_entry(const jblas::vec& entry)
      {
        JFR_ASSERT(entry.size() == m_model_points.size2(),
                   "entry must be of dimension " << m_model_points.size2()) 
        // JFR_ASSERT(m_model_nb_points < max_nb_points,
        //                                IcpException,
        //                                IcpException::OVERFLOW,
        //            "max number of entries already reached for model")
        row(m_model_points,m_model_nb_points) = entry;
        m_model_nb_points++;
      }

      void add_scene_entry(const jblas::vec& entry)
      {
        JFR_ASSERT(entry.size() == m_scene_points.size2(),
                   "entry must be of dimension " << m_scene_points.size2()) 
        // JFR_ASSERT(m_scene_nb_points < max_nb_points,
        //            //                    IcpException,
        //            //                    IcpException::OVERFLOW,
        //            "max number of entries already reached for scene")
        row(m_scene_points,m_scene_nb_points) = entry;
        m_scene_nb_points++;
      }
      void compute_RT(const jblas::mat& model, const jblas::mat& scene,
                      jblas::mat& R, jblas::vec& T);
      void run();
    };
  }
}

#endif