This article describes how to find camera matrix, including calibration matrix, from 6 or more known 3D points which have been projected to the camera sensor. Good reference for the article is in 1.
Problem description
We know at least six 3D points in the scene (
Basics
If your object has 6 known points (known 3D coordinates,
First some basics.
Homogenous coordinate is vector presentation of euclidean coordinate
Camera matrix is
Where
We remember that cross product between two 3-vectors can be defined as matrix-vector-multiplication such that:
It is also useful to note that cross production
Now lets try to solve the projection matrix
Aha! The result must be zero vector. If we now open the equation we get:
With little bit of refactoring we can get the projection matrix
Where
Small pause so we can gather our toughs. Note that the previous matrix equation has to be formed for each known 3D->2D correspondence (there must be at least 6 of them).
Now, for each point correspondence, calculate the first two rows of the matrix above, stack the
As we have 12 unknowns and (at least) 12 equations this can be solved. Only problem is that we don’t want to have the trivial answer where
Fortunately we can use singular value decomposition (SVD) to force
So to solve the the equations, calculate SVD of matrix
Now you wanted to know the distance to the object.
where
Finally, when you calculate the cameras location for two frames, you can calculate the unknown objects locations (or locations of some of the points of the object) by solving two equations for
Which goes pretty much the same way as how we solved the camera matrices:
And so on.