diff --git a/Mapping.py b/Mapping.py index 4de920c..42b6c16 100644 --- a/Mapping.py +++ b/Mapping.py @@ -62,8 +62,7 @@ def updateInfo(self, globalPose: np.ndarray, self.pointCloud = getPointCloudPolarInd(radarPolarImg) self.velocity = velocity - print(featurePointsLocal.shape) - self.featurePointsLocalUndistorted = MotionDistortionSolver.undistort(velocity, featurePointsLocal-RADAR_CART_CENTER)[:, :2] + self.featurePointsLocalUndistorted = MotionDistortionSolver.undistort(velocity, featurePointsLocal)[:, :2] self.prunedUndistortedLocals = self.featurePointsLocalUndistorted def copyFromOtherKeyframe(self, keyframe) -> None: @@ -91,7 +90,7 @@ def convertFeaturesLocalToGlobal( # Rotate and translate to make into global coordinate system R = getRotationMatrix(th) t = np.array([x, y]).reshape(2, 1) - featurePointsGlobal = (R @ (featurePointsGlobal.T + t)).T + featurePointsGlobal = (R @ (featurePointsGlobal.T) + t).T return featurePointsGlobal @@ -102,17 +101,15 @@ def getPrunedFeaturesGlobalPosition(self): featurePointsGlobal = self.prunedUndistortedLocals # Then we need to convert to meters - featurePointsGlobal *= RANGE_RESOLUTION_CART_M # px * (m/px) = m # Center origin at pose center # Rotate and translate to make into global coordinate system R = getRotationMatrix(th) t = np.array([x, y]).reshape(2, 1) - featurePointsGlobal = (R @ (featurePointsGlobal.T + t)).T + featurePointsGlobal = (R @ (featurePointsGlobal.T) + t).T return featurePointsGlobal - return self.convertFeaturesLocalToGlobal() def pruneFeaturePoints(self, corrStatus: np.ndarray) -> None: ''' diff --git a/RawROAMSystem.py b/RawROAMSystem.py index 4c347fa..44c3097 100644 --- a/RawROAMSystem.py +++ b/RawROAMSystem.py @@ -14,7 +14,7 @@ # Bad solution. better solution is to save in config between mapping and this file RADAR_CART_CENTER = np.array([1012, 1012]) - +wantToPlot = -1 class RawROAMSystem(): def __init__(self, @@ -145,14 +145,14 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: # Get initial features from Cartesian image blobCoord = np.empty((0, 2)) blobCoord, _ = appendNewFeatures(prevImgCart, blobCoord) - print(f"blobCoord shape:{blobCoord.shape}") # Initialize first keyframe + metricCoord = (blobCoord - RADAR_CART_CENTER) * RANGE_RESOLUTION_CART_M zero_velocity = np.zeros((3,)) - old_kf = Keyframe(initPose, blobCoord, prevImgPolar, zero_velocity) # pointer to previous kf + old_kf = Keyframe(initPose, metricCoord, prevImgPolar, zero_velocity) # pointer to previous kf self.map.addKeyframe(old_kf) - possible_kf = Keyframe(initPose, blobCoord, prevImgPolar, zero_velocity) + possible_kf = Keyframe(initPose, metricCoord, prevImgPolar, zero_velocity) for seqInd in range(startSeqInd + 1, endSeqInd + 1): # Obtain polar and Cart image @@ -164,9 +164,19 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: good_old, good_new, rotAngleRad, corrStatus = tracker.track( prevImgCart, currImgCart, prevImgPolar, currImgPolar, blobCoord, seqInd) - + ''' + if seqInd == wantToPlot: + plt.figure() + plt.subplot(1, 2, 1) + plt.scatter(good_old[:,0], good_old[:,1]) + plt.subplot(1, 2, 2) + plt.title("Good old") + #applied = homogenize(centered_new) @ new_transform.T + plt.scatter(good_new[:,0], good_new[:,1]) + plt.title(f"Good new") + plt.show() + ''' # Keyframe updating - print(f"Pruning index shape {corrStatus.shape}") old_kf.pruneFeaturePoints(corrStatus) print("Detected", np.rad2deg(rotAngleRad), "[deg] rotation") @@ -177,6 +187,7 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: # Solve for Motion Compensated Transform p_w = old_kf.getPrunedFeaturesGlobalPosition() # centered + #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 @@ -185,22 +196,44 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: [np.zeros((2,)), 1]]) # Give Motion Distort info on two new frames - MDS.update_problem(prev_pose, p_w, centered_new, T_wj) + debug = False + if seqInd == wantToPlot: + debug = False + # Centered_new is in meters, p_w is in meters, T_wj is in meters, prev_pose is meters + MDS.update_problem(prev_pose, p_w, centered_new, T_wj, debug) undistort_solution = MDS.optimize_library() # Extract new info pose_vector = undistort_solution[3:] - transform = MDS.T_wj0_inv @ convertPoseToTransform(pose_vector) - R = transform[:2, :2] - h = transform[:2, 2:] + new_transform = convertPoseToTransform(pose_vector) + relative_transform = MDS.T_wj0_inv @ new_transform + ''' + if seqInd == wantToPlot: + plt.figure() + plt.subplot(1, 3, 1) + plt.scatter(p_w[:,0], p_w[:,1]) + plt.subplot(1, 3, 2) + plt.title("World coordinates") + applied = homogenize(centered_new) @ new_transform.T + plt.scatter(centered_new[:,0], centered_new[:,1]) + plt.title(f"Post-Transform: {(np.max(centered_new[:,0]) - np.min(centered_new[:,0]))/(np.max(p_w[:,0]) - np.min(p_w[:,0]))}") + plt.subplot(1, 3, 3) + diff = p_w - applied[:, :2] + plt.scatter(diff[:,0], diff[:,1]) + plt.show() + ''' + R = relative_transform[:2, :2] + h = relative_transform[:2, 2:] velocity = undistort_solution[:3] + #velocity[:2] /= RANGE_RESOLUTION_CART_M # Update trajectory #self.updateTrajectory(R, h, seqInd) self.updateTrajectoryAbsolute(pose_vector, seqInd) latestPose = pose_vector #self.estTraj.poses[-1] - possible_kf.updateInfo(latestPose, good_new, currImgPolar, velocity) + # Good new is given in pixels, given velocity in meters, uh oh, pose in meters + possible_kf.updateInfo(latestPose, centered_new, currImgPolar, velocity) # Add a keyframe if it fulfills criteria # 1) large enough translation from previous keyframe @@ -217,7 +250,7 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: print("\nAdding keyframe...\n") #old_kf.copyFromOtherKeyframe(possible_kf) - self.map.addKeyframe(possible_kf) + #self.map.addKeyframe(possible_kf) # TODO: Aliasing above? old_kf is never assigned the object possible_kf, # map ends up with a list of N pointers to the same keyframe @@ -226,21 +259,22 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None: old_kf = possible_kf # TODO: Never replenished blobCoord. Proposed fix: Done here. if retrack: - print(f"Restocking features for new keyframe") good_new, _ = appendNewFeatures(currImgCart, good_new) - old_kf.updateInfo(latestPose, good_new, currImgPolar, velocity) - print(f"{old_kf.featurePointsLocal.shape[0]} features now") - possible_kf = Keyframe(latestPose, blobCoord, currImgPolar, velocity) + centered_new = (good_new - RADAR_CART_CENTER) * RANGE_RESOLUTION_CART_M + old_kf.updateInfo(latestPose, centered_new, currImgPolar, velocity) + possible_kf = Keyframe(latestPose, centered_new, currImgPolar, velocity) # TODO: do bundle adjustment here - self.map.bundleAdjustment() + #self.map.bundleAdjustment() # Plotting and prints and stuff - self.plot(prevImgCart, currImgCart, good_old, good_new, R, h, - seqInd, show = True) - + if seqInd == endSeqInd: + self.plot(prevImgCart, currImgCart, good_old, good_new, R, h, + seqInd, save = True, show = False) + else: + self.plot(prevImgCart, currImgCart, good_old, good_new, R, h, + seqInd, save = False, show = False) # Update incremental variables blobCoord = good_new.copy() - print(f"Blob coord is now size: {blobCoord.shape}") prevImgCart = currImgCart prev_pose = convertPoseToTransform(latestPose) diff --git a/img/roam_mapping/tiny_traj/0001.jpg b/img/roam_mapping/tiny_traj/0001.jpg index 3a6e83d..b2a5ce8 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 58301be..37676ec 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 1e7a5cd..2237332 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 2cb1cee..dbb22df 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 163c6a9..ffeffc0 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 e3c6a77..3a758e6 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 488c22b..47b0af0 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 c2eb4b2..99e54b8 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 8b1855e..ff59f5f 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 0a1f621..1974303 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 917f41b..7f1dbba 100644 --- a/motionDistortion.py +++ b/motionDistortion.py @@ -77,7 +77,7 @@ def __init__(self, sigma_p, sigma_v, self.sigma_v = np.diag(sigma_v) # Info matrix, velocity, sigma_v pass - def update_problem(self, T_wj0, p_w, p_jt, T_wj): + def update_problem(self, T_wj0, p_w, p_jt, T_wj, debug= False): # e_p Parameters self.T_wj0 = T_wj0 # Prior transform, T_w,j0 self.T_wj0_inv = np.linalg.inv(T_wj0) @@ -85,6 +85,7 @@ def update_problem(self, T_wj0, p_w, p_jt, T_wj): self.p_jt = homogenize(p_jt) # Observed points at time t, N x 3 assert(p_w.shape == p_jt.shape) self.T_wj_initial = T_wj + self.debug = debug # e_v Parameters self.v_j_initial = self.infer_velocity(self.T_wj0_inv @ T_wj) @@ -118,7 +119,7 @@ def compute_time_deltas(period, points): y = points[:, 1] # scanline starts at positive x axis and moves clockwise (counter-clockwise?) # We take midpoint pi/2 as 0 - angles = np.arctan2(y, -x) + angles = np.arctan2(-y, -x) dT = period * angles / (2 * np.pi) return dT diff --git a/parseData.py b/parseData.py index 94fa1be..984221c 100644 --- a/parseData.py +++ b/parseData.py @@ -132,7 +132,6 @@ def convertPolarImageToCartesian( flags += cv2.WARP_POLAR_LOG imgCart = cv2.warpPolar(imgPolar, cartSize, center, maxRadius, flags) - return imgCart