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

merged master to imu_feature

Browse Source
This commit is contained in:
matlabbe
2019-05-31 15:39:49 -04:00
parent e887d462ce cd6e51a968
commit d5d00fbd7d
5 changed files with 22 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/DBDriver.cpp
View File

@@ -741,14 +741,15 @@ bool DBDriver::getNodeInfo(
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
pose = _trashSignatures.at(signatureId)->getPose();
pose = _trashSignatures.at(signatureId)->getPose().clone();
mapId = _trashSignatures.at(signatureId)->mapId();
weight = _trashSignatures.at(signatureId)->getWeight();
label = _trashSignatures.at(signatureId)->getLabel();
label = std::string(_trashSignatures.at(signatureId)->getLabel());
stamp = _trashSignatures.at(signatureId)->getStamp();
groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
gps = _trashSignatures.at(signatureId)->sensorData().gps();
sensors = _trashSignatures.at(signatureId)->sensorData().envSensors();
groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose().clone();
velocity = std::vector<float>(_trashSignatures.at(signatureId)->getVelocity());
gps = GPS(_trashSignatures.at(signatureId)->sensorData().gps());
sensors = EnvSensors(_trashSignatures.at(signatureId)->sensorData().envSensors());
found = true;
}
_trashesMutex.unlock();

12
corelib/src/Memory.cpp
View File

@@ -3764,15 +3764,15 @@ bool Memory::getNodeInfo(int signatureId,
const Signature * s = this->getSignature(signatureId);
if(s)
{
odomPose = s->getPose();
odomPose = s->getPose().clone();
mapId = s->mapId();
weight = s->getWeight();
label = s->getLabel();
label = std::string(s->getLabel());
stamp = s->getStamp();
groundTruth = s->getGroundTruthPose();
velocity = s->getVelocity();
gps = s->sensorData().gps();
sensors = s->sensorData().envSensors();
groundTruth = s->getGroundTruthPose().clone();
velocity = std::vector<float>(s->getVelocity());
gps = GPS(s->sensorData().gps());
sensors = EnvSensors(s->sensorData().envSensors());
return true;
}
else if(lookInDatabase && _dbDriver)

8
corelib/src/RegistrationVis.cpp
View File

@@ -621,11 +621,13 @@ Transform RegistrationVis::computeTransformationImpl(
}
kptsFrom3D = detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
if(detectorFrom->getMinDepth() > 0.0f || detectorFrom->getMaxDepth() > 0.0f)
if(!kptsFrom3D.empty() && (detectorFrom->getMinDepth() > 0.0f || detectorFrom->getMaxDepth() > 0.0f))
{
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
kptsFrom3D.size() == kptsFrom.size());
UASSERT_MSG((int)kptsFrom.size() == descriptorsFrom.rows &&
kptsFrom3D.size() == kptsFrom.size(),
uFormat("kptsFrom=%d descriptorsFrom=%d kptsFrom3D=%d",
(int)kptsFrom.size(), descriptorsFrom.rows, (int)kptsFrom3D.size()).c_str());
std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());

2
corelib/src/Rtabmap.cpp
View File

@@ -4263,7 +4263,7 @@ void Rtabmap::get3DMap(
signatures.at(*iter).setWords(words);
signatures.at(*iter).setWords3(words3);
signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
if(!velocity.empty())
if(velocity.size()==6)
{
signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}

7
corelib/src/VWDictionary.cpp
View File

@@ -526,6 +526,7 @@ void VWDictionary::update()
UTimer timer;
timer.start();
int dim = _visualWords.begin()->second->getDescriptor().cols;
int type;
if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
{
@@ -533,6 +534,7 @@ void VWDictionary::update()
if(_strategy == kNNFlannKdTree)
{
type = CV_32F;
dim *= 8;
}
else
{
@@ -543,7 +545,6 @@ void VWDictionary::update()
{
type = _visualWords.begin()->second->getDescriptor().type();
}
int dim = _visualWords.begin()->second->getDescriptor().cols;
UASSERT(type == CV_32F || type == CV_8U);
UASSERT(dim > 0);
@@ -570,8 +571,8 @@ void VWDictionary::update()
descriptor = iter->second->getDescriptor();
}
UASSERT(descriptor.cols == dim);
UASSERT(descriptor.type() == type);
UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
descriptor.copyTo(_dataTree.row(i));
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));