volumetric representation and integration

FusionLib/include/Frame.h

@@ -17,11 +17,7 @@ class Frame {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     Frame(double* depthMap, const Eigen::Matrix3d& depthIntrinsics,
-            const unsigned int width, const unsigned int height, double maxDistance = 2);
-
-	void computeNormals(double maxDistance=0.1);
-
-	bool readFromFile(const std::string& filename);
+            unsigned int width, unsigned int height, double maxDistance = 2);
 
     bool WriteMesh(const std::string& filename, std::string color);
 
@@ -47,15 +43,10 @@ class Frame {
 
     const unsigned int getHeight() const;
 
-    double *getRawDepthMap() const;
-
     bool contains(const Eigen::Vector2i& point);
 
     Eigen::Vector3d projectIntoCamera(const Eigen::Vector3d& globalCoord);
     Eigen::Vector2i projectOntoPlane(const Eigen::Vector3d& cameraCoord);
-    Eigen::Vector2i findClosestPoint( const unsigned int u, const unsigned int v, Eigen::Vector3d target, const unsigned int range );
-    Eigen::Vector2i findClosestDistancePoint( const unsigned int u, const unsigned int v, Eigen::Vector3d target, const unsigned int range );
-
 
 private:
 
@@ -80,5 +71,4 @@ class Frame {
 
     const unsigned int m_width;
     const unsigned int m_height;
-    double* _rawDepthMap;
 };

FusionLib/include/Fusion.hpp

@@ -1,45 +1,51 @@
-//
-// Created by pbo on 08.07.19.
-//
-
 #pragma once
 
 #include "Volume.hpp"
 #include <Frame.h>
 #include <memory>
 class Fusion {
 public:
-//THIS method expects frame to hold all camera paramerters as well as the estimated pose --> TODO: check if those values are set or redefine method parameters
+    Fusion(double max_truncation_dist, double min_truncation_dist, double tsdf_max_weight);
 
-    bool reconstructSurface(std::shared_ptr<Frame> currentFrame,std::shared_ptr<Volume> volume,double truncationDistance);
+    //THIS method expects frame to hold all camera paramerters as well as the estimated pose --> TODO: check if those values are set or redefine method parameters
 
-private:
+    bool reconstructSurface(const std::shared_ptr<Frame>& currentFrame,const std::shared_ptr<Volume>& volume);
 
-    void reconstruct(std::shared_ptr<Frame> currentFrame,std::shared_ptr<Volume> volume,float truncationDistance);
-    //Reconstruction
-    bool calculateGlobal2CameraPoint(Eigen::Vector3d &currentCameraPosition, int x, int y, int z,
-									 Eigen::Matrix<double, 3, 3, Eigen::DontAlign> rotation,
-									 Eigen::Vector3d translation, double voxelScale);
+private:
 
-    bool pi(Eigen::Vector2i& unhomogenized,Eigen::Vector3d homogenized, Eigen::Matrix3d intrinsics,int width, int height);
+    /*bool calculateGlobal2CameraPoint(Eigen::Vector3d &currentCameraPosition, int x, int y, int z,
+									 const Eigen::Matrix<double, 3, 3>& rotation,
+									 const Eigen::Vector3d& translation, double voxelScale);
 
+    bool pi(Eigen::Vector2i& unhomogenized,const Eigen::Vector3d& homogenized, const Eigen::Matrix3d& intrinsics,int width, int height);
+*/
         /*!
          * The original implementation actually takes a raw depth Value, as we already calculated the camereSpacePoints
          * only the normalization has to be done.
          * TODO: move normalization to frame.cpp ; check if the cameraSpaceTransformation in frame.cpp equals the one used in Paper
          * @param cameraSpacePoint
          * @return the normalized cameraSpaceCoordinates
          */
-    double calculateLamdas(Eigen::Vector2i& cameraSpacePoint,Eigen::Matrix3d intrinsics);
-    /*!
+    /*double calculateLamdas(Eigen::Vector2i& cameraSpacePoint,const Eigen::Matrix3d& intrinsics);
+    *//*!
      *
      * @param lambda
      * @param cameraPosition
      * @param rawDepthValue
      * @return the signed-distance-function for the specific depth value lambda is based on
      */
 
-    double calculateSDF(double& lambda,Eigen::Vector3d& cameraPosition,double rawDepthValue);
+    double calculateSDF(Eigen::Vector3d& globalPoint ,Eigen::Vector3d& translation, double rawDepthValue);
+
+    void updateTSDFValue(const std::shared_ptr<Volume>& volume,
+            size_t x, size_t y, size_t z,
+            double current_tsdf,
+            double current_weight);
+
+
+    double _max_truncation_dist;
+    double _min_truncation_dist;
+    double _tsdf_max_weight;
 
 };
 

FusionLib/include/Raycast.hpp

@@ -51,7 +51,7 @@ class Raycast {
      */
 
     bool calculateCurrentPointOnRay(Eigen::Vector3d& currentPoint, double& rayParameter,
-            const Eigen::Vector3i& volumeSize,
+            const Eigen::Matrix<size_t,3,1>& volumeSize,
             const double voxelScale,
             const Eigen::Vector3d& origin, const Eigen::Vector3d& direction);
 

FusionLib/include/Volume.hpp

@@ -1,7 +1,3 @@
-//
-// Created by pbo on 08.07.19.
-//
-
 #pragma once
 
 #include <Eigen/Dense>
@@ -10,22 +6,30 @@
 
 class Volume {
 public:
-    Volume(const Eigen::Vector3i volumeSize, const double voxelScale);
+    Volume(const Eigen::Matrix<size_t,3,1>& volumeSize, double voxelScale, const double initial_tsdf);
 
     ~Volume()= default;
 
-    std::vector<std::pair<double,double>> &getPoints() ;
+    const std::vector<std::pair<double,double>> &getData() ;
+
+    const std::pair<double,double>& getVoxelData(size_t x, size_t y, size_t z);
 
+    void updateVoxelData(size_t x, size_t y, size_t z, double current_tsdf, double current_weight);
 
-	const Eigen::Vector3i &getVolumeSize() const;
+    const Eigen::Matrix<size_t,3,1> &getVolumeSize() const;
 
     float getVoxelScale() const;
 
 private:
-    //TODO: _points should not containt points, but tuples of tsdf & Weight
-    std::vector<std::pair<double,double>> _points;
-    const Eigen::Vector3i _volumeSize;
-    const double _voxelScale;
+    //tsdf_data contain tuple of tsdf & weight value for each point p in the global model
+    // vector size would be x * y * z
+    std::vector<std::pair<double,double>> _tsdfData;
+
+    // Overall size of the volume in mm
+    Eigen::Matrix<size_t,3,1> _volumeSize;
+
+    // Amount of mm one voxel will represent in each dimension
+    double _voxelScale;
 
 };
 

FusionLib/src/Frame.cpp

@@ -4,11 +4,10 @@
 
 Frame::Frame(double * depthMap, const Eigen::Matrix3d &depthIntrinsics,
         const unsigned int width, const unsigned int height, double maxDistance)
-        : m_intrinsic_matrix(depthIntrinsics),m_width(width),m_height(height),_rawDepthMap(depthMap){
+        : m_intrinsic_matrix(depthIntrinsics),m_width(width),m_height(height){
 
     m_depth_map.reserve(width*height);
 
-    std::cout << std::endl;
     for (size_t x = 0; x < width*height; x++) {
             m_depth_map.push_back(depthMap[x]);
     }
@@ -59,7 +58,6 @@ void Frame::addValidPoints(std::vector<Eigen::Vector3d> points, std::vector<Eige
             m_normals.push_back(normal);
         }
         else{
-            //m_depth_map[i] = MINF;
             m_points.emplace_back(Eigen::Vector3d(MINF, MINF, MINF));
             m_normals.emplace_back(Eigen::Vector3d(MINF, MINF, MINF));
         }
@@ -155,44 +153,6 @@ std::vector<Eigen::Vector3d> Frame::computeNormals(std::vector<Eigen::Vector3d>
     return normalsTmp;
 }
 
-Eigen::Vector2i Frame::findClosestPoint( const unsigned int u, const unsigned int v, Eigen::Vector3d target, const unsigned int range ){
-    double lowest_err = 10000;
-    int best_u = -1;
-    int best_v = -1;
-
-    for (size_t x = std::max((unsigned int)1, u - range); x < std::min(u + range, m_width-1); x++){
-        for (size_t y = std::max((unsigned int)1, v - range); y < std::min(v + range, m_height-1); y++){
-            double err = (m_normals_global[y*m_width + x].transpose() *
-                (m_points_global[ y*m_width + x ] - target)) ;
-            if (err < lowest_err){
-                best_u = x;
-                best_v = y;
-                lowest_err = err;
-            }
-        }
-    }
-
-    return Eigen::Vector2i(best_u, best_v);
-}
-
-Eigen::Vector2i Frame::findClosestDistancePoint( const unsigned int u, const unsigned int v, Eigen::Vector3d target, const unsigned int range ){
-    double lowest_err = 10000;
-    int best_u = -1;
-    int best_v = -1;
-
-    for (unsigned int x = std::max((unsigned int)0, u - range); x < std::min(u + range, m_width); x++){
-        for (unsigned int y = std::max((unsigned int)0, v - range); y < std::min(v + range, m_height); y++){
-            double err = (m_points_global[ y*m_width + x ] - target).norm() ;
-            if (err < lowest_err){
-                best_u = x;
-                best_v = y;
-                lowest_err = err;
-            }
-        }
-    }
-    return Eigen::Vector2i(best_u, best_v);
-}
-
 void Frame::applyGlobalPose(Eigen::Matrix4d& estimated_pose){
     Eigen::Matrix3d rotation = estimated_pose.block(0,0,3,3);
 
@@ -266,7 +226,3 @@ const unsigned int Frame::getWidth() const{
 const unsigned int Frame:: getHeight() const{
     return m_height;
 }
-
-double *Frame::getRawDepthMap() const {
-    return _rawDepthMap;
-}