From daaf833d8635a411db3dd8b06f781d8949823cb3 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Sun, 24 Apr 2022 21:19:59 -0400
Subject: [PATCH 01/14] Initial commit

---
 motionDistortion.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index b668b07..8f54e86 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -70,9 +70,9 @@ def compute_time_deltas(self):
         points = self.undistort()#self.p_jt # provide in N x 3
         x = points[:, 0]
         y = points[:, 1]
-        angles = np.arctan2(-y, x) # scanline starts at positive x axis and moves clockwise
+        angles = np.arctan2(y, -x) # scanline starts at positive x axis and moves clockwise, we take midpoint pi/2 as 0
         dT = self.total_scan_time * angles / (2 * np.pi)
-        dT -= self.total_scan_time / 2 # offset range, as defined in [-scan_time /2 , scan_time/2]
+        #dT -= self.total_scan_time / 2 # offset range, as defined in [-scan_time /2 , scan_time/2]
         self.dT = dT
 
     def undistort(self, v_j):

From 67884c102dc922da4593a7ab9c4b5bfe324bb886 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Mon, 25 Apr 2022 03:51:31 -0400
Subject: [PATCH 02/14] Some work on the motion distort testing

---
 genFakeData.py      | 39 ++++++++++++++++++++++-----------------
 getTransformKLT.py  |  9 +++++----
 motionDistortion.py |  6 +++---
 utils.py            | 11 +++++++++++
 4 files changed, 41 insertions(+), 24 deletions(-)

diff --git a/genFakeData.py b/genFakeData.py
index 1477d18..7d88997 100644
--- a/genFakeData.py
+++ b/genFakeData.py
@@ -1,6 +1,6 @@
 import numpy as np
 import matplotlib.pyplot as plt
-from utils import getRotationMatrix
+from utils import getRotationMatrix, invert_transform
 from parseData import *
 
 from parseData import RANGE_RESOLUTION_CART_M
@@ -40,7 +40,7 @@ def plotFakeFeatures(srcCoord,
                     color='blue',
                     marker='.',
                     alpha=alpha,
-                    label=f'Features 0{title_append}')
+                    label=f'Instantaneous Radar Scan{title_append}')
 
     if targetCoord is not None:
         plt.scatter(targetCoord[:, 0],
@@ -48,7 +48,7 @@ def plotFakeFeatures(srcCoord,
                     color='red',
                     marker='+',
                     alpha=alpha,
-                    label=f'Features 1{title_append}')
+                    label=f'Scan with Distortion{title_append}')
 
     if targetCoord2 is not None:
         plt.scatter(targetCoord2[:, 0],
@@ -56,7 +56,7 @@ def plotFakeFeatures(srcCoord,
                     color='green',
                     marker='x',
                     alpha=alpha,
-                    label=f'Features 2{title_append}')
+                    label=f'Original Points{title_append}')
 
     if plotDisplace:
         for i in range(targetCoord.shape[0]):
@@ -116,39 +116,44 @@ def convertPolarPointsToCartesian(points):
     y = np.expand_dims(ranges * np.sin(angles), axis = 1)
     return np.hstack((x, y))
 
-def generateFakeCorrespondencesPolar(srcCoord=None,
+def generateFakeCorrespondencesPolar(currentFrame=None,
                                     n_points=100,
                                     theta_max_deg=20,
                                     max_translation_m=3):
     '''
     @brief Generate fake correspondences with transform, randomly generated from max range and degree
-    @param[in] srcCoord Source coordinate to transform from. If none, will randomly generate features
-    @param[in] n_points Number of points to generate, only applies if srcCoord = None
+    @param[in] currentFrame Source coordinate to transform from. If none, will randomly generate features
+    @param[in] n_points Number of points to generate, only applies if currentFrame = None
     @param[in] theta_max_deg Maximum degree of rotation
     @param[in] max_range_m Maximum range (for translation) in meters
 
-    @return srcCoord Generated or passed in srcCoord
+    @return currentFrame Generated or passed in currentFrame
     @return targetCoord Corresponding targetCoord generated using (theta_deg, h)
     @return theta_deg Theta component of transform
     @return h Translation component of transform
     '''
 
-    if srcCoord is None:
+    if currentFrame is None:
         print("Generating fake features..")
         max_range_m = max_translation_m * 3
-        srcCoord = generateFakeFeaturesPolar(n_points, max_range_m)
-        #print(srcCoord.shape)
-        srcCoord = convertPolarPointsToCartesian(srcCoord)
+        currentFrame = generateFakeFeaturesPolar(n_points, max_range_m)
+        #print(currentFrame.shape)
+        currentFrame = convertPolarPointsToCartesian(currentFrame)
     else:
-        n_points = srcCoord.shape[0]
+        n_points = currentFrame.shape[0]
 
     theta_deg = np.random.random() * theta_max_deg
     R = getRotationMatrix(theta_deg, degrees=True)
     h = generateTranslationVector(max_translation_m)
-    #print(srcCoord.shape)
-    targetCoord = transformCoords(srcCoord, R, h)
-
-    return srcCoord, targetCoord, theta_deg, h
+    transform = np.block([[R, h],
+                          [np.zeros((2,)), 1]])
+    T_inv = invert_transform(transform)
+    R_inv = T_inv[:2, :2]
+    h_inv = T_inv[:2, 2:]
+    #print(currentFrame.shape)
+    groundTruth = transformCoords(currentFrame, R_inv, h_inv)
+
+    return groundTruth, currentFrame, theta_deg, h
 
 def distort(coords, velocity, frequency, h):
     
diff --git a/getTransformKLT.py b/getTransformKLT.py
index af73c65..45e914e 100644
--- a/getTransformKLT.py
+++ b/getTransformKLT.py
@@ -468,15 +468,16 @@ def getTrackedPointsKLT(
             good_old, good_new = rejectOutliers(good_old, good_new)
 
             # Obtain transforms
-            R, h = calculateTransformDxDth(good_old, good_new)
-            # R, h = calculateTransformSVD(good_old, good_new)
+            #R, h = calculateTransformDxDth(good_old, good_new)
+            R, h = calculateTransformSVD(good_old, good_new)
             # print(h)
             # h[0] += 0
             # for i in range(good_old.shape[0]):
             #     plotFakeFeatures(good_old[i:i+1,:], (R @ good_new[i:i+1,:].T + h).T, show= True)
-            # transformed_pts = (R @ good_new.T + h).T
+            transformed_pts = (R @ good_new.T + h).T
             # print(f"RMSE = {np.sum(np.square(good_old - transformed_pts))}")
-            # plotFakeFeatures(good_old, transformed_pts, good_new, show = True)
+            #plotFakeFeatures(good_old, good_new, show = True)
+            plotFakeFeatures(good_old, transformed_pts, good_new, show = True)
             h *= RANGE_RESOLUTION_CART_M
 
             #R, h = estimateTransformUsingDelats(good_old, good_new)
diff --git a/motionDistortion.py b/motionDistortion.py
index 8f54e86..02a5b81 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -57,7 +57,7 @@ def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, lambda_p, lambda_v):
         self.total_scan_time = 1 / 4 # assuming 4 Hz
         pass
 
-    def compute_time_deltas(self):
+    def compute_time_deltas(self, points):
         '''
         Get the time deltas for each point. This depends solely on where the
         points are in scan angle. The further away from center, the greater the
@@ -67,7 +67,7 @@ def compute_time_deltas(self):
         idea to re-run this function once an undistorted frame is obtained for
         better estimates.
         '''
-        points = self.undistort()#self.p_jt # provide in N x 3
+        #points = self.undistort()#self.p_jt # provide in N x 3
         x = points[:, 0]
         y = points[:, 1]
         angles = np.arctan2(y, -x) # scanline starts at positive x axis and moves clockwise, we take midpoint pi/2 as 0
@@ -81,7 +81,7 @@ def undistort(self, v_j):
         best estimate of v_T, T_wj, dT
         '''
         displacement = np.expand_dims(v_j, axis = 1) * self.dT # 3 x N
-        assert(displacement.shape = (3,points.shape[0]))
+        #assert(displacement.shape == (3,points.shape[0]))
         theta = displacement[2, :]
         dx = displacement[0, :]
         dy = displacement[1, :]
diff --git a/utils.py b/utils.py
index 6b60797..8aa02f6 100644
--- a/utils.py
+++ b/utils.py
@@ -143,3 +143,14 @@ def plt_savefig_by_axis(filePath, fig, ax, pad=0.0):
     extent = plt_full_extent(ax, pad).transformed(fig.dpi_scale_trans.inverted())
     # extent = ax.get_tightbbox(fig.canvas.get_renderer())
     fig.savefig(filePath, bbox_inches=extent)
+
+def invert_transform(T):
+    theta = np.arctan2(T[1, 0], T[0, 0])
+    x = T[0, 2]
+    y = T[1, 2]
+    c = np.cos(theta)
+    s = np.sin(theta)
+    T_inv = np.array([[ c, s, -s * y - c * x],
+                      [-s, c, -c * y + s * x],
+                      [ 0, 0, 1]])
+    return T_inv
\ No newline at end of file

From d8f63834f756d2c937a1742d95666bfb4e5b2e6d Mon Sep 17 00:00:00 2001
From: liukeskywalker98 <57304384+liukeskywalker98@users.noreply.github.com>
Date: Sun, 1 May 2022 21:23:59 -0400
Subject: [PATCH 03/14] Testing pipeline written. About to start testing.
 Issues with importing numpy due to upgrade of Python version

---
 genFakeData.py      | 21 +++++++++++----------
 motionDistortion.py | 29 ++++++++++++++++++++---------
 parseData.py        | 12 ++++++------
 3 files changed, 37 insertions(+), 25 deletions(-)

diff --git a/genFakeData.py b/genFakeData.py
index 7d88997..e718b8c 100644
--- a/genFakeData.py
+++ b/genFakeData.py
@@ -144,21 +144,22 @@ def generateFakeCorrespondencesPolar(currentFrame=None,
 
     theta_deg = np.random.random() * theta_max_deg
     R = getRotationMatrix(theta_deg, degrees=True)
-    h = generateTranslationVector(max_translation_m)
-    transform = np.block([[R, h],
-                          [np.zeros((2,)), 1]])
-    T_inv = invert_transform(transform)
-    R_inv = T_inv[:2, :2]
-    h_inv = T_inv[:2, 2:]
+    #h = generateTranslationVector(max_translation_m)
+    h = np.array([[0], [0]])
+    # transform = np.block([[R, h],
+    #                       [np.zeros((2,)), 1]])
+    # T_inv = invert_transform(transform)
+    # R_inv = T_inv[:2, :2]
+    # h_inv = T_inv[:2, 2:]
     #print(currentFrame.shape)
-    groundTruth = transformCoords(currentFrame, R_inv, h_inv)
+    groundTruth = transformCoords(currentFrame, R, h)
 
     return groundTruth, currentFrame, theta_deg, h
 
 def distort(coords, velocity, frequency, h):
     
     coords_norm = coords - h.flatten() # N x 2
-    angles = np.arctan2(coords_norm[:, 1], -coords_norm[:, 0]) # - y to follow clockwise convention
+    angles = np.arctan2(coords_norm[:, 1], -coords_norm[:, 0]) # - x to follow clockwise convention
     period = 1 / frequency
     times = angles / (2 * np.pi) * period
     #print(angles) # lesson: need to use arctan2 wisely, it wraps [-pi, pi]
@@ -178,8 +179,8 @@ def distort(coords, velocity, frequency, h):
     s = np.sin(dtheta)
     ones = np.ones(times.shape)
     zeros = np.zeros(times.shape)
-    distortion = np.array([[c, s, -s* dy - c*dx],
-                           [-s,  c, -c*dy + s*dx],
+    distortion = np.array([[ c, s, -s*dy - c*dx],
+                           [-s, c, -c*dy + s*dx],
                            [zeros, zeros, ones]]) # 3 x 3 x N, need to invert?
     distorted = np.transpose(distortion, axes = (2, 0, 1)) @ np.expand_dims(coords, axis = 2) # N x 3 x 1
     distorted = distorted[:, :2, 0]
diff --git a/motionDistortion.py b/motionDistortion.py
index 02a5b81..29ece32 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -30,7 +30,7 @@
 
 '''
 class MotionDistortionSolver():
-    def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, lambda_p, lambda_v):
+    def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, sigma_p, sigma_v):
         # e_p Parameters
         self.T_wj0 = T_wj0 # Prior transform, T_w,j0
         self.T_wj0_inv = np.linalg.inv(T_wj0)
@@ -42,13 +42,13 @@ def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, lambda_p, lambda_v):
         self.T_wj_initial = T_wj # Initial Transform guess (T from SVD solution)
 
         # Optimization parameters
-        self.lambda_p = lambda_p # Info matrix, point error, lamdba_p
-        self.lambda_v = lambda_v # Info matrix, velocity, lambda_v
-        nv = self.lambda_p.shape[0]
-        np = self.lambda_v.shape[0]
-        self.lambda_total = np.block([[lambda_v, np.zeros((nv, np))],
-                                      [np.zeros((np, nv)), lambda_p]])
-        self.info_sqrt = sp.linalg.sqrtm(np.linalg.inv(self.lambda_total)) # 5 x 5
+        self.sigma_p = sigma_p # Info matrix, point error, lamdba_p
+        self.sigma_v = sigma_v # Info matrix, velocity, sigma_v
+        nv = self.sigma_p.shape[0]
+        np = self.sigma_v.shape[0]
+        self.sigma_total = np.block([[sigma_v, np.zeros((nv, np))],
+                                      [np.zeros((np, nv)), sigma_p]])
+        self.info_sqrt = sp.linalg.sqrtm(np.linalg.inv(self.sigma_total)) # 5 x 5
         self.dT = None
         self.T_wj_best = T_wj
         self.v_j_best = v_j0 # might not be good enough a guess, too far from optimal
@@ -103,6 +103,11 @@ def expected_observed_pts(self, T_wj):
         return np.linalg.inv(T_wj) @ self.p_w.T
 
     def error_vector(self, params):
+        '''
+        Because we are optimizing over rotations, we choose to keep the rotation
+        in a theta form, we have to do matrix exponential in here to convert
+        into the SO(1) form, then augment to the rotation-translation transform
+        '''
         theta = params[2]
         x = params[0]
         y = params[1]
@@ -118,6 +123,7 @@ def error(self, v_j, T_wj):
         '''
         # Compute point error
         undistorted = self.undistort(v_j)
+        #self.compute_time_deltas(undistorted)
         expected = self.expected_observed_pts(self, T_wj)
         naive_e_p = expected - np.squeeze(undistorted).T # 3 x N
         # Actual loss is the Cauchy robust loss, defined here:
@@ -125,6 +131,7 @@ def error(self, v_j, T_wj):
         e_p = np.sum(e_p_i, axis = 1) # 2 x 1
         
         # Compute velocity error
+        # Matrix log operation
         T_j_j1 = self.T_wj0_inv @ T_wj
         dx = T_j_j1[0, 2]
         dy = T_j_j1[1, 2]
@@ -158,7 +165,7 @@ def jacobian(self, v_j, T_wj):
         input = expected - np.squeeze(undistorted).T # 3 x N
         cauchy_derivative = input / (np.square(input[:2, ]) / 2 + 1) # 2 x N
 
-        # Compute J_p: derivative of e_p wrt
+        # Compute J_p: derivative of errors wrt the point position
         c0 = self.T_wj0[0, 0]
         s0 = self.T_wj0[1, 0]
         c1 = T_wj[0, 0]
@@ -218,6 +225,10 @@ def optimize(self, max_iters = 20):
         pass
 
     def optimize_library(self):
+        '''
+        Optimize using the LM implementation in the scipy library.
+        '''
+        self.compute_time_deltas(self.p_jt)
         # Initialize v, T
         T0 = self.T_wj_initial
         T_params = np.array([T0[0, 2], T0[1, 2], np.arctan2(T0[1, 0], T0[0, 0])])
diff --git a/parseData.py b/parseData.py
index 864ffc0..94fa1be 100644
--- a/parseData.py
+++ b/parseData.py
@@ -1,5 +1,5 @@
 from tkinter.messagebox import NO
-from typing import Tuple
+from typing import Tuple, List
 import numpy as np
 import cv2
 import os, sys
@@ -55,7 +55,7 @@ def extractDataFromRadarImage(
 
 def drawCVPoint(img: np.ndarray,
                 point: CartCoord,
-                point_color: tuple[int, int, int] = (0, 0, 255)):
+                point_color: Tuple[int, int, int] = (0, 0, 255)):
     if isinstance(point, CartCoord):
         point = point.asTuple()
 
@@ -159,7 +159,7 @@ def convertPolarImgToLogPolar(imgPolar: np.ndarray):
 
 
 def getDataFromImgPathsByIndex(
-    imgPathArr: list[str], index: int
+    imgPathArr: List[str], index: int
 ) -> Tuple[np.ndarray, float, float, np.ndarray, np.ndarray, np.ndarray]:
     '''
     @brief Get information from image path array, indexing accordingly
@@ -180,7 +180,7 @@ def getDataFromImgPathsByIndex(
     return extractDataFromRadarImage(imgPolarData)
 
 
-def getPolarImageFromImgPaths(imgPathArr: list[str], index: int) -> np.ndarray:
+def getPolarImageFromImgPaths(imgPathArr: List[str], index: int) -> np.ndarray:
     '''
     @brief Get polar image from image path array, indexing accordingly
     @param[in] imgPathArr List of image path as strings
@@ -193,7 +193,7 @@ def getPolarImageFromImgPaths(imgPathArr: list[str], index: int) -> np.ndarray:
     return imgPolar
 
 
-def getCartImageFromImgPaths(imgPathArr: list[str], index: int) -> np.ndarray:
+def getCartImageFromImgPaths(imgPathArr: List[str], index: int) -> np.ndarray:
     '''
     @brief Get polar image from image path array, indexing accordingly
     @param[in] imgPathArr List of image path as strings
@@ -206,7 +206,7 @@ def getCartImageFromImgPaths(imgPathArr: list[str], index: int) -> np.ndarray:
     return convertPolarImageToCartesian(imgPolar)
 
 
-def getRadarImgPaths(dataPath: str, timestampPath: str) -> list[str]:
+def getRadarImgPaths(dataPath: str, timestampPath: str) -> List[str]:
     '''
     @brief Obtain list of radar image paths
     

From a2b18d2fd94dd1472f43bc193d53c99015449e89 Mon Sep 17 00:00:00 2001
From: liukeskywalker98 <57304384+liukeskywalker98@users.noreply.github.com>
Date: Sun, 1 May 2022 21:58:05 -0400
Subject: [PATCH 04/14] Adding testing files

---
 testMotionDistortion.py | 73 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 73 insertions(+)
 create mode 100644 testMotionDistortion.py

diff --git a/testMotionDistortion.py b/testMotionDistortion.py
new file mode 100644
index 0000000..ffa3cac
--- /dev/null
+++ b/testMotionDistortion.py
@@ -0,0 +1,73 @@
+from getTransformKLT import *
+from genFakeData import *
+from motionDistortion import *
+
+if __name__ == "__main__":
+    N = 100
+    outlier_rate = 0.4
+    noisy = False
+    useOld = False
+    frequency = 4
+    period = 1 / frequency
+
+    # Generate fake data
+    # srcCoord = frame 1
+    # currentFrame = frame 2
+    # R_theta_deg @ currentFrame + h = srcCoord
+    # pose at frame 1 (srcCoord) is I
+    groundTruth, currentFrame, theta_deg, h = generateFakeCorrespondencesPolar(n_points=N)
+    # This theta_deg reflects the radar's own motion. To distort points, the opposite must be used
+    velocity = np.array([h[0, 0], h[1, 0], theta_deg]) / period
+    distorted = distort(currentFrame, velocity, frequency, h)
+    if noisy:
+        currentFrame, outlier_ind = createOutliers(currentFrame, int(N * outlier_rate), 
+                                    noiseToAdd=10)
+
+    '''
+    Naive Fit: rotation and translation
+    '''
+    if useOld:
+        R_fit, h_fit = calculateTransform(groundTruth, distorted)
+
+        A = np.block([[R_fit, h_fit],
+                    [np.zeros((1, 2)), 1]])
+        A_inv = np.linalg.inv(A)
+        R_fit = A_inv[:2, :2]
+        h_fit = A_inv[:2, 2:]
+    else:
+        R_fit, h_fit = calculateTransformSVD(groundTruth, distorted)
+    #R_fit, h_fit = calculateTransformDxDth(srcCoord, currentFrame)
+
+    theta_fit = np.arctan2(R_fit[1, 0], R_fit[0, 0]) * 180 / np.pi
+    print(f"Actual Transform:\ntheta:\n{theta_deg}\nh:\n{h}")
+    print(f"Fitted Transform:\ntheta:\n{theta_fit}\nh:\n{h_fit}")
+
+    '''
+    Applying Motion Distortion Solving
+    '''
+    # Prior Transform
+    T_wj0 = np.eye(3)
+    # Point world positions
+    p_w = groundTruth
+    # Observed points at time 1
+    p_jt = distorted
+    # Initial velocity guess
+    v_j0 = np.array([h_fit[0,0], h_fit[1,0], theta_fit]) / period
+    # Initial Transform guess
+    T_wj = np.block([[R_fit,            h_fit],
+                     [np.zeros((2,)),    1]])
+    # Covariance matrix, point errors
+    cov_p = np.diag([1, 1])
+    # Covariance matrix, velocity errors
+    cov_v = np.diag([16, 16, 8 * np.pi / 180]) # 4 ^2 since 4 Hz
+    # Information matrix, 
+    MDS = MotionDistortionSolver(T_wj0, p_w, p_jt, v_j0, T_wj, cov_p, cov_v)
+    params = MDS.optimize_library
+    print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{np.flatten(params)}")
+
+    # Visualize
+    srcCoord2 = (R_fit @ distorted.T + h_fit).T
+    plotFakeFeatures(groundTruth, srcCoord2, 
+                     title_append="", alpha=0.5, clear=False, show=True, 
+                     plotDisplace = True)
+    
\ No newline at end of file

From 3685bd1a5cd91b614c25985cec2ebb910837835a Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Sun, 1 May 2022 23:07:58 -0400
Subject: [PATCH 05/14] Lots of bug fixes on the way to compilation

---
 motionDistortion.py     | 34 +++++++++++++++++++---------------
 testMotionDistortion.py |  2 +-
 utils.py                | 11 ++++++++++-
 3 files changed, 30 insertions(+), 17 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index 29ece32..573d8a2 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -1,5 +1,6 @@
 import numpy as np
 import scipy as sp
+from utils import *
 #sp.linalg.sqrtm
 #sp.lin
 
@@ -29,14 +30,16 @@
 Debug
 
 '''
+
+
 class MotionDistortionSolver():
     def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, sigma_p, sigma_v):
         # e_p Parameters
         self.T_wj0 = T_wj0 # Prior transform, T_w,j0
         self.T_wj0_inv = np.linalg.inv(T_wj0)
-        self.p_w = p_w # Estimated point world positions, N x 3
-        self.p_jt = p_jt # Observed points at time t, N x 3
-
+        self.p_w = homogenize(p_w) # Estimated point world positions, N x 3
+        self.p_jt = homogenize(p_jt) # Observed points at time t, N x 3
+        
         # e_v Parameters
         self.v_j_initial = v_j0 # Initial velocity guess (prior velocity/ velocity from SVD solution)
         self.T_wj_initial = T_wj # Initial Transform guess (T from SVD solution)
@@ -44,10 +47,10 @@ def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, sigma_p, sigma_v):
         # Optimization parameters
         self.sigma_p = sigma_p # Info matrix, point error, lamdba_p
         self.sigma_v = sigma_v # Info matrix, velocity, sigma_v
-        nv = self.sigma_p.shape[0]
-        np = self.sigma_v.shape[0]
-        self.sigma_total = np.block([[sigma_v, np.zeros((nv, np))],
-                                      [np.zeros((np, nv)), sigma_p]])
+        n_v = self.sigma_v.shape[0]
+        n_p = self.sigma_p.shape[0]
+        self.sigma_total = np.block([[sigma_v, np.zeros((n_v, n_p))],
+                                      [np.zeros((n_p, n_v)), sigma_p]])
         self.info_sqrt = sp.linalg.sqrtm(np.linalg.inv(self.sigma_total)) # 5 x 5
         self.dT = None
         self.T_wj_best = T_wj
@@ -85,13 +88,15 @@ def undistort(self, v_j):
         theta = displacement[2, :]
         dx = displacement[0, :]
         dy = displacement[1, :]
+        shape = theta.shape
         # Correction matrix for time drift, 3 x 3 x N
         T_j_jt = np.array([[np.cos(theta), -np.sin(theta), dx],
                            [np.sin(theta), np.cos(theta),  dy],
-                           [0,             0,              1]])
-        
-        p_jt_col = np.expand_dims(self.p_jt, axis = 1).transpose(axis = (2, 0, 1)) # N x 3 x 1
-        undistorted = T_j_jt.transpose(axis = (2, 0, 1)) @ p_jt_col # N x 3 x 1
+                           [np.zeros(shape), np.zeros(shape), np.ones(shape)]])
+        print(T_j_jt.shape)
+        p_jt_col = np.expand_dims(self.p_jt, axis = 2) # N x 3 x 1
+        print(p_jt_col.shape)
+        undistorted = T_j_jt.transpose((2, 0, 1)) @ p_jt_col # N x 3 x 1
         return undistorted
 
 
@@ -124,11 +129,11 @@ def error(self, v_j, T_wj):
         # Compute point error
         undistorted = self.undistort(v_j)
         #self.compute_time_deltas(undistorted)
-        expected = self.expected_observed_pts(self, T_wj)
+        expected = self.expected_observed_pts(T_wj)
         naive_e_p = expected - np.squeeze(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)
-        e_p = np.sum(e_p_i, axis = 1) # 2 x 1
+        e_p = np.sum(e_p_i, axis = 1, keepdims = True) # 2 x 1
         
         # Compute velocity error
         # Matrix log operation
@@ -137,8 +142,7 @@ def error(self, v_j, T_wj):
         dy = T_j_j1[1, 2]
         dtheta = np.arctan2(T_j_j1[1, 0], T_j_j1[0, 0])
         v_j_prior = np.array([dx, dy, dtheta]) / self.total_scan_time
-        e_v = (v_j - v_j_prior) * e_p.shape[1] # 3 x 1
-
+        e_v = np.expand_dims((v_j - v_j_prior) * e_p_i.shape[1], axis = 1) # 3 x 1
         e = np.vstack((e_v, e_p))
         return e
 
diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index ffa3cac..3838226 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -62,7 +62,7 @@
     cov_v = np.diag([16, 16, 8 * np.pi / 180]) # 4 ^2 since 4 Hz
     # Information matrix, 
     MDS = MotionDistortionSolver(T_wj0, p_w, p_jt, v_j0, T_wj, cov_p, cov_v)
-    params = MDS.optimize_library
+    params = MDS.optimize_library()
     print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{np.flatten(params)}")
 
     # Visualize
diff --git a/utils.py b/utils.py
index 8aa02f6..1b1e1f1 100644
--- a/utils.py
+++ b/utils.py
@@ -153,4 +153,13 @@ def invert_transform(T):
     T_inv = np.array([[ c, s, -s * y - c * x],
                       [-s, c, -c * y + s * x],
                       [ 0, 0, 1]])
-    return T_inv
\ No newline at end of file
+    return T_inv
+
+def homogenize(points):
+    result = None
+    if points.shape[1] == 2:
+        ones = np.ones((points.shape[0], 1))
+        result = np.concatenate((points, ones) , axis = 1)
+    else:
+        result = points.copy()
+    return result
\ No newline at end of file

From dd755e6d8b059c702bf9f93c384fab7ef7104887 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Mon, 2 May 2022 00:31:42 -0400
Subject: [PATCH 06/14] LM in working state

---
 motionDistortion.py     | 34 +++++++++++++++++++++-------------
 testMotionDistortion.py |  8 ++++----
 2 files changed, 25 insertions(+), 17 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index 573d8a2..262a908 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -93,9 +93,7 @@ def undistort(self, v_j):
         T_j_jt = np.array([[np.cos(theta), -np.sin(theta), dx],
                            [np.sin(theta), np.cos(theta),  dy],
                            [np.zeros(shape), np.zeros(shape), np.ones(shape)]])
-        print(T_j_jt.shape)
         p_jt_col = np.expand_dims(self.p_jt, axis = 2) # N x 3 x 1
-        print(p_jt_col.shape)
         undistorted = T_j_jt.transpose((2, 0, 1)) @ p_jt_col # N x 3 x 1
         return undistorted
 
@@ -133,7 +131,7 @@ def error(self, v_j, T_wj):
         naive_e_p = expected - np.squeeze(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)
-        e_p = np.sum(e_p_i, axis = 1, keepdims = True) # 2 x 1
+        e_p = np.sum(e_p_i, axis = 1) # (2,)
         
         # Compute velocity error
         # Matrix log operation
@@ -142,8 +140,9 @@ def error(self, v_j, T_wj):
         dy = T_j_j1[1, 2]
         dtheta = np.arctan2(T_j_j1[1, 0], T_j_j1[0, 0])
         v_j_prior = np.array([dx, dy, dtheta]) / self.total_scan_time
-        e_v = np.expand_dims((v_j - v_j_prior) * e_p_i.shape[1], axis = 1) # 3 x 1
-        e = np.vstack((e_v, e_p))
+        e_v = (v_j - v_j_prior) * e_p_i.shape[1] # (3,)
+        e = np.hstack((e_p, np.zeros((1,)), e_v))
+        #print(e)
         return e
 
     def jacobian_vector(self, params):
@@ -165,9 +164,10 @@ def jacobian(self, v_j, T_wj):
                 vx, vy, vtheta
         '''
         undistorted = self.undistort(v_j)
-        expected = self.expected_observed_pts(self, T_wj)
+        expected = self.expected_observed_pts(T_wj)
         input = expected - np.squeeze(undistorted).T # 3 x N
-        cauchy_derivative = input / (np.square(input[:2, ]) / 2 + 1) # 2 x N
+        denom = np.vstack((input[:2], np.ones((1, input.shape[1]))))
+        cauchy_derivative = input / (np.square(denom) / 2 + 1) # 3 x N
 
         # Compute J_p: derivative of errors wrt the point position
         c0 = self.T_wj0[0, 0]
@@ -179,11 +179,14 @@ def jacobian(self, v_j, T_wj):
         pwx = self.p_w[:, 0] # 1 x N
         pwy = self.p_w[:, 1]
         ones = np.ones(pwx.shape)
+        zeros = np.zeros(pwx.shape)
 
         # 2 x 3 x N
         J_p1 = np.array([[-c1 * ones, -s1 * ones, -pwx * s1 + pwy * c1 - c1 * Ty + s1 * Tx],
-                        [s1 * ones,  -c1 * ones, -pwx * c1 - pwy * s1 + s1 * Ty + c1 * Tx]])
+                        [s1 * ones,  -c1 * ones, -pwx * c1 - pwy * s1 + s1 * Ty + c1 * Tx],
+                         [zeros, zeros, zeros]])
         J_p1 *= np.expand_dims(cauchy_derivative, axis = 1)
+        J_p1 = np.sum(J_p1, axis = 2)
         J_p2 = np.array([[ c0, s0, 0],
                          [-s0, c0, 0],
                          [0,   0,  1]]) / self.total_scan_time
@@ -195,10 +198,12 @@ def jacobian(self, v_j, T_wj):
         y = points[:, 1]
         displacement = np.expand_dims(v_j, axis = 1) * self.dT # 3 x N
         theta = displacement[2, :]
-        J_v = np.array([[-self.dT, 0, np.sin(theta) * self.dT * x + np.cos(theta) * self.dT * y ],
-                        [0, -self.dT, -np.cos(theta) * self.dT * x + np.sin(theta) * self.dT * y]])
+        zeros = np.zeros(theta.shape)
+        J_v = np.array([[-self.dT, zeros, np.sin(theta) * self.dT * x + np.cos(theta) * self.dT * y ],
+                        [zeros, -self.dT, -np.cos(theta) * self.dT * x + np.sin(theta) * self.dT * y],
+                        [zeros, zeros, zeros]])
         J_v *= np.expand_dims(cauchy_derivative, axis = 1)
-        J_v = np.sum(J_v, axis = -1) # 3 x 2
+        J_v = np.sum(J_v, axis = -1) # 2 x 3
         J_v = np.vstack((J_v, np.eye(3) * x.shape[0]))
         
         # J = [J_v, J_p]
@@ -237,8 +242,10 @@ def optimize_library(self):
         T0 = self.T_wj_initial
         T_params = np.array([T0[0, 2], T0[1, 2], np.arctan2(T0[1, 0], T0[0, 0])])
         initial_guess = np.hstack((self.v_j_initial, T_params))
+        print(f"Initial v guess: {self.v_j_initial}")
+        print(f"Initial T guess: {T_params}")
 
-        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = self.jacobian_vector, method = 'lm')
+        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = '2-point', method = 'lm')
 
         # return v, T
         best_params = result.x
@@ -250,7 +257,8 @@ def optimize_library(self):
                         2 : "ftol termination condition is satisfied",
                         3 : "xtol termination condition is satisfied",
                         4 : "Both ftol and xtol termination conditions are satisfied"}
-        print(f"Final v: {best_params[:3]}, t: {best_params[3:]}")
+        print(f"Final v: {best_params[:3]}")
+        print(f"Final t: {best_params[3:]}")
         print(f"Used {num_evals} evaluations")
         print(status_dict[status])
         return best_params
diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index 3838226..4c6f620 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -52,18 +52,18 @@
     # Observed points at time 1
     p_jt = distorted
     # Initial velocity guess
-    v_j0 = np.array([h_fit[0,0], h_fit[1,0], theta_fit]) / period
+    v_j0 = np.array([h_fit[0,0], h_fit[1,0], theta_fit * np.pi / 180]) / period
     # Initial Transform guess
     T_wj = np.block([[R_fit,            h_fit],
                      [np.zeros((2,)),    1]])
     # Covariance matrix, point errors
-    cov_p = np.diag([1, 1])
+    cov_p = np.diag([8, 8, 8])
     # Covariance matrix, velocity errors
-    cov_v = np.diag([16, 16, 8 * np.pi / 180]) # 4 ^2 since 4 Hz
+    cov_v = np.diag([4, 4, (2 * np.pi / 180) ** 2]) # 4 ^2 since 4 Hz
     # Information matrix, 
     MDS = MotionDistortionSolver(T_wj0, p_w, p_jt, v_j0, T_wj, cov_p, cov_v)
     params = MDS.optimize_library()
-    print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{np.flatten(params)}")
+    print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{params.flatten()}")
 
     # Visualize
     srcCoord2 = (R_fit @ distorted.T + h_fit).T

From 6be05afe9e788ec65037e06d312501cb1f4075ee Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Mon, 2 May 2022 02:53:01 -0400
Subject: [PATCH 07/14] Testing code. Fixed a bug in the wrapper for
 jacobian_vector. Wrote code to visualize undistort

---
 genFakeData.py          | 10 ++--------
 motionDistortion.py     | 20 +++++++++++---------
 testMotionDistortion.py | 26 +++++++++++++++++++++-----
 3 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/genFakeData.py b/genFakeData.py
index e718b8c..99b36b8 100644
--- a/genFakeData.py
+++ b/genFakeData.py
@@ -144,14 +144,8 @@ def generateFakeCorrespondencesPolar(currentFrame=None,
 
     theta_deg = np.random.random() * theta_max_deg
     R = getRotationMatrix(theta_deg, degrees=True)
-    #h = generateTranslationVector(max_translation_m)
-    h = np.array([[0], [0]])
-    # transform = np.block([[R, h],
-    #                       [np.zeros((2,)), 1]])
-    # T_inv = invert_transform(transform)
-    # R_inv = T_inv[:2, :2]
-    # h_inv = T_inv[:2, 2:]
-    #print(currentFrame.shape)
+    h = generateTranslationVector(max_translation_m)
+    #h = np.array([[0], [0]])
     groundTruth = transformCoords(currentFrame, R, h)
 
     return groundTruth, currentFrame, theta_deg, h
diff --git a/motionDistortion.py b/motionDistortion.py
index 262a908..218fbd2 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -83,11 +83,12 @@ def undistort(self, v_j):
         Computes a new set of undistorted observed points, based on the current
         best estimate of v_T, T_wj, dT
         '''
-        displacement = np.expand_dims(v_j, axis = 1) * self.dT # 3 x N
-        #assert(displacement.shape == (3,points.shape[0]))
-        theta = displacement[2, :]
-        dx = displacement[0, :]
-        dy = displacement[1, :]
+        v_j_column = np.expand_dims(v_j, axis = 1)
+        displacement = v_j_column * self.dT # 3 x N
+
+        theta = displacement[2, :] # (N,)
+        dx = displacement[0, :] # (N,)
+        dy = displacement[1, :] # (N,)
         shape = theta.shape
         # Correction matrix for time drift, 3 x 3 x N
         T_j_jt = np.array([[np.cos(theta), -np.sin(theta), dx],
@@ -146,13 +147,14 @@ def error(self, v_j, T_wj):
         return e
 
     def jacobian_vector(self, params):
-        theta = params[2]
-        x = params[0]
-        y = params[1]
+        theta = params[5]
+        x = params[3]
+        y = params[4]
         T = np.array([[np.cos(theta), -np.sin(theta), x],
                       [np.sin(theta),  np.cos(theta), y],
                       [0            ,  0            , 1]])
-        return self.info_sqrt @ self.jacobian(params[:3], T)
+        velocity = params[:3]
+        return self.info_sqrt @ self.jacobian(velocity, T)
 
     def jacobian(self, v_j, T_wj):
         '''
diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index 4c6f620..4336cd8 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -1,6 +1,7 @@
 from getTransformKLT import *
 from genFakeData import *
 from motionDistortion import *
+import matplotlib.pyplot as plt
 
 if __name__ == "__main__":
     N = 100
@@ -42,6 +43,13 @@
     print(f"Actual Transform:\ntheta:\n{theta_deg}\nh:\n{h}")
     print(f"Fitted Transform:\ntheta:\n{theta_fit}\nh:\n{h_fit}")
 
+    # Visualize
+    plt.subplot(1,2,1)
+    srcCoord2 = (R_fit @ distorted.T + h_fit).T    
+    plotFakeFeatures(groundTruth, srcCoord2, 
+                     title_append="", alpha=0.5, clear=False, show=False, 
+                     plotDisplace = True)
+
     '''
     Applying Motion Distortion Solving
     '''
@@ -62,12 +70,20 @@
     cov_v = np.diag([4, 4, (2 * np.pi / 180) ** 2]) # 4 ^2 since 4 Hz
     # Information matrix, 
     MDS = MotionDistortionSolver(T_wj0, p_w, p_jt, v_j0, T_wj, cov_p, cov_v)
+    MDS.compute_time_deltas(p_jt)
+    undistorted = MDS.undistort(v_j0)
+    print(undistorted.shape)
+    undistorted = undistorted[:, :2, 0]
+    R_fit, h_fit = calculateTransformSVD(groundTruth, undistorted)
+    srcCoord3 = (R_fit @ undistorted.T + h_fit).T 
+    plt.subplot(1,2,2)
+    plotFakeFeatures(groundTruth, srcCoord3, 
+                     title_append="", alpha=0.5, clear=False, show=True, 
+                     plotDisplace = True)
+
+
     params = MDS.optimize_library()
     print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{params.flatten()}")
 
-    # Visualize
-    srcCoord2 = (R_fit @ distorted.T + h_fit).T
-    plotFakeFeatures(groundTruth, srcCoord2, 
-                     title_append="", alpha=0.5, clear=False, show=True, 
-                     plotDisplace = True)
+    
     
\ No newline at end of file

From e21a348d701f9cdbe15f3b25782938769db9491a Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Mon, 2 May 2022 03:03:19 -0400
Subject: [PATCH 08/14] motionDistort: some debugging comments

---
 motionDistortion.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index 218fbd2..d9f941f 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -104,7 +104,7 @@ def expected_observed_pts(self, T_wj):
         Returns the estimated positions of points based on their world location
         estimates and the current best fit transform
         '''
-        return np.linalg.inv(T_wj) @ self.p_w.T
+        return T_wj @ self.p_w.T#np.linalg.inv(T_wj) @ self.p_w.T
 
     def error_vector(self, params):
         '''

From fe5fd5363ba108ad3ba34ab03de9e43cb5cdb80b Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Mon, 2 May 2022 22:18:56 -0400
Subject: [PATCH 09/14] Found and fixed bug in the motion distort Jacobian

---
 genFakeData.py      | 1 -
 motionDistortion.py | 9 +++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/genFakeData.py b/genFakeData.py
index 99b36b8..0f681c3 100644
--- a/genFakeData.py
+++ b/genFakeData.py
@@ -166,7 +166,6 @@ def distort(coords, velocity, frequency, h):
     #print(displacement)
     #print(displacement)
     dx = displacement[0, :]
-    print(dx)
     dy = displacement[1, :]
     dtheta = displacement[2, :] / 180 * np.pi
     c = np.cos(dtheta)
diff --git a/motionDistortion.py b/motionDistortion.py
index d9f941f..c439a57 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -98,13 +98,13 @@ def undistort(self, v_j):
         undistorted = T_j_jt.transpose((2, 0, 1)) @ p_jt_col # N x 3 x 1
         return undistorted
 
-
+    # TODO: Check if this really should be inverted
     def expected_observed_pts(self, T_wj):
         '''
         Returns the estimated positions of points based on their world location
         estimates and the current best fit transform
         '''
-        return T_wj @ self.p_w.T#np.linalg.inv(T_wj) @ self.p_w.T
+        return np.linalg.inv(T_wj) @ self.p_w.T
 
     def error_vector(self, params):
         '''
@@ -191,7 +191,7 @@ def jacobian(self, v_j, T_wj):
         J_p1 = np.sum(J_p1, axis = 2)
         J_p2 = np.array([[ c0, s0, 0],
                          [-s0, c0, 0],
-                         [0,   0,  1]]) / self.total_scan_time
+                         [0,   0,  1]]) / self.total_scan_time * pwx.shape[0]
         J_p = np.vstack((J_p1, J_p2))
 
         # Compute J_v: derivative of the errors wrt velocity
@@ -205,7 +205,7 @@ def jacobian(self, v_j, T_wj):
                         [zeros, -self.dT, -np.cos(theta) * self.dT * x + np.sin(theta) * self.dT * y],
                         [zeros, zeros, zeros]])
         J_v *= np.expand_dims(cauchy_derivative, axis = 1)
-        J_v = np.sum(J_v, axis = -1) # 2 x 3
+        J_v = np.sum(J_v, axis = -1) # 3 x 3
         J_v = np.vstack((J_v, np.eye(3) * x.shape[0]))
         
         # J = [J_v, J_p]
@@ -262,5 +262,6 @@ def optimize_library(self):
         print(f"Final v: {best_params[:3]}")
         print(f"Final t: {best_params[3:]}")
         print(f"Used {num_evals} evaluations")
+        print(f"Residuals were {result.fun}")
         print(status_dict[status])
         return best_params

From a02ee2a048fa8d8065080de42bbdcba4fb9ee771 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Tue, 3 May 2022 00:37:25 -0400
Subject: [PATCH 10/14] Fixed bug in error vector formatting

---
 motionDistortion.py     | 17 ++++++++++++-----
 testMotionDistortion.py |  4 ++--
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index c439a57..24c6ee2 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -52,6 +52,10 @@ def __init__(self, T_wj0, p_w, p_jt, v_j0, T_wj, sigma_p, sigma_v):
         self.sigma_total = np.block([[sigma_v, np.zeros((n_v, n_p))],
                                       [np.zeros((n_p, n_v)), sigma_p]])
         self.info_sqrt = sp.linalg.sqrtm(np.linalg.inv(self.sigma_total)) # 5 x 5
+        sigma_p_vector = np.tile(np.diag(sigma_p), p_jt.shape[0])
+        sigma_v_vector = np.diag(sigma_v)
+        sigma_vector = np.concatenate((sigma_p_vector, sigma_v_vector))
+        self.info_vector = 1 / sigma_vector
         self.dT = None
         self.T_wj_best = T_wj
         self.v_j_best = v_j0 # might not be good enough a guess, too far from optimal
@@ -118,7 +122,8 @@ def error_vector(self, params):
         T = np.array([[np.cos(theta), -np.sin(theta), x],
                       [np.sin(theta),  np.cos(theta), y],
                       [0            ,  0            , 1]])
-        return self.info_sqrt @ self.error(params[:3], T)
+        #return self.info_sqrt @ self.error(params[:3], T)
+        return self.info_vector * self.error(params[:3], T)
 
     def error(self, v_j, T_wj):
         '''
@@ -131,8 +136,9 @@ def error(self, v_j, T_wj):
         expected = self.expected_observed_pts(T_wj)
         naive_e_p = expected - np.squeeze(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)
-        e_p = np.sum(e_p_i, axis = 1) # (2,)
+        e_p_i = np.log(np.square(naive_e_p[:2, :]) / 2 + 1) # 2 x N
+        e_p = e_p_i.flatten(order='F')
+        #e_p = np.sum(e_p_i, axis = 1) # (2,)
         
         # Compute velocity error
         # Matrix log operation
@@ -141,8 +147,9 @@ def error(self, v_j, T_wj):
         dy = T_j_j1[1, 2]
         dtheta = np.arctan2(T_j_j1[1, 0], T_j_j1[0, 0])
         v_j_prior = np.array([dx, dy, dtheta]) / self.total_scan_time
+        print(f"Prior velocity: {v_j_prior}")
         e_v = (v_j - v_j_prior) * e_p_i.shape[1] # (3,)
-        e = np.hstack((e_p, np.zeros((1,)), e_v))
+        e = np.hstack((e_p, e_v))
         #print(e)
         return e
 
@@ -155,7 +162,7 @@ def jacobian_vector(self, params):
                       [0            ,  0            , 1]])
         velocity = params[:3]
         return self.info_sqrt @ self.jacobian(velocity, T)
-
+        
     def jacobian(self, v_j, T_wj):
         '''
         Compute the Jacobian. This has two parts, as defined by the RadarSLAM
diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index 4336cd8..e06395c 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -65,9 +65,9 @@
     T_wj = np.block([[R_fit,            h_fit],
                      [np.zeros((2,)),    1]])
     # Covariance matrix, point errors
-    cov_p = np.diag([8, 8, 8])
+    cov_p = np.diag([4, 4])
     # Covariance matrix, velocity errors
-    cov_v = np.diag([4, 4, (2 * np.pi / 180) ** 2]) # 4 ^2 since 4 Hz
+    cov_v = np.diag([1, 1, (5 * np.pi / 180) ** 2]) # 4 ^2 since 4 Hz
     # Information matrix, 
     MDS = MotionDistortionSolver(T_wj0, p_w, p_jt, v_j0, T_wj, cov_p, cov_v)
     MDS.compute_time_deltas(p_jt)

From 59b5dc28907df5ae6ff47dec29103207e0cabad1 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Tue, 3 May 2022 00:51:25 -0400
Subject: [PATCH 11/14] Finally fixed it

---
 motionDistortion.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index 24c6ee2..85def52 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -116,9 +116,9 @@ def error_vector(self, params):
         in a theta form, we have to do matrix exponential in here to convert
         into the SO(1) form, then augment to the rotation-translation transform
         '''
-        theta = params[2]
-        x = params[0]
-        y = params[1]
+        theta = params[5]
+        x = params[4]
+        y = params[3]
         T = np.array([[np.cos(theta), -np.sin(theta), x],
                       [np.sin(theta),  np.cos(theta), y],
                       [0            ,  0            , 1]])
@@ -147,7 +147,7 @@ def error(self, v_j, T_wj):
         dy = T_j_j1[1, 2]
         dtheta = np.arctan2(T_j_j1[1, 0], T_j_j1[0, 0])
         v_j_prior = np.array([dx, dy, dtheta]) / self.total_scan_time
-        print(f"Prior velocity: {v_j_prior}")
+        #print(f"Prior velocity: {v_j_prior}")
         e_v = (v_j - v_j_prior) * e_p_i.shape[1] # (3,)
         e = np.hstack((e_p, e_v))
         #print(e)

From bd1a837ac62ebe77f24812c10cabeecd6f3e7a7d Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Tue, 3 May 2022 02:55:40 -0400
Subject: [PATCH 12/14] Jacobians still do not work. Too much error for
 velocity

---
 motionDistortion.py | 28 +++++++++++++++-------------
 1 file changed, 15 insertions(+), 13 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index 85def52..ab1424f 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -148,7 +148,10 @@ def error(self, v_j, T_wj):
         dtheta = np.arctan2(T_j_j1[1, 0], T_j_j1[0, 0])
         v_j_prior = np.array([dx, dy, dtheta]) / self.total_scan_time
         #print(f"Prior velocity: {v_j_prior}")
-        e_v = (v_j - v_j_prior) * e_p_i.shape[1] # (3,)
+        v_diff = (v_j - v_j_prior)
+        v_diff[2] = normalize_angles(v_diff[2])
+        e_v = v_diff * e_p_i.shape[1] # (3,)
+        # ideally should warp2pi here on theta error
         e = np.hstack((e_p, e_v))
         #print(e)
         return e
@@ -161,7 +164,8 @@ def jacobian_vector(self, params):
                       [np.sin(theta),  np.cos(theta), y],
                       [0            ,  0            , 1]])
         velocity = params[:3]
-        return self.info_sqrt @ self.jacobian(velocity, T)
+        #return self.info_sqrt @ self.jacobian(velocity, T)
+        return np.expand_dims(self.info_vector, axis=1) * self.jacobian(velocity, T)
         
     def jacobian(self, v_j, T_wj):
         '''
@@ -175,8 +179,8 @@ def jacobian(self, v_j, T_wj):
         undistorted = self.undistort(v_j)
         expected = self.expected_observed_pts(T_wj)
         input = expected - np.squeeze(undistorted).T # 3 x N
-        denom = np.vstack((input[:2], np.ones((1, input.shape[1]))))
-        cauchy_derivative = input / (np.square(denom) / 2 + 1) # 3 x N
+        naive_e_p = input[:2]
+        cauchy_derivative = naive_e_p / (np.square(naive_e_p) / 2 + 1) # 3 x N
 
         # Compute J_p: derivative of errors wrt the point position
         c0 = self.T_wj0[0, 0]
@@ -192,10 +196,9 @@ def jacobian(self, v_j, T_wj):
 
         # 2 x 3 x N
         J_p1 = np.array([[-c1 * ones, -s1 * ones, -pwx * s1 + pwy * c1 - c1 * Ty + s1 * Tx],
-                        [s1 * ones,  -c1 * ones, -pwx * c1 - pwy * s1 + s1 * Ty + c1 * Tx],
-                         [zeros, zeros, zeros]])
+                        [s1 * ones,  -c1 * ones, -pwx * c1 - pwy * s1 + s1 * Ty + c1 * Tx]])
         J_p1 *= np.expand_dims(cauchy_derivative, axis = 1)
-        J_p1 = np.sum(J_p1, axis = 2)
+        J_p1 = np.squeeze(np.vstack(np.split(J_p1, J_p1.shape[2], axis = 2)))
         J_p2 = np.array([[ c0, s0, 0],
                          [-s0, c0, 0],
                          [0,   0,  1]]) / self.total_scan_time * pwx.shape[0]
@@ -208,11 +211,10 @@ def jacobian(self, v_j, T_wj):
         displacement = np.expand_dims(v_j, axis = 1) * self.dT # 3 x N
         theta = displacement[2, :]
         zeros = np.zeros(theta.shape)
-        J_v = np.array([[-self.dT, zeros, np.sin(theta) * self.dT * x + np.cos(theta) * self.dT * y ],
-                        [zeros, -self.dT, -np.cos(theta) * self.dT * x + np.sin(theta) * self.dT * y],
-                        [zeros, zeros, zeros]])
+        J_v = np.array([[-self.dT, zeros, self.dT * (np.sin(theta) * x + np.cos(theta) * y) ],
+                        [zeros, -self.dT, self.dT * (-np.cos(theta) * x + np.sin(theta) * y)]])
         J_v *= np.expand_dims(cauchy_derivative, axis = 1)
-        J_v = np.sum(J_v, axis = -1) # 3 x 3
+        J_v = np.squeeze(np.vstack(np.split(J_v, J_v.shape[2], axis = 2))) # 2N x 3
         J_v = np.vstack((J_v, np.eye(3) * x.shape[0]))
         
         # J = [J_v, J_p]
@@ -254,8 +256,8 @@ def optimize_library(self):
         print(f"Initial v guess: {self.v_j_initial}")
         print(f"Initial T guess: {T_params}")
 
-        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = '2-point', method = 'lm')
-
+        #result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = '2-point', method = 'lm')
+        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = self.jacobian_vector, method = 'lm')
         # return v, T
         best_params = result.x
         num_evals = result.nfev # number of function evaluations: measure of how quickly we converged

From 58760c7e4e0717ccbca19d3004b4f8b5e09126f3 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Tue, 3 May 2022 03:22:58 -0400
Subject: [PATCH 13/14] Added final code to test file to visualize application
 of undistort

---
 motionDistortion.py     | 8 ++++----
 testMotionDistortion.py | 7 +++++++
 2 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/motionDistortion.py b/motionDistortion.py
index ab1424f..47e0b07 100644
--- a/motionDistortion.py
+++ b/motionDistortion.py
@@ -117,8 +117,8 @@ def error_vector(self, params):
         into the SO(1) form, then augment to the rotation-translation transform
         '''
         theta = params[5]
-        x = params[4]
-        y = params[3]
+        x = params[3]
+        y = params[4]
         T = np.array([[np.cos(theta), -np.sin(theta), x],
                       [np.sin(theta),  np.cos(theta), y],
                       [0            ,  0            , 1]])
@@ -256,8 +256,8 @@ def optimize_library(self):
         print(f"Initial v guess: {self.v_j_initial}")
         print(f"Initial T guess: {T_params}")
 
-        #result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = '2-point', method = 'lm')
-        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = self.jacobian_vector, method = 'lm')
+        result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = '2-point', method = 'lm')
+        # result = sp.optimize.least_squares(self.error_vector, initial_guess, jac = self.jacobian_vector, method = 'lm')
         # return v, T
         best_params = result.x
         num_evals = result.nfev # number of function evaluations: measure of how quickly we converged
diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index e06395c..c36a5c8 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -84,6 +84,13 @@
 
     params = MDS.optimize_library()
     print(f"Parameters:\nvx, vy, dx, dy, dtheta\n{params.flatten()}")
+    final_undistorted = MDS.undistort(params[:3])
+    transform = convertPoseToTransform(params[3:])
+    solution = (transform @ final_undistorted)[:, :2, 0]
+    plt.figure()
+    plotFakeFeatures(groundTruth, solution, 
+                     title_append="", alpha=0.5, clear=False, show=True, 
+                     plotDisplace = True)
 
     
     
\ No newline at end of file

From 69f17fd722a529b13ad55b89ffd63deebf30c5f7 Mon Sep 17 00:00:00 2001
From: Kevin <kevinliu@andrew.cmu.edu>
Date: Tue, 3 May 2022 03:44:15 -0400
Subject: [PATCH 14/14] Added comments guiding next steps in
 testMotionDistortion.py

---
 testMotionDistortion.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/testMotionDistortion.py b/testMotionDistortion.py
index c36a5c8..5840d2d 100644
--- a/testMotionDistortion.py
+++ b/testMotionDistortion.py
@@ -53,7 +53,7 @@
     '''
     Applying Motion Distortion Solving
     '''
-    # Prior Transform
+    # Prior frame's pose
     T_wj0 = np.eye(3)
     # Point world positions
     p_w = groundTruth
@@ -92,5 +92,9 @@
                      title_append="", alpha=0.5, clear=False, show=True, 
                      plotDisplace = True)
 
-    
+    #TODO: In main loop: for each iteration, track the previous frame's points.
+    # If we need to generate new points, create a new keyframe and add its points
+    # to the global map, using the undistort's velocity and transform to determine
+    # true position. We need to be able to associate local points to global points
+    # in each keyframe, and associate global points to those
     
\ No newline at end of file