-
Notifications
You must be signed in to change notification settings - Fork 6
Kinect Fusion node summary
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 betweenvolume_pose
andcamera_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