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

Refactoring: Renamed rtabmap::Image to rtabmap::SensorData

Browse Source 
removed keypoints and descriptors stuff from CameraEvent and Image/SensorData
removed keypoints3 from Image/SensorData

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1653 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-18 23:14:04 +00:00
parent 9aca634b28
commit e460cb592f
34 changed files with 499 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -26,7 +26,7 @@ SET(SRC_FILES
util3d.cpp
Odometry.cpp
Image.cpp
SensorData.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp

2
corelib/src/CameraThread.cpp
View File

@@ -104,8 +104,6 @@ void CameraThread::mainLoop()
{
UTimer timer;
UDEBUG("");
cv::Mat descriptors;
std::vector<cv::KeyPoint> keypoints;
cv::Mat rgb, depth;
float fx = 0.0f;
float fy = 0.0f;

2
corelib/src/DBReader.cpp
View File

@@ -145,7 +145,7 @@ void DBReader::mainLoop()
{
if(!_odometryIgnored)
{
Image data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
this->post(new OdometryEvent(data));
if(pose.isNull())
{

113
corelib/src/Features2d.cpp
View File

@@ -109,66 +109,6 @@ void filterKeypointsByDepth(
}
}
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
float maxDepth)
{
cv::Mat descriptors;
filterKeypointsByDepth(keypoints, keypoints3, descriptors, maxDepth);
}
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & keypoints3,
cv::Mat & descriptors,
float maxDepth)
{
UASSERT(keypoints.size() == keypoints3.size());
UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
if(keypoints.size())
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<cv::Point3f> output3(keypoints3.size());
std::vector<int> indexes(keypoints.size(), 0);
int oi=0;
for(unsigned int i=0; i<keypoints.size(); ++i)
{
if(uIsFinite(keypoints3[i].z) && keypoints3[i].z < maxDepth)
{
output3[oi] = keypoints3[i];
output[oi++] = keypoints[i];
indexes[i] = 1;
}
}
output.resize(oi);
output3.resize(oi);
keypoints = output;
keypoints3 = output3;
if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
{
if(keypoints.size() == 0)
{
descriptors = cv::Mat();
}
else
{
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
int di = 0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
if(indexes[i] == 1)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
}
}
descriptors = newDescriptors;
}
}
}
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
{
cv::Mat descriptors;
@@ -219,59 +159,6 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
}
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints)
{
cv::Mat descriptors;
limitKeypoints(keypoints, keypoints3, descriptors, maxKeypoints);
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints)
{
UASSERT(((int)keypoints.size() == descriptors.rows && keypoints.size() == keypoints3.size()) || descriptors.rows == 0);
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Remove them from the signature
int removed = hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
std::vector<cv::Point3f> kpts3Tmp(maxKeypoints);
cv::Mat descriptorsTmp;
if(descriptors.rows)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
kpts3Tmp[k] = keypoints3[iter->second];
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
keypoints3 = kpts3Tmp;
if(descriptors.rows)
{
descriptors = descriptorsTmp;
}
}
}
cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
if(!image.empty() && roiRatios.size() == 4)

254
corelib/src/Memory.cpp
View File

@@ -429,7 +429,7 @@ void Memory::preUpdate()
}
}
bool Memory::update(const Image & image, Statistics * stats)
bool Memory::update(const SensorData & data, Statistics * stats)
{
UDEBUG("");
UTimer timer;
@@ -449,7 +449,7 @@ bool Memory::update(const Image & image, Statistics * stats)
//============================================================
// Create a signature with the image received.
//============================================================
Signature * signature = this->createSignature(image, this->isRawDataKept());
Signature * signature = this->createSignature(data, this->isRawDataKept());
if (signature == 0)
{
UERROR("Failed to create a signature...");
@@ -1628,7 +1628,7 @@ void Memory::rejectLoopClosure(int oldId, int newId)
}
// compute transform newId -> oldId
Transform Memory::computeVisualTransform(int oldId, int newId) const
Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg) const
{
const Signature * oldS = this->getSignature(oldId);
const Signature * newS = this->getSignature(newId);
@@ -1637,19 +1637,25 @@ Transform Memory::computeVisualTransform(int oldId, int newId) const
if(oldS && newId)
{
return computeVisualTransform(*oldS, *newS);
return computeVisualTransform(*oldS, *newS, rejectedMsg);
}
else
{
UWARN("Did not find nodes %d and/or %d", oldId, newId);
std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId);
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UWARN(msg.c_str());
}
return Transform();
}
// compute transform newId -> oldId
Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS) const
Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg) const
{
Transform transform;
std::string msg;
// Guess transform from visual words
if(!oldS.getWords3().empty() && !newS.getWords3().empty())
{
@@ -1679,23 +1685,37 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
}
else if(inliersCount < _bowMinInliers)
{
UINFO("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
else if(inliersCount == (int)inliersOld->size())
{
msg = uFormat("Rejected identity with full inliers.");
UINFO(msg.c_str());
}
}
else
{
UINFO("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature())
{
UERROR("Words 3D empty?!?");
msg = "Words 3D empty?!?";
UERROR(msg.c_str());
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
return transform;
}
// compute transform newId -> oldId
Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D)
Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg)
{
Signature * oldS = this->_getSignature(oldId);
Signature * newS = this->_getSignature(newId);
@@ -1738,13 +1758,22 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
Transform t;
if(oldS && newS)
{
t = computeIcpTransform(*oldS, *newS, guess, icp3D);
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", oldId, newId);
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UWARN(msg.c_str());
}
return t;
}
// get transform from the new to old node
Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const
Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg) const
{
if(guess.isNull())
{
@@ -1758,6 +1787,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
}
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
std::string msg;
Transform transform;
// ICP with guess transform
@@ -1816,33 +1846,36 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
_icpMaxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
UDEBUG("fitness=%f", fitness);
if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
{
transform = icpT * guess;
transform = transform.inverse();
}
else
{
msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness);
UINFO(msg.c_str());
}
}
else
{
UWARN("Clouds empty ?!?");
}
//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
UDEBUG("fitness=%f", fitness);
if(hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
{
transform = icpT * guess;
transform = transform.inverse();
}
else
{
UWARN("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness);
msg = "Clouds empty ?!?";
UWARN(msg.c_str());
}
}
else
{
UERROR("Depths 3D empty?!?");
msg = "Depths 3D empty?!?";
UERROR(msg.c_str());
}
}
else // icp 2D
@@ -1907,31 +1940,39 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
correspondences,
(int)oldCloud->size(),
correspondencesRatio*100.0f);
}
else
{
UWARN("Clouds empty ?!?");
}
if(hasConverged &&
(_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = icpT * guess;
transform = transform.inverse();
if(!icpT.isNull() && hasConverged &&
(_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = icpT * guess;
transform = transform.inverse();
}
else
{
msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f correspondencesRatio=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness, correspondencesRatio, _icp2CorrespondenceRatio);
UINFO(msg.c_str());
}
}
else
{
UWARN("Cannot compute transform (hasConverged=%s fitness=%f/%f correspondencesRatio=%f/%f)",
hasConverged?"true":"false", fitness, _icpMaxFitness, correspondencesRatio, _icp2CorrespondenceRatio);
msg = "Clouds 2D empty ?!?";
UWARN(msg.c_str());
}
}
else
{
UERROR("Depths 2D empty?!?");
msg = "Depths 2D empty?!?";
UERROR(msg.c_str());
}
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;
}
@@ -1940,7 +1981,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
Transform Memory::computeScanMatchingTransform(
int newId,
int oldId,
const std::map<int, Transform> & poses)
const std::map<int, Transform> & poses,
std::string * rejectedMsg)
{
// make sure that all depth2D are loaded
std::list<Signature*> depthToLoad;
@@ -1958,6 +2000,7 @@ Transform Memory::computeScanMatchingTransform(
_dbDriver->loadNodeData(depthToLoad, true);
}
std::string msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledOldClouds(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2023,7 +2066,7 @@ Transform Memory::computeScanMatchingTransform(
(int)newCloud->size(),
correspondencesRatio);
if(hasConverged &&
if(!icpT.isNull() && hasConverged &&
(_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) &&
correspondencesRatio >= _icp2CorrespondenceRatio)
{
@@ -2034,14 +2077,25 @@ Transform Memory::computeScanMatchingTransform(
}
else
{
UWARN("Constraints failed... hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)",
msg = uFormat("Constraints failed... hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)",
hasConverged?"true":"false",
fitness,
correspondences,
(int)newCloud->size(),
correspondencesRatio);
UINFO(msg.c_str());
}
}
else
{
msg = "Empty data ?!?";
UWARN(msg.c_str());
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
return transform;
}
@@ -2840,6 +2894,14 @@ void Memory::copyData(const Signature * from, Signature * to)
UDEBUG("Merging time = %fs", timer.ticks());
}
void Memory::extractKeypointsAndDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const
{
extractKeypointsAndDescriptors(image, cv::Mat(), 0,0,0,0, keypoints, descriptors);
}
void Memory::extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
@@ -2848,7 +2910,7 @@ void Memory::extractKeypointsAndDescriptors(
float cx,
float cy,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors)
cv::Mat & descriptors) const
{
if(_wordsPerImageTarget >= 0)
{
@@ -2895,20 +2957,19 @@ private:
VWDictionary * _vwp;
};
Signature * Memory::createSignature(const Image & image, bool keepRawData)
Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
{
UASSERT(image.image().empty() || image.image().type() == CV_8UC1 || image.image().type() == CV_8UC3);
UASSERT(image.depth().empty() || image.depth().type() == CV_16UC1);
UASSERT(image.depth2d().empty() || image.depth2d().type() == CV_32FC2);
UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1);
UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
PreUpdateThread preUpdateThread(_vwd);
UTimer timer;
timer.start();
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> keypoints3;
cv::Mat descriptors;
int id = image.id();
int id = data.id();
if(_generateIds)
{
id = this->getNextId();
@@ -2951,48 +3012,24 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
preUpdateThread.start();
}
if(!image.descriptors().empty() && image.featureType() == _featureType)
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
{
// DESCRIPTORS
if(image.descriptors().rows && image.descriptors().rows >= _badSignRatio * float(meanWordsPerLocation))
{
UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
descriptors = image.descriptors();
keypoints = image.keypoints();
keypoints3 = image.keypoints3();
}
if(keypoints3.size())
{
filterKeypointsByDepth(keypoints, keypoints3, descriptors, _wordsMaxDepth);
limitKeypoints(keypoints, keypoints3, descriptors, _wordsPerImageTarget);
}
else
{
filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
// IMAGE RAW
cv::Mat imageMono;
// convert to grayscale
if(image.image().channels() > 1)
{
cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = image.image();
}
imageMono = data.image();
}
this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
this->extractKeypointsAndDescriptors(imageMono, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), keypoints, descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
if(_parallelized)
@@ -3021,7 +3058,6 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3;
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
@@ -3029,18 +3065,14 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
if(i < keypoints3.size())
{
words3.insert(std::pair<int, pcl::PointXYZ>(*iter, pcl::PointXYZ(keypoints3[i].x, keypoints3[i].y, keypoints3[i].z)));
}
}
}
UASSERT(keypoints3.size() == 0 || words3.size() == words.size());
//3d words
if(words3.size() == 0 && !image.depth().empty() && image.depthFx() && image.depthFy())
std::multimap<int, pcl::PointXYZ> words3;
if(!data.depth().empty() && data.depthFx() && data.depthFy())
{
words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
words3 = util3d::generateWords3(words, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), data.localTransform());
}
Signature * s;
@@ -3048,8 +3080,8 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
{
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
util3d::CompressionThread ctImage(image.image(), std::string(".jpg"));
util3d::CompressionThread ctDepth(image.depth(), std::string(".png"));
util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
util3d::CompressionThread ctDepth(data.depth(), std::string(".png"));
ctImage.start();
ctDepth.start();
ctImage.join();
@@ -3061,15 +3093,15 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
_idMapCount,
words,
words3,
image.pose(),
util3d::compressData(image.depth2d()),
data.pose(),
util3d::compressData(data.depth2d()),
imageBytes,
depthBytes,
image.depthFx(),
image.depthFy(),
image.depthCx(),
image.depthCy(),
image.localTransform());
data.depthFx(),
data.depthFy(),
data.depthCx(),
data.depthCy(),
data.localTransform());
}
else
{
@@ -3077,8 +3109,8 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
_idMapCount,
words,
words3,
image.pose(),
util3d::compressData(image.depth2d()));
data.pose(),
util3d::compressData(data.depth2d()));
}

95
corelib/src/Odometry.cpp
View File

@@ -93,9 +93,9 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
fabs(transform.z()) > _linearUpdate;
}
Transform Odometry::process(Image & image, int * quality)
Transform Odometry::process(SensorData & data, int * quality)
{
Transform t = this->computeTransform(image, quality);
Transform t = this->computeTransform(data, quality);
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
@@ -175,42 +175,35 @@ void OdometryBOW::reset()
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(Image & image, int * quality)
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
{
UTimer timer;
Transform output;
cv::Mat imageMono;
// convert to grayscale
if(image.image().channels() > 1)
if(data.image().channels() > 1)
{
cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = image.image();
}
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), keypoints, descriptors);
image.setDescriptors(descriptors, _memory->getFeatureType());
image.setKeypoints(keypoints);
if(this->getLocalHistory() && this->getLocalHistory() < descriptors.rows)
{
UWARN("Local history words size (%d) is smaller than extracted features from the current frame (%d).",
this->getLocalHistory(), descriptors.rows);
imageMono = data.image();
}
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
const Signature * previousSignature = _memory->getLastWorkingSignature();
if(_memory->update(image))
if(_memory->update(data))
{
const Signature * newSignature = _memory->getLastWorkingSignature();
if(newSignature)
{
nFeatures = newSignature->getWords().size();
}
if(previousSignature && newSignature)
{
Transform transform;
@@ -360,7 +353,7 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
timer.elapsed(),
descriptors.rows,
nFeatures,
inliers,
correspondences,
(int)_memory->getVWDictionary()->getVisualWords().size(),
@@ -398,7 +391,7 @@ void OdometryICP::reset()
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(Image & image, int * quality)
Transform OdometryICP::computeTransform(const SensorData & data, int * quality)
{
UTimer timer;
Transform output;
@@ -406,19 +399,19 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
bool hasConverged = false;
double fitness = 0;
unsigned int minPoints = 100;
if(!image.depth().empty())
if(!data.depth().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
image.depth(),
image.depthFx(),
image.depthFy(),
image.depthCx(),
image.depthCy(),
data.depth(),
data.depthFx(),
data.depthFy(),
data.depthCx(),
data.depthCy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
image.localTransform());
data.localTransform());
if(_pointToPlane)
{
@@ -538,7 +531,7 @@ void OdometryThread::handleEvent(UEvent * event)
CameraEvent * cameraEvent = (CameraEvent*)event;
if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
{
this->addImage(cameraEvent->image());
this->addData(cameraEvent->data());
}
else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
{
@@ -554,7 +547,7 @@ void OdometryThread::handleEvent(UEvent * event)
void OdometryThread::mainLoopKill()
{
_imageAdded.release();
_dataAdded.release();
}
//============================================================
@@ -568,51 +561,51 @@ void OdometryThread::mainLoop()
_resetOdometry = false;
}
Image image;
getImage(image);
if(!image.empty())
SensorData data;
getData(data);
if(data.isValid())
{
int quality = -1;
Transform pose = _odometry->process(image, &quality);
image.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(image, quality));
Transform pose = _odometry->process(data, &quality);
data.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, quality));
}
}
void OdometryThread::addImage(const Image & image)
void OdometryThread::addData(const SensorData & data)
{
if(image.empty() || image.depth().empty() || image.depthFx() == 0.0f || image.depthFy() == 0.0f)
if(data.image().empty() || data.depth().empty() || data.depthFx() == 0.0f || data.depthFy() == 0.0f)
{
ULOGGER_ERROR("image empty !?");
return;
}
bool notify = true;
_imageMutex.lock();
_dataMutex.lock();
{
notify = _imageBuffer.empty();
_imageBuffer = image;
notify = !_dataBuffer.isValid();
_dataBuffer = data;
}
_imageMutex.unlock();
_dataMutex.unlock();
if(notify)
{
_imageAdded.release();
_dataAdded.release();
}
}
void OdometryThread::getImage(Image & image)
void OdometryThread::getData(SensorData & data)
{
_imageAdded.acquire();
_imageMutex.lock();
_dataAdded.acquire();
_dataMutex.lock();
{
if(!_imageBuffer.empty())
if(_dataBuffer.isValid())
{
image = _imageBuffer;
_imageBuffer = Image();
data = _dataBuffer;
_dataBuffer = SensorData();
}
}
_imageMutex.unlock();
_dataMutex.unlock();
}
} /* namespace rtabmap */

59
corelib/src/Rtabmap.cpp
View File

@@ -667,7 +667,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
//============================================================
// MAIN LOOP
//============================================================
bool Rtabmap::process(const Image & image)
bool Rtabmap::process(const SensorData & data)
{
UDEBUG("");
@@ -724,9 +724,9 @@ bool Rtabmap::process(const Image & image)
// Wait for an image...
//============================================================
ULOGGER_INFO("getting data...");
if(image.empty())
if(!data.isValid())
{
ULOGGER_INFO("image is null...");
ULOGGER_INFO("image is not valid...");
return false;
}
@@ -743,10 +743,10 @@ bool Rtabmap::process(const Image & image)
//============================================================
if(_rgbdSlamMode)
{
if(image.pose().isNull())
if(data.pose().isNull())
{
UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
"Image %d is ignored!", image.id());
"Image %d is ignored!", data.id());
return false;
}
else
@@ -755,7 +755,7 @@ bool Rtabmap::process(const Image & image)
if(_memory->getLastWorkingSignature())
{
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
if(!lastPose.isIdentity() && image.pose().isIdentity())
if(!lastPose.isIdentity() && data.pose().isIdentity())
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
@@ -764,7 +764,7 @@ bool Rtabmap::process(const Image & image)
}
else
{
Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
@@ -774,7 +774,7 @@ bool Rtabmap::process(const Image & image)
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
image.pose().prettyPrint().c_str());
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
@@ -787,7 +787,7 @@ bool Rtabmap::process(const Image & image)
// Memory Update : Location creation + Add to STM + Weight Update (Rehearsal)
//============================================================
ULOGGER_INFO("Updating memory...");
if(!_memory->update(image, &statistics_))
if(!_memory->update(data, &statistics_))
{
return false;
}
@@ -870,17 +870,22 @@ bool Rtabmap::process(const Image & image)
{
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses);
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg);
if(!t.isNull())
{
scanMatchingSuccess = true;
UDEBUG("Update neighbor link (%d->%d) from %s to %s",
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getNeighbors().at(oldId).prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t);
}
else
{
UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
}
}
else
{
@@ -909,11 +914,12 @@ bool Rtabmap::process(const Image & image)
signature->getNeighbors().find(*iter) == signature->getNeighbors().end() &&
_memory->getSignature(*iter)->mapId() == signature->mapId())
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
Transform transform = _memory->computeVisualTransform(*iter, signature->id());
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -947,6 +953,11 @@ bool Rtabmap::process(const Image & image)
*iter, signature->id());
}
}
else
{
UINFO("Local loop closure (time) between %d and %d rejected: %s",
*iter, signature->id(), rejectedMsg.c_str());
}
}
}
}
@@ -1249,10 +1260,11 @@ bool Rtabmap::process(const Image & image)
Transform transform;
if(_rgbdSlamMode)
{
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
std::string rejectedMsg;
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1270,7 +1282,7 @@ bool Rtabmap::process(const Image & image)
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Cannot compute a loop closure transform between %d and %d", _lcHypothesisId, signature->id());
UWARN("Cannot compute a loop closure transform between %d and %d: %s", _lcHypothesisId, signature->id(), rejectedMsg.c_str());
}
}
if(!rejectedHypothesis)
@@ -1316,14 +1328,15 @@ bool Rtabmap::process(const Image & image)
localSpaceDetectionPosesCount = (int)poses.size()-1;
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
localSpaceNearestId &&
localSpaceNearestId &&
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
{
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses);
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
UDEBUG("Add local loop closure in SPACE (%d->%d) %s",
UINFO("Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
@@ -1334,6 +1347,10 @@ bool Rtabmap::process(const Image & image)
UASSERT(oldS != 0);
_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
}
else
{
UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
}
}
}
timeLocalSpaceDetection = timer.ticks();
@@ -1720,9 +1737,9 @@ bool Rtabmap::process(const Image & image)
return true;
}
bool Rtabmap::process(const cv::Mat & image, int id)
bool Rtabmap::process(const cv::Mat & sensorData, int id)
{
return this->process(Image(image, id));
return this->process(SensorData(sensorData, id));
}
// SETTERS

72
corelib/src/RtabmapThread.cpp
View File

@@ -42,7 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rate(Parameters::defaultRtabmapDetectionRate()),
_frameRateTimer(new UTimer()),
_rtabmap(rtabmap),
@@ -74,16 +74,16 @@ void RtabmapThread::pushNewState(State newState, const ParametersMap & parameter
}
_stateMutex.unlock();
_imageAdded.release();
_dataAdded.release();
}
void RtabmapThread::clearBufferedData()
{
_imageMutex.lock();
_dataMutex.lock();
{
_imageBuffer.clear();
_dataBuffer.clear();
}
_imageMutex.unlock();
_dataMutex.unlock();
}
void RtabmapThread::setDetectorRate(float rate)
@@ -95,7 +95,7 @@ void RtabmapThread::setDetectorRate(float rate)
void RtabmapThread::setBufferSize(int bufferSize)
{
UASSERT(bufferSize >= 0);
_imageBufferMaxSize = bufferSize;
_dataBufferMaxSize = bufferSize;
}
void RtabmapThread::publishMap(bool optimized, bool full) const
@@ -177,7 +177,7 @@ void RtabmapThread::mainLoopKill()
this->clearBufferedData();
// this will post the newData semaphore
_imageAdded.release();
_dataAdded.release();
}
void RtabmapThread::mainLoop()
@@ -203,9 +203,9 @@ void RtabmapThread::mainLoop()
this->process();
break;
case kStateChangingParameters:
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _imageBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
UASSERT(_imageBufferMaxSize >= 0);
UASSERT(_dataBufferMaxSize >= 0);
UASSERT(_rate >= 0.0f);
_rtabmap->parseParameters(parameters);
break;
@@ -270,11 +270,9 @@ void RtabmapThread::handleEvent(UEvent* event)
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage ||
e->getCode() == CameraEvent::kCodeFeatures ||
e->getCode() == CameraEvent::kCodeImageDepth)
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
{
this->addImage(e->image());
this->addData(e->data());
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
@@ -283,7 +281,7 @@ void RtabmapThread::handleEvent(UEvent* event)
OdometryEvent * e = (OdometryEvent*)event;
if(e->isValid())
{
this->addImage(e->data());
this->addData(e->data());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
@@ -416,26 +414,26 @@ void RtabmapThread::handleEvent(UEvent* event)
//============================================================
void RtabmapThread::process()
{
Image image;
getImage(image);
if(!image.empty())
SensorData data;
getData(data);
if(data.isValid())
{
_rtabmap->process(image);
_rtabmap->process(data);
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
}
}
void RtabmapThread::addImage(const Image & image)
void RtabmapThread::addData(const SensorData & sensorData)
{
if(!_paused)
{
if(image.empty())
if(!sensorData.isValid())
{
ULOGGER_ERROR("image empty !?");
ULOGGER_ERROR("data not valid !?");
return;
}
@@ -449,42 +447,42 @@ void RtabmapThread::addImage(const Image & image)
_frameRateTimer->start();
bool notify = true;
_imageMutex.lock();
_dataMutex.lock();
{
_imageBuffer.push_back(image);
while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
_dataBuffer.push_back(sensorData);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
_imageBuffer.pop_front();
_dataBuffer.pop_front();
notify = false;
}
}
_imageMutex.unlock();
_dataMutex.unlock();
if(notify)
{
_imageAdded.release();
_dataAdded.release();
}
}
}
void RtabmapThread::getImage(Image & image)
void RtabmapThread::getData(SensorData & image)
{
ULOGGER_DEBUG("");
ULOGGER_INFO("waiting for data");
_imageAdded.acquire();
_dataAdded.acquire();
ULOGGER_INFO("wake-up");
_imageMutex.lock();
_dataMutex.lock();
{
if(!_imageBuffer.empty())
if(!_dataBuffer.empty())
{
image = _imageBuffer.front();
_imageBuffer.pop_front();
image = _dataBuffer.front();
_dataBuffer.pop_front();
}
}
_imageMutex.unlock();
_dataMutex.unlock();
}
void RtabmapThread::setDataBufferSize(int size)
@@ -492,11 +490,11 @@ void RtabmapThread::setDataBufferSize(int size)
if(size < 0)
{
ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
_imageBufferMaxSize = 0;
_dataBufferMaxSize = 0;
}
else
{
_imageBufferMaxSize = size;
_dataBufferMaxSize = size;
}
}

42
corelib/src/Image.cpp → corelib/src/SensorData.cpp
View File

@@ -26,9 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Image.h"
#include <rtabmap/utilite/ULogger.h>
#include "rtabmap/core/SensorData.h"
namespace rtabmap
{
@@ -36,16 +34,21 @@ namespace rtabmap
/**
* An id is automatically generated if id=0.
*/
Image::Image(const cv::Mat & image,
int id,
const cv::Mat & descriptors,
Feature2D::Type featureType,
const std::vector<cv::KeyPoint> & keypoints) :
SensorData::SensorData() :
_image(cv::Mat()),
_id(0),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
{
}
SensorData::SensorData(const cv::Mat & image,
int id) :
_image(image),
_id(id),
_descriptors(descriptors),
_featureType(featureType),
_keypoints(keypoints),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
@@ -55,7 +58,7 @@ Image::Image(const cv::Mat & image,
}
// Metric constructor
Image::Image(const cv::Mat & image,
SensorData::SensorData(const cv::Mat & image,
const cv::Mat & depth,
float fx,
float fy,
@@ -66,7 +69,6 @@ Image::Image(const cv::Mat & image,
int id) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_fx(fx),
_fy(fy),
@@ -78,7 +80,7 @@ Image::Image(const cv::Mat & image,
}
// Metric constructor + 2d depth
Image::Image(const cv::Mat & image,
SensorData::SensorData(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depth2d,
float fx,
@@ -90,7 +92,6 @@ Image::Image(const cv::Mat & image,
int id) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depth2d(depth2d),
_fx(fx),
@@ -102,16 +103,9 @@ Image::Image(const cv::Mat & image,
{
}
void Image::setKeypoints(
const std::vector<cv::KeyPoint> & keypoints,
const std::vector<cv::Point3f> * keypoints3)
bool SensorData::empty() const
{
_keypoints = keypoints;
if(keypoints3 && keypoints3->size())
{
UASSERT(_keypoints.size() == keypoints3->size());
_keypoints3 = *keypoints3;
}
return _image.empty();
}
} // namespace rtabmap

9
corelib/src/util3d.cpp
View File

@@ -1193,14 +1193,11 @@ Transform transformFromXYZCorrespondences(
if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
{
//Wrong transform
UINFO("Wrong transform: identity");
UDEBUG("Wrong transform: identity with full inliers");
transform.setNull();
if(inliers)
{
*inliers = 0;
}
}
else if(inliers)
if(inliers)
{
*inliers = (int)correspondencesInliers.size();
}