i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-13 05:19:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdomF2M: Scan gravity optimization

Browse Source
This commit is contained in:
matlabbe
2026-10-10 23:41:39 -07:00
parent 9e27446236
commit c036e03b3e
6 changed files with 249 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -529,6 +529,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
RTABMAP_PARAM(OdomF2M, ScanGravity, bool, false, uFormat("[Geometry] With an IMU, keep the local scan map aligned with gravity: the scan keyframes of the local map are kept in a local graph, with their registration links and a gravity link from the IMU orientation at each of them (weighted by \"%s\"), optimized when a keyframe is added (the oldest one fixed). The map is then reassembled from the keyframes' clouds at their optimized poses, so roll and pitch don't drift. Requires g2o or GTSAM.", kOptimizerGravitySigma().c_str()));
RTABMAP_PARAM(OdomF2M, ScanRange, float, 0, "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
RTABMAP_PARAM(OdomF2M, ValidDepthRatio, float, 0.75, "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
corelib/include/rtabmap/core/odometry/OdometryF2M.h
+16 -1
View File
@@ -82,7 +82,22 @@ private:
Signature * map_;
Signature * lastFrame_;
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
// The scan keyframes the local scan map is assembled from
struct ScanKeyFrame
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud; // in odometry frame, at pose
pcl::IndicesPtr indices; // points kept in the map (not subtracted), all if empty
int id = 0;
Transform pose; // odometry pose of the keyframe
Transform gravity; // IMU orientation at the keyframe (null if not available)
Link link; // registration from the previous keyframe (not set on the first)
};
std::vector<ScanKeyFrame> scansBuffer_;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembleScanMap() const;
bool alignScanKeyFramesWithGravity();
bool scanGravity_;
Optimizer * scanOptimizer_;
int scanKeyFrameSeq_;
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
std::map<int, Transform> bundlePoses_;