robotics专项课程Perception Assignment 3: Image Projection

function [ corners ] = track_corners(images, img_pts_init)
%TRACK_CORNERS
% This function tracks the corners in the image sequence and visualizes a
% virtual box projected into the image
% Inputs:
%     images - size (N x 1) cell containing the sequence of images to track
%     img_pts_init - size (4 x 2) matrix containing points to initialize
%       the tracker
% Outputs:
%     corners - size (4 x 2 x N) array of where the corners are tracked to

corners = zeros(4,2,size(images,1));

%%%% INITIALIZATION CODE FOR TRACKER HERE %%%%

img_pts = img_pts_init; % img_pts is where you will store the tracked points
corners(:,:,1) = img_pts;
% frame =imread('data/apriltagims/image001.jpg');
% Initialize the images 
PointTracker=vision.PointTracker('MaxBidirectionalError', 2);
initialize(PointTracker,img_pts,images{1});
cell2mat(images(1));
% Process all the images
for i = 2:size(images,1)
    %%%% CODE FOR TRACKING HERE %%%%
%     frame = imread(sprintf('data/apriltagims/image%03d.jpg',i));
    [img_pts,validity]=step(PointTracker,images{i});
    % Store corners and visualize results (if desired)
    corners(:,:,i) = img_pts;
   
end

end

function [proj_points, t, R] = ar_cube(H,render_points,K)
%% ar_cube
% Estimate your position and orientation with respect to a set of 4 points on the ground
% Inputs:
%    H - the computed homography from the corners in the image
%    render_points - size (N x 3) matrix of world points to project
%    K - size (3 x 3) calibration matrix for the camera
% Outputs:
%    proj_points - size (N x 2) matrix of the projected points in pixel
%      coordinates
%    t - size (3 x 1) vector of the translation of the transformation
%    R - size (3 x 3) matrix of the rotation of the transformation
% Written by Stephen Phillips for the Coursera Robotics:Perception course

% YOUR CODE HERE: Extract the pose from the homography
[nrows, ncols] = size(render_points);
proj_points=zeros(nrows, 2);
h = inv(K)*H;
h1 = h(:,1);
h2 = h(:,2);
h3 = h(:,3);
A = [h1 h2 cross(h1,h2)];
[U S V] = svd(A);
s = [1 1 det(U*V')];
R = U*diag(s)*V';
t = h3/norm(h1);

% YOUR CODE HERE: Project the points using the pose
for i = 1:nrows
    X = render_points(i,:)';
    Xc = K*(R*X+t);
    proj_points(i, :) = [Xc(1)/Xc(3) Xc(2)/Xc(3)];
end
end