i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated rehearsal behavior when there are intermediate nodes and RGBD/LinearUpdate,RGBD/AngularUpdate are set.

Browse Source
This commit is contained in:
matlabbe
2015-08-28 23:05:45 -04:00
parent ce2bbd8feb
commit a2b9c9f9a0
8 changed files with 168 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/include/rtabmap/core/Link.h
View File

@@ -118,25 +118,29 @@ public:
infMatrix_.at<double>(5,5) = 1.0/rotVariance;
}
Link merge(const Link & link) const
Link merge(const Link & link, Type outputType) const
{
UASSERT(to_ == link.from());
UASSERT(type_ == link.type());
UASSERT(!transform_.isNull());
UASSERT(!link.transform().isNull());
UASSERT(outputType != Link::kUndef);
UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
return Link(
from_,
link.to(),
type_,
transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
infMatrix_ + link.infMatrix());
outputType,
transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
}
Link inverse() const
{
return Link(to_, from_, type_, transform_.inverse(), infMatrix_);
return Link(
to_,
from_,
type_,
transform_.isNull()?Transform():transform_.inverse(),
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
}
private:

4
corelib/include/rtabmap/core/Parameters.h
View File

@@ -286,8 +286,8 @@ class RTABMAP_EXP Parameters
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");

1
corelib/include/rtabmap/core/Statistics.h
View File

@@ -83,6 +83,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Signatures_retrieved,);
RTABMAP_STATS(Memory, Images_buffered,);
RTABMAP_STATS(Memory, Rehearsal_sim,);
RTABMAP_STATS(Memory, Rehearsal_id,);
RTABMAP_STATS(Memory, Rehearsal_merged,);
RTABMAP_STATS(Memory, Local_graph_size,);