RGBD Odometry fixes

Core/Cuda/cudafuncs.cu

@@ -361,6 +361,33 @@ void copyMaps(
   cudaSafeCall(cudaGetLastError());
 }
 
+__global__ void copyVmapsTmpKernel(
+    PtrStepSz<float> vmap_src,
+    int rows,
+    int cols,
+    float* vmaps_tmp) {
+
+  int x = threadIdx.x + blockIdx.x * blockDim.x;
+  int y = threadIdx.y + blockIdx.y * blockDim.y;
+
+  if (x < cols && y < rows) {
+    vmaps_tmp[y * cols * 4 + (x * 4) + 2] = vmap_src.ptr(y + 2 * rows)[x];
+  }
+}
+
+void copyVmapsTmp(
+    DeviceArray2D<float>& vmap_src,
+    const size_t srcWidth,
+    const size_t srcHeight,
+    DeviceArray<float>& vmaps_tmp) {
+  dim3 block(32, 8);
+  dim3 grid(1, 1, 1);
+  grid.x = getGridDim(srcWidth, block.x);
+  grid.y = getGridDim(srcHeight, block.y);
+
+  copyVmapsTmpKernel<<<grid, block>>>(vmap_src, srcHeight, srcWidth, vmaps_tmp);
+}
+
 __global__ void
 pyrDownKernelGaussF(const PtrStepSz<float> src, PtrStepSz<float> dst, float* gaussKernel) {
   int x = blockIdx.x * blockDim.x + threadIdx.x;

Core/Cuda/cudafuncs.cuh

@@ -143,6 +143,12 @@ void copyMaps(
     DeviceArray2D<float>& vmap_dst,
     DeviceArray2D<float>& nmap_dst);
 
+void copyVmapsTmp(
+    DeviceArray2D<float>& vmap_src,
+    const size_t srcWidth,
+    const size_t srcHeight,
+    DeviceArray<float>& vmaps_tmp);
+
 void resizeVMap(const DeviceArray2D<float>& input, DeviceArray2D<float>& output);
 
 void resizeNMap(const DeviceArray2D<float>& input, DeviceArray2D<float>& output);

Core/ElasticFusion.cpp

@@ -330,7 +330,7 @@ void ElasticFusion::processFrame(
           Eigen::MatrixXd covariance = frameToModel.getCovariance();
 
           for (int i = 0; i < 6; i++) {
-            if (covariance(i, i) > 1e-04) {
+            if (covariance(i, i) > 1e-03) {
               trackingOk = false;
               break;
             }
@@ -349,7 +349,7 @@ void ElasticFusion::processFrame(
           Eigen::MatrixXd covariance = frameToModel.getCovariance();
 
           for (int i = 0; i < 6; i++) {
-            if (covariance(i, i) > 1e-04) {
+            if (covariance(i, i) > 1e-03) {
               trackingOk = false;
               break;
             }

Core/Utils/RGBDOdometry.cpp

@@ -143,6 +143,8 @@ void RGBDOdometry::initICP(GPUTexture* filteredDepth, const float depthCutoff) {
     createNMap(vmaps_curr_[i], nmaps_curr_[i]);
   }
 
+  copyVmapsTmp(vmaps_curr_[0], width, height, vmaps_tmp);
+
   cudaDeviceSynchronize();
 }
 
@@ -439,7 +441,7 @@ void RGBDOdometry::getIncrementalTransformation(
         TOCK("computeRgbResidual");
       }
 
-      float sigmaVal = std::sqrt((float)sigma / rgbSize == 0 ? 1 : rgbSize);
+      float sigmaVal = std::sqrt((float)sigma / (rgbSize == 0 ? 1 : rgbSize));
       float rgbError = std::sqrt(sigma) / (rgbSize == 0 ? 1 : rgbSize);
 
       if (rgbOnly && rgbError > lastRGBError) {
@@ -521,7 +523,7 @@ void RGBDOdometry::getIncrementalTransformation(
 
       if (icp && rgb) {
         double w = icpWeight;
-        lastA = dA_rgbd + w * w * dA_icp;
+        lastA = dA_rgbd + w * dA_icp;
         lastb = db_rgbd + w * db_icp;
         result = lastA.ldlt().solve(lastb);
       } else if (icp) {

MainController.cpp

@@ -70,7 +70,7 @@ MainController::MainController(int argc, char* argv[])
   depth = 3.0f;
   icp = 10.0f;
   icpErrThresh = 4e-05;
-  covThresh = 1e-05;
+  covThresh = 1e-04;
   photoThresh = 115;
   fernThresh = 0.3095f;