Left TODO comments for tomorrow's work

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
 

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]

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)