sdf_tracker.h
Go to the documentation of this file.
00001 #include <boost/thread/mutex.hpp>
00002 
00003 #include <fstream>
00004 #include <iostream>
00005 #include <Eigen/Core>
00006 #include <unsupported/Eigen/MatrixFunctions>
00007 
00008 #include <opencv2/core/core.hpp>
00009 #include <opencv2/imgproc/imgproc.hpp>
00010 #include <opencv2/highgui/highgui.hpp>
00011 #include <Eigen/StdVector>
00012 #include <time.h>
00013 
00014 #define EIGEN_USE_NEW_STDVECTOR
00015 
00016 #ifndef SDF_TRACKER
00017 #define SDF_TRACKER
00018 
00019 class SDF_Parameters
00020 {
00021 public:
00022   bool interactive_mode;
00023   int XSize;
00024   int YSize;
00025   int ZSize;
00026   int raycast_steps;
00027   int image_height;
00028   int image_width;
00029   double fx;
00030   double fy;
00031   double cx;
00032   double cy;
00033   double Wmax;
00034   double resolution;
00035   double Dmax; 
00036   double Dmin;
00037   Eigen::Matrix4d pose_offset;
00038   double robust_statistic_coefficient;
00039   double regularization;
00040   double min_parameter_update;
00041   double min_pose_change;
00042   std::string render_window;
00043 
00044   SDF_Parameters();
00045   virtual ~SDF_Parameters();
00046 };
00047 
00048 typedef Eigen::Matrix<double,6,1> Vector6d; 
00049 
00050 class SDFTracker
00051 {
00052   protected:
00053   // variables
00054   std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > transformations_;
00055   std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > interest_points_;
00056   std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > Points_;
00057 
00058   Eigen::Matrix4d Transformation_;
00059   Vector6d Pose_;
00060   Vector6d cumulative_pose_;
00061   cv::Mat *depthImage_;
00062   cv::Mat *depthImage_denoised_;
00063 
00064   boost::mutex transformation_mutex_;
00065   boost::mutex depth_mutex_;
00066   boost::mutex points_mutex_;
00067   boost::mutex depthDenoised_mutex_;
00068   std::string camera_name_;
00069   
00070   bool** validityMask_;
00071   float*** myGrid_; 
00072   bool first_frame_;
00073   bool quit_;
00074   SDF_Parameters parameters_;
00075   // functions 
00076   Eigen::Vector4d VertexInterp(double iso, Eigen::Vector4d &p1d, Eigen::Vector4d &p2d,double valp1, double valp2);
00077   void MarchingTetrahedrons(Eigen::Vector4d &Origin, int tetrahedron);
00078   virtual void Init(SDF_Parameters &parameters);
00079   virtual void DeleteGrids(void);
00080 
00081   public:
00082   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00083 
00084   std::vector<Eigen::Vector4d> triangles_;
00085   
00087   virtual double SDF(const Eigen::Vector4d &location);
00088 
00090   virtual double SDFGradient(const Eigen::Vector4d &location, int dim, int stepSize);
00091 
00093   virtual void SaveSDF(const std::string &filename = std::string("sdf_volume.vti"));
00094   
00096   virtual void LoadSDF(const std::string &filename);
00097 
00099   bool ValidGradient(const Eigen::Vector4d &location);
00100 
00102   virtual void UpdateDepth(const cv::Mat &depth);   
00103   
00105   virtual void UpdatePoints(const std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > &Points);   
00106 
00107   
00109   virtual Vector6d EstimatePoseFromDepth(void); 
00110 
00112   virtual Vector6d EstimatePoseFromPoints(void); 
00113 
00115   virtual void FuseDepth(void);
00116 
00118   virtual void FusePoints(void);
00119 
00121   virtual void FuseDepth(const cv::Mat &depth);
00122 
00124   virtual void Render(void);
00125 
00127   cv::Point2d To2D(const Eigen::Vector4d &location, double fx, double fy, double cx, double cy);
00128 
00130   Eigen::Vector4d To3D(int row, int column, double depth, double fx, double fy, double cx, double cy);
00131   
00133   Eigen::Matrix4d Twist(const Vector6d &xi);
00134 
00136   void MakeTriangles(void);
00137 
00139   void SaveTriangles(const std::string filename = std::string("triangles.obj"));
00140 
00142   void SaveTrianglesSTL(const std::string filename = std::string("triangles.stl"));
00144   void GetDenoisedImage(cv::Mat &img); 
00145 
00147   Eigen::Matrix4d GetCurrentTransformation(void);
00148 
00150   void SetCurrentTransformation(const Eigen::Matrix4d &T); 
00151   
00153   bool Quit(void);
00154 
00156   SDFTracker();
00157 
00159   SDFTracker(SDF_Parameters &parameters);
00160   virtual ~SDFTracker();   
00161 
00162 };
00163 
00164 #endif


sdf_tracker
Author(s): Daniel Canelhas
autogenerated on Mon Oct 6 2014 03:19:15