Made various fixes to coordinate systems, plotting apparatus

Mapping.py

@@ -63,7 +63,7 @@ def updateInfo(self, globalPose: np.ndarray,
 
         self.velocity = velocity
         print(featurePointsLocal.shape)
-        self.featurePointsLocalUndistorted = MotionDistortionSolver.undistort(velocity, featurePointsLocal)[:, :2]
+        self.featurePointsLocalUndistorted = MotionDistortionSolver.undistort(velocity, featurePointsLocal-RADAR_CART_CENTER)[:, :2]
         self.prunedUndistortedLocals = self.featurePointsLocalUndistorted
 
     def copyFromOtherKeyframe(self, keyframe) -> None:
@@ -99,7 +99,7 @@ def getPrunedFeaturesGlobalPosition(self):
         x, y, th = self.pose
 
         # First translate local points to origin at center
-        featurePointsGlobal = self.prunedUndistortedLocals - RADAR_CART_CENTER
+        featurePointsGlobal = self.prunedUndistortedLocals
 
         # Then we need to convert to meters
         #featurePointsGlobal *= RANGE_RESOLUTION_CART_M  # px * (m/px) = m

RawROAMSystem.py

@@ -12,6 +12,9 @@
 from motionDistortion import MotionDistortionSolver
 from utils import *
 
+# Bad solution. better solution is to save in config between mapping and this file
+RADAR_CART_CENTER = np.array([1012, 1012])
+
 class RawROAMSystem():
 
     def __init__(self,
@@ -173,15 +176,21 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
             # R = estR
 
             # Solve for Motion Compensated Transform
-            p_w = old_kf.getPrunedFeaturesGlobalPosition()
-
+            p_w = old_kf.getPrunedFeaturesGlobalPosition() # centered
+            centered_new = good_new - RADAR_CART_CENTER
             # Initial Transform guess
-            T_wj = np.block([[R,                h],
-                             [np.zeros((2,)),   1]])
+            T_wj = prev_pose @ np.block([[R,                h],
+                                         [np.zeros((2,)),   1]])
 
-            MDS.update_problem(prev_pose, p_w, good_new, T_wj)
+            # Give Motion Distort info on two new frames
+            MDS.update_problem(prev_pose, p_w, centered_new, T_wj)
             undistort_solution = MDS.optimize_library()
+
+            # Extract new info
             pose_vector = undistort_solution[3:]
+            transform = convertPoseToTransform(pose_vector)
+            R = transform[:2, :2]
+            h = transform[:2, 2:]
             velocity = undistort_solution[:3]
 
             # Update trajectory
@@ -224,8 +233,8 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
                 self.map.bundleAdjustment()
 
             # Plotting and prints and stuff
-            # self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
-            #           seqInd)
+            self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
+                      seqInd, show = True)
 
             # Update incremental variables
             blobCoord = good_new.copy()
@@ -274,7 +283,7 @@ def plot(self,
 
         ax2 = self.fig.add_subplot(1, 2, 2)
         # TODO: Plotting for map points
-        self.map.plot(self.fig, show=False)
+        #self.map.plot(self.fig, show=False)
 
         self.plotTraj(seqInd, R, h, save=False, show=False)
 
@@ -286,14 +295,14 @@ def plot(self,
         self.fig.savefig(trajSavePathInd)
 
         # # Save by subplot
-        # if save:
-        #     imgSavePath = self.filePaths["imgSave"]
-        #     imgSavePathInd = os.path.join(imgSavePath, f"{seqInd:04d}.jpg")
-        #     plt_savefig_by_axis(imgSavePathInd, self.fig, ax1)
+        if save:
+            imgSavePath = self.filePaths["imgSave"]
+            imgSavePathInd = os.path.join(imgSavePath, f"{seqInd:04d}.jpg")
+            plt_savefig_by_axis(imgSavePathInd, self.fig, ax1)
 
-        #     trajSavePath = self.filePaths["trajSave"]
-        #     trajSavePathInd = os.path.join(trajSavePath, f"{seqInd:04d}.jpg")
-        #     plt_savefig_by_axis(trajSavePathInd, self.fig, ax2)
+            trajSavePath = self.filePaths["trajSave"]
+            trajSavePathInd = os.path.join(trajSavePath, f"{seqInd:04d}.jpg")
+            plt_savefig_by_axis(trajSavePathInd, self.fig, ax2)
 
         if show:
             plt.pause(0.01)
@@ -371,7 +380,7 @@ def plotTraj(self, seqInd, R, h, save=False, show=False):
     print("Generating mp4 with script (requires bash and FFMPEG command)...")
     try:
         # Save video sequence
-        os.system(f"./img/mp4-from-folder.sh {imgSavePath} {startSeqInd + 1}")
+        os.system(f"./img/mp4-from-folder.sh {imgSavePath} {startSeqInd + 1} 20")
         print(f"mp4 saved to {imgSavePath.strip(os.path.sep)}.mp4")
 
         if REMOVE_OLD_RESULTS:

motionDistortion.py

@@ -1,6 +1,7 @@
 import numpy as np
 import scipy as sp
 from utils import *
+import matplotlib.pyplot as plt
 #sp.linalg.sqrtm
 #sp.lin
 
@@ -46,7 +47,7 @@ def __init__(self, T_wj0, p_w, p_jt, T_wj, sigma_p, sigma_v,
         self.total_scan_time = 1 / frequency
 
         # e_v Parameters
-        self.v_j_initial = self.infer_velocity(T_wj)
+        self.v_j_initial = self.infer_velocity(self.T_wj0_inv @ T_wj)
         # Initial velocity guess (prior velocity/ velocity from SVD solution)
 
         # Optimization parameters
@@ -84,7 +85,7 @@ def update_problem(self, T_wj0, p_w, p_jt, T_wj):
         self.T_wj_initial = T_wj
 
         # e_v Parameters
-        self.v_j_initial = self.infer_velocity(T_wj)
+        self.v_j_initial = self.infer_velocity(self.T_wj0_inv @ T_wj)
         # Initial velocity guess (prior velocity/ velocity from SVD solution)
         self.dT = MotionDistortionSolver.compute_time_deltas(self.total_scan_time, p_jt)
 
@@ -179,6 +180,7 @@ def error(self, v_j, T_wj):
         undistorted = MotionDistortionSolver.undistort(v_j, self.p_jt, times=self.dT)
         expected = self.expected_observed_pts(T_wj)
         naive_e_p = expected - undistorted.T # 3 x N
+
         # Actual loss is the Cauchy robust loss, defined here:
         e_p_i = np.log(np.square(naive_e_p[:2, :]) / 2 + 1) # 2 x N
         e_p = e_p_i.flatten(order='F')

trajectoryPlotting.py

@@ -127,7 +127,7 @@ def plotGtAndEstTrajectory(gtTraj,
                            title='GT and EST Trajectories',
                            info=None,
                            savePath=None,
-                           arrow=True):
+                           arrow=False):
     '''
     @brief Plot ground truth trajectory and estimated trajectory
     @param[in] gtTrajectory Ground truth trajectory