Skip to content

Commit

Permalink
Update comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@Samleo8
Samleo8 committed May 5, 2022
1 parent 39852cd commit 4bc3256
Show file tree
Hide file tree
Showing 2 changed files with 138 additions and 90 deletions.
57 changes: 13 additions & 44 deletions Mapping.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,11 @@ def convertFeaturesLocalToGlobal(

return featurePointsGlobal

def getPrunedFeaturesGlobalPosition(self):
def getPrunedFeaturesGlobalPosition(self) -> np.ndarray:
'''
@brief Get global position of pruned features (stored internally)
@return Global position of pruned features (K x 2)
'''
x, y, th = self.pose

# First translate local points to origin at center
Expand All @@ -120,14 +124,6 @@ def pruneFeaturePoints(self, corrStatus: np.ndarray) -> None:
self.prunedFeaturePoints = self.prunedFeaturePoints[corrStatus.flatten().astype(bool)]
self.prunedUndistortedLocals = self.prunedUndistortedLocals[corrStatus.flatten().astype(bool)]

# def isVisible(self, keyframe):
# '''
# @brief Return points that are visible from keyframe
# @deprecated
# '''
# MAX_RANGE_CLIP_DEFAULT


# Map class
class Map():

Expand All @@ -143,14 +139,13 @@ def __init__(self, sequenceName: str, estTraj: Trajectory,
self.estTraj = estTraj

# TODO: Instead of storing set of all keyframes, only store the last keyframe, and then a big array of map points in global coordinates
self.mapPoints = [
] # should be a large np.array of global feature points
# should be a large np.array of global feature points
self.mapPoints = []
self.keyframes = []

def updateInternalTraj(self, traj: Trajectory):
self.estTraj = traj

# TODO: might not want to make keyframe before adding it
def isGoodKeyframe(self, keyframe: Keyframe) -> bool:
'''
@brief Check if a keyframe is good for adding using information about relative rotation and translation
Expand All @@ -164,7 +159,6 @@ def isGoodKeyframe(self, keyframe: Keyframe) -> bool:
targetPose = keyframe.pose

# Check rotation condition relative to last keyframe
# TODO: Check if radians or degrees?
deltaTh = np.abs(srcPose[2] - targetPose[2])

if (deltaTh >= ROT_THRESHOLD):
Expand All @@ -186,46 +180,21 @@ def addKeyframe(self, keyframe: Keyframe) -> None:
'''
self.keyframes.append(keyframe)

def bundleAdjustment(self) -> None:
'''
@brief Perform bundle adjustment on the last 2 keyframes
# TODO: Should actually bundle adjust on all keyframes within range
@return None
'''
# Cannot do BA on only 1 KF
if len(self.keyframes) <= 1:
return

# TODO: Perform bundle adjustment on last 2 keyframes
old_kf = self.keyframes[-2]
new_kf = self.keyframes[-1]

# Obtain relevant information
pose_old = old_kf.pose
pose_new = new_kf.pose
points_old_local = old_kf.prunedFeaturePoints
points_new_local = new_kf.featurePointsLocal

# NOTE: remember ot convert to global coordinates before doing anything with the keyframes
points_old = old_kf.convertFeaturesLocalToGlobal(points_old_local)
points_new = new_kf.convertFeaturesLocalToGlobal(points_new_local)

# TODO: iterative optimisation and related solvers here
# 2d bundle adjustment

pass

def plot(self, fig: plt.figure, show: bool = False):
def plot(self, fig: plt.figure, subsampleFactor: int = 5, show: bool = False) -> None:
'''
@brief Plot map points on plt figure
@param[in] fig plt figure to plot on @todo do this
@param[in] fig plt figure to plot on @todo Currently unused
@param[in] subsampleFactor Subsampling amount to do for feature points plotting
Controls density of plotted points. Higher = less dense
@param[in] show Whether to plt.show()
'''

# TODO: For now, plot all the keyframe feature points
points_global = np.empty((0, 2))
for kf in self.keyframes:
points_global = np.vstack((points_global,kf.getPrunedFeaturesGlobalPosition()))

subsampleFactor = 5
plt.scatter(points_global[::subsampleFactor, 0],
points_global[::subsampleFactor, 1],
marker='+',
Expand Down
171 changes: 125 additions & 46 deletions RawROAMSystem.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
# 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,
Expand Down Expand Up @@ -130,9 +132,10 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:

# Initialialize Motion Distortion Solver
# Covariance matrix, point errors
cov_p = np.diag([4, 4]) # sigma = 2 pixels
cov_p = np.diag([4, 4]) # sigma = 2 pixels
# Covariance matrix, velocity errors
cov_v = np.diag([1, 1, (5 * np.pi / 180) ** 2]) # 1 pixel/s, 1 pixel/s, 5 degrees/s
cov_v = np.diag([1, 1, (5 * np.pi / 180)**2
]) # 1 pixel/s, 1 pixel/s, 5 degrees/s
MDS = MotionDistortionSolver(cov_p, cov_v)
# Prior frame's pose
prev_pose = convertPoseToTransform(initPose)
Expand All @@ -148,11 +151,13 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:

# Initialize first keyframe
metricCoord = (blobCoord - RADAR_CART_CENTER) * RANGE_RESOLUTION_CART_M
zero_velocity = np.zeros((3,))
old_kf = Keyframe(initPose, metricCoord, prevImgPolar, zero_velocity) # pointer to previous kf
zero_velocity = np.zeros((3, ))
old_kf = Keyframe(initPose, metricCoord, prevImgPolar,
zero_velocity) # pointer to previous kf
self.map.addKeyframe(old_kf)

possible_kf = Keyframe(initPose, metricCoord, prevImgPolar, zero_velocity)
possible_kf = Keyframe(initPose, metricCoord, prevImgPolar,
zero_velocity)

for seqInd in range(startSeqInd + 1, endSeqInd + 1):
# Obtain polar and Cart image
Expand All @@ -178,22 +183,22 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
'''
# Keyframe updating
old_kf.pruneFeaturePoints(corrStatus)

print("Detected", np.rad2deg(rotAngleRad), "[deg] rotation")
estR = getRotationMatrix(-rotAngleRad)
R, h = tracker.getTransform(good_old, good_new, pixel = False)

R, h = tracker.getTransform(good_old, good_new, pixel=False)
# R = estR

# Solve for Motion Compensated Transform
p_w = old_kf.getPrunedFeaturesGlobalPosition() # centered
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
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]])
T_wj = prev_pose @ np.block([[R, h], [np.zeros((2, )), 1]])

# Give Motion Distort info on two new frames
debug = False
Expand Down Expand Up @@ -231,14 +236,14 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
#self.updateTrajectory(R, h, seqInd)
self.updateTrajectoryAbsolute(pose_vector, seqInd)

latestPose = pose_vector #self.estTraj.poses[-1]
latestPose = pose_vector #self.estTraj.poses[-1]
# Good new is given in pixels, given velocity in meters, uh oh, pose in meters
possible_kf.updateInfo(latestPose, centered_new, currImgPolar, velocity)
possible_kf.updateInfo(latestPose, centered_new, currImgPolar,
velocity)

# Add a keyframe if it fulfills criteria
# 1) large enough translation from previous keyframe
# 2) large enough rotation from previous KF
# TODO: Not sure if this criteria is actually correct, perhaps we should be adding the previous keyframe instead
# 3) not enough features in current keyframe (ie about to have new features coming up)
# NOTE: Feature check and appending will only be checked in the next iteration,
# so we can prematuraly do it here and add a keyframe first
Expand All @@ -252,58 +257,111 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
#old_kf.copyFromOtherKeyframe(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
# Proposed fix: old_kf = possible_kf # switch ptr to new object
# Initialize new poss_kf for new ptr
old_kf = possible_kf
# TODO: Never replenished blobCoord. Proposed fix: Done here.

if retrack:
good_new, _ = appendNewFeatures(currImgCart, good_new)
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()
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)

# Plotting and prints and stuff
if seqInd == endSeqInd:
self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
seqInd, save = True, show = False)
self.plot(prevImgCart,
currImgCart,
good_old,
good_new,
R,
h,
seqInd,
save=True,
show=False)
else:
if seqInd % 3 == 0:
self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
seqInd, save = False, show = False)
self.plot(prevImgCart,
currImgCart,
good_old,
good_new,
R,
h,
seqInd,
save=False,
show=False)
# Update incremental variables
blobCoord = good_new.copy()
prevImgCart = currImgCart
prev_pose = convertPoseToTransform(latestPose)

# TODO: Move into trajectory class?
def updateTrajectory(self, R, h, seqInd):
def updateTrajectory(self, R: np.ndarray, h: np.ndarray,
seqInd: np.ndarray) -> None:
'''
@brief Update trajectory using R,h matrices
@param[in] R (2 x 2) rotation matrix
@param[in] h (2 x 1) translation vector
@param[in] seqInd Sequence index, used for visualization/plotting
@note Updates internal trajectory
'''

imgPathArr = self.imgPathArr

timestamp = radarImgPathToTimestamp(imgPathArr[seqInd])
est_deltas = convertRandHtoDeltas(R, h)
self.estTraj.appendRelativeDeltas(timestamp, est_deltas)
# self.estTraj.appendRelativeTransform(timestamp, R, h)

def updateTrajectoryAbsolute(self, pose_vector, seqInd):
def updateTrajectoryAbsolute(self, pose_vector: np.ndarray,
seqInd: int) -> None:
'''
@brief Update trajectory using pose vector
@param[in] pose_vector (3, ) pose vector
@param[in] seqInd Sequence index, used for visualization/plotting
@note Updates internal trajectory
'''

imgPathArr = self.imgPathArr

timestamp = radarImgPathToTimestamp(imgPathArr[seqInd])
self.estTraj.appendAbsoluteTransform(timestamp, pose_vector)

def plot(self,
prevImg,
currImg,
good_old,
good_new,
R,
h,
seqInd,
save=True,
show=False):
prevImg: np.ndarray,
currImg: np.ndarray,
good_old: np.ndarray,
good_new: np.ndarray,
R: np.ndarray,
h: np.ndarray,
seqInd: np.ndarray,
plotMapPoints: bool = True,
useArrow: bool = False,
save: bool = True,
show: bool = False) -> None:
'''
@brief Perform global plotting of everything, including trajectory and map points
@param[in] prevImg (M x N) Previous Cartesian radar image to plot
@param[in] currImg (M x N) Current Cartesian radar image to plot
@param[in] good_old (K x 2) Good correspondence points from previous image in scan frame
@param[in] good_new (K x 2) Good correspondence points from current image in scan frame
@param[in] R (2 x 2) rotation matrix
@param[in] h (2 x 1) translation vector
@param[in] seqInd Sequence index
@param[in] useArrow Whether to plot with arrows/triangles to indicate pose direction.
Otherwise uses plain lines.
@param[in] save Whether to save image as png/jpg
@param[in] show Whether to plt.show image
'''

# Draw as subplots
plt.clf()
Expand All @@ -319,10 +377,12 @@ def plot(self,
show=False)

ax2 = self.fig.add_subplot(1, 2, 2)
# TODO: Plotting for map points
self.map.plot(self.fig, show=False)

self.plotTraj(seqInd, R, h, save=False, show=False)
# Plotting for map points
if plotMapPoints:
self.map.plot(self.fig, show=False)

self.plotTraj(seqInd, R, h, useArrow=useArrow, save=False, show=False)

trajSavePath = self.filePaths["trajSave"]
trajSavePathInd = os.path.join(trajSavePath, f"{seqInd:04d}.jpg")
Expand All @@ -344,7 +404,24 @@ def plot(self,
if show:
plt.pause(0.01)

def plotTraj(self, seqInd, R, h, save=False, show=False):
def plotTraj(self,
seqInd: int,
R: np.ndarray,
h: np.ndarray,
useArrow: bool = False,
save: bool = False,
show: bool = False) -> None:
'''
@brief Plot trajectory
@param[in] seqInd Sequence index
@param[in] R (2 x 2) rotation matrix
@param[in] h (2 x 1) translation vector
@param[in] useArrow Whether to plot with arrows/triangles to indicate pose direction.
Otherwise uses plain lines.
@param[in] save Whether to save image as png/jpg
@param[in] show Whether to plt.show image
'''

# Init locals
gtTraj = self.gtTraj
estTraj = self.estTraj
Expand Down Expand Up @@ -377,6 +454,7 @@ def plotTraj(self, seqInd, R, h, save=False, show=False):
estTraj,
title=f'[{seqInd}]',
info=info,
arrow=useArrow,
savePath=toSaveTrajPath)

if show:
Expand Down Expand Up @@ -417,7 +495,8 @@ 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} 20")
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:
Expand Down

0 comments on commit 4bc3256

Please sign in to comment.