           
           // As the pointer is incremented in the for loop, we have to make sure that the pointer is never outside the memory
           if(pos > buffer.tsdf_memory_end)
-            pos -= (buffer.tsdf_memory_end - buffer.tsdf_memory_start);
+            pos -= (buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1);
           
           float inv_z = 1.0f / (v_z + Rcurr_inv.data[2].z * z_scaled);
           if (inv_z < 0)

                int z_step = buffer.voxels_size.y * volume.step / sizeof(*pos);
                            
                ///Get the size of the whole TSDF memory 
                int size = buffer.tsdf_memory_end - buffer.tsdf_memory_start;
                              
                ///Move pointer to the Z origin
                pos+= minBounds.z * z_step;

                int z_step = buffer.voxels_size.y * volume.step / sizeof(*pos);
                                    
                ///Get the size of the whole TSDF memory
                int size = buffer.tsdf_memory_end - buffer.tsdf_memory_start + 1;
                                  
                ///Move along z axis
      #pragma unroll
