svdicp.hpp

#ifndef ICP_SVDICP_HPP
#define ICP_SVDICP_HPP

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

namespace jafar {
  namespace icp {
    
    class SVDICP {
    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;

      /*
       * @param qr: input quaternion rotation part only or full vector in which case
       * the rotation part is assumed to be in the 4th first elements
       * @return R: computed rotation matrix
       */

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

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

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

      ~SVDICP()
      {
        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.size1()) 
        row(m_model_points,m_model_nb_points) = entry;
        m_model_nb_points++;
      }

      void add_scene_entry(const jblas::vec3& entry)
      {
        JFR_ASSERT(entry.size() == m_scene_points.size2(),
                   "entry must be of dimension "<<m_scene_points.size1()) 
        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);
      //geom::T3D *guess = NULL
      void run();
    };
  }
}

#endif