Skip to content

Kinect Fusion node summary

Aaditya Saraiya edited this page Aug 12, 2018 · 1 revision

The function calls and processes are in the order of which they are executed when the following command is executed.

roslaunch yak launch_gazebo_robot.launch

  • KinFuServer app(&camera, fixedFrame, cameraFrame);

  • Kinfu Server- Create raycastImg publisher, get_tsdf_server and get_sparse_tsdf services are advertised

  • tf.Listener- looks up initial transform


  • app.ExecuteBlocking()

  • Initialise()- Initialise the camera device

  • ConnectCamera()- Connect to the camera and find new depth, if no devices connected, show the error message

  • LoadParams()- Get the depth and width of the image, get depth intrinsics, load all the parameters which have been provided by ROS Params

  • Update()

  • Grab()- Grab the last_depth and last_color image in form of OpenCV matrices

  • waitForTransform() and lookupTransform()- Find transformation between volume_pose and camera_depth_frame at that instant.


Note- volume_pose is the original position from where the reconstruction process has started. camera_depth_frame is the tf frame which gives us depth information as seen from the Kinect sensor.


  • past_to_current_sensor- Utilise previous_volume_to_sensor and current volume_to_sensor to find past_to_current_sensor, basically we are tracking the camera pose as the camera has moved

  • currentCameraMotionHint- tf -> affine transform (past_to_current_sensor_)

  • currentCameraPoseHint- tf -> affine transform (current_volume_to_sensor_)

  • previousCameraPoseHint- tf -> affine transform (past_volume_to_sensor)

  • previous_volume_to_sensor_transform

  • current_volume_to_sensor_transform

  • has_image() - Use the kinfu_ pointer to do one step of Kinect fusion

  • Kinfu::Kinfu- Set volume dimensions, truncated distance, set max weight, set volume size, get volume pose, setRaycastStepFactor, setGradientDeltaFactor, set distance threshold, angle threshold and number of iterations for ICP.

  • allocate_buffer()- Resize current and previous depth and normals pyramid, create rows and columns for current and previous depth and normal pyramids, and current and previous points pyramid (Allocation of CUDA memory is being done in this case).

  • resetvolume()- Remove older volume

  • poses_.reserve()- Reserve poses in the start

  • poses_.push_back()- Allow loading of robot pose instead of default volume pose

  • cuda::computeDists()- Compute euclidean distances.

  • cuda::depthBilateralFilter()- Reduction of noise

  • cuda::depthTruncation()- Reduce depth to a particular level beyond which everything is set to 1

  • cuda::depthBuildPyramid()- Build pyramid with previous and current depth image which is separated by bilateral_sigma_depth

  • computeNormalsAndMaskDepth()

  • computePointNormals()


Note on working of ICP

  • ICP compares the points and normals between the current and previous depth images to estimate the affine transform from the previous image pose to the current image pose.

  • If it finds a transform with error below the threshold, the previous pose is multiplied by the affine transform to produce the new pose.

  • If no transform is found (for one reason or another) a reset is triggered.


  • icp -> estimateTransform()- Estimate a transformation from previousCameraPose to correctedCameraPose