diff --git a/Mapping.py b/Mapping.py index 18a15cc..dae89a5 100644 --- a/Mapping.py +++ b/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 diff --git a/RawROAMSystem.py b/RawROAMSystem.py index d227550..13b9b83 100644 --- a/RawROAMSystem.py +++ b/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: diff --git a/img/roam_mapping/tiny_traj.mp4 b/img/roam_mapping/tiny_traj.mp4 index 9e5cd2b..a2d3de9 100644 Binary files a/img/roam_mapping/tiny_traj.mp4 and b/img/roam_mapping/tiny_traj.mp4 differ diff --git a/img/roam_mapping/tiny_traj/0001.jpg b/img/roam_mapping/tiny_traj/0001.jpg index 5ac3f75..3a6e83d 100644 Binary files a/img/roam_mapping/tiny_traj/0001.jpg and b/img/roam_mapping/tiny_traj/0001.jpg differ diff --git a/img/roam_mapping/tiny_traj/0002.jpg b/img/roam_mapping/tiny_traj/0002.jpg index d03baae..58301be 100644 Binary files a/img/roam_mapping/tiny_traj/0002.jpg and b/img/roam_mapping/tiny_traj/0002.jpg differ diff --git a/img/roam_mapping/tiny_traj/0003.jpg b/img/roam_mapping/tiny_traj/0003.jpg index 00aecc1..1e7a5cd 100644 Binary files a/img/roam_mapping/tiny_traj/0003.jpg and b/img/roam_mapping/tiny_traj/0003.jpg differ diff --git a/img/roam_mapping/tiny_traj/0004.jpg b/img/roam_mapping/tiny_traj/0004.jpg index 905cf01..2cb1cee 100644 Binary files a/img/roam_mapping/tiny_traj/0004.jpg and b/img/roam_mapping/tiny_traj/0004.jpg differ diff --git a/img/roam_mapping/tiny_traj/0005.jpg b/img/roam_mapping/tiny_traj/0005.jpg index e72978d..163c6a9 100644 Binary files a/img/roam_mapping/tiny_traj/0005.jpg and b/img/roam_mapping/tiny_traj/0005.jpg differ diff --git a/img/roam_mapping/tiny_traj/0006.jpg b/img/roam_mapping/tiny_traj/0006.jpg index 5eb3226..e3c6a77 100644 Binary files a/img/roam_mapping/tiny_traj/0006.jpg and b/img/roam_mapping/tiny_traj/0006.jpg differ diff --git a/img/roam_mapping/tiny_traj/0007.jpg b/img/roam_mapping/tiny_traj/0007.jpg index fcce2d2..488c22b 100644 Binary files a/img/roam_mapping/tiny_traj/0007.jpg and b/img/roam_mapping/tiny_traj/0007.jpg differ diff --git a/img/roam_mapping/tiny_traj/0008.jpg b/img/roam_mapping/tiny_traj/0008.jpg index 6a6c762..c2eb4b2 100644 Binary files a/img/roam_mapping/tiny_traj/0008.jpg and b/img/roam_mapping/tiny_traj/0008.jpg differ diff --git a/img/roam_mapping/tiny_traj/0009.jpg b/img/roam_mapping/tiny_traj/0009.jpg index 0ed1188..8b1855e 100644 Binary files a/img/roam_mapping/tiny_traj/0009.jpg and b/img/roam_mapping/tiny_traj/0009.jpg differ diff --git a/img/roam_mapping/tiny_traj/0010.jpg b/img/roam_mapping/tiny_traj/0010.jpg index 8725796..0a1f621 100644 Binary files a/img/roam_mapping/tiny_traj/0010.jpg and b/img/roam_mapping/tiny_traj/0010.jpg differ diff --git a/motionDistortion.py b/motionDistortion.py index 398b3cc..19f3255 100644 --- a/motionDistortion.py +++ b/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') diff --git a/trajectoryPlotting.py b/trajectoryPlotting.py index 3706c7e..4e0687e 100644 --- a/trajectoryPlotting.py +++ b/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