kinect_modeler.hpp

#ifndef MODELER_KINECT_MODELER_HPP
#define MODELER_KINECT_MODELER_HPP

#include "fdetect/DetectionResult.hpp"
#include "fdetect/Detector.hpp"
#include "fdetect/FloatDescriptor.hpp"
#include "fdetect/SURFDetector.hpp"
#include "gfm/GroupsMatcher.hpp"
#include "gfm/MatchingResult.hpp"
#include "gfm/EngineTracking.hpp"
#include "gfm/DescriptorsBasedMatcher.hpp"
#include "image/Image.hpp"
#include "jmath/jblas.hpp"
#include "jmath/ublasExtra.hpp"
#include "modeler/mesher.hpp"
#include "modeler/bpa.hpp"
#include "modeler/vcgDeclarations.hpp"
#include "jmath/pca.hpp"
//#include "icp/gicp.hpp"
#include "geom/t3d.hpp"
#include "geom/t3dPointTools.hpp"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp_nl.h>
#include <boost/bind.hpp>
#include <boost/bimap.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/graphviz.hpp>

namespace jafar {
  namespace modeler {

    class KinectModeler {
    public:
      typedef ublas::bounded_vector<float, 3> fvec3;
    protected:
      typedef fdetect_v2::InterestFeatureWithData<fdetect_v2::FloatDescriptor, cv::Point3f> Feature;
      typedef fdetect_v2::DetectionResult<Feature> DR;
      typedef gfm_v2::MatchingResult<Feature> MR;
      typedef gfm_v2::GroupsMatcher<Feature> GroupsMatcher;
      typedef gfm_v2::MatchSourceInfo<Feature> MatchSourceInfoType;
      fdetect_v2::Detector<Feature> *detector;
      gfm_v2::Matcher<Feature> *matcher;
      gfm_v2::EngineTracking<Feature> *tracker;
      typedef boost::bimap<Feature*, unsigned int> bm_type;
      bm_type features_indices;
      typedef bm_type::left_map::const_iterator features_citerator;
      typedef bm_type::right_map::const_iterator indices_citerator;
      std::map<unsigned int, DR > sequence_detect;
      size_t counter;
      unsigned int index;
      unsigned int global;
      int status;
      struct vertex
      {
        JfrMesh::CoordType p;
        std::vector<unsigned int> origine;
        vertex() {};
        vertex(const JfrMesh::CoordType& _p, 
               const std::vector<unsigned int>& _origine) : 
          p(_p), origine(_origine) {};
        void setP(const jblas::vec3& c) { p[0] = c[0]; p[1] = c[1]; p[2] = c[2]; }
      };
      std::map<unsigned int, vertex> vertices;
      // ublas::triangular_matrix<MR, ublas::upper> matches;
    public:
      KinectModeler(fdetect_v2::Detector<Feature> *_detector = new fdetect_v2::SURFDetectorT<Feature>(),
                    gfm_v2::Matcher<Feature> *_matcher = new gfm_v2::GroupsMatcher<Feature>( new gfm::WindowMatchFocus(100000, 100000), 10, 10, 2, 20.0, 0.6, 0.6, 0.1, 0.2, 10000, 10, 0.4, 0.0)):
        detector(_detector), matcher(_matcher),
        counter(0), index(0), global(0), images_graph(3) 
        
      {}
      void add(const image::Image& color_img, const ublas::matrix<float>& depth_image);

      void build();
      void analyse();
      inline void write(const std::string& filename) {
        std::ofstream outfile;
        outfile.open(filename.c_str());
        if(outfile.is_open())
          write_graphviz(outfile, images_graph);
        outfile.close();
      }
    private:
      // typedef jmath::PCA_T<float> FPCA;
      // std::map<size_t, FPCA> pcaz;
      // ublas::matrix<float> entry;
      // ublas::matrix<float> previous_entry;
      // ublas::matrix<float> reference;
      std::map<size_t, ublas::matrix<float> > depth_map;
      std::map<size_t, image::Image > images_map;
      std::map<size_t, pcl::PointCloud<pcl::PointXYZ> > cloud_map;
      std::map<size_t, MatchSourceInfoType *> match_infos;
      int previous_width;
      MatchSourceInfoType *m_source_info;
      // geom::T3D* final;
      // geom::T3D *src_to_dst;
//      icp::GICP *gicp;
      jblas::mat44 H;
      fvec3 ref_mean, match_mean, transformed_mean;
      std::vector<unsigned char> outliers;
      //graph stuff
      struct edge_type{
        MR matches;
        geom::T3D *trans;
        double error;
      };
      typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, boost::no_property, edge_type> Graph;
      Graph images_graph;
      typedef Graph::edge_descriptor Edge;
      typedef Graph::vertex_descriptor Vertex;
      int nb_componenets;
      inline bool is_invalid(const ublas::matrix<float>& M, size_t r)
      {
        return (std::abs(M(r,2)) == 0) || 
               (std::abs(M(r,1)) == 0) || 
               (std::abs(M(r,0)) == 0);
      }
      inline bool is_invalid(Feature* f, const ublas::matrix<float>& M, int width)
      {
        return is_invalid(M, (int(f->v())*width)+int(f->u()));
      }
      void means(const MR& paired, fvec3& ref_mean, fvec3& match_mean) 
      {
        cv::Point3f ref_pt;
        cv::Point3f match_pt;
        for(MR::const_iterator it = paired.begin(); it != paired.end(); ++it) {
          ref_pt+=it->point_ref->getData();
          match_pt+=it->point_match->getData();
        }
        ref_mean[0] = ref_pt.x; ref_mean[1] = ref_pt.y; ref_mean[2] = ref_pt.z;
        match_mean[0] = match_pt.x; match_mean[1] = match_pt.y; match_mean[2] = match_pt.z;
        ref_mean/=paired.size();
        match_mean/=paired.size();        
      }
      void erase(MR& mr, const std::vector<unsigned char>& outliers) {
        MR::iterator result = mr.begin();
        size_t counter = 0;
        for (MR::iterator first = mr.begin(); first != mr.end(); ++first, counter++)
        {
          cout << "outliers[counter] "<<outliers[counter]<<endl;
          if (!outliers[counter]) *result++ = *first;
        }
        if(result != mr.end())
          mr.erase(result, mr.end());
      }
      void fill_cloud_with_contours(size_t image_idx)
      {
        vector<vector<cv::Point> > contours;
        vector<cv::Vec4i> hierarchy;
        image::Image gray;
        images_map[image_idx].copyTo(gray);
        cv::threshold( gray, gray, 128, 255, CV_THRESH_BINARY );
        cv::findContours( gray, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE );
        if( contours.size() > 0 )
        {
          size_t nb_points = 0;
          for(vector<vector<cv::Point> >::const_iterator this_contour = contours.begin();
              this_contour != contours.end();
              ++this_contour)
            nb_points+=this_contour->size();
          
          pcl::PointCloud<pcl::PointXYZ> &cloud = cloud_map[image_idx];
          cloud.width = 1;
          cloud.height = nb_points;
          cloud.points.resize(nb_points);
          cloud.is_dense = false;
          // iterate through all the top-level contours,
          size_t pos = 0;
          for(int counter =0; counter >= 0; counter = hierarchy[counter][0]) 
          {
            //iterate through each contour points
            for(std::vector<cv::Point>::iterator pit = contours[counter].begin(); 
                pit != contours[counter].end(); 
                ++pit, pos++) 
            {
              size_t idx = (pit->x * images_map[idx].width()) + pit->y;
              const ublas::matrix<float> &entry = depth_map[image_idx];
              if(!is_invalid(entry, idx))
              {
                cloud.points[pos].x = entry(idx,0);
                cloud.points[pos].y = entry(idx,1);
                cloud.points[pos].z = entry(idx,2);
              }
            }
          }
          assert(pos == nb_points);
        }
      }

      template <class InputIterator, class OutputIterator, class T>
      void copy_others ( InputIterator first, InputIterator last,
                         OutputIterator result, const T& value )
      {
        for ( ; first != last; ++first)
          if (!(*first == value)) *result++ = first;
      }
    };//kinect modeler
  }// namespace modeler
}//namespace jafar

#endif