diff --git a/Doxyfile b/Doxyfile index 85ddfe5..bda8a42 100644 --- a/Doxyfile +++ b/Doxyfile @@ -743,7 +743,7 @@ WARN_LOGFILE = # spaces. # Note: If this tag is empty the current directory is searched. -INPUT = ./doc/python/netconf.py +INPUT = # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses @@ -810,7 +810,7 @@ FILE_PATTERNS = *.c \ # be searched for input files as well. # The default value is: NO. -RECURSIVE = YES +RECURSIVE = NO # The EXCLUDE tag can be used to specify files and/or directories that should be # excluded from the INPUT source files. This way you can easily exclude a @@ -819,7 +819,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = +EXCLUDE = g2o/* # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/doc/html/annotated.html b/doc/html/annotated.html new file mode 100644 index 0000000..989f199 --- /dev/null +++ b/doc/html/annotated.html @@ -0,0 +1,119 @@ + + +
+ + + + +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
▼NCoord | |
CCartCoord | |
CPolarCoord | |
▼NMapping | |
CKeyframe | |
CMap | |
▼NmotionDistortion | |
CMotionDistortionSolver | |
▼NPoseGraphLib | |
CBundleAdjustment | |
CPoseGraphOptimization | |
▼NRawROAMSystem | |
CRawROAMSystem | |
▼NTracker | |
CTracker | |
▼NtrajectoryPlotting | |
CTrajectory |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for CartCoord, including all inherited members.
+__init__(self, float x, float y) | CartCoord | |
__str__(self) | CartCoord | |
add(self, float dx, float dy) | CartCoord | |
addCoord(self, other) | CartCoord | |
asTuple(self) | CartCoord | |
getAngle(self, other) | CartCoord | |
getDistance(self, other) | CartCoord | |
getX(self) | CartCoord | |
getY(self) | CartCoord | |
scale(self, float scaleFactor) | CartCoord | |
scaleX(self, float scaleFactor) | CartCoord | |
scaleY(self, float scaleFactor) | CartCoord | |
sub(self, float dx, float dy) | CartCoord | |
x | CartCoord | |
y | CartCoord |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self, float x, float y) |
def | __str__ (self) |
None | addCoord (self, other) |
None | add (self, float dx, float dy) |
None | sub (self, float dx, float dy) |
None | scale (self, float scaleFactor) |
None | scaleX (self, float scaleFactor) |
None | scaleY (self, float scaleFactor) |
float | getX (self) |
float | getY (self) |
float | getDistance (self, other) |
def | getAngle (self, other) |
tuple | asTuple (self) |
+Public Attributes | |
x | |
y | |
Creates a point on a Cartesian coordinate plane with values x and y.
def __init__ | +( | ++ | self, | +
+ | + | float | +x, | +
+ | + | float | +y | +
+ | ) | ++ |
Defines x and y variables+
def __str__ | +( | ++ | self | ) | ++ |
None add | +( | ++ | self, | +
+ | + | float | +dx, | +
+ | + | float | +dy | +
+ | ) | ++ |
None addCoord | +( | ++ | self, | +
+ | + | + | other | +
+ | ) | ++ |
tuple asTuple | +( | ++ | self | ) | ++ |
def getAngle | +( | ++ | self, | +
+ | + | + | other | +
+ | ) | ++ |
float getDistance | +( | ++ | self, | +
+ | + | + | other | +
+ | ) | ++ |
float getX | +( | ++ | self | ) | ++ |
float getY | +( | ++ | self | ) | ++ |
None scale | +( | ++ | self, | +
+ | + | float | +scaleFactor | +
+ | ) | ++ |
None scaleX | +( | ++ | self, | +
+ | + | float | +scaleFactor | +
+ | ) | ++ |
None scaleY | +( | ++ | self, | +
+ | + | float | +scaleFactor | +
+ | ) | ++ |
None sub | +( | ++ | self, | +
+ | + | float | +dx, | +
+ | + | float | +dy | +
+ | ) | ++ |
x | +
y | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for PolarCoord, including all inherited members.
+__init__(self, float r, float theta) | PolarCoord | |
__str__(self) | PolarCoord | |
asTuple(self) | PolarCoord | |
getR(self) | PolarCoord | |
getTheta(self) | PolarCoord | |
r | PolarCoord | |
scaleR(self, scaleFactor) | PolarCoord | |
theta | PolarCoord | |
toCart(self) | PolarCoord |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self, float r, float theta) |
def | __str__ (self) |
float | getR (self) |
float | getTheta (self) |
None | scaleR (self, scaleFactor) |
CartCoord | toCart (self) |
tuple | asTuple (self) |
+Public Attributes | |
r | |
theta | |
Creates a point on a Cartesian coordinate plane with values x and y.
def __init__ | +( | ++ | self, | +
+ | + | float | +r, | +
+ | + | float | +theta | +
+ | ) | ++ |
Defines x and y variables+
def __str__ | +( | ++ | self | ) | ++ |
tuple asTuple | +( | ++ | self | ) | ++ |
float getR | +( | ++ | self | ) | ++ |
float getTheta | +( | ++ | self | ) | ++ |
None scaleR | +( | ++ | self, | +
+ | + | + | scaleFactor | +
+ | ) | ++ |
CartCoord toCart | +( | ++ | self | ) | ++ |
r | +
theta | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for Keyframe, including all inherited members.
+__init__(self, np.ndarray globalPose, np.ndarray featurePointsLocal, np.ndarray radarPolarImg, np.ndarray velocity) | Keyframe | |
convertFeaturesLocalToGlobal(self, np.ndarray featurePointsLocal) | Keyframe | |
copyFromOtherKeyframe(self, keyframe) | Keyframe | |
featurePointsLocal | Keyframe | |
featurePointsLocalUndistorted | Keyframe | |
getPrunedFeaturesGlobalPosition(self) | Keyframe | |
pointCloud | Keyframe | |
pose | Keyframe | |
prunedFeaturePoints | Keyframe | |
prunedUndistortedLocals | Keyframe | |
pruneFeaturePoints(self, np.ndarray corrStatus) | Keyframe | |
radarPolarImg | Keyframe | |
updateInfo(self, np.ndarray globalPose, np.ndarray featurePointsLocal, np.ndarray radarPolarImg, np.ndarray velocity) | Keyframe | |
velocity | Keyframe |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
None | __init__ (self, np.ndarray globalPose, np.ndarray featurePointsLocal, np.ndarray radarPolarImg, np.ndarray velocity) |
None | updateInfo (self, np.ndarray globalPose, np.ndarray featurePointsLocal, np.ndarray radarPolarImg, np.ndarray velocity) |
None | copyFromOtherKeyframe (self, keyframe) |
np.ndarray | convertFeaturesLocalToGlobal (self, np.ndarray featurePointsLocal) |
np.ndarray | getPrunedFeaturesGlobalPosition (self) |
None | pruneFeaturePoints (self, np.ndarray corrStatus) |
+Public Attributes | |
pose | |
radarPolarImg | |
featurePointsLocal | |
prunedFeaturePoints | |
pointCloud | |
velocity | |
featurePointsLocalUndistorted | |
prunedUndistortedLocals | |
None __init__ | +( | ++ | self, | +
+ | + | np.ndarray | +globalPose, | +
+ | + | np.ndarray | +featurePointsLocal, | +
+ | + | np.ndarray | +radarPolarImg, | +
+ | + | np.ndarray | +velocity | +
+ | ) | ++ |
@brief Keyframe class. Contains pose, feature points and point cloud information +@param[in] globalPose (3 x 1) Pose information [x, y, th] in global coordinates, + units of [m, m, rad] # TODO: Confirm these units +@param[in] featurePointsLocal (K x 2) Tracked feature points from previous keyframe, + in local coordinates (pixels) +@param[in] radarPolarImg (M x N) Radar polar (range-azimuth) image + +@see updateInfo() ++
np.ndarray convertFeaturesLocalToGlobal | +( | ++ | self, | +
+ | + | np.ndarray | +featurePointsLocal | +
+ | ) | ++ |
@brief Local feature points to convert into global coordinates, given internal pose +@param[in] featurePointsLocal (K x 2) Tracked feature points from previous keyframe, + in local coordinates (pixels) +@return featurePointsGlobal (K x 2) Feature points in global coordinates, meters ++
None copyFromOtherKeyframe | +( | ++ | self, | +
+ | + | + | keyframe | +
+ | ) | ++ |
np.ndarray getPrunedFeaturesGlobalPosition | +( | ++ | self | ) | ++ |
@brief Get global position of pruned features (stored internally) +@return Global position of pruned features (K x 2) ++
None pruneFeaturePoints | +( | ++ | self, | +
+ | + | np.ndarray | +corrStatus | +
+ | ) | ++ |
@brief Prune feature points based on correspondence status +@param[in] corrStatus +@note In place changing of `self.prunedFeaturePoints` function, which aims to track and prune away the feature points that are part of this keyframe ++
None updateInfo | +( | ++ | self, | +
+ | + | np.ndarray | +globalPose, | +
+ | + | np.ndarray | +featurePointsLocal, | +
+ | + | np.ndarray | +radarPolarImg, | +
+ | + | np.ndarray | +velocity | +
+ | ) | ++ |
@brief Update internal information: pose, feature points and point cloud information +@param[in] globalPose (3 x 1) Pose information [x, y, th] in global coordinates, + units of [m, m, rad] # TODO: Confirm these units +@param[in] featurePointsLocal (K x 2) Tracked feature points from previous keyframe, + in local coordinates (pixels) +@param[in] radarPolarImg (M x N) Radar polar (range-azimuth) image ++
featurePointsLocal | +
featurePointsLocalUndistorted | +
pointCloud | +
pose | +
prunedFeaturePoints | +
prunedUndistortedLocals | +
radarPolarImg | +
velocity | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for Map, including all inherited members.
+__init__(self, str sequenceName, Trajectory estTraj, list[str] imgPathArr, dict[str] filePaths) | Map | |
addKeyframe(self, Keyframe keyframe) | Map | |
estTraj | Map | |
filePaths | Map | |
imgPathArr | Map | |
isGoodKeyframe(self, Keyframe keyframe) | Map | |
keyframes | Map | |
mapPoints | Map | |
plot(self, plt.figure fig, int subsampleFactor=5, bool show=False) | Map | |
sequenceName | Map | |
sequenceSize | Map | |
updateInternalTraj(self, Trajectory traj) | Map |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
None | __init__ (self, str sequenceName, Trajectory estTraj, list[str] imgPathArr, dict[str] filePaths) |
def | updateInternalTraj (self, Trajectory traj) |
bool | isGoodKeyframe (self, Keyframe keyframe) |
None | addKeyframe (self, Keyframe keyframe) |
None | plot (self, plt.figure fig, int subsampleFactor=5, bool show=False) |
+Public Attributes | |
sequenceName | |
imgPathArr | |
sequenceSize | |
filePaths | |
estTraj | |
mapPoints | |
keyframes | |
None __init__ | +( | ++ | self, | +
+ | + | str | +sequenceName, | +
+ | + | Trajectory | +estTraj, | +
+ | + | list[str] | +imgPathArr, | +
+ | + | dict[str] | +filePaths | +
+ | ) | ++ |
None addKeyframe | +( | ++ | self, | +
+ | + | Keyframe | +keyframe | +
+ | ) | ++ |
@brief Add a keyframe to the running pose graph +@param[in] keyframe Keyframe to add ++
bool isGoodKeyframe | +( | ++ | self, | +
+ | + | Keyframe | +keyframe | +
+ | ) | ++ |
@brief Check if a keyframe is good for adding using information about relative rotation and translation +@return If keyframe passes checks ++
None plot | +( | ++ | self, | +
+ | + | plt.figure | +fig, | +
+ | + | int | +subsampleFactor = 5 , |
+
+ | + | bool | +show = False |
+
+ | ) | ++ |
@brief Plot map points on plt figure +@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() ++
def updateInternalTraj | +( | ++ | self, | +
+ | + | Trajectory | +traj | +
+ | ) | ++ |
estTraj | +
filePaths | +
imgPathArr | +
keyframes | +
mapPoints | +
sequenceName | +
sequenceSize | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for BundleAdjustment, including all inherited members.
+__init__(self) | BundleAdjustment | |
add_edge(self, point_id, pose_id, measurement, information=np.eye(2), robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))) | BundleAdjustment | |
add_point(self, point_id, point, fixed=False, marginalized=True) | BundleAdjustment | |
add_pose(self, pose_id, pose, cam, fixed=False) | BundleAdjustment | |
get_point(self, point_id) | BundleAdjustment | |
get_pose(self, pose_id) | BundleAdjustment | |
optimize(self, max_iterations=10) | BundleAdjustment |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self) |
def | optimize (self, max_iterations=10) |
def | add_pose (self, pose_id, pose, cam, fixed=False) |
def | add_point (self, point_id, point, fixed=False, marginalized=True) |
def | add_edge (self, point_id, pose_id, measurement, information=np.eye(2), robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))) |
def | get_pose (self, pose_id) |
def | get_point (self, point_id) |
def __init__ | +( | ++ | self | ) | ++ |
def add_edge | +( | ++ | self, | +
+ | + | + | point_id, | +
+ | + | + | pose_id, | +
+ | + | + | measurement, | +
+ | + | + | information = np.eye(2) , |
+
+ | + | + | robust_kernel = g2o.RobustKernelHuber(
+ np.sqrt(5.991)) |
+
+ | ) | ++ |
def add_point | +( | ++ | self, | +
+ | + | + | point_id, | +
+ | + | + | point, | +
+ | + | + | fixed = False , |
+
+ | + | + | marginalized = True |
+
+ | ) | ++ |
def add_pose | +( | ++ | self, | +
+ | + | + | pose_id, | +
+ | + | + | pose, | +
+ | + | + | cam, | +
+ | + | + | fixed = False |
+
+ | ) | ++ |
def get_point | +( | ++ | self, | +
+ | + | + | point_id | +
+ | ) | ++ |
def get_pose | +( | ++ | self, | +
+ | + | + | pose_id | +
+ | ) | ++ |
def optimize | +( | ++ | self, | +
+ | + | + | max_iterations = 10 |
+
+ | ) | ++ |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for PoseGraphOptimization, including all inherited members.
+__init__(self) | PoseGraphOptimization | |
add_edge(self, vertices, measurement, information=np.eye(6), robust_kernel=None) | PoseGraphOptimization | |
add_vertex(self, id, pose, fixed=False) | PoseGraphOptimization | |
get_pose(self, id) | PoseGraphOptimization | |
optimize(self, max_iterations=20) | PoseGraphOptimization |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self) |
def | optimize (self, max_iterations=20) |
def | add_vertex (self, id, pose, fixed=False) |
def | add_edge (self, vertices, measurement, information=np.eye(6), robust_kernel=None) |
def | get_pose (self, id) |
def __init__ | +( | ++ | self | ) | ++ |
def add_edge | +( | ++ | self, | +
+ | + | + | vertices, | +
+ | + | + | measurement, | +
+ | + | + | information = np.eye(6) , |
+
+ | + | + | robust_kernel = None |
+
+ | ) | ++ |
def add_vertex | +( | ++ | self, | +
+ | + | + | id, | +
+ | + | + | pose, | +
+ | + | + | fixed = False |
+
+ | ) | ++ |
def get_pose | +( | ++ | self, | +
+ | + | + | id | +
+ | ) | ++ |
def optimize | +( | ++ | self, | +
+ | + | + | max_iterations = 20 |
+
+ | ) | ++ |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for RawROAMSystem, including all inherited members.
+__init__(self, str sequenceName, bool paramFlags=dict(), bool hasGroundTruth=True) | RawROAMSystem | |
estTraj | RawROAMSystem | |
fig | RawROAMSystem | |
filePaths | RawROAMSystem | |
gtTraj | RawROAMSystem | |
hasGroundTruth | RawROAMSystem | |
imgPathArr | RawROAMSystem | |
map | RawROAMSystem | |
paramFlags | RawROAMSystem | |
plot(self, np.ndarray prevImg, np.ndarray currImg, np.ndarray good_old, np.ndarray good_new, np.ndarray R, np.ndarray h, np.ndarray seqInd, bool plotMapPoints=True, bool useArrow=False, bool save=True, bool show=False) | RawROAMSystem | |
plotTraj(self, int seqInd, np.ndarray R, np.ndarray h, bool useArrow=False, bool save=False, bool show=False) | RawROAMSystem | |
run(self, int startSeqInd=0, int endSeqInd=-1) | RawROAMSystem | |
sequenceName | RawROAMSystem | |
sequenceSize | RawROAMSystem | |
tracker | RawROAMSystem | |
updateTrajectory(self, np.ndarray R, np.ndarray h, np.ndarray seqInd) | RawROAMSystem | |
updateTrajectoryAbsolute(self, np.ndarray pose_vector, int seqInd) | RawROAMSystem | |
updateTrajFromTracker(self) | RawROAMSystem |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
None | __init__ (self, str sequenceName, bool paramFlags=dict(), bool hasGroundTruth=True) |
def | updateTrajFromTracker (self) |
None | run (self, int startSeqInd=0, int endSeqInd=-1) |
None | updateTrajectory (self, np.ndarray R, np.ndarray h, np.ndarray seqInd) |
None | updateTrajectoryAbsolute (self, np.ndarray pose_vector, int seqInd) |
None | plot (self, np.ndarray prevImg, np.ndarray currImg, np.ndarray good_old, np.ndarray good_new, np.ndarray R, np.ndarray h, np.ndarray seqInd, bool plotMapPoints=True, bool useArrow=False, bool save=True, bool show=False) |
None | plotTraj (self, int seqInd, np.ndarray R, np.ndarray h, bool useArrow=False, bool save=False, bool show=False) |
+Public Attributes | |
sequenceName | |
paramFlags | |
hasGroundTruth | |
imgPathArr | |
sequenceSize | |
filePaths | |
fig | |
gtTraj | |
estTraj | |
tracker | |
map | |
None __init__ | +( | ++ | self, | +
+ | + | str | +sequenceName, | +
+ | + | bool | +paramFlags = dict() , |
+
+ | + | bool | +hasGroundTruth = True |
+
+ | ) | ++ |
@brief Initializer for ROAM system +@param[in] sequenceName Name of sequence. Should be in ./data folder +@param[in] pararmFlags Set of flags indicating turning on and off of certain algorithm features + - rejectOutliers: Whether to use graph-based outlier rejection + - useANMS: Whether to use ANMS + - useFMT: Whether to use FMT to correct things + - correctMotionDistortion: Whether to correct for motion distortion +@param[in] hasGroundTruth Whether sequence has ground truth to be used for plotting @deprecated ++
None plot | +( | ++ | self, | +
+ | + | np.ndarray | +prevImg, | +
+ | + | np.ndarray | +currImg, | +
+ | + | np.ndarray | +good_old, | +
+ | + | np.ndarray | +good_new, | +
+ | + | np.ndarray | +R, | +
+ | + | np.ndarray | +h, | +
+ | + | np.ndarray | +seqInd, | +
+ | + | bool | +plotMapPoints = True , |
+
+ | + | bool | +useArrow = False , |
+
+ | + | bool | +save = True , |
+
+ | + | bool | +show = False |
+
+ | ) | ++ |
@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 ++
None plotTraj | +( | ++ | self, | +
+ | + | int | +seqInd, | +
+ | + | np.ndarray | +R, | +
+ | + | np.ndarray | +h, | +
+ | + | bool | +useArrow = False , |
+
+ | + | bool | +save = False , |
+
+ | + | bool | +show = False |
+
+ | ) | ++ |
@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 ++
None run | +( | ++ | self, | +
+ | + | int | +startSeqInd = 0 , |
+
+ | + | int | +endSeqInd = -1 |
+
+ | ) | ++ |
@brief Do a full run the ROAMing algorithm on sequence, + starting from and ending at specified indices, + incrementally calling the @see Tracker.track() function + +@param[in] startImgInd Starting index of sequence. Default 0. +@param[in] startImgInd Ending index of sequence. Default -1. + Negative numbers to indicate end. ++
None updateTrajectory | +( | ++ | self, | +
+ | + | np.ndarray | +R, | +
+ | + | np.ndarray | +h, | +
+ | + | np.ndarray | +seqInd | +
+ | ) | ++ |
@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 ++
None updateTrajectoryAbsolute | +( | ++ | self, | +
+ | + | np.ndarray | +pose_vector, | +
+ | + | int | +seqInd | +
+ | ) | ++ |
@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 ++
def updateTrajFromTracker | +( | ++ | self | ) | ++ |
estTraj | +
fig | +
filePaths | +
gtTraj | +
hasGroundTruth | +
imgPathArr | +
map | +
paramFlags | +
sequenceName | +
sequenceSize | +
tracker | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for Tracker, including all inherited members.
+__init__(self, str sequenceName, list[str] imgPathArr, dict[str] filePaths, dict[bool] paramFlags) | Tracker | |
estTraj | Tracker | |
filePaths | Tracker | |
getTransform(self, np.ndarray srcCoord, np.ndarray targetCoord, bool pixel) | Tracker | |
gtTraj | Tracker | |
imgPathArr | Tracker | |
initTraj(self, Trajectory estTraj, Trajectory gtTraj=None) | Tracker | |
paramFlags | Tracker | |
plot(self, prevImg, currImg, good_old, good_new, seqInd, save=True, show=False) | Tracker | |
sequenceName | Tracker | |
sequenceSize | Tracker | |
track(self, np.ndarray prevImgCart, np.ndarray currImgCart, np.ndarray prevImgPolar, np.ndarray currImgPolar, np.ndarray featureCoord, int seqInd) | Tracker |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
None | __init__ (self, str sequenceName, list[str] imgPathArr, dict[str] filePaths, dict[bool] paramFlags) |
def | initTraj (self, Trajectory estTraj, Trajectory gtTraj=None) |
Tuple[np.ndarray, np.ndarray, np.ndarray] | track (self, np.ndarray prevImgCart, np.ndarray currImgCart, np.ndarray prevImgPolar, np.ndarray currImgPolar, np.ndarray featureCoord, int seqInd) |
Tuple[np.ndarray, np.ndarray] | getTransform (self, np.ndarray srcCoord, np.ndarray targetCoord, bool pixel) |
def | plot (self, prevImg, currImg, good_old, good_new, seqInd, save=True, show=False) |
+Public Attributes | |
sequenceName | |
imgPathArr | |
sequenceSize | |
filePaths | |
paramFlags | |
estTraj | |
gtTraj | |
None __init__ | +( | ++ | self, | +
+ | + | str | +sequenceName, | +
+ | + | list[str] | +imgPathArr, | +
+ | + | dict[str] | +filePaths, | +
+ | + | dict[bool] | +paramFlags | +
+ | ) | ++ |
Tuple[np.ndarray, np.ndarray] getTransform | +( | ++ | self, | +
+ | + | np.ndarray | +srcCoord, | +
+ | + | np.ndarray | +targetCoord, | +
+ | + | bool | +pixel | +
+ | ) | ++ |
@brief Obtain transform from coordinate correspondnces + +@param[in] srcCoord Coordinates of src feature points (K' x 2) in [x, y] format +@param[in] targetCoord Coordinates of target feature points (K' x 2) in [x, y] format + +@note target = R @ src + h +@return R rotation matrix (2 x 2) +@return h translation matrix (2 x 1), units in meters [m] ++
def initTraj | +( | ++ | self, | +
+ | + | Trajectory | +estTraj, | +
+ | + | Trajectory | +gtTraj = None |
+
+ | ) | ++ |
def plot | +( | ++ | self, | +
+ | + | + | prevImg, | +
+ | + | + | currImg, | +
+ | + | + | good_old, | +
+ | + | + | good_new, | +
+ | + | + | seqInd, | +
+ | + | + | save = True , |
+
+ | + | + | show = False |
+
+ | ) | ++ |
Tuple[np.ndarray, np.ndarray, np.ndarray] track | +( | ++ | self, | +
+ | + | np.ndarray | +prevImgCart, | +
+ | + | np.ndarray | +currImgCart, | +
+ | + | np.ndarray | +prevImgPolar, | +
+ | + | np.ndarray | +currImgPolar, | +
+ | + | np.ndarray | +featureCoord, | +
+ | + | int | +seqInd | +
+ | ) | ++ |
@brief Track based on previous and current image + +@param[in] prevImg Previous Cartesian radar image (N x N) +@param[in] prevImg Current Cartesian radar image (N x N) +@param[in] prevImg Previous polar radar image (? x ?) +@param[in] prevImg Current polar radar image (? x ?) + +@param[in] blobCoord Coordinates of feature points (K x 2) in [x, y] format + +@return good_old Coordinates of old good feature points (K' x 2) in [x, y] format +@return good_new Coordinates of new good feature points (K' x 2) in [x, y] format +@return angleRotRad Angle used to rotate image +@return corrStatus (K x 2) correspondence status @note Needed for mapping to track keyframe points ++
estTraj | +
filePaths | +
gtTraj | +
imgPathArr | +
paramFlags | +
sequenceName | +
sequenceSize | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for MotionDistortionSolver, including all inherited members.
+
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self, T_wj0, p_w, p_jt, T_wj, sigma_p, sigma_v, frequency=RADAR_SCAN_FREQUENCY) |
def | __init__ (self, sigma_p, sigma_v, frequency=RADAR_SCAN_FREQUENCY) |
def | update_problem (self, T_wj0, p_w, p_jt, T_wj, debug=False) |
def | infer_velocity (self, transform) |
def | expected_observed_pts (self, T_wj) |
def | error_vector (self, params) |
def | error (self, v_j, T_wj) |
def | jacobian_vector (self, params) |
def | jacobian (self, v_j, T_wj) |
def | optimize (self, max_iters=20) |
def | optimize_library (self) |
+Static Public Member Functions | |
def | compute_time_deltas (period, points) |
def | undistort (v_j, points, period=1/RADAR_SCAN_FREQUENCY, times=None) |
+Public Attributes | |
T_wj0 | |
T_wj0_inv | |
p_w | |
p_jt | |
T_wj_initial | |
total_scan_time | |
v_j_initial | |
sigma_p | |
sigma_v | |
info_vector | |
dT | |
debug | |
def __init__ | +( | ++ | self, | +
+ | + | + | T_wj0, | +
+ | + | + | p_w, | +
+ | + | + | p_jt, | +
+ | + | + | T_wj, | +
+ | + | + | sigma_p, | +
+ | + | + | sigma_v, | +
+ | + | + | frequency = RADAR_SCAN_FREQUENCY |
+
+ | ) | ++ |
def __init__ | +( | ++ | self, | +
+ | + | + | sigma_p, | +
+ | + | + | sigma_v, | +
+ | + | + | frequency = RADAR_SCAN_FREQUENCY |
+
+ | ) | ++ |
+
|
+ +static | +
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 +time displacement, and therefore the higher time delta. We use this time +delta to help us transform the points into an undistorted frame. Note +that this is an estimate computed from distorted images. It is a good +idea to re-run this function once an undistorted frame is obtained for +better estimates. ++
def error | +( | ++ | self, | +
+ | + | + | v_j, | +
+ | + | + | T_wj | +
+ | ) | ++ |
Return the Cauchy robust error between the undistorted points and their +estimated observed positions and the velocity error. ++
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 ++
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 ++
def infer_velocity | +( | ++ | self, | +
+ | + | + | transform | +
+ | ) | ++ |
def jacobian | +( | ++ | self, | +
+ | + | + | v_j, | +
+ | + | + | T_wj | +
+ | ) | ++ |
Compute the Jacobian. This has two parts, as defined by the RadarSLAM +paper: +J_p - gradient of point error and velocity error wrt pose terms Tx, + Ty, Ttheta +J_v - gradient of point error and velocity error wrt velocity terms + vx, vy, vtheta ++
def jacobian_vector | +( | ++ | self, | +
+ | + | + | params | +
+ | ) | ++ |
def optimize | +( | ++ | self, | +
+ | + | + | max_iters = 20 |
+
+ | ) | ++ |
def optimize_library | +( | ++ | self | ) | ++ |
Optimize using the LM implementation in the scipy library. ++
+
|
+ +static | +
Computes a new set of undistorted observed points, based on the current +best estimate of v_T, T_wj, dT ++
def update_problem | +( | ++ | self, | +
+ | + | + | T_wj0, | +
+ | + | + | p_w, | +
+ | + | + | p_jt, | +
+ | + | + | T_wj, | +
+ | + | + | debug = False |
+
+ | ) | ++ |
debug | +
dT | +
info_vector | +
p_jt | +
p_w | +
sigma_p | +
sigma_v | +
T_wj0 | +
T_wj0_inv | +
T_wj_initial | +
total_scan_time | +
v_j_initial | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
This is the complete list of members for Trajectory, including all inherited members.
+__init__(self, timestamps, poses) | Trajectory | |
appendAbsoluteTransform(self, time, pose) | Trajectory | |
appendRelativeDeltas(self, time, d_xyth) | Trajectory | |
appendRelativeTransform(self, time, R, h) | Trajectory | |
getGroundTruthDeltasAtTime(self, time) | Trajectory | |
getPoseAtTimes(self, times) | Trajectory | |
plot(self, title='My Trajectory', savePath=False) | Trajectory | |
pose_transform | Trajectory | |
poses | Trajectory | |
timestamps | Trajectory |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+Public Member Functions | |
def | __init__ (self, timestamps, poses) |
def | getGroundTruthDeltasAtTime (self, time) |
def | appendRelativeDeltas (self, time, d_xyth) |
def | appendRelativeTransform (self, time, R, h) |
def | appendAbsoluteTransform (self, time, pose) |
def | getPoseAtTimes (self, times) |
def | plot (self, title='My Trajectory', savePath=False) |
+Public Attributes | |
timestamps | |
poses | |
pose_transform | |
def __init__ | +( | ++ | self, | +
+ | + | + | timestamps, | +
+ | + | + | poses | +
+ | ) | ++ |
@param[in] timestamps np.ndarray of sorted timestamps (N) +@param[in] pose_matrices np.ndarray of poses (N x 3) ++
def appendAbsoluteTransform | +( | ++ | self, | +
+ | + | + | time, | +
+ | + | + | pose | +
+ | ) | ++ |
@brief Append a relative transform to the trajectory + h should already be scaled by radar resolution +@param[in] time timestamp of the transform +@param[in] pose pose vector (3, ) ++
def appendRelativeDeltas | +( | ++ | self, | +
+ | + | + | time, | +
+ | + | + | d_xyth | +
+ | ) | ++ |
def appendRelativeTransform | +( | ++ | self, | +
+ | + | + | time, | +
+ | + | + | R, | +
+ | + | + | h | +
+ | ) | ++ |
@brief Append a relative transform to the trajectory + h should already be scaled by radar resolution +@param[in] time timestamp of the transform +@param[in] R rotation matrix (2 x 2) +@param[in] h translation vector (2 x 1) ++
def getGroundTruthDeltasAtTime | +( | ++ | self, | +
+ | + | + | time | +
+ | ) | ++ |
@brief Given a timestamp, return the ground truth deltas at that time in (dx, dy, dth) list for debugging ++
def getPoseAtTimes | +( | ++ | self, | +
+ | + | + | times | +
+ | ) | ++ |
@brief Given timestamps, return the pose at that time using cubic interpolation +@param[in] times np.ndarray of sorted timestamps ++
def plot | +( | ++ | self, | +
+ | + | + | title = 'My Trajectory' , |
+
+ | + | + | savePath = False |
+
+ | ) | ++ |
pose_transform | +
poses | +
timestamps | +
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
CKeyframe | |
CMap | |
CMotionDistortionSolver | |
▼Cobject | |
CCartCoord | |
CPolarCoord | |
CRawROAMSystem | |
▼CSparseOptimizer | |
CBundleAdjustment | |
CPoseGraphOptimization | |
CTracker | |
CTrajectory |
+ RAW-ROAM
+
+ Python Re-implementation of RadarSLAM's odometry and mapping (ROAM) components
+ |
+
A Python reimplementation of odometry and mapping component of RadarSLAM by Hong et al. [1,2].
+Final Paper: RAW-ROAM: An Open-Source Implementation of Adverse Weather RadarSLAM
+With Motion Compensation
+ +Without Motion Compensation:
+ +Requires Python. Tested on Python >= 3.9.
+Radar sequences can be obtained from Oxford Radar RobotCar Dataset and should be placed in the ./data
folder. The folder organization listed as full_seq_1
is an example of how the directory looks like, and is taken from the 10-11-46-21
sequence.
See the docs
+diff --git a/doc/html/md__r_e_a_d_m_e.html b/doc/html/md__r_e_a_d_m_e.html new file mode 100644 index 0000000..59ddc3d --- /dev/null +++ b/doc/html/md__r_e_a_d_m_e.html @@ -0,0 +1,125 @@ + + +
+ + + + +
+ + + + + + + + + + + + +
+