Cache whether an update performs a shift, and expose a method to dete…

…rmine after an update if a shift occurred.

gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h

@@ -235,6 +235,13 @@ namespace pcl
             PCL_WARN("ICP is %s\n", !disable_icp_?"ENABLED":"DISABLED");
           }
 
+          /** \brief Return whether the last update resulted in a shift */
+          inline bool
+          hasShifted () const
+          {
+            return (has_shifted_);
+          }
+
         private:
 
           /** \brief Allocates all GPU internal buffers.
@@ -440,6 +447,9 @@ namespace pcl
 
 
           bool disable_icp_;
+
+          /** \brief True or false depending on if there was a shift in the last pose update */
+          bool has_shifted_;
 
         public:
           EIGEN_MAKE_ALIGNED_OPERATOR_NEW

gpu/kinfu_large_scale/src/kinfu.cpp

@@ -251,6 +251,7 @@ pcl::gpu::kinfuLS::KinfuTracker::reset ()
 
 
   lost_=false;
+  has_shifted_=false;
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -653,8 +654,8 @@ pcl::gpu::kinfuLS::KinfuTracker::operator() (const DepthMap& depth_raw)
 
   ///////////////////////////////////////////////////////////////////////////////////////////  
   // check if we need to shift
-  bool has_shifted = cyclical_.checkForShift(tsdf_volume_, getCameraPose (), 0.6 * volume_size_, true, perform_last_scan_); // TODO make target distance from camera a param
-  if(has_shifted)
+  has_shifted_ = cyclical_.checkForShift(tsdf_volume_, getCameraPose (), 0.6 * volume_size_, true, perform_last_scan_); // TODO make target distance from camera a param
+  if(has_shifted_)
     PCL_WARN ("SHIFTING\n");
 
   ///////////////////////////////////////////////////////////////////////////////////////////
@@ -702,7 +703,7 @@ pcl::gpu::kinfuLS::KinfuTracker::operator() (const DepthMap& depth_raw)
     pcl::device::kinfuLS::sync ();
   }
 
-  if(has_shifted && perform_last_scan_)
+  if(has_shifted_ && perform_last_scan_)
     extractAndSaveWorld ();