gicp.hpp

#ifndef ICP_GICP_HPP
#define ICP_GICP_HPP

#include "jmath/jblas.hpp"
#include "kernel/jafarMacro.hpp"
#include "geom/t3dEuler.hpp"
#include <lbfgs.h>
#include <ANN.h>
#include <gsl/gsl_vector.h>
namespace jafar {
  namespace icp {
    class GICP {
    private:
      ANNpointArray m_model_points;
      ANNpointArray m_scene_points;
      ANNkd_tree *m_model_tree;
      ANNkd_tree *m_scene_tree;
      unsigned int m_model_nb_points;
      unsigned int m_scene_nb_points;
      
      jblas::mat R,t;
      int m_K;
      double m_epsilon;
      double m_max_dist;
      //      typedef std::vector< cv::Mat > covariances;
      typedef std::vector< jblas::mat_column_major > covariances;
      // typedef typename std:vector< cv::Mat >::iterator a_covariance;
      covariances m_scene_covs;
      covariances m_model_covs;
      int m_max_iter;
      int m_lbfgs_max_iter;
      lbfgs_parameter_t m_lbfgs_param;
      double m_lbfgs_ftol;
      double m_lbfgs_gtol;
      double* m_x;
      double m_fx;
      int nb_matches;
      geom::T3D *initial_t3d;
      geom::T3D *final_t3d;
      std::vector<jblas::mat_column_major> mahalanobis;
      //      std::vector< cv::Mat > indexes;
      //      std::vector< cv::Mat > distances;
      ANNidx *indexes;
      std::vector< ANNdist > distances;

      static double _evaluate(void* instance,
                              const double *x,
                              double *g,
                              const int n,
                              const double step);
      double evaluate(const double *x,
                      double *g,
                      const int n,
                      const double step);

      static int _progress(void* instance,
                           const double *x,
                   const double *g,
                   const double fx,
                   const double xnorm,
                   const double gnorm,
                   const double step,
                   int n,
                   int k,
                   int ls);

      int progress(const double *x,
                   const double *g,
                   const double fx,
                   const double xnorm,
                   const double gnorm,
                   const double step,
                   int n,
                   int k,
                   int ls);

// compute trace of a^t.b
inline double trace_at_b(const jblas::mat &a, const jblas::mat& b) {
  JFR_ASSERT(a.size1() == b.size2(), "matrices need to be multipliable")
  double trace = 0.;
  int n = a.size1();
  for(int i = 0; i < n; i++)
    for(int j = 0; j < n; j++)
      trace+= a(j,i) * b(i,j);

  return trace;
}

public: 
      GICP(size_t max_nb_points = 1024, double epsilon = 1e-3, double max_dist = 5., 
           int K = 20, int max_iter = 100,
           int inner_max_iter = 8, double ftol = 0.01, double gtol = 0.01) : 
        m_K(K), m_epsilon(epsilon), m_max_dist(max_dist), m_max_iter(max_iter), 
        m_lbfgs_max_iter(inner_max_iter), m_lbfgs_ftol(ftol), m_lbfgs_gtol(gtol),
        m_model_nb_points(0), m_scene_nb_points(0), 
        mahalanobis(0),
        indexes(0), 
        distances(0)
      {
        m_model_points = annAllocPts(max_nb_points, 3);
        m_scene_points = annAllocPts(max_nb_points, 3);
      }

      GICP(const jblas::mat& model, jblas::mat& scene, 
           double epsilon = 1e-3, double max_dist = 5., 
           int K = 20, int max_iter = 100, 
           int inner_max_iter = 8, double ftol = 0.01, double gtol = 0.01) : 
        m_K(K), m_epsilon(epsilon), m_max_dist(max_dist), m_max_iter(max_iter), 
        m_lbfgs_max_iter(inner_max_iter), m_lbfgs_ftol(ftol), m_lbfgs_gtol(gtol),
        // indexes(m_model.rows, cv::Mat(1, 1, CV_32S)), 
        // distances(m_model.rows, cv::Mat(1, 1, CV_32F)),
        m_model_nb_points(model.size1()),
        m_scene_nb_points(scene.size1()),
        mahalanobis(0)
      {
        using namespace std;
        m_model_points = annAllocPts(m_model_nb_points, 3);
        m_scene_points = annAllocPts(m_scene_nb_points, 3);
        for (unsigned int mrow = 0; mrow < m_model_nb_points; mrow++) {
          m_model_points[mrow][0] = model(mrow,0);
          m_model_points[mrow][1] = model(mrow,1);
          m_model_points[mrow][2] = model(mrow,2);
        }
        cout << "loaded "<<m_model_nb_points<<" points for the model"<<endl;
        for (unsigned int srow = 0; srow < m_scene_nb_points; srow++) {
          m_scene_points[srow][0] = scene(srow,0);
          m_scene_points[srow][1] = scene(srow,1);
          m_scene_points[srow][2] = scene(srow,2);
        }
        cout << "loaded "<<m_scene_nb_points<<" points for the scene"<<endl;
      }

      void addModelEntry(const jblas::vec3& entry)
      {
        m_model_points[m_model_nb_points][0] = entry[0];
        m_model_points[m_model_nb_points][1] = entry[1];
        m_model_points[m_model_nb_points][2] = entry[2];
        ++m_model_nb_points;
      }

      void addSceneEntry(const jblas::vec3& entry)
      {
        m_scene_points[m_scene_nb_points][0] = entry[0];
        m_scene_points[m_scene_nb_points][1] = entry[1];
        m_scene_points[m_scene_nb_points][2] = entry[2];
        ++m_scene_nb_points;
      }

      inline void transform(const geom::T3D* t, jblas::vec3& pt) {
        jblas::mat44 M = t->getM();
        assert(M(3,3) == 1);
        double x, y, z;
        x = (M(0,0) * pt[0]) + (M(0,1) * pt[1]) + (M(0,2) * pt[2]) + M(0,3);
        y = (M(1,0) * pt[0]) + (M(1,1) * pt[1]) + (M(1,2) * pt[2]) + M(1,3);
        z = (M(2,0) * pt[0]) + (M(2,1) * pt[1]) + (M(2,2) * pt[2]) + M(2,3);
        pt[0] = x;
        pt[1] = y;
        pt[2] = z;
      }

      void computeCovarianceMatrices(const ANNpointArray& points, 
                                     size_t nb_points,
                                     ANNkd_tree* points_tree,
                                     covariances& points_covariances);

      int align(const ANNpointArray& model_points, unsigned int nb_model_points, 
                const covariances& model_covariances,
                const ANNpointArray& scene_points, ANNkd_tree* scene_tree, 
                const covariances& scene_covariances,
                const geom::T3D *guess, geom::T3D *t);
      int align_gsl(const ANNpointArray& model, unsigned int nb_model_points, 
                    const covariances& model_covariances,
                    const ANNpointArray& scene, ANNkd_tree* scene_tree,
                    const covariances& scene_covariances,
                    const geom::T3D *guess, geom::T3D *t);
      void run(geom::T3DEuler *guess = NULL) 
      {
        using namespace std;
        //build kd tree for the model points
        m_model_tree = new ANNkd_tree(m_model_points, m_model_nb_points, 3, 10);
        cout << "m_model_index created" << endl;
        computeCovarianceMatrices(m_model_points, m_model_nb_points, m_model_tree, m_model_covs);
        //build kd tree for the scene points
        m_scene_tree = new ANNkd_tree(m_scene_points, m_scene_nb_points, 3, 10);
        cout << "m_scene_index created" << endl;
        computeCovarianceMatrices(m_scene_points, m_scene_nb_points, m_scene_tree, m_scene_covs);
        
        final_t3d = geom::T3D::create(geom::T3D::EULER);
        initial_t3d = geom::T3D::create(geom::T3D::EULER);
        initial_t3d->set(jblas::identity_mat(3,3), jblas::zero_vec(3));
        final_t3d->set(jblas::identity_mat(3,3), jblas::zero_vec(3));
        if(guess != NULL) 
        {
          initial_t3d = guess;
        } 

        //  align(m_model_points, m_model_nb_points, m_scene_points, m_scene_tree, initial_t3d, t1);
        //align(m_model_points, m_model_nb_points, m_model_covs, m_scene_points, m_scene_tree, m_scene_covs, initial_t3d, final_t3d);
        align_gsl(m_model_points, m_model_nb_points, m_model_covs, m_scene_points, m_scene_tree, m_scene_covs, initial_t3d, final_t3d);
        //  cout << "guess " << *guess << endl;
        cout << "initial t3d" << initial_t3d->getM() << endl;
        cout << "final t3d" << final_t3d->getM() << endl;
      }
      double f(const gsl_vector *x, void *params);
      void df(const gsl_vector *x, void *params, gsl_vector *g);
      void fdf(const gsl_vector *x, void *params, double *f, gsl_vector *g);
      geom::T3D* get_final_t3d() 
      {
        return final_t3d;
      }
    };
  }
}

#endif