sdf_tracker.h

Go to the documentation of this file.

#include <boost/thread/mutex.hpp>

#include <fstream>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/StdVector>
#include <time.h>

#define EIGEN_USE_NEW_STDVECTOR

#ifndef SDF_TRACKER
#define SDF_TRACKER

class SDF_Parameters
{
public:
  bool interactive_mode;
  int XSize;
  int YSize;
  int ZSize;
  int raycast_steps;
  int image_height;
  int image_width;
  double fx;
  double fy;
  double cx;
  double cy;
  double Wmax;
  double resolution;
  double Dmax; 
  double Dmin;
  Eigen::Matrix4d pose_offset;
  double robust_statistic_coefficient;
  double regularization;
  double min_parameter_update;
  double min_pose_change;
  std::string render_window;

  SDF_Parameters();
  virtual ~SDF_Parameters();
};

typedef Eigen::Matrix<double,6,1> Vector6d; 

class SDFTracker
{
  protected:
  // variables
  std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > transformations_;
  std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > interest_points_;
  std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > Points_;

  Eigen::Matrix4d Transformation_;
  Vector6d Pose_;
  Vector6d cumulative_pose_;
  cv::Mat *depthImage_;
  cv::Mat *depthImage_denoised_;

  boost::mutex transformation_mutex_;
  boost::mutex depth_mutex_;
  boost::mutex points_mutex_;
  boost::mutex depthDenoised_mutex_;
  std::string camera_name_;
  
  bool** validityMask_;
  float*** myGrid_; 
  bool first_frame_;
  bool quit_;
  // functions 
  Eigen::Vector4d VertexInterp(double iso, Eigen::Vector4d &p1d, Eigen::Vector4d &p2d,double valp1, double valp2);
  void MarchingTetrahedrons(Eigen::Vector4d &Origin, int tetrahedron);
  virtual void Init(SDF_Parameters &parameters);
  virtual void DeleteGrids(void);

  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  std::vector<Eigen::Vector4d> triangles_;
  SDF_Parameters parameters_;
  
  virtual double SDF(const Eigen::Vector4d &location);

  virtual double SDFGradient(const Eigen::Vector4d &location, int dim, int stepSize);

  virtual void SaveSDF(const std::string &filename = std::string("sdf_volume.vti"));
  
  virtual void LoadSDF(const std::string &filename);

  bool ValidGradient(const Eigen::Vector4d &location);

  virtual void UpdateDepth(const cv::Mat &depth);   
  
  virtual void UpdatePoints(const std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > &Points);   

  
  virtual Vector6d EstimatePoseFromDepth(void); 

  virtual Vector6d EstimatePoseFromPoints(void); 

  virtual void FuseDepth(void);

  virtual void FusePoints(void);

  virtual void FuseDepth(const cv::Mat &depth);

  virtual void Render(void);

  cv::Point2d To2D(const Eigen::Vector4d &location, double fx, double fy, double cx, double cy);

  Eigen::Vector4d To3D(int row, int column, double depth, double fx, double fy, double cx, double cy);
  
  Eigen::Matrix4d Twist(const Vector6d &xi);

  void MakeTriangles(void);

  void SaveTriangles(const std::string filename = std::string("triangles.obj"));

  void SaveTrianglesSTL(const std::string filename = std::string("triangles.stl"));
  void GetDenoisedImage(cv::Mat &img); 

  Eigen::Matrix4d GetCurrentTransformation(void);

  void SetCurrentTransformation(const Eigen::Matrix4d &T); 

  Eigen::Vector3d ShootSingleRay(int row, int col, Eigen::Matrix4d &pose);

  Eigen::Vector3d ShootSingleRay(int row, int col);
  
  Eigen::Vector3d ShootSingleRay(Eigen::Vector3d &start, Eigen::Vector3d &direction);

  cv::Mat* GetCurrentDepthImg() {
      return depthImage_;
  }
  bool Quit(void);

  SDFTracker();

  SDFTracker(SDF_Parameters &parameters);
  virtual ~SDFTracker();   

};

#endif