roughly working, need coarse-to-fine

Author: theNded

examples/Cuda/Experiment/RegisterRGBDToTSDFVolume.cpp

@@ -56,7 +56,9 @@ int main(int argc, char **argv) {
             (float)config.tsdf_truncation_, trans, 40000, 60000);
     cuda::RGBDImageCuda rgbd((float)config.max_depth_,
                              (float)config.depth_factor_);
-    for (int i = begin; i < end; ++i) {
+
+    Eigen::Matrix4d pose, prev_pose;
+    for (int i = begin; i < begin + 13; ++i) {
         utility::LogInfo("Integrating frame {} ...", i);
 
         Image depth, color;
@@ -65,15 +67,23 @@ int main(int argc, char **argv) {
         rgbd.Upload(depth, color);
 
         /* Use ground truth trajectory */
-        Eigen::Matrix4d pose = pose_graph.nodes_[i - begin].pose_;
-        trans.FromEigen(pose);
-
-        tsdf_volume.Integrate(rgbd, intrinsic, trans);
+        if (i - begin <= 10) {
+            pose = pose_graph.nodes_[i - begin].pose_;
+            trans.FromEigen(pose);
+        } else {
+            trans.FromEigen(prev_pose);
+            auto result =
+                    RGBDToTSDFRegistration(rgbd, tsdf_volume, intrinsic, trans);
+            pose = std::get<1>(result);
+            trans.FromEigen(pose);
 
-        if (i > 10) {
-            RGBDToTSDFRegistration(rgbd, tsdf_volume, intrinsic, trans);
-            break;
+            std::cout << "------\n";
+            std::cout << pose << "\n";
+            std::cout << pose_graph.nodes_[i - begin].pose_ << "\n";
+            std::cout << "------\n";
         }
+        tsdf_volume.Integrate(rgbd, intrinsic, trans);
+        prev_pose = pose;
     }
 
     tsdf_volume.GetAllSubvolumes();

src/Cuda/Integration/RGBDToTSDF.cpp

@@ -20,7 +20,7 @@ std::tuple<bool, Eigen::Matrix4d, float> RGBDToTSDFRegistration(
     Eigen::Matrix4d cam_to_world = transform_camera_to_world.ToEigen();
     Eigen::Matrix4d delta;
 
-    for (int iter = 0; iter < 3; ++iter) {
+    for (int iter = 0; iter < 200; ++iter) {
         linear_system.Memset(0);
         transform_camera_to_world.FromEigen(cam_to_world);
 
@@ -32,7 +32,6 @@ std::tuple<bool, Eigen::Matrix4d, float> RGBDToTSDFRegistration(
 
         utility::LogInfo("> Iter {}: loss = {}, avg loss = {}, inliers = {}",
                          iter, loss, loss / inliers, inliers);
-        std::cout << cam_to_world << "\n";
 
         std::tie(is_success, delta) =
                 utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JtJ, Jtr);

src/Cuda/Integration/RGBDToTSDFKernel.cuh

@@ -103,7 +103,7 @@ __global__ void BuildLinearSystemRGBDToTSDFKernel(
     /** Reduce Sum loss and inlier **/
     const int OFFSET2 = 27;
     {
-        local_sum0[tid] = mask ? residual : 0;
+        local_sum0[tid] = mask ? residual * residual : 0;
         local_sum1[tid] = mask ? 1 : 0;
         __syncthreads();