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

Removed parameter "OdomF2M/FixedMapPath". GUI: added odometry disabled option. Rtabmap can now localize even if odometry input is null (only in localization mode).

Browse Source
This commit is contained in:
matlabbe
2016-11-27 14:30:11 -05:00
parent 693f623e5e
commit 121446d648
15 changed files with 695 additions and 763 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
corelib/src/Memory.cpp
View File

@@ -3456,52 +3456,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(!pose.isNull() &&
data.cameraModels().size() == 1 &&
words.size() &&
words3D.size() == 0)
words3D.size() == 0 &&
_signatures.size() &&
_signatures.rbegin()->second->mapId() == _idMapCount) // same map
{
bool fillWithNaN = true;
if(_signatures.size())
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
fillWithNaN = false;
}
}
if(fillWithNaN)
{
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
}
}

101
corelib/src/Odometry.cpp
View File

@@ -206,56 +206,63 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
// Ground alignment
if(_pose.isIdentity() && _alignWithGround)
{
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
cloud = util3d::voxelize(cloud, indices, 0.01);
bool success = false;
if(cloud->size())
if(data.depthOrRightRaw().empty())
{
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
if(ground->size())
{
pcl::ModelCoefficients coefficients;
util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
if(coefficients.values.at(3) >= 0)
{
UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
else
{
UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
_pose *= rotation;
success = true;
}
UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
}
if(!success)
else
{
UERROR("Odometry failed to detect the ground. You have this "
"error because parameter \"Odom/AlignWithGround\" is true. "
"Make sure the camera is seeing the ground (e.g., tilt ~30 "
"degrees toward the ground).");
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
cloud = util3d::voxelize(cloud, indices, 0.01);
bool success = false;
if(cloud->size())
{
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
if(ground->size())
{
pcl::ModelCoefficients coefficients;
util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
if(coefficients.values.at(3) >= 0)
{
UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
else
{
UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
coefficients.values.at(0),
coefficients.values.at(1),
coefficients.values.at(2),
coefficients.values.at(3),
alignTimer.ticks());
}
Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
_pose *= rotation;
success = true;
}
}
if(!success)
{
UERROR("Odometry failed to detect the ground. You have this "
"error because parameter \"%s\" is true. "
"Make sure the camera is seeing the ground (e.g., tilt ~30 "
"degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
}
}
}

747
corelib/src/OdometryF2M.cpp
View File

@@ -62,7 +62,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
regPipeline_(Registration::create(parameters)),
@@ -76,7 +75,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleAdjustmentMaxFrames_);
bundleParameters_ = parameters;
@@ -84,90 +82,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
if(!fixedMapPath_.empty())
{
UINFO("Init odometry from a fixed database: \"%s\"", fixedMapPath_.c_str());
// init the local map with a all 3D features contained in the database
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
Memory memory(customParameters);
if(!memory.init(fixedMapPath_, false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
else if(memory.getLastWorkingSignature())
{
// get the graph
std::map<int, int> ids = memory.getNeighborsId(memory.getLastWorkingSignature()->id(), 0, -1);
UDEBUG("ids=%d", (int)ids.size());
std::map<int, Transform> poses;
std::multimap<int, Link> links;
memory.getMetricConstraints(uKeysSet(ids), poses, links, true);
if(poses.size())
{
//optimize the graph
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
delete optimizer;
UDEBUG("optimizedPoses=%d", (int)optimizedPoses.size());
std::multimap<int, cv::Point3f> words3D;
std::multimap<int, cv::Mat> wordsDescriptors;
// fill the local map
for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
posesIter!=optimizedPoses.end();
++posesIter)
{
const Signature * s = memory.getSignature(posesIter->first);
if(s)
{
UDEBUG("%d has %d words", posesIter->first, (int)s->getWords3().size());
// Transform 3D points accordingly to pose and add them to local map
for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=s->getWords3().begin();
pointsIter!=s->getWords3().end();
++pointsIter)
{
if(!uContains(words3D, pointsIter->first))
{
words3D.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
if(s->getWordsDescriptors().size() == s->getWords3().size())
{
UASSERT(uContains(s->getWordsDescriptors(), pointsIter->first));
wordsDescriptors.insert(std::make_pair(pointsIter->first, s->getWordsDescriptors().find(pointsIter->first)->second));
}
else // load descriptor from dictionary
{
UASSERT(memory.getVWDictionary()->getWord(pointsIter->first) != 0);
wordsDescriptors.insert(std::make_pair(pointsIter->first, memory.getVWDictionary()->getWord(pointsIter->first)->getDescriptor()));
}
}
}
}
}
UASSERT(words3D.size() == wordsDescriptors.size());
UDEBUG("words3D=%d", (int)words3D.size());
map_->setWords3(words3D);
map_->setWordsDescriptors(wordsDescriptors);
}
else
{
UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
}
}
if((int)map_->getWords3().size() < regPipeline_->getMinVisualCorrespondences() || map_->getWords3().size() == 0)
{
// TODO: support geometric-only maps?
UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
fixedMapPath_.c_str(), (int)map_->getWords3().size());
}
}
}
OdometryF2M::~OdometryF2M()
@@ -188,15 +102,7 @@ void OdometryF2M::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
if(fixedMapPath_.empty())
{
*map_ = Signature(-1);
}
else
{
UWARN("Odometry cannot be reset when a fixed local map is set.");
}
*map_ = Signature(-1);
}
// return not null transform if odometry is correctly computed
@@ -358,300 +264,293 @@ Transform OdometryF2M::computeTransform(
{
output = transform;
if(fixedMapPath_.empty())
bool modified = false;
Transform newFramePose = this->getPose()*output;
// fields to update
cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
//Visual
int added = 0;
int removed = 0;
UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
if(regPipeline_->isImageRequired() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
{
bool modified = false;
Transform newFramePose = this->getPose()*output;
UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
// fields to update
cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
// update local map
UASSERT(mapWords.size() == mapPoints.size());
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
//Visual
int added = 0;
int removed = 0;
UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
if(regPipeline_->isImageRequired() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
if(bundleAdjustment_>0)
{
UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
// update local map
UASSERT(mapWords.size() == mapPoints.size());
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
if(bundleAdjustment_>0)
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapPoints.count(iter->first) == 1);
mapPoints.find(iter->first)->second = iter->second;
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
UASSERT(mapPoints.count(iter->first) == 1);
mapPoints.find(iter->first)->second = iter->second;
}
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
{
if(util3d::isFinite(iter->second))
{
if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
{
newIds.insert(
std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
std::make_pair(iter->first,
std::make_pair(iter2D->second,
std::make_pair(iter->second, iterDesc->second)))));
}
else if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
}
}
}
}
}
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
iter!=newIds.end();
++iter)
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->second.first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
}
}
}
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
++added;
}
}
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
{
if(matches.find(iter->first) == matches.end())
{
std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
{
if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterKp->first) -= 1;
if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
{
bundlePoses_.erase(iterKp->first);
bundleLinks_.erase(iterKp->first);
bundleModels_.erase(iterKp->first);
bundlePoseReferences_.erase(iterKp->first);
UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
}
}
}
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(iter++);
mapDescriptors.erase(iterMapDescriptors++);
mapWords.erase(iterMapWords++);
++removed;
}
else
{
++iter;
++iterMapDescriptors;
++iterMapWords;
}
}
}
modified = true;
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
}
// Geometric
UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
if(regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
{
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
UTimer tmpTimer;
if(lastFrame_->sensorData().laserScanRaw().cols)
if(util3d::isFinite(iter->second))
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
{
frameCloudNormalsIndices = util3d::subtractFiltering(
frameCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
mapCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
scanSubtractRadius_,
0.0f);
newPoints = frameCloudNormalsIndices->size();
newIds.insert(
std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
std::make_pair(iter->first,
std::make_pair(iter2D->second,
std::make_pair(iter->second, iterDesc->second)))));
}
else
else if(bundleAdjustment_>0)
{
newPoints = mapCloudNormals->size();
}
if(newPoints)
{
scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
(int)scansBuffer_.size(),
int(mapCloudNormals->size()),
newPoints,
scanMaximumMapSize_);
if(newPoints < 20)
if(lastFrame_->getWords().count(iter->first) == 1)
{
UWARN("The number of new scan points added to local odometry "
"map is low (%d), you may want to decrease the parameter \"%s\" "
"(current value=%f and ICP inliers ratio is %f)",
newPoints,
Parameters::kOdomScanKeyFrameThr().c_str(),
scanKeyFrameThr_,
regInfo.icpInliersRatio);
}
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
if(scansBuffer_.size() > 1 &&
int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
{
//regenerate the local map
mapCloudNormals->clear();
std::list<int> toRemove;
int i = int(scansBuffer_.size())-1;
for(; i>=0; --i)
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
i == 0)
{
*mapCloudNormals += *scansBuffer_[i].first;
break;
}
else
{
if(scansBuffer_[i].second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_[i].first;
}
}
}
// remove old clouds
if(i > 0)
{
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
int oi = 0;
for(; i<(int)scansBuffer_.size(); ++i)
{
UASSERT(oi < (int)scansTmp.size());
scansTmp[oi++] = scansBuffer_[i];
}
scansBuffer_ = scansTmp;
}
}
else
{
// just append the last cloud
if(scansBuffer_.back().second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
*mapCloudNormals += tmp;
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
*mapCloudNormals += *scansBuffer_.back().first;
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
}
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
modified=true;
}
}
UDEBUG("Update local map = %fs", tmpTimer.ticks());
}
if(modified)
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
iter!=newIds.end();
++iter)
{
*map_ = tmpMap;
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->second.first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
map_->setWords(mapWords);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
}
}
}
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
++added;
}
}
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
{
if(matches.find(iter->first) == matches.end())
{
std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
{
if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterKp->first) -= 1;
if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
{
bundlePoses_.erase(iterKp->first);
bundleLinks_.erase(iterKp->first);
bundleModels_.erase(iterKp->first);
bundlePoseReferences_.erase(iterKp->first);
UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
}
}
}
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(iter++);
mapDescriptors.erase(iterMapDescriptors++);
mapWords.erase(iterMapWords++);
++removed;
}
else
{
++iter;
++iterMapDescriptors;
++iterMapWords;
}
}
}
modified = true;
}
else
// Geometric
UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
if(regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
{
// fixed local map, don't update with the new signature
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
UTimer tmpTimer;
if(lastFrame_->sensorData().laserScanRaw().cols)
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
{
frameCloudNormalsIndices = util3d::subtractFiltering(
frameCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
mapCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
scanSubtractRadius_,
0.0f);
newPoints = frameCloudNormalsIndices->size();
}
else
{
newPoints = mapCloudNormals->size();
}
if(newPoints)
{
scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
(int)scansBuffer_.size(),
int(mapCloudNormals->size()),
newPoints,
scanMaximumMapSize_);
if(newPoints < 20)
{
UWARN("The number of new scan points added to local odometry "
"map is low (%d), you may want to decrease the parameter \"%s\" "
"(current value=%f and ICP inliers ratio is %f)",
newPoints,
Parameters::kOdomScanKeyFrameThr().c_str(),
scanKeyFrameThr_,
regInfo.icpInliersRatio);
}
if(scansBuffer_.size() > 1 &&
int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
{
//regenerate the local map
mapCloudNormals->clear();
std::list<int> toRemove;
int i = int(scansBuffer_.size())-1;
for(; i>=0; --i)
{
int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
i == 0)
{
*mapCloudNormals += *scansBuffer_[i].first;
break;
}
else
{
if(scansBuffer_[i].second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_[i].first;
}
}
}
// remove old clouds
if(i > 0)
{
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
int oi = 0;
for(; i<(int)scansBuffer_.size(); ++i)
{
UASSERT(oi < (int)scansTmp.size());
scansTmp[oi++] = scansBuffer_[i];
}
scansBuffer_ = scansTmp;
}
}
else
{
// just append the last cloud
if(scansBuffer_.back().second->size())
{
pcl::PointCloud<pcl::PointNormal> tmp;
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_.back().first;
}
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
modified=true;
}
}
UDEBUG("Update local map = %fs", tmpTimer.ticks());
}
if(modified)
{
*map_ = tmpMap;
map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
map_->setWords(mapWords);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
}
}
@@ -690,72 +589,69 @@ Transform OdometryF2M::computeTransform(
if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
{
frameValid = true;
if (fixedMapPath_.empty())
// update local map
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter, ++descIter, ++wordsIter)
{
// update local map
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter, ++descIter, ++wordsIter)
if (util3d::isFinite(iter->second))
{
if (util3d::isFinite(iter->second))
{
words.insert(*wordsIter);
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
descriptors.insert(*descIter);
}
words.insert(*wordsIter);
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
descriptors.insert(*descIter);
}
if(bundleAdjustment_>0)
{
// update bundleWordReferences_: used for bundle adjustment
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
}
UASSERT(model.isValidForProjection());
UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
//origin
bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
bundleModels_.insert(std::make_pair(0, model));
}
map_->setWords(words);
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
}
if(bundleAdjustment_>0)
{
// update bundleWordReferences_: used for bundle adjustment
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
}
UASSERT(model.isValidForProjection());
UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
//origin
bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
bundleModels_.insert(std::make_pair(0, model));
}
map_->setWords(words);
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
}
else
{
@@ -767,12 +663,9 @@ Transform OdometryF2M::computeTransform(
if (lastFrame_->sensorData().laserScanRaw().cols)
{
frameValid = true;
if (fixedMapPath_.empty())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
}
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
}
else
{

4
corelib/src/OdometryThread.cpp
View File

@@ -109,7 +109,7 @@ void OdometryThread::mainLoop()
void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
{
@@ -119,7 +119,7 @@ void OdometryThread::addData(const SensorData & data)
}
else
{
// Mono and BOW can accept RGB only
// Mono can accept RGB only
if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");

5
corelib/src/Parameters.cpp
View File

@@ -226,6 +226,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// 0.11.12
removedParameters_.insert(std::make_pair("Optimizer/Slam2D", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("OdomF2M/FixedMapPath", std::make_pair(false, "")));
// 0.11.10 typos
removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected", std::make_pair(true, Parameters::kGridFlatObstacleDetected())));
@@ -242,13 +243,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// 0.11.2
removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
// 0.11.0
removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));

3
corelib/src/RegistrationVis.cpp
View File

@@ -236,6 +236,7 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.sensorData().descriptors().rows == 0 ||
fromSignature.getWordsDescriptors().size() == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() && toSignature.getWords3().empty())||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
@@ -601,7 +602,7 @@ Transform RegistrationVis::computeTransformationImpl(
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
}
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points

60
corelib/src/Rtabmap.cpp
View File

@@ -810,7 +810,7 @@ void Rtabmap::resetMemory()
//============================================================
bool Rtabmap::process(
const SensorData & data,
const Transform & odomPose,
Transform odomPose,
const cv::Mat & covariance)
{
UDEBUG("");
@@ -883,9 +883,20 @@ bool Rtabmap::process(
{
if(odomPose.isNull())
{
UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
"Image %d is ignored!", data.id());
return false;
if(_memory->isIncremental())
{
UERROR("RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. "
"Image %d is ignored!", data.id());
return false;
}
else // fake localization
{
if(_lastLocalizationPose.isNull())
{
_lastLocalizationPose = Transform::getIdentity();
}
odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
}
}
else if(_memory->isIncremental()) // only in mapping mode
{
@@ -1185,10 +1196,11 @@ bool Rtabmap::process(
}
// For proximity by time, correspondences should be already enough precise, so don't recompute them
Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info, true);
Transform transform = _memory->computeTransform(*iter, signature->id(), guess, &info, true);
if(!transform.isNull())
{
transform = transform.inverse();
UDEBUG("Add local loop closure in TIME (%d->%d) %s",
signature->id(),
*iter,
@@ -1740,7 +1752,7 @@ bool Rtabmap::process(
info.variance = 1.0f;
if(_rgbdSlamMode)
{
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info);
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
loopClosureVisualInliers = info.inliers;
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1748,6 +1760,10 @@ bool Rtabmap::process(
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
}
else
{
transform = transform.inverse();
}
}
if(!rejectedHypothesis)
{
@@ -1848,9 +1864,10 @@ bool Rtabmap::process(
++localVisualPathsChecked;
RegistrationInfo info;
Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
if(!transform.isNull())
{
transform = transform.inverse();
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
{
UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
@@ -2086,9 +2103,9 @@ bool Rtabmap::process(
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
Transform mapCorrectionInv = _mapCorrection.inverse();
Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
Transform up = u * oldPose.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
@@ -2197,6 +2214,7 @@ bool Rtabmap::process(
}
// Update map correction, it should be identify when optimizing from the last node
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
@@ -2333,6 +2351,11 @@ bool Rtabmap::process(
}
Signature lastSignatureData(signature->id());
Transform lastSignatureOptimizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
{
lastSignatureOptimizedPose = _optimizedPoses.at(signature->id());
}
if(_publishLastSignatureData)
{
lastSignatureData = *signature;
@@ -2537,28 +2560,29 @@ bool Rtabmap::process(
UDEBUG("Get all node infos...");
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
Transform odomPoseLocal;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
Transform groundTruth;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, false);
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
weight,
stamp,
label,
odomPose,
odomPoseLocal,
groundTruth)));
}
localGraphSize = (int)poses.size();
poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureOptimizedPose)); // in case we are in localization
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
statistics_.setSignatures(signatures);
statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
localGraphSize = (int)poses.size();
UDEBUG("");
}
@@ -3210,13 +3234,13 @@ void Rtabmap::get3DMap(
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
Transform odomPose;
Transform odomPoseLocal;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
Transform groundTruth;
_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, groundTruth, true);
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
@@ -3229,7 +3253,7 @@ void Rtabmap::get3DMap(
weight,
stamp,
label,
odomPose,
odomPoseLocal,
groundTruth,
data)));
signatures.at(*iter).setWords(words);
@@ -3280,20 +3304,20 @@ void Rtabmap::getGraph(
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
Transform odomPoseLocal;
int weight = -1;
int mapId = -1;
std::string label;
double stamp = 0;
Transform groundTruth;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, global);
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
weight,
stamp,
label,
odomPose,
odomPoseLocal,
groundTruth)));
std::multimap<int, cv::KeyPoint> words;

11
corelib/src/RtabmapThread.cpp
View File

@@ -336,7 +336,7 @@ void RtabmapThread::handleEvent(UEvent* event)
{
if (_rtabmap->isRGBDMode())
{
if (!e->info().odomPose.isNull())
if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
{
this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
}
@@ -347,7 +347,7 @@ void RtabmapThread::handleEvent(UEvent* event)
}
else
{
this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
}
}
@@ -356,7 +356,7 @@ void RtabmapThread::handleEvent(UEvent* event)
{
UDEBUG("OdometryEvent");
OdometryEvent * e = (OdometryEvent*)event;
if(!e->pose().isNull())
if(!e->pose().isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
{
this->addData(*e);
}
@@ -630,7 +630,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
_transVariance = 0;
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
if(_rate > 0.0f)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
}
_dataBuffer.pop_front();
notify = false;
}