title: camera pose estimation with solvePnP() and SOLVEPNP_IPPE_SQUARE method
body: <p>I'm working with ARKit and trying to get camera position from QR code with known size (0.16m).
  To detect QR code I'am using Vision framework so i can get each corner point on image. </p>
  
  <p>Data preparation:</p>
  
  <pre><code>let intrinsics = arFrame.camera.intrinsics
  let imageResolution = arFrame.camera.imageResolution
  
  let imagePointsArray = [NSValue(cgPoint: visionResult.topLeft), NSValue(cgPoint: visionResult.topRight), NSValue(cgPoint: visionResult.bottomLeft), NSValue(cgPoint: visionResult.bottomRight)]
  let intrinsicsArray = (0..&lt;3).flatMap { x in (0..&lt;3).map { y in NSNumber(value: intrinsics[x][y]) } }
  let squareLength = NSNumber(value: 0.16)
  let res = OpenCVWrapper.findPose(imagePointsArray, intrinsics: intrinsicsArray, size: imageResolution, squareLength: squareLength)
  </code></pre>
  
  <p>To get camera position I'm using OpenCV solution <a href="https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d" rel="nofollow noreferrer">solvePnP()</a> with flag = <a href="https://docs.opencv.org/master/d9/d0c/group__calib3d.html#gga357634492a94efe8858d0ce1509da869ac5d28b2805d3ac32fd477eee4479406f" rel="nofollow noreferrer">SOLVEPNP_IPPE_SQUARE</a></p>
  
  <p>OpenCV in Objective-C++ based on <a href="https://stackoverflow.com/questions/44008003/camera-pose-estimation-from-homography-or-with-solvepnp-function">this answer</a>:</p>
  
  <pre><code>+(Pose)findPose: (NSArray&lt;NSValue *&gt; *) imagePoints
       intrinsics: (NSArray&lt;NSNumber *&gt; *) intrinsics
  imageResolution: (CGSize) imageResolution
     squareLength: (NSNumber *) squareLength {
  
      cv::Mat distCoeffs(4,1,cv::DataType&lt;double&gt;::type, 0.0);
      cv::Mat rvec(3,1,cv::DataType&lt;double&gt;::type);
      cv::Mat tvec(3,1,cv::DataType&lt;double&gt;::type);
  
      cv::Mat cameraMatrix = [self intrinsicMatrixWithArray:intrinsics];
      vector&lt;Point2f&gt; cvImagePoints = [self convertImagePoints:imagePoints toSize: imageResolution];
      vector&lt;Point3f&gt; cvObjectPoints = [self getObjectPointsWithSquareLength:squareLength];
  
      std::cout &lt;&lt; "object points: \n" &lt;&lt; cvObjectPoints &lt;&lt; std::endl;
      std::cout &lt;&lt; "image points: \n" &lt;&lt; cvImagePoints &lt;&lt; std::endl;
      std::cout &lt;&lt; "cameraMatrix points: \n" &lt;&lt; cameraMatrix &lt;&lt; std::endl;
  
      cv::solvePnP(cvObjectPoints, cvImagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, SOLVEPNP_IPPE_SQUARE);
  
      std::cout &lt;&lt; "rvec: \n" &lt;&lt; rvec &lt;&lt; std::endl;
      std::cout &lt;&lt; "tvec: \n" &lt;&lt; tvec &lt;&lt; std::endl;
  
      cv::Mat RotX(3, 3, cv::DataType&lt;double&gt;::type);
      cv::setIdentity(RotX);
      RotX.at&lt;double&gt;(4) = -1; //cos(180) = -1
      RotX.at&lt;double&gt;(8) = -1;
  
      cv::Mat R;
      cv::Rodrigues(rvec, R);
  
      R = R.t();  // rotation of inverse
      Mat rvecConverted;
      Rodrigues(R, rvecConverted); //
      std::cout &lt;&lt; "rvec in world coords:\n" &lt;&lt; rvecConverted &lt;&lt; std::endl;
      rvecConverted = RotX * rvecConverted;
      std::cout &lt;&lt; "rvec scenekit :\n" &lt;&lt; rvecConverted &lt;&lt; std::endl;
  
      Mat tvecConverted = -R * tvec;
      std::cout &lt;&lt; "tvec in world coords:\n" &lt;&lt; tvecConverted &lt;&lt; std::endl;
      tvecConverted = RotX * tvecConverted;
      std::cout &lt;&lt; "tvec scenekit :\n" &lt;&lt; tvecConverted &lt;&lt; std::endl;
  
      SCNVector4 rotationVector = SCNVector4Make(rvecConverted.at&lt;double&gt;(0), rvecConverted.at&lt;double&gt;(1), rvecConverted.at&lt;double&gt;(2), norm(rvecConverted));
      SCNVector3 translationVector = SCNVector3Make(tvecConverted.at&lt;double&gt;(0), tvecConverted.at&lt;double&gt;(1), tvecConverted.at&lt;double&gt;(2));
  
      return Pose{rotationVector, translationVector};
  }
  
  + (vector&lt;Point3f&gt;) getObjectPointsWithSquareLength: (NSNumber*) squareLength {
      vector&lt;Point3f&gt; points;
      double squareLengthDouble = [squareLength doubleValue];
      points.push_back(Point3f(-squareLengthDouble/2, squareLengthDouble/2, 0));
      points.push_back(Point3f(squareLengthDouble/2, squareLengthDouble/2, 0));
      points.push_back(Point3f(squareLengthDouble/2, -squareLengthDouble/2, 0));
      points.push_back(Point3f(-squareLengthDouble/2, -squareLengthDouble/2, 0));
      return points;
  }
  
  + (vector&lt;Point2f&gt;) convertImagePoints: (NSArray&lt;NSValue *&gt; *) array
                                  toSize: (CGSize) size {
      vector&lt;Point2f&gt; points;
      for (NSValue * value in array) {
          CGPoint point = [value CGPointValue];
          points.push_back(Point2f((point.x * size.width), (point.y * size.height)));
      }
      return points;
  }
  
  + (cv::Mat) intrinsicMatrixWithArray: (NSArray&lt;NSNumber *&gt; *) intrinsics {
      Mat result(3,3,cv::DataType&lt;double&gt;::type);
      cv::setIdentity(result);
      result.at&lt;double&gt;(0) = [intrinsics[0] doubleValue]; //fx
      result.at&lt;double&gt;(4) = [intrinsics[4] doubleValue]; //fy
      result.at&lt;double&gt;(2) = [intrinsics[6] doubleValue]; //cx
      result.at&lt;double&gt;(5) = [intrinsics[7] doubleValue]; //cy
      result.at&lt;double&gt;(8) = [intrinsics[8] doubleValue]; //1
      return result;
  }
  </code></pre>
  
  <p>The problem is when I point camera directly to QR code with 2 meters distance, the result of translationVector.<strong>z</strong> (<strong>tvec scenekit</strong>) should be equal to 2 meters, but instead there is a random positive or negative numbers.</p>
  
  <p>Output:</p>
  
  <pre><code>Calculated distance to QR 2.0856588
  object points: 
  [-0.079999998, 0.079999998, 0;
   0.079999998, 0.079999998, 0;
   0.079999998, -0.079999998, 0;
   -0.079999998, -0.079999998, 0]
  image points: 
  [795.98724, 717.27045;
   684.5592, 715.80487;
   793.31567, 826.06146;
   684.40692, 824.39771]
  cameraMatrix points: 
  [1454.490478515625, 0, 935.6685791015625;
   0, 1454.490478515625, 717.999267578125;
   0, 0, 1]
  rvec: 
  [-0.9251278749049585;
   1.185890362907954;
   -0.9989977018022447]
  tvec: 
  [0.04753833193572054;
   -0.009999648596310796;
   -0.3527916723601041]
  rvec in world coords:
  [0.9251278749049584;
   -1.185890362907954;
   0.9989977018022447]
  rvec scenekit :
  [0.9251278749049584;
   1.185890362907954;
   -0.9989977018022447]
  tvec in world coords:
  [-0.1159248829391864;
   -0.3366933247327607;
   0.004569098144615695]
  tvec scenekit :
  [-0.1159248829391864;
   0.3366933247327607;
   -0.004569098144615695]
  </code></pre>
  
  <p>Thanks for any help</p>