From 68fed569b1a4d14273ee1dbfa41bf597c4c9b7b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Calvo?= Date: Wed, 22 Dec 2021 00:43:05 +0100 Subject: [PATCH 1/3] Bundle adjustment implementation This implementation works with the formulation described in exercise 9. In my laptop it takes a couple of seconds to solve the BA problem for 4 poses. --- src/HomogMatrix2twist.m | 44 ++++++++++ src/runBA.m | 163 ++++++++++++++++++++++++++++++++++++++ src/twist2MatOperations.m | 36 +++++++++ 3 files changed, 243 insertions(+) create mode 100644 src/HomogMatrix2twist.m create mode 100644 src/runBA.m create mode 100644 src/twist2MatOperations.m diff --git a/src/HomogMatrix2twist.m b/src/HomogMatrix2twist.m new file mode 100644 index 0000000..2c36e85 --- /dev/null +++ b/src/HomogMatrix2twist.m @@ -0,0 +1,44 @@ +function twist = HomogMatrix2twist(H) + +% HomogMatrix2twist Convert 4x4 homogeneous matrix to twist coordinates +% +% Input: +% -H(4,4): Euclidean transformation matrix (rigid body motion) +% +% Output: +% -twist(6,1): twist coordinates. Stack linear and angular parts [v;w] +% +% Observe that the same H might be represented by different twist vectors +% Here, twist(4:6) is a rotation vector with norm in [0,pi] + +se_matrix = logm(H); + +% careful for rotations of pi; the top 3x3 submatrix of the returned +% se_matrix by logm is not skew-symmetric (bad). + +v = se_matrix(1:3,4); + +w = Matrix2Cross(se_matrix(1:3,1:3)); + +twist = [v; w]; + +end + +% MATRIX2CROSS Compute 3D vector corresponding to an antisymmetric matrix +% +% Computes the 3D vector x corresponding to an antisymmetric matrix M such +% that M*y = cross(x,y) for all 3D vectors y. +% +% Input: +% - M(3,3) : antisymmetric matrix +% +% Output: +% - x(3,1) : column vector +% +% See also CROSS2MATRIX + +function x = Matrix2Cross(M) + +x = [-M(2,3); M(1,3); -M(1,2)]; + +end diff --git a/src/runBA.m b/src/runBA.m new file mode 100644 index 0000000..d421811 --- /dev/null +++ b/src/runBA.m @@ -0,0 +1,163 @@ +function hidden_state = runBA(hidden_state, observations, K) +%RUNBA runs the bundle adjustment algorithm, finding the landmarks and extrinsic +% parameters that minimize the reprojection error. It does so by finding a local +% optimum of the non linear least squares problem. +% INPUT: +% - hidden_state: unidimensional vector that contains all the camera positions +% as 6D vectors and the set of 3D landmarks, all unrolled. This +% are all the values that we seek to tune in order to reduce the +% reprojection error. +% [pose_1, pose_2, ..., pose_n, l_1, l_2, ..., l_m] +% - observations: unidimensional vector that contains: +% [n m O_1, O_2, ..., O_n]. Where n is the number of camera +% poses, m is the number of landmarks and O_i is the observation +% obtained at pose i and contains: +% [k_i, p_1, p_2, ..., p_k, l_1, l_2, ..., l_k], where: +% - k_i: is the number of landmarks observed in pose i. +% - p_j: 2D position in pixel space of landmark l_j. +% - l_j: index pointing to the corresponding 3D landmark in +% the set of all 3D landmarks. +% - K: camera projection matrix (3x3) +% OUTPUT: +% - hidden_state: unidimensional vector that contains all the camera positions +% landmarks as described previously. It is the result of the +% minimization of the nonlinear least squares problem. + +debug = false; +sparse_gradients = true; + +% Extract the data +% Define the error function of the BA +error_function = @(x) errorBA(x, observations, K); + +if sparse_gradients + % Get the sparse jacobian filter matrix + M = getSparseJacobianFilter(hidden_state, observations); + + % Plot the sparsity pattern of M + if debug + figure(4); + spy(M); + end + + % Tell the optimizer to only compute the gradients that are non-zero + options = optimoptions('lsqnonlin', ... + 'JacobPattern', M, ... + 'Display', 'iter', ... + 'UseParallel', false, ... + 'MaxIter', 20); +else + options = optimoptions('lsqnonlin', ... + 'Display', 'iter', ... + 'UseParallel', false, ... + 'MaxIter', 20); +end + +[hidden_state, resnorm] = lsqnonlin(error_function, hidden_state, ... + [], [], options); + +end + +function e = errorBA(hidden_state, observations, K) +%ERRORBA given the hidden state, the observations and the camera projection +%matrix, it computes the reprojection error used as the objective of the bundle +%adjustment. +% INPUT: +% - hidden_state: as described in the function RUNBA. +% - observations: as described in the function RUNBA. +% - K: camera projection matrix (3x3) + +debug = false; + +n = observations(1); +m = observations(2); +taus = hidden_state(1:(n*6)); % Camera twist (obtained from VO) +taus = reshape(taus, 6, n); % 6xn +P = hidden_state((n*6+1):end); % 3D Landmarks +P = reshape(P, 3, m); % 3xm + +% The number of observations is the same as the number of frames +curr_id = 3; +Y = []; +f_x = []; +for f=1:n + % Get the camera homogeneous matrix in the world frame + hs_id = (f-1)*6+1; + T_WC_i = twist2HomogMatrix(taus(:,f)); + T_CW_i = inv(T_WC_i); + + % Get the number of keypoints for this frame + ki = observations(curr_id); + % Generate the vector of reference 2D points + % The observed points are in (row, cols) and they should be in x, y + aux_y = flipud(reshape(observations(curr_id+1:curr_id+2*ki), 2, ki)); + Y = [Y; aux_y(:)]; % 2 numbers per point + + % Generate the vector of reprojected 3D points from the camera twist + l_i = floor(observations(curr_id+1+2*ki:curr_id+3*ki)); % landmark ids + P_i = P(:,l_i); % retrieve the seen landmarks by order + p_i = K * T_CW_i(1:3,:) * [P_i; ones(1, size(P_i, 2))]; % Reprojection + p_i = p_i(1:2,:) ./ p_i(3,:); + f_x = [f_x; p_i(:)]; + + % DEBUG: This is used to check that the projections make sense + if debug + figure(3) + plot(p_i(1,:), p_i(2,:), 'x'); + hold on; + plot(aux_y(1,:), aux_y(2,:), 'x'); + hold off; + axis equal; + pause(0.01); + end + + % Update the current id for the next iteration + curr_id = curr_id+1+3*ki; % 2 numbers + landmark id per point +end + +e = f_x - Y; + +end + +function M = getSparseJacobianFilter(hidden_state, observations) +%GETSPARSEJACOBIANFILTER calculates the sparse matrix that represents all the +%positions where the jacobian used for the computation of the reprojection error +%are not zero. +% INPUT: +% - hidden_state: as described in RUNBA. +% - observations: as described in RUNBA. +% OUTPUT: +% - M: the sparse matrix. + +% Populate the M matrix +n = observations(1); +m = observations(2); +taus = hidden_state(1:(n*6)); % Camera twist (obtained from VO) +taus = reshape(taus, 6, n); % 6xn +P = hidden_state((n*6+1):end); % 3D Landmarks +P = reshape(P, 3, m); % 3xm +numel_obs = (numel(observations)-2-n)/3; +% Prepare the sparse jacobian matrix +M = spalloc(numel_obs, length(hidden_state), 9*numel_obs); + +curr_id = 3; +n_obs = 1; +for f=1:n + ki = observations(curr_id); + l_i = floor(observations(curr_id+1+2*ki:curr_id+3*ki)); + + % Dependence on the camera pose + M(n_obs:n_obs+ki*2-1, (f-1)*6+1:(f-1)*6+6) = 1; + + % Dependence on the landmark + for l_id=1:numel(l_i) + M(n_obs+2*(l_id-1):n_obs-1+l_id*2, ... + n*6+3*(l_i(l_id)-1)+1:n*6+3*l_i(l_id)) = 1; + end + + % Update the current id for the next iteration + n_obs = n_obs + ki*2; + curr_id = curr_id+1+3*ki; % 2 numbers + landmark id per point +end + +end diff --git a/src/twist2MatOperations.m b/src/twist2MatOperations.m new file mode 100644 index 0000000..6569cc2 --- /dev/null +++ b/src/twist2MatOperations.m @@ -0,0 +1,36 @@ +function H = twist2HomogMatrix(twist) + +% twist2HomogMatrix Convert twist coordinates to 4x4 homogeneous matrix +% +% Input: +% -twist(6,1): twist coordinates. Stack linear and angular parts [v;w] +% Output: +% -H(4,4): Euclidean transformation matrix (rigid body motion) + +v = twist(1:3); % linear part +w = twist(4:6); % angular part + +se_matrix = [Cross2Matrix(w), v; 0 0 0 0]; % Lie algebra matrix +H = expm(se_matrix); + +end + +% CROSS2MATRIX Antisymmetric matrix corresponding to a 3D vector +% +% Computes the antisymmetric matrix M corresponding to a 3D vector x such +% that M*y = cross(x,y) for all 3D vectors y. +% +% Input: +% - x(3,1) : vector +% +% Output: +% - M(3,3) : antisymmetric matrix +% +% See also MATRIX2CROSS + +function M = Cross2Matrix(x) + +M = [0,-x(3),x(2); x(3),0,-x(1);-x(2),x(1),0]; + +end + From 0eeaa457c7a3d9aae87e51b1e3c5ba2a24ac2aaf Mon Sep 17 00:00:00 2001 From: Joshua O'Reilly Date: Mon, 20 Dec 2021 21:35:06 -0500 Subject: [PATCH 2/3] Add basic plotting functionality Plots in (more or less) same format as example GUI from project PDF. imshow() will probably need to be replaced with something else once we know how we'd like to integrate the keypoints to the current frame; additionally, the top view will need a new data structure, since it should only be holding the points from the previous 20 frames, and Im not sure how to do that efficiently off the top of my head --- src/createFigure.m | 21 +++++++++++++++++++++ test/testPlotting.m | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+) create mode 100644 src/createFigure.m create mode 100644 test/testPlotting.m diff --git a/src/createFigure.m b/src/createFigure.m new file mode 100644 index 0000000..61b0276 --- /dev/null +++ b/src/createFigure.m @@ -0,0 +1,21 @@ +function fig = createFigure(numPointsNumLandmarks) + fig = struct; + fig.f = figure; + + fig.currentFramePlot = subplot(2,2,1); + fig.currentFrameData = []; + + fig.topViewPlot = subplot(2,2,2); + % should only be tracking last 20 frames, so need some clever way of tracking and updating these arrays (efficiently) + fig.TopViewX = []; + fig.TopViewY = []; + axis equal; + + fig.numLandmarksPlot = subplot(2,2,3); + fig.numLandmarksData = animatedline('MaximumNumPoints',numPointsNumLandmarks); + axis equal; + + fig.fullTrajectoryPlot = subplot(2,2,4); + fig.fullTrajectoryData = animatedline; + axis equal; +end \ No newline at end of file diff --git a/test/testPlotting.m b/test/testPlotting.m new file mode 100644 index 0000000..ffbb3b1 --- /dev/null +++ b/test/testPlotting.m @@ -0,0 +1,33 @@ +fig = createFigure(50); + +pause('on') + +numPoints = 200; + +x = linspace(1,numPoints,numPoints); +y = sin(x); +z = cos(x); + +pointX = rand(1,numPoints); +pointY = rand(1,numPoints); + +for i = 1:numel(x) + pause(0.01); + + % update animatedlines + addpoints(fig.numLandmarksData, x(i), y(i)) + addpoints(fig.fullTrajectoryData, x(i), z(i)) + + % update point cloud (animatedlines automatically connects points) + fig.TopViewX = [fig.TopViewX, pointX(i)]; + fig.TopViewY = [fig.TopViewY, pointY(i)]; + plot(fig.topViewPlot, fig.TopViewX(1:i), fig.TopViewY(1:i), 'o'); + + % new camera frame + img = imread(strcat("data/kitti05/kitti", '/05/image_0/', ... + sprintf('%06d.png',i))); + % some modification of the image here... + imshow(img, 'Parent', fig.currentFramePlot); + + drawnow +end \ No newline at end of file From 24b01e27bf5f52c346f1058ca6fcc2dd89fec370 Mon Sep 17 00:00:00 2001 From: Joshua O'Reilly Date: Tue, 21 Dec 2021 14:09:29 -0500 Subject: [PATCH 3/3] Add updateFigure.m, add scaling, image support, amongst others Big ol commit which adds images with point annotation, two sets of points for the top view plot (since it needs vehicle position and 3D landmark location), titles (mostly, see TODO in createFigure.m), and rebuids testPlotting.m to test all this --- src/createFigure.m | 44 +++++++++++++++++++++++++++++++++++++------- src/updateFigure.m | 38 ++++++++++++++++++++++++++++++++++++++ test/testPlotting.m | 34 ++++++++++++++++++++++------------ 3 files changed, 97 insertions(+), 19 deletions(-) create mode 100644 src/updateFigure.m diff --git a/src/createFigure.m b/src/createFigure.m index 61b0276..7fbdf7a 100644 --- a/src/createFigure.m +++ b/src/createFigure.m @@ -1,21 +1,51 @@ -function fig = createFigure(numPointsNumLandmarks) +function [fig, topViewLandmarksX, topViewLandmarksY, topViewCarX, topViewCarY] = createFigure(numPointsNumLandmarks, numFrames) + %CREATEFIGURE + % Creates and returns a figure which can be updated with each frame processed + + % Inputs + % + % numPointsNumLandmarks = number of points to show in the Number of Landmarks plot before sliding the window + % numFrames = number of frame data to show in the top-view plot + + % Outputs + % + % fig = figure itself + % topViewLandmarksX = cell array to update landmarks to show in last numFrames frames + % topViewLandmarksY = cell array to update landmarks to show in last numFrames frames + % topViewCarX = cell array to update car positions to show in last numFrames frames + % topViewCarY = cell array to update car positions to show in last numFrames frames + fig = struct; fig.f = figure; + fig.numFrames = numFrames; + % Top left corner: plot current frame and keypoints or whatever else we'd like fig.currentFramePlot = subplot(2,2,1); - fig.currentFrameData = []; + title('Current Image') + hold(fig.currentFramePlot, 'on') + % Top right corner: plot 3D landmarks and car position as seen from above fig.topViewPlot = subplot(2,2,2); - % should only be tracking last 20 frames, so need some clever way of tracking and updating these arrays (efficiently) - fig.TopViewX = []; - fig.TopViewY = []; + % TODO: get this title to show? + title(sprintf('Trajectory of last %d frames and landmarks', numFrames)); axis equal; - + % each cell contains the values for a given frame + % since matlab is pass by value only, we need to update these outside this function + topViewLandmarksX = cell(1,numFrames); + topViewLandmarksY = cell(1,numFrames); + topViewCarX = cell(1,numFrames); + topViewCarY = cell(1,numFrames); + + % Bottom left corner: number of keypoints tracked over the last numFrames frames fig.numLandmarksPlot = subplot(2,2,3); fig.numLandmarksData = animatedline('MaximumNumPoints',numPointsNumLandmarks); + xlim([-numFrames, 0]) + title(sprintf('Number of landmarks tracked over last %d frames', numFrames)); + addpoints(fig.numLandmarksData, 0, 0) axis equal; + % Bottom right corner: position of vehicle over time? fig.fullTrajectoryPlot = subplot(2,2,4); fig.fullTrajectoryData = animatedline; - axis equal; + title('Full Trajectory') end \ No newline at end of file diff --git a/src/updateFigure.m b/src/updateFigure.m new file mode 100644 index 0000000..2bb524c --- /dev/null +++ b/src/updateFigure.m @@ -0,0 +1,38 @@ +function [] = updateFigure(fig, img, imgPoints, numLandmarksPoint, fullTrajectoryPoints, topViewLandmarkPointsX, topViewLandmarkPointsY, topViewCarPointsX, topViewCarPointsY) + % UPDATEFIGURE + % updates plots with data from newest frame + + % Inputs + % + % fig = figure to update + % img = raw frame + % imgPoints = set of keypoints to project on frame + % numLandmarksPoint = number of landmarks in current frame + % fullTrajectoryPoints = x and y coordinates of new vehicle position (can be batched) + % topViewLandmarkPointsX = cell array to update landmarks to show in last numFrames frames + % topViewLandmarkPointsY = cell array to update landmarks to show in last numFrames frames + % topViewCarPointsX = cell array to update car positions to show in last numFrames frames + % topViewCarPointsY = cell + + % Top left corner: plot current frame and keypoints or whatever else we'd like + imshow(img, 'Parent', fig.currentFramePlot); + plot(fig.currentFramePlot, imgPoints(:,1), imgPoints(:,2), 'co'); + + % Top right corner: plot 3D landmarks and car position as seen from above in last numFrames frames + % concatenate cells to get full X and Y sets + plot(fig.topViewPlot, cell2mat(topViewLandmarkPointsX), cell2mat(topViewLandmarkPointsY), 'ko'); + hold(fig.topViewPlot, 'on') + plot(fig.topViewPlot, cell2mat(topViewCarPointsX), cell2mat(topViewCarPointsY), 'ro'); + hold(fig.topViewPlot, 'off') + + % Bottom left corner: number of keypoints tracked over the last numFrames frames + % uber janky way of getting this to plot from -numFrames to 0 + [x,y] = getpoints(fig.numLandmarksData); + addpoints(fig.numLandmarksData, x-(max(x)+1), y); + addpoints(fig.numLandmarksData, x(end)+1, numLandmarksPoint) + + % Bottom right corner: position of vehicle over time? + addpoints(fig.fullTrajectoryData, fullTrajectoryPoints(:,1), fullTrajectoryPoints(:,2)) + + drawnow +end \ No newline at end of file diff --git a/test/testPlotting.m b/test/testPlotting.m index ffbb3b1..b3c5961 100644 --- a/test/testPlotting.m +++ b/test/testPlotting.m @@ -1,4 +1,7 @@ -fig = createFigure(50); +numFrames = 20; + +[fig, topViewLandmarksX, topViewLandmarksY, topViewCarX, topViewCarY] = createFigure(50, numFrames); +topViewLastCell = 1; pause('on') @@ -10,24 +13,31 @@ pointX = rand(1,numPoints); pointY = rand(1,numPoints); +pointX2 = rand(1,numPoints); +pointY2 = rand(1,numPoints); for i = 1:numel(x) pause(0.01); - % update animatedlines - addpoints(fig.numLandmarksData, x(i), y(i)) - addpoints(fig.fullTrajectoryData, x(i), z(i)) - - % update point cloud (animatedlines automatically connects points) - fig.TopViewX = [fig.TopViewX, pointX(i)]; - fig.TopViewY = [fig.TopViewY, pointY(i)]; - plot(fig.topViewPlot, fig.TopViewX(1:i), fig.TopViewY(1:i), 'o'); - % new camera frame img = imread(strcat("data/kitti05/kitti", '/05/image_0/', ... sprintf('%06d.png',i))); % some modification of the image here... - imshow(img, 'Parent', fig.currentFramePlot); - drawnow + % annoying work we need to do because pass-by-value, this will be in the final script + topViewLandmarksX{topViewLastCell} = pointX(i); + topViewLandmarksY{topViewLastCell} = pointY(i); + topViewCarX{topViewLastCell} = pointX2(i); + topViewCarY{topViewLastCell} = pointY2(i); + topViewLastCell = mod(topViewLastCell + 1, numFrames); + if topViewLastCell == 0 + topViewLastCell = 1; + end + + % just for test + landMarkPositionsX = cell2mat(topViewLandmarksX)' * 1226; + landMarkPositionsY = cell2mat(topViewLandmarksY)' * 370; + + % lots of transposes and concatenation because x and y are separate structures in this little test script; when extracting keypoints using matlab's built-in functions, these will already be in the right kx2 format + updateFigure(fig, img, [landMarkPositionsX,landMarkPositionsY], y(i), [x(i), z(i)], topViewLandmarksX, topViewLandmarksY, topViewCarX, topViewCarY); end \ No newline at end of file