From 08a03b31783d3af5e7572f739d3fdac6fd27ccda Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 23 Feb 2020 22:11:07 -0800
Subject: [PATCH] reproject corners in solvePNP test

---
 .../vision/pipeline/SolvePNPtest.java         | 21 +++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
index 66d237210..9fc54dbba 100644
--- a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
+++ b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
@@ -54,16 +54,17 @@ public class SolvePNPtest {
         // make a tvec and rvec
 //        positive x to the right, positive y to the bottom, positive z away from the image
         // tvec example:
-//        [45.92180012026041;
-//        24.94979431168253;
-//        88.00905002660249]
+//        [-3.377348429632199;
+//        9.132802434424915;
+//        67.79662519667924]
+
         // rvec:
 //        [1.990867004634147;
 //        -0.1508389335122144;
 //        -1.552061845576413]
 
         Mat tvec = new Mat(3, 1, 6);
-        tvec.put(0, 0, 0, 0, 200); // 10ft away?
+        tvec.put(0, 0, 0, 40, 200); // 10ft away?
         Mat rvec = new Mat(3, 1, 6);
         rvec.put(0, 0, 0, 0, 0);
 
@@ -75,6 +76,18 @@ public class SolvePNPtest {
             Imgproc.circle(blank, p, 3, new Scalar(0, 0, 255), 4);
         }
 
+        // go backwards to solvePNP
+        Mat rvec_ = new Mat(), tvec_ = new Mat();
+        Calib3d.solvePnP(target, imagePoints, calibration.getCameraMatrixAsMat(), calibration.getDistortionCoeffsAsMat(), rvec_, tvec_);
+
+        // what we projected
+        var initTvec = tvec.dump();
+        var initRvec = rvec.dump();
+
+        // what solvePNP gives
+        var retedTvec = tvec_.dump();
+        var rettedRvec = rvec_.dump();
+
         displayImage(mat2BufferedImage(blank));
     }