title: Camera pose estimation using QR codes in MATLAB
body: <p>i'm trying to learn camera pose estimation and have been been through quite a lot of material. I want to calculate the pose of the camera using QR codes (quick response code) set up like this on an standard A4:</p>
  
  <p>A0 A1</p>
  
  <p>B0 B1</p>
  
  <p>C0 C1</p>
  
  <ul>
  <li>Took some photos of the QR codes which i printed out on A4 paper, which have dimensions 210mm x 297 mm</li>
  </ul>
  
  <p>This is a short code i wrote in MATLAB to get the coordinates of the world points where i assume the bottom left corner is (0,0)</p>
  
  <pre><code>n=2; %number of qr in x direction
  m=3; %number of qr in y direction
  x_FE=50; %distance from left edge to point1 in mm
  y_FE=270; %distance from bottom to top qr code
  sqr_dist=27.6; %distance between qr codes [mm]
  sqr_size=47.4; %size of qr code height or width [mm]
  QRnames=['A0'; 'A1';'B0'; 'B1';'C0'; 'C1'];
  
  for i=1:n %number of squares in x directions
      x1(i)=x_FE+sqr_dist*(i-1);
      x2(i)=x_FE+sqr_size+sqr_dist*(i-1);
  end
  
  for j=1:m %number of squares in y directions
      y1(j)=y_FE-sqr_dist*(j-1);
      y2(j)=y_FE-sqr_size-sqr_dist*(j-1);
  end
  
   n=1;
  for k=1:3 %qr in y direction
      for j=1:2 %qr in x direction
          p1=[x1(j),y1(k)]
          p2=[x1(j),y2(k)]
          p3=[x2(j),y2(k)]
          p4=[x2(j),y1(k)]
          varname1=(['p' int2str(i)]);
          varname2=(['x' int2str(j)]);
          varname3=(['y' int2str(j)]);
          eval([QRnames(n) int2str(j-1) 'w=[p1 p2 p3 p4]' ';' ]);
          n=n+1;
      end
  end
  </code></pre>
  
  <p>Ive also checked the coordinates by measuring with a ruler</p>
  
  <p>Using a Python script called ZBar which is a QR code reader. i can extract the image coordinates from the QR codes which is embedded in the QR code itself (i think).</p>
  
  <p>The coordinates get saved in a .txt file which is setup this way</p>
  
  <p>QRcode name  x1 y1 x2 y2 x3 y3 x4 y4</p>
  
  <p>Here are the images: <a href="http://imgur.com/a/xPkWB#0" rel="nofollow">http://imgur.com/a/xPkWB#0</a></p>
  
  <p>here are some of the results coordinates:</p>
  
  <p><strong>image2</strong></p>
  
  <p>A0  452 495 454 878 836 878 834 493 </p>
  
  <p>B0  455 1099    453 1479    836 1477    836 1099</p>
  
  <p>B1  1056    1099    1056    1477    1442    1481    1442    1096    </p>
  
  <p>C0  449 1702    444 2091    831 2093    833 1700</p>
  
  <p>C1  1056    1701    1056    2095    1447    2102    1445    1706    </p>
  
  <p>A1  1057    491 1057    878 1443    873 1445    485 </p>
  
  <p><strong>image3</strong></p>
  
  <p>A1  1046    375 1051    737 1417    725 1412    364</p>
  
  <p>B0  479 955 482 1316    848 1310    843 950</p>
  
  <p>A0  475 389 477 747 841 740 835 379</p>
  
  <p>B1  1053    946 1058    1307    1426    1302    1421    938</p>
  
  <p>C1  1061    1518    1066    1895    1444    1895    1432    1517</p>
  
  <p>C0  481 1528    479 1901    851 1896    848 1521    </p>
  
  <p><strong>image8</strong></p>
  
  <p>A0  528 484 483 738 879 728 891 476 </p>
  
  <p>A1  1104    472 1109    728 1512    716 1473    463 </p>
  
  <p>B0  452 906 389 1240    852 1237    870 899</p>
  
  <p>B1  1113    896 1119    1234    1595    1226    1540    884</p>
  
  <p>C0  343 1472    248 1957    809 1953    839 1465    </p>
  
  <p>C1  1126    1463    1139    1952    1717    1955    1633    1461    </p>
  
  <p><strong>image9</strong></p>
  
  <p>A0  2040    560 1741    553 1818    812 2138    811 </p>
  
  <p>B0  1562    548 1242    537 1282    806 1625    809</p>
  
  <p>A1  2200    977 1866    980 1969    1311    2325    1301    </p>
  
  <p>C0  1047    528 691 515 689 797 1073    804 </p>
  
  <p>B1  1665    979 1308    984 1359    1328    1751    1318    </p>
  
  <p>C1  1090    983 685 985 677 1359    1119    1340    </p>
  
  <p><strong>image10</strong></p>
  
  <p>A1  562 823 943 814 993 517 643 531</p>
  
  <p>B1  1169    805 1571    793 1574    484 1201    504 </p>
  
  <p>A0  393 1409    835 1410    907 1010    506 1017    </p>
  
  <p>C1  1819    782 2268    768 2209    449 1801    471 </p>
  
  <p>B0  1098    1408    1571    1410    1570    997 1146    1003    </p>
  
  <p>C0  1862    1415    2390    1420    2309    982 1833    991 </p>
  
  <p>Here is information about the camera i used (ipod touch 5.gen):</p>
  
  <pre><code>         ExposureTime: 0.0167
                  FNumber: 2.4000
          ExposureProgram: 'Normal program'
          ISOSpeedRatings: 32
              ExifVersion: [4x1 double]
         DateTimeOriginal: '2013:11:23 11:44:52'
        DateTimeDigitized: '2013:11:23 11:44:52'
  ComponentsConfiguration: 'YCbCr'
        ShutterSpeedValue: 5.9069
            ApertureValue: 2.5261
          BrightnessValue: 5.9402
             MeteringMode: 'Pattern'
                    Flash: [1x148 char]
              FocalLength: 3.3000
              SubjectArea: [4x1 double]
                MakerNote: [196x1 double]
       SubsecTimeOriginal: '317'
      SubsecTimeDigitized: '317'
          FlashpixVersion: [4x1 double]
               ColorSpace: 'sRGB'
         CPixelXDimension: 1936
         CPixelYDimension: 2592
            SensingMethod: 'One-chip color area sensor'
                SceneType: 'A directly photographed image'
             ExposureMode: 'Auto exposure'
             WhiteBalance: 'Auto white balance'
         DigitalZoomRatio: 1.0021
    FocalLengthIn35mmFilm: 33
         SceneCaptureType: 'Standard'
              UnknownTags: [3x1 struct]
  </code></pre>
  
  <p>and info about the first test image</p>
  
  <blockquote>
    <blockquote>
      <p>info=imfinfo('testimage1002.jpg')</p>
    </blockquote>
  </blockquote>
  
  <p>info = </p>
  
  <pre><code>        Filename: 'D:\dropbox\Dropbox\campose\testimage_1\testimage1002.jpg'
       FileModDate: '23-Nov-2013 11:45:16'
          FileSize: 1188331
            Format: 'jpg'
     FormatVersion: ''
             Width: 1936
            Height: 2592
          BitDepth: 24
         ColorType: 'truecolor'
   FormatSignature: ''
   NumberOfSamples: 3
      CodingMethod: 'Huffman'
     CodingProcess: 'Sequential'
           Comment: {}
              Make: 'Apple'
             Model: 'iPod touch'
       Orientation: 1
       XResolution: 72
       YResolution: 72
    ResolutionUnit: 'Inch'
          Software: '7.0.4'
          DateTime: '2013:11:23 11:45:16'
  YCbCrPositioning: 'Centered'
     DigitalCamera: [1x1 struct]
           GPSInfo: [1x1 struct]
     ExifThumbnail: [1x1 struct]
  </code></pre>
  
  <p>i read the .txt files and set them up like this</p>
  
  <pre><code>txt=fopen('testimage1002.txt');
  datascan01=textscan(txt,'%s%f%f%f%f%f%f%f%f');
  
  QRname01=[datascan01{1}];
  x1=[datascan01{2}];
  y1=[datascan01{3}];
  
  
  % point 2
  x2=[datascan01{4}];
  y2=[datascan01{5}];
  
  
  % point 3
  x3=[datascan01{6}];
  y3=[datascan01{7}];
  
  
  % point 4
  x4=[datascan01{8}];
  y4=[datascan01{9}];
  
  p1=0;p2=0;p3=0;p4=0;
  
  for n=1:length(datascan01{1})
      for i=1:4
          for j=1:4
              varname1=(['p' int2str(i)]);
              varname2=(['x' int2str(i)]);
              varname3=(['y' int2str(i)]);
              varname4=([QRname01{n}]);
              eval(['p' int2str(i) '=[x' int2str(i) '(n)' ' y' int2str(i) '(n)];'])
              eval([varname4 '=[p1 p2 p3 p4]' ';' ]);
          end
      end
  end
  </code></pre>
  
  <p>Now, i have image coordinates and world coordinates and want to find how the camera was standing when the image was captured. </p>
  
  <p>ive looked at  Jean- Yves Bouget's Camera Calibration Toolbox but couldnt figure out how to use the functions using QR codes</p>
  
  <p>also looked at 'Finding optimal rotation and translation between corresponding 3D points' (<a href="http://nghiaho.com/?page_id=671" rel="nofollow">http://nghiaho.com/?page_id=671</a>)</p>
  
  <p>But the resulting rotation matrix, translation vector and rmse seemed wrong. (got rmse=440?)</p>
  
  <p><strong>Calibration matrix</strong></p>
  
  <p>%Image Resolution: 2592 x 1936 pixels</p>
  
  <p>ImgRes=[2592 1936];% pixels</p>
  
  <p>Focallength= 3.3; % mm</p>
  
  <p>%--- Calibration Results ---
  % Focals (pixels) - </p>
  
  <p>Fx= 2329.13; </p>
  
  <p>Fy= 2324.04;</p>
  
  <p>% Optical center - </p>
  
  <p>Cx= 1307; Cy= 950.382;</p>
  
  <p>%Radial distortion (Brown's Model)
  K1= 0.236384; </p>
  
  <p>K2= -0.680501; </p>
  
  <p>K3= 0;</p>
  
  <p>P1= 0.000319648;</p>
  
  <p>P2= 0.000751849;</p>
  
  <p>%pixel size fx=f/sx</p>
  
  <p>sx=Focallength/Fx;</p>
  
  <p>sy=Focallength/Fy;</p>
  
  <p>distortion = [K1 K2 K3 P1 P2];</p>
  
  <p>Any ideas on how i can do this (hopefully in matlab)? all help is appreciated</p>
  
  <ul>
  <li>Jonathan Brian </li>
  </ul>