Skip to content

Commit 42faef3

Browse files
matlabbematlabbe
authored
DBReader: added ignore imu option, GUI: added tf overrides option (#1637)
* DBReader: added ignore imu option, GUI: added tf overrides option * transform offset
1 parent 7db1453 commit 42faef3

16 files changed

Lines changed: 251 additions & 87 deletions

‎corelib/include/rtabmap/core/DBReader.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
5959
int startMapId = 0,
6060
int stopMapId = -1,
6161
bool priorsIgnored = false,
62+
bool imuIgnored = false,
6263
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
6364
DBReader(const std::list<std::string> & databasePaths,
6465
float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -74,6 +75,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
7475
int startMapId = 0,
7576
int stopMapId = -1,
7677
bool priorsIgnored = false,
78+
bool imuIgnored = false,
7779
const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
7880
virtual ~DBReader();
7981

@@ -107,6 +109,7 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera {
107109
bool _landmarksIgnored;
108110
bool _featuresIgnored;
109111
bool _priorsIgnored;
112+
bool _imuIgnored;
110113
int _startMapId;
111114
int _stopMapId;
112115
std::vector<Transform> _cameraLocalTransformOverrides;

‎corelib/include/rtabmap/core/RegistrationInfo.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@ class RegistrationInfo
4141
inliersMeanDistance(0.0f),
4242
inliersDistribution(0.0f),
4343
matches(0),
44+
variance(0.0f),
4445
icpInliersRatio(0),
4546
icpTranslation(0.0f),
4647
icpRotation(0.0f),
@@ -64,6 +65,7 @@ class RegistrationInfo
6465
output.inliersDistribution = inliersDistribution;
6566
output.matches = matches;
6667
output.matchesPerCam = matchesPerCam;
68+
output.variance = variance;
6769
output.icpInliersRatio = icpInliersRatio;
6870
output.icpTranslation = icpTranslation;
6971
output.icpRotation = icpRotation;
@@ -85,6 +87,7 @@ class RegistrationInfo
8587
float inliersDistribution;
8688
std::vector<int> inliersIDs;
8789
int matches;
90+
float variance;
8891
std::vector<int> matchesIDs;
8992
std::vector<int> projectedIDs; // "From" IDs
9093
std::vector<int> inliersPerCam;

‎corelib/include/rtabmap/core/Statistics.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@ class RTABMAP_CORE_EXPORT Statistics
6767
RTABMAP_STATS(Loop, Visual_inliers,);
6868
RTABMAP_STATS(Loop, Visual_inliers_ratio,);
6969
RTABMAP_STATS(Loop, Visual_matches,);
70+
RTABMAP_STATS(Loop, Visual_variance,);
7071
RTABMAP_STATS(Loop, Distance_since_last_loc, m);
7172
RTABMAP_STATS(Loop, Last_id,);
7273
RTABMAP_STATS(Loop, Optimization_max_error, m);

‎corelib/include/rtabmap/core/util3d_features.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ std::map<int, cv::Point3f> RTABMAP_CORE_EXPORT generateWords3DMono(
8080
Transform & cameraTransform,
8181
float ransacReprojThreshold = 3.0f,
8282
float ransacConfidence = 0.99f,
83+
int varianceMedianRatio = 4,
8384
const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
8485
double * variance = 0,
8586
std::vector<int> * matchesOut = 0);

‎corelib/src/DBReader.cpp‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,7 @@ DBReader::DBReader(const std::string & databasePath,
5656
int startMapId,
5757
int stopMapId,
5858
bool priorsIgnored,
59+
bool imuIgnored,
5960
const std::vector<Transform> & cameraLocalTransformOverrides) :
6061
Camera(frameRate),
6162
_paths(uSplit(databasePath, ';')),
@@ -69,6 +70,7 @@ DBReader::DBReader(const std::string & databasePath,
6970
_landmarksIgnored(landmarksIgnored),
7071
_featuresIgnored(featuresIgnored),
7172
_priorsIgnored(priorsIgnored),
73+
_imuIgnored(imuIgnored),
7274
_startMapId(startMapId),
7375
_stopMapId(stopMapId),
7476
_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -96,6 +98,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
9698
int startMapId,
9799
int stopMapId,
98100
bool priorsIgnored,
101+
bool imuIgnored,
99102
const std::vector<Transform> & cameraLocalTransformOverrides) :
100103
Camera(frameRate),
101104
_paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
109112
_landmarksIgnored(landmarksIgnored),
110113
_featuresIgnored(featuresIgnored),
111114
_priorsIgnored(priorsIgnored),
115+
_imuIgnored(imuIgnored),
112116
_startMapId(startMapId),
113117
_stopMapId(stopMapId),
114118
_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -463,14 +467,17 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
463467
}
464468

465469
Transform gravityTransform;
466-
std::multimap<int, Link> gravityLinks;
467-
_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
468-
if( gravityLinks.size() &&
469-
!gravityLinks.begin()->second.transform().isNull() &&
470-
gravityLinks.begin()->second.infMatrix().cols == 6 &&
471-
gravityLinks.begin()->second.infMatrix().rows == 6)
470+
if(!_imuIgnored)
472471
{
473-
gravityTransform = gravityLinks.begin()->second.transform();
472+
std::multimap<int, Link> gravityLinks;
473+
_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
474+
if( gravityLinks.size() &&
475+
!gravityLinks.begin()->second.transform().isNull() &&
476+
gravityLinks.begin()->second.infMatrix().cols == 6 &&
477+
gravityLinks.begin()->second.infMatrix().rows == 6)
478+
{
479+
gravityTransform = gravityLinks.begin()->second.transform();
480+
}
474481
}
475482

476483
Landmarks landmarks;

‎corelib/src/Features2d.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -303,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
303303
cv::Mat descriptorsTmp;
304304
if(ssc)
305305
{
306-
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
306+
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
307307

308308
// Sorting keypoints by deacreasing order of strength
309309
std::vector<float> responseVector;
@@ -419,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
419419
inliers.resize(keypoints.size(), false);
420420
if(ssc)
421421
{
422-
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
422+
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
423423

424424
// Sorting keypoints by deacreasing order of strength
425425
std::vector<float> responseVector;

‎corelib/src/Memory.cpp‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5836,7 +5836,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
58365836
cameraModels.size() == 1 &&
58375837
words.size() &&
58385838
(words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
5839-
_registrationPipeline->isImageRequired() &&
58405839
_signatures.size() &&
58415840
_signatures.rbegin()->second->mapId() == _idMapCount) // same map
58425841
{
@@ -5880,11 +5879,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
58805879

58815880
// The following is used only to re-estimate the correspondences, the returned transform is ignored
58825881
Transform tmpt;
5883-
RegistrationVis reg(parameters_);
5882+
ParametersMap tmpParams = parameters_;
5883+
// Pure 2D-2D without guess would generate variance=1
5884+
uInsert(tmpParams, ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1"));
5885+
RegistrationVis reg(tmpParams);
58845886
if(_registrationPipeline->isScanRequired())
58855887
{
58865888
// If icp is used, remove it to just do visual registration
5887-
RegistrationVis vis(parameters_);
5889+
RegistrationVis vis(tmpParams);
58885890
tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
58895891
}
58905892
else
@@ -5906,11 +5908,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
59065908
{
59075909
previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
59085910
}
5911+
float reprojError = Parameters::defaultVisPnPReprojError();
5912+
int varianceMedianRatio = Parameters::defaultVisPnPVarianceMedianRatio();
5913+
Parameters::parse(parameters_, Parameters::kVisPnPReprojError(), reprojError);
5914+
Parameters::parse(parameters_, Parameters::kVisPnPVarianceMedianRatio(), varianceMedianRatio);
59095915
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
59105916
currentWords,
59115917
previousWords,
59125918
cameraModels[0],
5913-
cameraTransform);
5919+
cameraTransform,
5920+
reprojError,
5921+
0.99f,
5922+
varianceMedianRatio);
59145923

59155924
UDEBUG("inliers=%d", (int)inliers.size());
59165925

‎corelib/src/RegistrationIcp.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -529,6 +529,7 @@ Transform RegistrationIcp::computeTransformationImpl(
529529
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
530530
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
531531
info.icpStructuralComplexity = complexity;
532+
UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
532533
if(complexity < _pointToPlaneMinComplexity)
533534
{
534535
tooLowComplexityForPlaneToPlane = true;

‎corelib/src/RegistrationVis.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -321,6 +321,7 @@ Transform RegistrationVis::computeTransformationImpl(
321321
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
322322
UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
323323
UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
324+
UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
324325
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
325326
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
326327
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1632,6 +1633,7 @@ Transform RegistrationVis::computeTransformationImpl(
16321633
cameraTransform,
16331634
_PnPReprojError,
16341635
0.99f,
1636+
_PnPVarMedianRatio,
16351637
words3A, // for scale estimation
16361638
&variance,
16371639
&matchesV);
@@ -2204,6 +2206,10 @@ Transform RegistrationVis::computeTransformationImpl(
22042206
info.matches = matchesCount;
22052207
info.rejectedMsg = msg;
22062208
info.covariance = covariance;
2209+
if(!covariance.empty())
2210+
{
2211+
info.variance = covariance.at<double>(0,0);
2212+
}
22072213

22082214
UDEBUG("inliers=%d/%d", info.inliers, info.matches);
22092215
UDEBUG("transform=%s", transform.prettyPrint().c_str());

‎corelib/src/Rtabmap.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2613,6 +2613,7 @@ bool Rtabmap::process(
26132613
int loopClosureVisualInliers = 0; // for statistics
26142614
float loopClosureVisualInliersRatio = 0.0f;
26152615
int loopClosureVisualMatches = 0;
2616+
float loopClosureVisualVariance = 0.0f;
26162617
float loopClosureLinearVariance = 0.0f;
26172618
float loopClosureAngularVariance = 0.0f;
26182619
float loopClosureVisualInliersMeanDist = 0;
@@ -2795,6 +2796,7 @@ bool Rtabmap::process(
27952796
loopClosureVisualInliers = info.inliers;
27962797
loopClosureVisualInliersRatio = info.inliersRatio;
27972798
loopClosureVisualMatches = info.matches;
2799+
loopClosureVisualVariance = info.variance;
27982800

27992801
cv::Mat information = getInformation(info.covariance);
28002802
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -3062,6 +3064,7 @@ bool Rtabmap::process(
30623064
loopClosureVisualInliers = info.inliers;
30633065
loopClosureVisualInliersRatio = info.inliersRatio;
30643066
loopClosureVisualMatches = info.matches;
3067+
loopClosureVisualVariance = info.variance;
30653068
rejectedLoopClosure = transform.isNull();
30663069
if(rejectedLoopClosure)
30673070
{
@@ -4049,6 +4052,7 @@ bool Rtabmap::process(
40494052
statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
40504053
statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
40514054
statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
4055+
statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance);
40524056
statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
40534057
statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
40544058
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());

0 commit comments

Comments
 (0)