i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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(); _trashesMutex.lock();
if(uContains(_trashSignatures, signatureId)) if(uContains(_trashSignatures, signatureId))
{ {
pose = _trashSignatures.at(signatureId)->getPose(); pose = _trashSignatures.at(signatureId)->getPose().clone();
mapId = _trashSignatures.at(signatureId)->mapId(); mapId = _trashSignatures.at(signatureId)->mapId();
weight = _trashSignatures.at(signatureId)->getWeight(); weight = _trashSignatures.at(signatureId)->getWeight();
label = _trashSignatures.at(signatureId)->getLabel(); label = std::string(_trashSignatures.at(signatureId)->getLabel());
stamp = _trashSignatures.at(signatureId)->getStamp(); stamp = _trashSignatures.at(signatureId)->getStamp();
groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose(); groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose().clone();
gps = _trashSignatures.at(signatureId)->sensorData().gps(); velocity = std::vector<float>(_trashSignatures.at(signatureId)->getVelocity());
sensors = _trashSignatures.at(signatureId)->sensorData().envSensors(); gps = GPS(_trashSignatures.at(signatureId)->sensorData().gps());
sensors = EnvSensors(_trashSignatures.at(signatureId)->sensorData().envSensors());
found = true; found = true;
} }
_trashesMutex.unlock(); _trashesMutex.unlock();

12
corelib/src/Memory.cpp
View File

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

8
corelib/src/RegistrationVis.cpp
View File

@@ -621,11 +621,13 @@ Transform RegistrationVis::computeTransformationImpl(
} }
kptsFrom3D = detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom); kptsFrom3D = detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size()); 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 //remove all keypoints/descriptors with no valid 3D points
UASSERT((int)kptsFrom.size() == descriptorsFrom.rows && UASSERT_MSG((int)kptsFrom.size() == descriptorsFrom.rows &&
kptsFrom3D.size() == kptsFrom.size()); 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::KeyPoint> validKeypoints(kptsFrom.size());
std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size()); std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type()); 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).setWords(words);
signatures.at(*iter).setWords3(words3); signatures.at(*iter).setWords3(words3);
signatures.at(*iter).setWordsDescriptors(wordsDescriptors); 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]); 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; UTimer timer;
timer.start(); timer.start();
int dim = _visualWords.begin()->second->getDescriptor().cols;
int type; int type;
if(_visualWords.begin()->second->getDescriptor().type() == CV_8U) if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
{ {
@@ -533,6 +534,7 @@ void VWDictionary::update()
if(_strategy == kNNFlannKdTree) if(_strategy == kNNFlannKdTree)
{ {
type = CV_32F; type = CV_32F;
dim *= 8;
} }
else else
{ {
@@ -543,7 +545,6 @@ void VWDictionary::update()
{ {
type = _visualWords.begin()->second->getDescriptor().type(); type = _visualWords.begin()->second->getDescriptor().type();
} }
int dim = _visualWords.begin()->second->getDescriptor().cols;
UASSERT(type == CV_32F || type == CV_8U); UASSERT(type == CV_32F || type == CV_8U);
UASSERT(dim > 0); UASSERT(dim > 0);
@@ -570,8 +571,8 @@ void VWDictionary::update()
descriptor = iter->second->getDescriptor(); descriptor = iter->second->getDescriptor();
} }
UASSERT(descriptor.cols == dim); UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
UASSERT(descriptor.type() == type); UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
descriptor.copyTo(_dataTree.row(i)); descriptor.copyTo(_dataTree.row(i));
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id())); _mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));