Orientation and position from point correspondences

Assuming the intrinsics K, d, and g are known from a previous calibration, the extrinsic parameters R and t are estimated from a set of point correspondences (rho, dev) using the 'oriapos' method of the dpdev class.

Distorted pinhole camera
Point correspondences
Plotting arrangement
Obtaining pose using a homography (pinhole)
Obtaining pose using 'dpdev.oriapos'

Distorted pinhole camera

A camera is simulated using known parameters.

pars.DT = 'dpin';
pars.MN = [600 800];
pars.K  = [ 1, 2, 3
            0, 4, 5
            0, 0, 1 ];
pars.R  = rmat( [-1.5, 2.5, 2] );
pars.t  = [10; -20; 30];
pars.d  = [-1.9, -1.8];
pars.g  = [ 0.1, -0.2];
cam = dpdev(pars);

Point correspondences

A set of points on the reference plane (xy-plane) are created. This points represent the corners of a checkerboard.

rx = linspace(-10, 10, 11);
ry = linspace( -5,  5,  6);
[Rx, Ry] = meshgrid(rx, ry);
rho = [Rx(:), Ry(:)]';      % Reference plane

The corresponding points are obtained by 'capturing' the observed points 'rho' using the 'rsp2dev' method. As a result, the associated points 'dev' on the camera image plane are obtained.

dev = cam.rsp2dev( rho );   % Image plane

Plotting arrangement

subplot(1,2,1); cam.draw('cam','Camera');
hold on
    surf( Rx, Ry, zeros(size(Rx)) )
hold off
title("Arrangement")

subplot(2,2,2)
surf( reshape( dev(1,:), size(Rx) ), ...
      reshape( dev(2,:), size(Rx) ), zeros(size(Rx)) ); view(2)
title("Captured image")

Obtaining pose using a homography (pinhole)

The classical homography-based extrinsic parameter estimation is used for illustration. This process is not accurate because lens distortion is not taken into account.

G = Gme( rho, dev );        % Homography estimation
[R2, t2] = Lme(G, cam.K );  % Extrinsics from homography

disp("R2 ="); disp( R2 )
disp("t2 ="); disp( t2 )

disp("Exact extrinsics (for comparison)")
disp("cam.R ="); disp( cam.R )
disp("cam.t ="); disp( cam.t )

[BEGIN Gme]	 	RPe_1 = 0.01343	 	CTime = 1.8432 msec.	[END Gme]
[BEGIN Lme]	 	Rte_1 = 0.00017658	 	CTime = 0.6929 msec.	[END Lme]
R2 =
    0.9278    0.2665   -0.2611
    0.3656   -0.7890    0.4938
   -0.0744   -0.5536   -0.8294

t2 =
   11.5176
  -20.1321
   34.4557

Exact extrinsics (for comparison)
cam.R =
    0.9306    0.2682   -0.2491
    0.3636   -0.7561    0.5442
   -0.0423   -0.5970   -0.8011

cam.t =
    10
   -20
    30

Obtaining pose using 'dpdev.oriapos'

A new camera 'cam2' is built with the known intrinsics taken from the 'cam' object and empty extrinsic parameters.

cam2 = cam.setpose(  [] , []  ); % New camera object
cam2 = cam2.oriapos( rho, dev ); % Extrinsic estimation

disp("cam2.R ="); disp( cam2.R )
disp("cam2.t ="); disp( cam2.t )