├── README.md ├── calculateWarpingJacobian.m ├── computeResidualVariance.m ├── constructNormalizedCoordinate.m ├── constructPyramid.m ├── convertMotionVectorToMatrix.m ├── demo.m ├── estimateVisualOdometry.m ├── examples ├── case_1 │ ├── dep_curr.png │ ├── dep_prev.png │ ├── img_curr.png │ ├── img_prev.png │ ├── pose_curr.txt │ └── pose_prev.txt ├── case_2 │ ├── dep_curr.png │ ├── dep_prev.png │ ├── img_curr.png │ ├── img_prev.png │ ├── pose_curr.txt │ └── pose_prev.txt └── kinect_params.txt ├── getRelativePose.m ├── projectPointCloud.m ├── reprojectDepthImage.m ├── twistexp.m ├── warpImage.m └── warpPointCloud.m /README.md: -------------------------------------------------------------------------------- 1 | # Dense Visual Odometry from a Monocular RGB-D Camera 2 | 3 | This repository contains an algorithm to estimate visual odometry with a monocular RGB-D camera. It has been tested on Matlab 2015b and Ubuntu 14.04. 4 | 5 | This algorithm is closely related to the following papers: 6 | 7 | - F. Steinbrucker, J. Sturm and D. Cremers, [**Real-Time Visual Odometry from Dense RGB-D Images**](https://vision.in.tum.de/_media/spezial/bib/steinbruecker_sturm_cremers_iccv11.pdf), ICCV workshop 2011. 8 | - C. Kerl, J. Sturm and D. Cremers, [**Robust Odometry Estimation for RGB-D Cameras**](https://vision.in.tum.de/_media/spezial/bib/kerl13icra.pdf), ICRA 2013. 9 | 10 | ## Usage 11 | 12 | - Exemplar frame sequences are listed in `examples`, which are randomly picked from [TUM RGB-D SLAM Dataset](https://vision.in.tum.de/data/datasets/rgbd-dataset). Please check the [dataset configurations](https://vision.in.tum.de/data/datasets/rgbd-dataset/download). 13 | 14 | - Please test the dense visual odometry demo via running `demo.m` 15 | 16 | -------------------------------------------------------------------------------- /calculateWarpingJacobian.m: -------------------------------------------------------------------------------- 1 | function jacobian = calculateWarpingJacobian(warped_pointclouds, pointclouds, pose, K) 2 | % calculate the warping jacobian matrix 3 | % 4 | % INPUT: 5 | % warped_pointclouds: a matrix of size [num_pixels, 3] 6 | % pointclouds: a matrix of size [num_pixels, 3] 7 | % pose: the pose for the warping 8 | % K: the intrinsic matrix of the camera 9 | % 10 | % OUTPUT: 11 | % jacobian: jacobian matrix of the warping operation 12 | 13 | fx = K(1, 1); 14 | fy = K(2, 2); 15 | 16 | num_points = size(pointclouds, 1); 17 | 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | % step-1: projection gradient 20 | % pointcloud after warping 21 | warped_x = warped_pointclouds(:, 1); 22 | warped_y = warped_pointclouds(:, 2); 23 | warped_z = warped_pointclouds(:, 3); 24 | 25 | proj_grad = cat(1, ... 26 | [fx./warped_z, zeros(num_points, 1), -fx.*warped_x./warped_z.^2], ... 27 | [zeros(num_points, 1), fy./warped_z, -fy.*warped_y./warped_z.^2]); 28 | proj_grad = reshape(proj_grad, [], 2, 3); 29 | 30 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 | % step-2: rigid point-transformation gradient 32 | % pointcloud before warping 33 | x = pointclouds(:, 1); 34 | y = pointclouds(:, 2); 35 | z = pointclouds(:, 3); 36 | 37 | rot_grad = kron(eye(3), cat(2, x, y, z)); 38 | rot_grad = reshape(rot_grad, [], 3, 9); 39 | trans_grad = kron(eye(3), ones(num_points, 1)); 40 | trans_grad = reshape(trans_grad, [], 3, 3); 41 | rigid_trans_grad = cat(3, rot_grad, trans_grad); 42 | 43 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 44 | % step-3: twist motion gradient 45 | twist_rot_grad = cat(1, ... 46 | [zeros(3, 1), pose(1:3, 3), -pose(1:3, 2), zeros(3, 3)], ... 47 | [-pose(1:3, 3), zeros(3, 1), pose(1:3, 1), zeros(3, 3)], ... 48 | [pose(1:3, 2), -pose(1:3, 1), zeros(3, 1), zeros(3, 3)]); 49 | twist_trans_grad = cat(1, ... 50 | [0, pose(3, 4), -pose(2, 4), 1, 0, 0], ... 51 | [-pose(3, 4), 0, pose(1, 4), 0, 1, 0], ... 52 | [pose(2, 4), -pose(1, 4), 0, 0, 0, 1]); 53 | twist_grad = cat(1, twist_rot_grad, twist_trans_grad); 54 | 55 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 56 | % calculate the chained gradient 57 | chained_gradient = zeros(num_points, 2, 12); 58 | for i = 1:2 59 | for j = 1:12 60 | proj_grad_temp = squeeze(proj_grad(:, i, :)); 61 | rigid_trans_grad_temp = squeeze(rigid_trans_grad(:, :, j)); 62 | chained_gradient(:, i, j) = dot(proj_grad_temp, rigid_trans_grad_temp, 2); 63 | end 64 | end 65 | % multiply the twist gradient 66 | chained_gradient = reshape(chained_gradient, [], 12) * twist_grad; 67 | jacobian = reshape(chained_gradient, [], 2, 6); 68 | 69 | end -------------------------------------------------------------------------------- /computeResidualVariance.m: -------------------------------------------------------------------------------- 1 | function variance = computeResidualVariance(residual, nu) 2 | % compute the variance of the residuals, assuming standard student-t 3 | % distribution 4 | % 5 | % INPUT: 6 | % residual: a vector of size [num_pixels, 1] 7 | % nu: degrees of freedom nu 8 | % 9 | % OUTPUT: 10 | % variance: the scalar variance of the standard student-t distribution 11 | 12 | variance = 1; 13 | for iter = 1:10 14 | variance = mean(residual.^2 .* (nu + 1) ./ (nu + residual.^2./variance)); 15 | end 16 | 17 | end -------------------------------------------------------------------------------- /constructNormalizedCoordinate.m: -------------------------------------------------------------------------------- 1 | function normalized_coords = constructNormalizedCoordinate(height, width, K) 2 | % Construct normalized coordinate 3 | % 4 | % INPUT: 5 | % height: the height of the input image 6 | % width: the width of the input image 7 | % K: intrinsic parameters of the camera 8 | % 9 | % OUTPUT: 10 | % normalized_coords: as a 2-channel image 11 | 12 | fx = K(1, 1); 13 | fy = K(2, 2); 14 | u0 = K(1, 3); 15 | v0 = K(2, 3); 16 | 17 | [tilde_x, tilde_y] = meshgrid(1:width, 1:height); 18 | tilde_x = (tilde_x - u0)/fx; 19 | tilde_y = (tilde_y - v0)/fy; 20 | 21 | normalized_coords = cat(3, tilde_x, tilde_y); 22 | 23 | end -------------------------------------------------------------------------------- /constructPyramid.m: -------------------------------------------------------------------------------- 1 | function img_pyr = constructPyramid(img, num_levels) 2 | % construct image pyramid for the input image 3 | % 4 | % INPUT: 5 | % img: a grayscale image 6 | % num_levels: number of pyramid levels 7 | % 8 | % OUTPUT: 9 | % img_pyr: a cell of image pyramid 10 | 11 | if size(img, 3) > 1 12 | img = rgb2gray(img); 13 | end 14 | 15 | img_pyr = cell(num_levels, 1); 16 | img_pyr{1} = img; 17 | for i = 2:num_levels 18 | img = imresize(img, 0.5, 'nearest'); 19 | img_pyr{i} = img; 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /convertMotionVectorToMatrix.m: -------------------------------------------------------------------------------- 1 | function pose_mat = convertMotionVectorToMatrix(pose_vec) 2 | % convert the motion vector to motion matrix 3 | % 4 | % INPUT: 5 | % pose_vec: a vector as [tx, ty, tz, wx, wy, wz, wq] 6 | % 7 | % OUTPUT: 8 | % pose_mat: a matrix as [R, T; 0, 1] 9 | 10 | trans_vec = pose_vec(1:3); 11 | 12 | quaternion = [pose_vec(end), pose_vec(4:end-1)]; 13 | rot_mat = quat2rotm(quaternion); 14 | 15 | pose_mat = eye(4); 16 | pose_mat(1:3, 1:3) = rot_mat; 17 | pose_mat(1:3, 4) = trans_vec; 18 | 19 | end -------------------------------------------------------------------------------- /demo.m: -------------------------------------------------------------------------------- 1 | % example folder 2 | exp_folder = 'examples'; 3 | 4 | % Kinect intrinsic parameters 5 | K = dlmread(fullfile(exp_folder, 'kinect_params.txt')); 6 | 7 | % load (image, depth) pairs and ground truth relative pose 8 | case_folder = 'case_2'; 9 | 10 | %% load previous image and depth pairs 11 | img_prev = im2double(imread(fullfile(exp_folder, case_folder, 'img_prev.png'))); 12 | dep_prev = double(imread(fullfile(exp_folder, case_folder, 'dep_prev.png'))); 13 | 14 | % load current image 15 | img_curr = im2double(imread(fullfile(exp_folder, case_folder, 'img_curr.png'))); 16 | dep_curr = double(imread(fullfile(exp_folder, case_folder, 'dep_curr.png'))); 17 | 18 | % convert the image and depth data 19 | img_prev = rgb2gray(img_prev); img_curr = rgb2gray(img_curr); 20 | dep_prev = dep_prev/5000; dep_curr = dep_curr/5000; 21 | 22 | % load the ground-truth poses for two frames: pose_vec --> [tx, ty, tz, wx, wy, wz, wq] 23 | gt_pose_prev = dlmread(fullfile(exp_folder, case_folder, 'pose_prev.txt')); 24 | gt_pose_curr = dlmread(fullfile(exp_folder, case_folder, 'pose_curr.txt')); 25 | 26 | gt_pose_prev = convertMotionVectorToMatrix(gt_pose_prev); 27 | gt_pose_curr = convertMotionVectorToMatrix(gt_pose_curr); 28 | 29 | % calculate the relative pose 30 | gt_pose_rel = getRelativePose(gt_pose_curr, gt_pose_prev); 31 | 32 | %% warping by the ground-truth relative pose 33 | [warped_image, valid_mask] = warpImage(img_curr, dep_prev, gt_pose_rel, K); 34 | error = mean((warped_image(valid_mask) - img_prev(valid_mask)).^2); 35 | disp(error); 36 | 37 | figure(3); 38 | imshow(abs(warped_image - img_prev) .* valid_mask, []) 39 | 40 | %% calculate the relative pose from two RGB-D frames 41 | num_levels = 5; 42 | isVerbose = true; 43 | 44 | [pose_rel, score] = estimateVisualOdometry(img_curr, img_prev, dep_prev, K, num_levels, isVerbose); -------------------------------------------------------------------------------- /estimateVisualOdometry.m: -------------------------------------------------------------------------------- 1 | function [pose_rel, score] = estimateVisualOdometry(img_curr, img_prev, dep_prev, K, num_levels, isVerbose) 2 | % estimate visual odometry from two adjacent frames 3 | % 4 | % INPUT: 5 | % img_curr: current RGB frame 6 | % img_prev: previous RGB frame 7 | % dep_prev: previous depth frame 8 | % K: intrinsic parameters of the camera 9 | % num_levels: pyramid levels for the estimation 10 | % isVerbose: whether to show intermediate results 11 | % 12 | % OUTPUT: 13 | % pose_rel: relative pose 14 | % score: fitting score 15 | 16 | % construct image pyramids 17 | img_curr_pyr = constructPyramid(img_curr, num_levels); 18 | img_prev_pyr = constructPyramid(img_prev, num_levels); 19 | 20 | % construct depth pyramid for the previous frame 21 | dep_prev_pyr = constructPyramid(dep_prev, num_levels); 22 | 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | % estimate the relative pose from coarse to fine scales 25 | 26 | % initialize the relative pose and its increment 27 | pose_rel = eye(4); 28 | increment = zeros(6, 1); 29 | 30 | % convert the vectorize motion increment to update the relative pose 31 | increment = twistexp(increment); 32 | pose_rel = increment * pose_rel; 33 | 34 | % modify the camera parameters to fit each pyramid 35 | K_pyr = K; 36 | K_pyr(1:2, :) = K_pyr(1:2, :) / (2^(num_levels-1)); 37 | 38 | for n = num_levels:-1:1 39 | 40 | % image size 41 | [height, width] = size(dep_prev_pyr{n}); 42 | 43 | % get valid point clouds in the previous frame 44 | [pointclouds, valid_mask] = reprojectDepthImage(dep_prev_pyr{n}, K_pyr); 45 | 46 | % warp pointclouds and prune invalid points 47 | warped_pointclouds = warpPointCloud(pointclouds, pose_rel); 48 | [warped_img_coordinates, valid_points] = projectPointCloud(warped_pointclouds, K_pyr, height, width); 49 | 50 | warped_pointclouds = warped_pointclouds(valid_points, :); 51 | pointclouds = pointclouds(valid_points, :); 52 | 53 | % spatial gradient in the current frame 54 | [Gx_curr, Gy_curr] = imgradientxy(img_curr_pyr{n}, 'CentralDifference'); 55 | Gx_curr = interp2(Gx_curr, warped_img_coordinates(:, 1), warped_img_coordinates(:, 2), 'linear', 0); 56 | Gy_curr = interp2(Gy_curr, warped_img_coordinates(:, 1), warped_img_coordinates(:, 2), 'linear', 0); 57 | Gs_curr = cat(2, Gx_curr, Gy_curr); 58 | 59 | % temporal visual difference 60 | Gt_prev = img_prev_pyr{n}; 61 | Gt_prev = Gt_prev(valid_mask); 62 | Gt_prev = Gt_prev(valid_points); 63 | Gt_curr = img_curr_pyr{n}; 64 | Gt_curr = interp2(Gt_curr, warped_img_coordinates(:, 1), warped_img_coordinates(:, 2), 'linear', 0); 65 | Gt = Gt_curr - Gt_prev; 66 | 67 | % calculate the warping Jacobian 68 | warp_jacobian = calculateWarpingJacobian(warped_pointclouds, pointclouds, pose_rel, K_pyr); 69 | 70 | % calculate the compositive jacobian 71 | comp_jacobian = squeeze(sum(bsxfun(@times, Gs_curr, warp_jacobian), 2)); 72 | 73 | % % adjust the importance of each residual elements, when required 74 | % % necessary pruning of bad correspondences 75 | % % compute the weight 76 | % variance = computeResidualVariance(Gt, 5); 77 | % weights = sqrt((5 + 1) ./ (5 + Gt.^2./variance)); 78 | % comp_jacobian = bsxfun(@times, comp_jacobian, weights); 79 | % Gt = Gt.*weights; 80 | 81 | % calculate the increment motion 82 | increment = -(comp_jacobian'*comp_jacobian)\(comp_jacobian'*Gt); 83 | 84 | % get the current relative pose 85 | increment = twistexp(increment); 86 | pose_rel = increment * pose_rel; 87 | 88 | % intermediate results 89 | if isVerbose 90 | [warped_image, valid_mask] = warpImage(img_curr, dep_prev, pose_rel, K); 91 | error = mean((warped_image(valid_mask) - img_prev(valid_mask)).^2); 92 | disp(['visual consistency score in level ' num2str(n) ' is ' num2str(error)]); 93 | 94 | figure(1); 95 | imshow(abs(warped_image - img_prev).*valid_mask, []) 96 | end 97 | 98 | % increse the focal length 99 | K_pyr(1:2, :) = K_pyr(1:2, :) * 2; 100 | end 101 | 102 | % get the final score 103 | [warped_image, valid_mask] = warpImage(img_curr, dep_prev, pose_rel, K); 104 | score = mean((warped_image(valid_mask) - img_prev(valid_mask)).^2); 105 | 106 | if isVerbose 107 | disp(['The fitting score is ' num2str(error)]); 108 | end 109 | 110 | end 111 | -------------------------------------------------------------------------------- /examples/case_1/dep_curr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_1/dep_curr.png -------------------------------------------------------------------------------- /examples/case_1/dep_prev.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_1/dep_prev.png -------------------------------------------------------------------------------- /examples/case_1/img_curr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_1/img_curr.png -------------------------------------------------------------------------------- /examples/case_1/img_prev.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_1/img_prev.png -------------------------------------------------------------------------------- /examples/case_1/pose_curr.txt: -------------------------------------------------------------------------------- 1 | 1.3160 0.6254 1.6302 0.6609 0.6199 -0.2893 -0.3086 -------------------------------------------------------------------------------- /examples/case_1/pose_prev.txt: -------------------------------------------------------------------------------- 1 | 1.3303 0.6256 1.6464 0.6579 0.6161 -0.2932 -0.3189 -------------------------------------------------------------------------------- /examples/case_2/dep_curr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_2/dep_curr.png -------------------------------------------------------------------------------- /examples/case_2/dep_prev.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_2/dep_prev.png -------------------------------------------------------------------------------- /examples/case_2/img_curr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_2/img_curr.png -------------------------------------------------------------------------------- /examples/case_2/img_prev.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LucasSheng/dense-rgbd-visual-odometry/2973ca8d0dc55e636663e6fdb9bd09fd1f21a6ee/examples/case_2/img_prev.png -------------------------------------------------------------------------------- /examples/case_2/pose_curr.txt: -------------------------------------------------------------------------------- 1 | 1.3084 0.6254 1.6221 0.6610 0.6207 -0.2901 -0.3059 -------------------------------------------------------------------------------- /examples/case_2/pose_prev.txt: -------------------------------------------------------------------------------- 1 | 1.3434 0.6271 1.6606 0.6583 0.6112 -0.2938 -0.3266 -------------------------------------------------------------------------------- /examples/kinect_params.txt: -------------------------------------------------------------------------------- 1 | 525 0 319.5 2 | 0 525 239.5 3 | 0 0 1 -------------------------------------------------------------------------------- /getRelativePose.m: -------------------------------------------------------------------------------- 1 | function pose_rel = getRelativePose(pose_curr, pose_prev) 2 | % get relative pose from the pose parameters between adjacent frames 3 | % 4 | % INPUT: 5 | % pose_curr: a [4, 4] pose matrix from the current frame 6 | % pose_prev: a [4, 4] pose matrix from the previous frame 7 | % 8 | % OUTPUT: 9 | % pose_rel: a [4, 4] relative pose matrix from the previous frame to the 10 | % current frame 11 | 12 | pose_rel = pose_curr \ pose_prev; 13 | 14 | end -------------------------------------------------------------------------------- /projectPointCloud.m: -------------------------------------------------------------------------------- 1 | function [img_coords, valid_mask] = projectPointCloud(pointclouds, K, height, width) 2 | % project the pointcloud into image coorindates 3 | % 4 | % INPUT: 5 | % pointclouds: a matrix of size [num_points, 3] 6 | % K: intrinsic camera parameters 7 | % height: image height 8 | % width: image width 9 | % 10 | % OUTPUT: 11 | % img_coords: a matrix of size [num_pixels, 2] 12 | % valid_mask: binary mask for valid pixels 13 | 14 | img_coords = bsxfun(@rdivide, pointclouds, pointclouds(:, 3)); 15 | img_coords = img_coords * K'; 16 | img_coords = img_coords(:, 1:2); 17 | 18 | valid_mask = img_coords(:, 1) > 0 & img_coords(:, 1) <= width & img_coords(:, 2) > 0 & img_coords(:, 2) <= height; 19 | img_coords = img_coords(valid_mask, :); 20 | 21 | end -------------------------------------------------------------------------------- /reprojectDepthImage.m: -------------------------------------------------------------------------------- 1 | function [pointclouds, valid_mask] = reprojectDepthImage(depth, K) 2 | % Reproject the depth image to 3D point clouds 3 | % 4 | % INPUT: 5 | % depth: a grayscale image 6 | % K: intrinsic parameters of the camera 7 | % 8 | % OUTPUT: 9 | % pointclouds: a 3-channel point list 10 | % valid_mask: a binary mask 11 | 12 | height = size(depth, 1); 13 | width = size(depth, 2); 14 | 15 | valid_mask = depth > 0; 16 | 17 | normalized_coords = constructNormalizedCoordinate(height, width, K); 18 | pointclouds = cat(3, bsxfun(@times, normalized_coords, depth), depth); 19 | 20 | % convert pointclouds to a list 21 | pointclouds = reshape(pointclouds, [], 3); 22 | pointclouds = pointclouds(valid_mask, :); 23 | 24 | end -------------------------------------------------------------------------------- /twistexp.m: -------------------------------------------------------------------------------- 1 | function incre_pose = twistexp(increment) 2 | % calculate the increment pose 3 | % 4 | % INPUT: 5 | % increment: a vector as [wx, wy, wz, tx, ty, tz] 6 | % 7 | % OUTPUT: 8 | % incre_pose: a [4, 4] matrix about the rigid transformation 9 | 10 | wx = increment(1); 11 | wy = increment(2); 12 | wz = increment(3); 13 | 14 | incre_pose = eye(4); 15 | incre_pose(1:3, 4) = increment(4:6); 16 | incre_pose(1, 2) = -wz; 17 | incre_pose(1, 3) = wy; 18 | incre_pose(2, 1) = wz; 19 | incre_pose(2, 3) = -wx; 20 | incre_pose(3, 1) = -wy; 21 | incre_pose(3, 2) = wx; 22 | 23 | end -------------------------------------------------------------------------------- /warpImage.m: -------------------------------------------------------------------------------- 1 | function [warped_image, valid_mask] = warpImage(img_curr, dep_prev, pose_rel, K) 2 | % warp the current image to the previous image coordinate 3 | % 4 | % INPUT: 5 | % img_curr: current grayscale image 6 | % dep_prev: previous depth image 7 | % pose_rel: relative pose between the current and previous frame 8 | % K: intrinsic camera parameters 9 | % 10 | % OUTPUT: 11 | % warped_image: warped image by the rigid transformation 12 | % valid_mask: valid pixels in the warped image 13 | 14 | [height, width] = size(img_curr); 15 | 16 | [pointclouds, valid_mask] = reprojectDepthImage(dep_prev, K); 17 | warped_pointclouds = warpPointCloud(pointclouds, pose_rel); 18 | [warped_img_coords, warped_valid_mask] = projectPointCloud(warped_pointclouds, K, height, width); 19 | 20 | warped_intensities = interp2(img_curr, warped_img_coords(:, 1), warped_img_coords(:, 2), 'linear', 0); 21 | 22 | valid_indices = find(valid_mask); 23 | valid_indices = valid_indices(warped_valid_mask); 24 | valid_mask = zeros(height, width); 25 | valid_mask(valid_indices) = 1; 26 | valid_mask = logical(valid_mask); 27 | 28 | warped_image = zeros(height, width); 29 | warped_image(valid_indices) = warped_intensities; 30 | 31 | end -------------------------------------------------------------------------------- /warpPointCloud.m: -------------------------------------------------------------------------------- 1 | function warped_pointclouds = warpPointCloud(pointclouds, pose) 2 | % warp the point cloud in the 3D space 3 | % 4 | % INPUT: 5 | % pointclouds: a matrix of size [num_points, 3] 6 | % pose: a matrix of size [4, 4] 7 | % 8 | % OUTPUT: 9 | % warped_pointclouds: a matrix of size [num_points, 3] 10 | 11 | warped_pointclouds = bsxfun(@plus, pointclouds * pose(1:3, 1:3)', pose(1:3, 4)'); 12 | end --------------------------------------------------------------------------------