From e7d4d0c7e9e8cdb53327f2a1b4d324435445cf2c Mon Sep 17 00:00:00 2001
From: liukeskywalker98 <57304384+liukeskywalker98@users.noreply.github.com>
Date: Wed, 4 May 2022 06:11:31 -0400
Subject: [PATCH] Left TODO comments for tomorrow's work

---
 Mapping.py          |  2 +-
 RawROAMSystem.py    | 10 ++++++----
 motionDistortion.py |  4 +++-
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/Mapping.py b/Mapping.py
index dae89a5..4de920c 100644
--- a/Mapping.py
+++ b/Mapping.py
@@ -102,7 +102,7 @@ def getPrunedFeaturesGlobalPosition(self):
         featurePointsGlobal = self.prunedUndistortedLocals
 
         # Then we need to convert to meters
-        #featurePointsGlobal *= RANGE_RESOLUTION_CART_M  # px * (m/px) = m
+        featurePointsGlobal *= RANGE_RESOLUTION_CART_M  # px * (m/px) = m
 
         # Center origin at pose center
 
diff --git a/RawROAMSystem.py b/RawROAMSystem.py
index 13b9b83..4c347fa 100644
--- a/RawROAMSystem.py
+++ b/RawROAMSystem.py
@@ -5,7 +5,7 @@
 
 from Mapping import Keyframe, Map
 from getFeatures import N_FEATURES_BEFORE_RETRACK, appendNewFeatures
-from parseData import convertPolarImageToCartesian, getCartImageFromImgPaths, getPolarImageFromImgPaths, getRadarImgPaths
+from parseData import convertPolarImageToCartesian, getCartImageFromImgPaths, getPolarImageFromImgPaths, getRadarImgPaths, RANGE_RESOLUTION_CART_M
 from trajectoryPlotting import Trajectory, getGroundTruthTrajectory, plotGtAndEstTrajectory
 from utils import convertRandHtoDeltas, f_arr, getRotationMatrix, plt_savefig_by_axis, radarImgPathToTimestamp
 from Tracker import Tracker
@@ -172,12 +172,14 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
             print("Detected", np.rad2deg(rotAngleRad), "[deg] rotation")
             estR = getRotationMatrix(-rotAngleRad)
             
-            R, h = tracker.getTransform(good_old, good_new, pixel = True)
+            R, h = tracker.getTransform(good_old, good_new, pixel = False)
             # R = estR
             
             # Solve for Motion Compensated Transform
             p_w = old_kf.getPrunedFeaturesGlobalPosition() # centered
-            centered_new = good_new - RADAR_CART_CENTER
+            #TODO: Scatter p_w, then try the transform on the centered new points
+            # Scatter that on the same plot
+            centered_new = (good_new - RADAR_CART_CENTER) * RANGE_RESOLUTION_CART_M
             # Initial Transform guess
             T_wj = prev_pose @ np.block([[R,                h],
                                          [np.zeros((2,)),   1]])
@@ -188,7 +190,7 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
 
             # Extract new info
             pose_vector = undistort_solution[3:]
-            transform = convertPoseToTransform(pose_vector)
+            transform = MDS.T_wj0_inv @ convertPoseToTransform(pose_vector)
             R = transform[:2, :2]
             h = transform[:2, 2:]
             velocity = undistort_solution[:3]
diff --git a/motionDistortion.py b/motionDistortion.py
index 19f3255..917f41b 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -23,6 +23,8 @@
 TODO: Run parse data to look at how the scanline moves. Adjust code to match
 TODO: Verify code against math
 TODO: Integration with the code. Adding mapping capability
+TODO: Check arctan2 conventions for dT stuff. the image might be flipped because
+images have the origin in the top left.
 
 Plan:
 Check out parse data
@@ -114,7 +116,7 @@ def compute_time_deltas(period, points):
         '''
         x = points[:, 0]
         y = points[:, 1]
-        # scanline starts at positive x axis and moves clockwise
+        # scanline starts at positive x axis and moves clockwise (counter-clockwise?)
         # We take midpoint pi/2 as 0
         angles = np.arctan2(y, -x) 
         dT = period * angles / (2 * np.pi)