Implementation of the previous algorithem by octave

In the last article I have introduced a way how to solve the PnP problem theoretically, but every theory is always just beautiful beyond the reality. During the implementation the drawbacks of the algorithm come in front one by one. Here I would like to give a short description about my job:
1) build a virtual target with four markers, and set it into the camera coordinate system, in the system the four markers' CS is as follows:
1 0 4
0 1 4
-1 0 2
0 -1 3

With the pinhole camera projection, we can obtain the projected CS on the camera sensor as follows (actually just as x/z and y/z for every marker):
0.25 0
0 0.25
-0.5 0
0 -0.33333

in the previous two pictures you can get the geometry feeling more obviously, and here is another image about distribution inside the camera :

So with this model and the algorithm introduced before we can realize the pose reconstruction job with any implementation way. At the beginning I choose the octave programming (extrem similar to Matlab), and later I will rewrite it in C++. And the final result of the job is as follows and the comparison between the after reconstruction and original form is shown in the image

the target is in the gross reconstructed, but in fact not so perfectly. According to my program, and my design I guess one of the main reason is the target is relatively large for the camera coordinate system. The case is a little like push the body too close in front of the camera, so the next several days I will redesign a body with proper distance to the camera, and some great mathematical tools to optimize the statistic result is also relevant for a better reconstruction.
By the way, to implement the algorithm with massive parameter transmission is miserable and anguished. So I'd like to save your time to enjoy a cup of cafe, and here is the link of a matlab or octave file to complete the parameter matrix: click me

Posted in: Codes,tracking technology,Virtual Reality