title: Triangulation using the direct linear transform taken directly from Hartley & Zisserman
body: <p>Below is a Matlab function that implements the triangulation method as described in Multiple View Geometry. It takes the 8 corner points of a cube (OriginalPoints) and projects them on two screens. The screen points are then in CameraPoints and ProjectorPoints. The goal is to reconstruct the original points from these two views using the Direct Linear Transform. Unfortunately the result is gibberish. </p>
  
  <p>There is <a href="https://stackoverflow.com/questions/2276445/triangulation-direct-linear-transform">another topic here on SO</a>, from which the normalization of the rows of A are taken, but that did not improve the result.</p>
  
  <p>Three general questions:</p>
  
  <ul>
  <li>When normalizing the data before the SVD (origin = centroid and avg distance = sqrt(2)), do I calculate the average distance over (x,y) or (x, y, w)? Inhomogeneous or homogeneous?</li>
  <li>After every operation, where possible, I scale the homogeneous coordinate w back to 1.0, is this correct?</li>
  <li>Since the camera matrices are known completely (not just up to a projective transformation) the resulting points should differ no more from the original points than a Euclidean transformation, right?</li>
  </ul>
  
  <pre><code>
      % Demos the triangulation method using the direct linear transform
      % algorithm
      function testTriangulation()
          close all;
  
      camWidth = 800;
      camHeight = 600;
      projectorWidth = 5080;
      projectorHeight = 760;
  
      % camera matrices
      CameraMatrix =    [
                      -1.81066 0.00000 0.00000 0.00000;
                      0.00000 2.41421 0.00000 0.00000;
                      0.00000 0.00000 1.00000 1.50000
                  ];
  
      ProjectorMatrix =   [
                      -0.36118 0.00000 0.00000 0.00000
                      0.00000 2.00875 1.33916 0.00000
                      0.00000 -0.55470 0.83205 1.80278
                  ];
  
      % generating original points and displaying them
      OriginalPoints =    [
                              -0.2 -0.2 -0.2 -0.2 0.2 0.2 0.2 0.2;
                              -0.2 -0.2 0.2 0.2 -0.2 -0.2 0.2 0.2;
                              -0.2 0.2 -0.2 0.2 -0.2 0.2 -0.2 0.2;
                              1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0
                          ];    
      scatter3(OriginalPoints(1, :), OriginalPoints(2, :), OriginalPoints(3,:));
      title('Original Points');
  
      % projecting and normalizing camera points
      CameraPoints = CameraMatrix * OriginalPoints;
      for i = 1:3
          CameraPoints(i, :) = CameraPoints(i, :) ./ CameraPoints(3,:);
      end
      %     figure();
      %     scatter(CameraPoints(1,:), CameraPoints(2,:));
      %     title('Projected Camera Points');
  
      % transforming camera points to screen coordinates
      camXOffset = camWidth / 2;
      camYOffset = camHeight / 2; 
      CameraPoints(1,:) = CameraPoints(1,:) * camXOffset + camXOffset;
      CameraPoints(2,:) = CameraPoints(2,:) * camYOffset + camYOffset;
      figure();
      scatter(CameraPoints(1,:), CameraPoints(2,:));
      title('Projected Camera Points in Screen Coordinates');
  
  
      % projecting and normalizing projector points
      ProjectorPoints = ProjectorMatrix * OriginalPoints;
      for i = 1:3
          ProjectorPoints(i, :) = ProjectorPoints(i, :) ./ ProjectorPoints(3,:);
      end
      %     figure();
      %     scatter(ProjectorPoints(1,:), ProjectorPoints(2,:));
      %     title('Projected Projector Points');
  
      % transforming projector points to screen coordinates
      projectorXOffset = projectorWidth / 2;
      projectorYOffset = projectorHeight / 2; 
      ProjectorPoints(1,:) = ProjectorPoints(1,:) * projectorXOffset + projectorXOffset;
      ProjectorPoints(2,:) = ProjectorPoints(2,:) * projectorYOffset + projectorYOffset;
      figure();
      scatter(ProjectorPoints(1,:), ProjectorPoints(2,:));
      title('Projected Projector Points in Screen Coordinates');
  
  
      % normalizing camera points (i.e. origin is
      % the centroid and average distance is sqrt(2)).
      camCenter = mean(CameraPoints, 2);
      CameraPoints(1,:) =  CameraPoints(1,:) - camCenter(1);
      CameraPoints(2,:) = CameraPoints(2,:) - camCenter(2);
      avgDistance = 0.0;
      for i = 1:length(CameraPoints(1,:))
          avgDistance = avgDistance + norm(CameraPoints(:, i));
      end
      avgDistance = avgDistance / length(CameraPoints(1, :));
      scaleFactor = sqrt(2) / avgDistance;
      CameraPoints(1:2, :) = CameraPoints(1:2, :) * scaleFactor;
  
      % normalizing projector points (i.e. origin is
      % the centroid and average distance is sqrt(2)).
      projectorCenter = mean(ProjectorPoints, 2);
      ProjectorPoints(1,:) =  ProjectorPoints(1,:) - projectorCenter(1);
      ProjectorPoints(2,:) = ProjectorPoints(2,:) - projectorCenter(2);
      avgDistance = 0.0;
      for i = 1:length(ProjectorPoints(1,:))
          avgDistance = avgDistance + norm(ProjectorPoints(:, i));
      end
      avgDistance = avgDistance / length(ProjectorPoints(1, :));
      scaleFactor = sqrt(2) / avgDistance;
      ProjectorPoints(1:2, :) = ProjectorPoints(1:2, :) * scaleFactor;
  
  
      % triangulating points
      TriangulatedPoints = zeros(size(OriginalPoints));
      for i = 1:length(OriginalPoints(1, :))
          A = [
                  CameraPoints(1, i) * CameraMatrix(3, :) - CameraMatrix(1, :);
                  CameraPoints(2, i) * CameraMatrix(3, :) - CameraMatrix(2, :);
                  ProjectorPoints(1, i) * ProjectorMatrix(3,:) - ProjectorMatrix(1,:);
                  ProjectorPoints(2, i) * ProjectorMatrix(3,:) - ProjectorMatrix(2,:)
              ];
  
          % normalizing rows of A, as suggested in a topic on StackOverflow
          A(1,:) = A(1,:)/norm(A(1,:));
          A(2,:) = A(2,:)/norm(A(2,:));
          A(3,:) = A(3,:)/norm(A(3,:));
          A(4,:) = A(4,:)/norm(A(4,:));
  
          [U S V] = svd(A);
          TriangulatedPoints(:, i) = V(:, end);
      end
      for i = 1:4
          TriangulatedPoints(i, :) = TriangulatedPoints(i, :) ./ TriangulatedPoints(4, :);
      end
      figure()
      scatter3(TriangulatedPoints(1, :), TriangulatedPoints(2, :), TriangulatedPoints(3, :));
      title('Triangulated Points');
  </code></pre>