add features constraints to icp refinement

core/include/supersurfel_fusion/dense_registration.hpp

@@ -54,6 +54,22 @@ class DenseRegistration
                const CamParam& cam_param,
                Mat33& R,
                float3& t);
+    bool featureConstrainedSymmetricICP(const thrust::device_vector<float3>& source_positions,
+                                        const thrust::device_vector<float3>& source_colors,
+                                        const thrust::device_vector<Mat33>& source_orientations,
+                                        const thrust::device_vector<float3>& target_colors,
+                                        const thrust::device_vector<Mat33>& target_orientations,
+                                        const thrust::device_vector<float>& target_confidences,
+                                        const thrust::host_vector<float3>& source_features3D,
+                                        const thrust::host_vector<float3>& target_features3D,
+                                        int source_size,
+                                        const cv::Ptr<Texture<float>>& tex_depth,
+                                        const cv::Ptr<Texture<int>>& tex_index,
+                                        const Mat33& R_init,
+                                        const float3& t_init,
+                                        const CamParam& cam,
+                                        Mat33& R,
+                                        float3& t);
     inline int& setNbIter(){return nbIter;}
     inline const int&  getNbIter() const {return nbIter;}
     inline double& setCovThresh(){return covThresh;}

core/include/supersurfel_fusion/dense_registration_kernels.cuh

@@ -24,6 +24,7 @@
 #include <supersurfel_fusion/cuda_utils_dev.cuh>
 #include <supersurfel_fusion/reduce_dev.cuh>
 #include <supersurfel_fusion/matrix_math.cuh>
+#include <stdio.h>
 
 #define PI 3.141592654f
 
@@ -35,7 +36,6 @@ __global__ void makeCorrespondences(const float3* source_positions,
                                     const float3* source_colors,
                                     const Mat33* source_orientations,
                                     const float* source_confidences,
-                                    const float3* target_positions,
                                     const float3* target_colors,
                                     const Mat33* target_orientations,
                                     const float* target_confidences,
@@ -65,6 +65,9 @@ __device__ inline void operator+=(MotionTrackingData& a, const MotionTrackingDat
     #pragma unroll
     for(int i = 0; i < 6; i++)
         a.Jtr[i] += b.Jtr[i];
+
+    a.r += b.r;
+    a.inliers += b.inliers;
 }
 
 __device__ inline void atomicAdd(MotionTrackingData* a, const MotionTrackingData& b)
@@ -76,6 +79,9 @@ __device__ inline void atomicAdd(MotionTrackingData* a, const MotionTrackingData
     #pragma unroll
     for(int i = 0; i < 6; i++)
         ::atomicAdd((float*) &(a->Jtr[i]), b.Jtr[i]);
+
+    ::atomicAdd((float*) &(a->r), b.r);
+    ::atomicAdd((float*) &(a->inliers), b.inliers);
 }
 
 template <int BLOCK_SIZE>
@@ -102,6 +108,9 @@ __global__ void buildSymmetricPoint2PlaneSystem(MotionTrackingData* system,
     for(int i = 0; i < 6; i++)
         sh_data[tid].Jtr[i] = 0.0f;
 
+    sh_data[tid].r = 0.0f;
+    sh_data[tid].inliers = 0.0f;
+
     if(id < nb_pairs)
     {
         float3 ps = scale * (source_positions[id] - source_centroid);
@@ -150,6 +159,9 @@ __global__ void buildSymmetricPoint2PlaneSystem(MotionTrackingData* system,
         sh_data[tid].JtJ[19] = w * (x1[4] * x1[5] + x2[4] * x2[5]);
 
         sh_data[tid].JtJ[20] = w * (x1[5] * x1[5] + x2[5] * x2[5]);
+
+        sh_data[tid].r = w * dn2 * dn2;
+        sh_data[tid].inliers = 1.0f;
     }
 
     __syncthreads();
@@ -160,65 +172,124 @@ __global__ void buildSymmetricPoint2PlaneSystem(MotionTrackingData* system,
         atomicAdd(system, sh_data[tid]);
 }
 
-__inline__ __device__ MotionTrackingData warpReduceSum(MotionTrackingData mtd)
-{
-    for(int offset = warpSize/2; offset > 0; offset /= 2)
-    {
-        #pragma unroll
-        for(int i = 0; i < 21; i++)
-            mtd.JtJ[i] += __shfl_down(mtd.JtJ[i], offset);
-
-        #pragma unroll
-        for(int i = 0; i < 6; i++)
-            mtd.Jtr[i] += __shfl_down(mtd.Jtr[i], offset);
-    }
-
-    return mtd;
-}
-
-__inline__ __device__ MotionTrackingData blockReduceSum(MotionTrackingData mtd)
+template <int BLOCK_SIZE>
+__global__ void computeSymmetricICPSystem(MotionTrackingData* system,
+                                          const float3* source_positions,
+                                          const float3* source_colors,
+                                          const Mat33* source_orientations,
+                                          const float3* target_colors,
+                                          const Mat33* target_orientations,
+                                          const float* target_confidences,
+                                          Mat33 R,
+                                          float3 t,
+                                          float fx,
+                                          float fy,
+                                          float cx,
+                                          float cy,
+                                          cudaTextureObject_t tex_index,
+                                          cudaTextureObject_t tex_depth,
+                                          int width,
+                                          int height,
+                                          int nb_supersurfels)
 {
-    static __shared__ MotionTrackingData shared[32];
-    int lane = threadIdx.x % warpSize;
-    int wid = threadIdx.x / warpSize;
+    int source_id = blockIdx.x * blockDim.x + threadIdx.x;
+    int tid = threadIdx.x;
 
-    mtd = warpReduceSum(mtd);
+    __shared__ MotionTrackingData sh_data[BLOCK_SIZE];
 
-    if(lane == 0)
-        shared[wid] = mtd;
+    #pragma unroll
+    for(int i = 0; i < 21; i++)
+        sh_data[tid].JtJ[i] = 0.0f;
 
-    __syncthreads();
+    #pragma unroll
+    for(int i = 0; i < 6; i++)
+        sh_data[tid].Jtr[i] = 0.0f;
 
+    sh_data[tid].r = 0.0f;
+    sh_data[tid].inliers = 0.0f;
 
-     if(threadIdx.x < blockDim.x / warpSize)
-         mtd = shared[lane];
-     else
-     {
-         #pragma unroll
-         for(int i = 0; i < 21; i++)
-             mtd.JtJ[i] = 0.0f;
+    if(source_id < nb_supersurfels)
+    {
+       float3 ps = source_positions[source_id];
+       ps = R * ps + t;
+
+       int u = lroundf(ps.x * fx / ps.z + cx);
+       int v = lroundf(ps.y * fy / ps.z + cy);
+
+       if(u >= 0 && u < width && v >= 0 && v < height)
+       {
+           int target_id = tex2D<int>(tex_index, u, v);
+           float zt = tex2D<float>(tex_depth, u, v);
+
+           if(target_confidences[target_id] > 0.0f && zt >= 0.2f && zt <= 5.0f)
+           {
+               float dist_color = length(rgbToLab(source_colors[source_id]) - rgbToLab(target_colors[target_id]));
+
+               float3 pt = make_float3(zt * (u - cx) / fx, zt * (v - cy) / fy, zt);
+               float3 nt = target_orientations[target_id].rows[2];
+               float3 ns = normalize(R * source_orientations[source_id].rows[2]);
+
+               if(dist_color < 20.0f &&
+                  length(ps - pt) < 0.1f  &&
+                  fabsf(dot(nt, ns)) > 0.8f)
+               {
+                   //float w = dot(ns, nt);
+                   float w = 1.0f;
+
+                   float3 d = pt - ps;
+                   float3 c1 = cross(pt, ns);
+                   float3 c2 = cross(ps, nt);
+                   float dn1 = dot(d, ns);
+                   float dn2 = dot(d, nt);
+
+                   float x1[6] = {c1.x, c1.y, c1.z, ns.x, ns.y, ns.z};
+                   float x2[6] = {c2.x, c2.y, c2.z, nt.x, nt.y, nt.z};
+
+                   for(int i = 0; i < 6; i++)
+                       sh_data[tid].Jtr[i] = w * (dn1 * x1[i] + dn2 * x2[i]);
+
+                   sh_data[tid].JtJ[0] = w * (x1[0] * x1[0] + x2[0] * x2[0]);
+                   sh_data[tid].JtJ[1] = w * (x1[0] * x1[1] + x2[0] * x2[1]);
+                   sh_data[tid].JtJ[2] = w * (x1[0] * x1[2] + x2[0] * x2[2]);
+                   sh_data[tid].JtJ[3] = w * (x1[0] * x1[3] + x2[0] * x2[3]);
+                   sh_data[tid].JtJ[4] = w * (x1[0] * x1[4] + x2[0] * x2[4]);
+                   sh_data[tid].JtJ[5] = w * (x1[0] * x1[5] + x2[0] * x2[5]);
+
+                   sh_data[tid].JtJ[6] = w * (x1[1] * x1[1] + x2[1] * x2[1]);
+                   sh_data[tid].JtJ[7] = w * (x1[1] * x1[2] + x2[1] * x2[2]);
+                   sh_data[tid].JtJ[8] = w * (x1[1] * x1[3] + x2[1] * x2[3]);
+                   sh_data[tid].JtJ[9] = w * (x1[1] * x1[4] + x2[1] * x2[4]);
+                   sh_data[tid].JtJ[10] = w * (x1[1] * x1[5] + x2[1] * x2[5]);
+
+                   sh_data[tid].JtJ[11] = w * (x1[2] * x1[2] + x2[2] * x2[2]);
+                   sh_data[tid].JtJ[12] = w * (x1[2] * x1[3] + x2[2] * x2[3]);
+                   sh_data[tid].JtJ[13] = w * (x1[2] * x1[4] + x2[2] * x2[4]);
+                   sh_data[tid].JtJ[14] = w * (x1[2] * x1[5] + x2[2] * x2[5]);
+
+                   sh_data[tid].JtJ[15] = w * (x1[3] * x1[3] + x2[3] * x2[3]);
+                   sh_data[tid].JtJ[16] = w * (x1[3] * x1[4] + x2[3] * x2[4]);
+                   sh_data[tid].JtJ[17] = w * (x1[3] * x1[5] + x2[3] * x2[5]);
+
+                   sh_data[tid].JtJ[18] = w * (x1[4] * x1[4] + x2[4] * x2[4]);
+                   sh_data[tid].JtJ[19] = w * (x1[4] * x1[5] + x2[4] * x2[5]);
+
+                   sh_data[tid].JtJ[20] = w * (x1[5] * x1[5] + x2[5] * x2[5]);
+
+                   sh_data[tid].r = w * dn2 * dn2;
+                   sh_data[tid].inliers = 1.0f;
+               }
+           }
+       }
+    }
 
-         #pragma unroll
-         for(int i = 0; i < 6; i++)
-             mtd.Jtr[i] = 0.0f;
-     }
+     __syncthreads();
 
-    if(wid == 0)
-        mtd = warpReduceSum(mtd);
+     reduce<BLOCK_SIZE, MotionTrackingData>(sh_data, tid);
 
-    return mtd;
+     if(tid == 0)
+         atomicAdd(system, sh_data[tid]);
 }
 
-__global__ void buildSymmetricPoint2PlaneSystem2(MotionTrackingData* system,
-                                                 const float3* source_positions,
-                                                 const float3* source_normals,
-                                                 const float3* target_positions,
-                                                 const float3* target_normals,
-                                                 float3 source_centroid,
-                                                 float3 target_centroid,
-                                                 float scale,
-                                                 int nb_pairs);
-
 } // supersurfel_fusion
 
 #endif // DENSE_REGISTRATION_KERNELS_CUH

core/include/supersurfel_fusion/dense_registration_types.hpp

@@ -56,6 +56,7 @@ struct MotionTrackingData
 {
     float JtJ[21];
     float Jtr[6];
+    float r, inliers;
 };
 
 } // namespace supersurfel_fusion

core/include/supersurfel_fusion/local_map.hpp

@@ -57,8 +57,6 @@ class LocalMap
                    const std::vector<bool>& is_static/*,
                    const cv::Mat& mask*/);
     void clean();
-    void insert(const Eigen::Vector3f& position, const cv::Mat& descriptor);
-    void replace(int id, const Eigen::Vector3f& position, const cv::Mat& descriptor);
     void findMatches(const std::vector<cv::KeyPoint>& frame_keypoints,
                      const cv::Mat& frame_descriptors,
                      std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& matched_map_positions,
@@ -74,6 +72,7 @@ class LocalMap
                float range_min,
                float range_max,
                const cv::Mat& depth,
+               const cv::Mat& gray,
                const std::vector<cv::KeyPoint>& frame_keypoints,
                const cv::Mat& frame_descriptors);
 

core/include/supersurfel_fusion/pnp_solver.hpp

@@ -65,13 +65,14 @@ class PnPSolver
                               float cy);
 
     inline const std::vector<int>& getInlierMarks() const {return inlierMarks;}
-    //inline const double getRMSE() const {return rmse;}
+    inline int getNbInliers() const {return nbInliers;}
 
   private:
     g2o::SparseOptimizer *optimizer;
 
     std::vector<int> inlierMarks;
-    //double rmse;
+    std::vector<std::pair<int, float>> inliersIdDist;
+    int nbInliers;
 
 }; // class PnPSolver
 

core/include/supersurfel_fusion/sparse_vo.hpp

@@ -24,6 +24,7 @@
 #include <supersurfel_fusion/local_map.hpp>
 #include <Eigen/StdVector>
 #include <supersurfel_fusion/pnp_solver.hpp>
+//#include "KDTreeVectorOfVectorsAdaptor.h"
 
 
 namespace supersurfel_fusion
@@ -58,20 +59,17 @@ class SparseVO
                                     float range_min,
                                     float range_max);
     void reset(const Eigen::Isometry3f& reset_pose);
-
     inline const std::vector<cv::KeyPoint>& getKeypoints() const {return keypoints;}
     inline const cv::Mat& getDescriptors() const {return descriptors;}
     inline std::vector<cv::KeyPoint>& setKeypoints() {return keypoints;}
     inline cv::Mat& setDescriptors() {return descriptors;}
     inline const Eigen::Isometry3f& getPose() const {return pose;}
     inline Eigen::Isometry3f& setPose() {return pose;}
     inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& getLocalMapPoints() const {return localMap->getPositions();}
-
-    void computeInlierFeature3DCorrespondences(std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& map_features3D,
-                                               std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& frame_features3D,
-                                               //float& rmse,
-                                               float range_min,
-                                               float range_max);
+    inline const std::vector<int>& getInlierMarks() const {return solver.getInlierMarks();}
+    inline int getNbInliers() const {return solver.getNbInliers();}
+    inline const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& getMatchedMapPositions() const {return matchedMapPositions;}
+    inline const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>& getMatchedKeypointsPositions() const {return matchedKeypointsPositions;}
 
 private:
     cv::Mat rgb, depth, gray;

core/include/supersurfel_fusion/supersurfel_fusion.hpp

@@ -31,6 +31,8 @@
 #include <supersurfel_fusion/deformation_graph.hpp>
 #include <supersurfel_fusion/motion_detection.hpp>
 
+#include <opencv2/cudafilters.hpp>
+
 
 namespace supersurfel_fusion
 {
@@ -76,8 +78,9 @@ class SupersurfelFusion
     void exportModel(std::string filename);
     void computeSuperpixelSegIm(cv::Mat& seg_im);
     void computeSlantedPlaneIm(cv::Mat& slanted_plane_im);
-    void closeLoop(const std::vector<cv::KeyPoint>& curr_kpts,
-                   const cv::Mat& curr_desc);
+    void closeGlobalLoop(const std::vector<cv::KeyPoint>& curr_kpts,
+                         const cv::Mat& curr_desc);
+    void updateLocalMap();
 
     inline bool isInitialized() {return initialized;}
     inline const Supersurfels& getFrame() {return frame;}
@@ -110,7 +113,7 @@ class SupersurfelFusion
 
     Supersurfels model, frame;
     CachedAllocator allocator;
-    int nbSupersurfels, nbSupersurfelsMax, nbRemoved, nbActive;
+    int nbSupersurfels, nbSupersurfelsMax, nbRemoved, nbVisible;
     int *nbSupersurfelsDev, *nbRemovedDev;
 
     cv::cuda::GpuMat filteredDepth;
@@ -122,7 +125,7 @@ class SupersurfelFusion
 
     Ferns* ferns;
     bool enableLoopClosure, LCdone;
-    int previousFernId, stampLastLC;
+    int previousFernId, stampLastGlobalLC;
     DeformationGraph def;
 
     MotionDetection mod;
@@ -137,6 +140,8 @@ class SupersurfelFusion
 
     double runtime, modelSize;
 
+    cv::Ptr<cv::cuda::Filter> sobelFilterX, sobelFilterY;
+
 }; // class SupersurfelFusion
 
 } // namespace supersurfel_fusion