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

Fixed missing 3D keypoints when RGBD/LoopClosureReextractFeatures=true and Reg/Strategy=1 (https://github.com/introlab/rtabmap_ros/issues/668). DBViewer: fixed wrong poses optimization with GTSAM when showing scans of loop closures by proximity by space (multiscan) while there are GPS priors.

Browse Source
This commit is contained in:
matlabbe
2021-12-05 17:36:20 -05:00
parent 12906f4490
commit 580e35afb1
6 changed files with 43 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Graph.h
View File

@@ -133,7 +133,8 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays = true);
bool checkBothWays = true,
Link::Type type = Link::kUndef);
std::multimap<int, int>::iterator RTABMAP_EXP findLink(
std::multimap<int, int> & links,
int from,

7
corelib/src/Graph.cpp
View File

@@ -989,12 +989,13 @@ std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays)
bool checkBothWays,
Link::Type type)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}
@@ -1007,7 +1008,7 @@ std::multimap<int, Link>::iterator findLink(
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}

16
corelib/src/Memory.cpp
View File

@@ -2732,13 +2732,13 @@ Transform Memory::computeTransform(
// make sure we have all data needed
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && fromS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
(_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
{
fromS.sensorData() = getNodeData(fromS.id(), true, true, true, true);
}
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
if(((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) && toS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
(_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
{
@@ -2748,27 +2748,27 @@ Transform Memory::computeTransform(
cv::Mat imgBuf, depthBuf, userBuf;
LaserScan laserBuf;
fromS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
toS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&imgBuf:0,
(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
// compute transform fromId -> toId
std::vector<int> inliersV;
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
if((_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull())) ||
(fromS.getWords().size() && toS.getWords().size()) ||
(!guess.isNull() && !_registrationPipeline->isImageRequired()))
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
{
UDEBUG("");
tmpFrom.removeAllWords();

2
corelib/src/Signature.cpp
View File

@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
}
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());

25
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -106,29 +106,24 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::NonlinearFactorGraph graph;
// detect if there is a global pose prior set, if so remove rootId
bool gpsPriorOnly = false;
bool hasPriorPoses = false;
bool hasGPSPrior = false;
if(!priorsIgnored())
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
{
hasPriorPoses = true;
hasGPSPrior = true;
if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0))
{
// orientation is set, don't set root prior
gpsPriorOnly = false;
// orientation is set, don't set root prior (it is no GPS)
rootId = 0;
hasGPSPrior = false;
break;
}
else if(gravitySigma()<=0)
{
gpsPriorOnly = true;
}
}
}
}
@@ -138,25 +133,25 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
UASSERT(uContains(poses, rootId));
const Transform & initialPose = poses.at(rootId);
UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false");
if(isSlam2d())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits<double>::min()));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
}
else
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) <<
1e-2, 1e-2, hasPriorPoses?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
(gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2 // xyz
1e-2, 1e-2, hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
}
}
UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d gpsPriorOnly=%d landmarksIgnored=%d)",
rootId, priorsIgnored()?1:0, gpsPriorOnly?1:0, landmarksIgnored()?1:0);
UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
gtsam::Values initialEstimate;
std::map<int, bool> isLandmarkWithRotation;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)