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

New parameter: Mem/ImageDecimation (save downscaled images in database), Updated default parameter RGBD/LocalLoopDetectionMaxDiffID to 50

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-13 15:25:40 -05:00
parent 7c65dbf6bb
commit ad05c5902a
10 changed files with 193 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/include/rtabmap/core/Memory.h
View File

@@ -123,8 +123,7 @@ public:
bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
bool isIDsGenerated() const {return _generateIds;}
int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
const Feature2D * getFeature2D() const {return _feature2D;}
void setRoi(const std::string & roi);
@@ -209,12 +208,12 @@ private:
bool _idUpdatedToNewOneRehearsal;
bool _generateIds;
bool _badSignaturesIgnored;
int _imageDecimation;
int _idCount;
int _idMapCount;
Signature * _lastSignature;
int _lastGlobalLoopClosureParentId;
int _lastGlobalLoopClosureChildId;
int _lastGlobalLoopClosureId;
bool _memoryChanged; // False by default, become true only when Memory::update() is called.
bool _linksChanged; // False by default, become true when links are modified.
int _signaturesAdded;

6
corelib/include/rtabmap/core/Parameters.h
View File

@@ -192,7 +192,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location Ids, False=use input image ids.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1).");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -294,7 +296,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius, float, 15, "Maximum radius for space detection.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 0, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");

4
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -97,7 +97,7 @@ public:
float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
void setTimeThreshold(float maxTimeAllowed); // in ms
void triggerNewMap();
int triggerNewMap();
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool global);
void resetMemory();
@@ -186,6 +186,8 @@ private:
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
double _lastProcessTime;
int _lastLocalLoopClosureParentId;
int _lastLocalLoopClosureChildId;
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.

5
corelib/include/rtabmap/core/Statistics.h
View File

@@ -60,8 +60,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
RTABMAP_STATS(Loop, VisualInliers,);
RTABMAP_STATS(Loop, Last_loop_closure_parent,);
RTABMAP_STATS(Loop, Last_loop_closure_child,);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
@@ -69,6 +68,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(LocalLoop, Space_nearest_id,);
RTABMAP_STATS(LocalLoop, Space_neighbors,);
RTABMAP_STATS(LocalLoop, Space_diff_id,);
RTABMAP_STATS(LocalLoop, Space_last_parent,);
RTABMAP_STATS(LocalLoop, Space_last_child,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);

2
corelib/include/rtabmap/core/util3d.h
View File

@@ -401,6 +401,8 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
const pcl::IndicesPtr & indicesA,
const pcl::IndicesPtr & indicesB);
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
///////////////////
// Templated PCL methods
///////////////////

110
corelib/src/Memory.cpp
View File

@@ -66,11 +66,11 @@ Memory::Memory(const ParametersMap & parameters) :
_idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_imageDecimation(Parameters::defaultMemImageDecimation()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
_lastGlobalLoopClosureParentId(0),
_lastGlobalLoopClosureChildId(0),
_lastGlobalLoopClosureId(0),
_memoryChanged(false),
_linksChanged(false),
_signaturesAdded(0),
@@ -384,10 +384,12 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
UASSERT(_imageDecimation >= 1);
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
@@ -590,9 +592,6 @@ bool Memory::update(const SensorData & data, Statistics * stats)
UDEBUG("totalTimer = %fs", totalTimer.ticks());
if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_parent(), _lastGlobalLoopClosureParentId);
if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_child(), _lastGlobalLoopClosureChildId);
return true;
}
@@ -1003,8 +1002,7 @@ void Memory::clear()
}
UDEBUG("");
_lastSignature = 0;
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
_lastGlobalLoopClosureId = 0;
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
@@ -1303,10 +1301,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
bool recentWmImmunized = false;
// look for the position of the lastLoopClosureId in WM
int currentRecentWmSize = 0;
if(_lastGlobalLoopClosureParentId > 0 && _stMem.find(_lastGlobalLoopClosureParentId) == _stMem.end())
if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
{
// If set, it must be in WM
std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureParentId);
std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
while(iter != _workingMem.end())
{
++currentRecentWmSize;
@@ -1318,9 +1316,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
}
else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
{
UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureParentId);
UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
}
UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureParentId);
UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId);
}
// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
@@ -1332,8 +1330,8 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
{
if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureParentId) ||
*memIter == _lastGlobalLoopClosureParentId)
if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureId) ||
*memIter == _lastGlobalLoopClosureId)
{
// ignore recent memory
}
@@ -1390,7 +1388,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
removableSignatures.push_back(iter->second);
addedSignatures.insert(iter->second->id());
if(iter->second->id() > _lastGlobalLoopClosureParentId)
if(iter->second->id() > _lastGlobalLoopClosureId)
{
++recentWmCount;
if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
@@ -1400,7 +1398,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
}
}
}
else if(iter->second->id() < _lastGlobalLoopClosureParentId)
else if(iter->second->id() < _lastGlobalLoopClosureId)
{
UDEBUG("weight=%d, id=%d",
iter->first.weight,
@@ -1579,10 +1577,9 @@ void Memory::removeLink(int oldId, int newId)
break;
}
}
if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureParentId)
if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId)
{
_lastGlobalLoopClosureParentId = 0;
_lastGlobalLoopClosureChildId = 0;
_lastGlobalLoopClosureId = 0;
}
}
else
@@ -2181,8 +2178,7 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
if(_incrementalMemory && type == Link::kGlobalClosure)
{
_lastGlobalLoopClosureParentId = newS->id()>oldS->id()?newS->id():oldS->id();
_lastGlobalLoopClosureChildId = newS->id()>oldS->id()?oldS->id():newS->id();
_lastGlobalLoopClosureId = newS->id()>oldS->id()?newS->id():oldS->id();
// udpate weights only if the memory is incremental
if(newS->id() > oldS->id())
@@ -2470,9 +2466,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
// update weight
newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
if(_lastGlobalLoopClosureParentId == oldS->id())
if(_lastGlobalLoopClosureId == oldS->id())
{
_lastGlobalLoopClosureParentId = newS->id();
_lastGlobalLoopClosureId = newS->id();
}
}
else
@@ -2978,8 +2974,8 @@ private:
Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1);
UASSERT(data.depth().empty() || ((data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1) && data.depth().rows == data.image().rows && data.depth().cols == data.image().cols));
UASSERT(data.rightImage().empty() || (data.rightImage().type() == CV_8UC1 && data.rightImage().rows == data.image().rows && data.rightImage().cols == data.image().cols));
UASSERT(data.laserScan().empty() || data.laserScan().type() == CV_32FC2);
UASSERT(_feature2D != 0);
@@ -3372,7 +3368,18 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
unsigned int i=0;
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(_imageDecimation > 1)
{
cv::KeyPoint kpt = keypoints[i];
kpt.pt.x /= float(_imageDecimation);
kpt.pt.y /= float(_imageDecimation);
kpt.size /= float(_imageDecimation);
words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
}
else
{
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
}
if(keypoints3D->size())
{
words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
@@ -3380,25 +3387,44 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
}
cv::Mat image = data.image();
cv::Mat depthOrRightImage = data.depthOrRightImage();
float fx = data.fx();
float fyOrBaseline = data.fyOrBaseline();
float cx = data.cx();
float cy = data.cy();
// apply decimation?
if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1)
{
image = util3d::decimate(image, _imageDecimation);
depthOrRightImage = util3d::decimate(depthOrRightImage, _imageDecimation);
cx/=float(_imageDecimation);
cy/=float(_imageDecimation);
if(data.baseline() != 0.0f)
{
fx/=float(_imageDecimation);
}
else
{
fx/=float(_imageDecimation);
fyOrBaseline/=float(_imageDecimation);
}
}
Signature * s;
if(this->isBinDataKept())
{
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
if(data.depth().type() == CV_32FC1)
if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
{
UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage);
}
cv::Mat depthOrRightImage;
if(!data.depth().empty())
{
depthOrRightImage = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
}
else if(!data.rightImage().empty())
{
depthOrRightImage = data.rightImage();
}
rtabmap::CompressionThread ctImage(data.image(), std::string(".jpg"));
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(data.laserScan());
ctImage.start();
@@ -3416,10 +3442,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
ctDepth2d.getCompressedData(),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
data.fx(),
data.fy()>0.0f?data.fy():data.baseline(),
data.cx(),
data.cy(),
fx,
fyOrBaseline,
cx,
cy,
data.localTransform());
}
else
@@ -3433,8 +3459,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
if(this->isRawDataKept())
{
s->setImageRaw(data.image());
s->setDepthRaw(data.depth());
s->setImageRaw(image);
s->setDepthRaw(depthOrRightImage);
s->setLaserScanRaw(data.laserScan());
}

43
corelib/src/Rtabmap.cpp
View File

@@ -112,6 +112,8 @@ Rtabmap::Rtabmap() :
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
_lastLocalLoopClosureParentId(0),
_lastLocalLoopClosureChildId(0),
_epipolarGeometry(0),
_bayesFilter(0),
_memory(0),
@@ -584,15 +586,19 @@ Transform Rtabmap::getPose(int locationId) const
return Transform();
}
void Rtabmap::triggerNewMap()
int Rtabmap::triggerNewMap()
{
int mapId = -1;
if(_memory)
{
int mapId = _memory->incrementMapId();
UINFO("New map triggerred, new map = %d", mapId);
mapId = _memory->incrementMapId();
UINFO("New map triggered, new map = %d", mapId);
_optimizedPoses.clear();
_constraints.clear();
_lastLocalLoopClosureParentId = 0;
_lastLocalLoopClosureChildId = 0;
}
return mapId;
}
void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
@@ -653,6 +659,8 @@ void Rtabmap::resetMemory()
_highestHypothesis = std::make_pair(0,0.0f);
_loopClosureHypothesis = std::make_pair(0,0.0f);
_lastProcessTime = 0.0;
_lastLocalLoopClosureParentId = 0;
_lastLocalLoopClosureChildId = 0;
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
@@ -773,14 +781,12 @@ bool Rtabmap::process(const SensorData & data)
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
int mapId = triggerNewMap();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
}
}
@@ -1372,6 +1378,11 @@ bool Rtabmap::process(const SensorData & data)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
if(!rejectedHypothesis)
{
_lastLocalLoopClosureParentId = signature->id();
_lastLocalLoopClosureChildId = _loopClosureHypothesis.first;
}
}
if(rejectedHypothesis)
@@ -1444,6 +1455,9 @@ bool Rtabmap::process(const SensorData & data)
t.prettyPrint().c_str());
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
_lastLocalLoopClosureParentId = signature->id();
_lastLocalLoopClosureChildId = localSpaceNearestId;
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
UASSERT(oldS != 0);
@@ -1555,25 +1569,26 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
if(localSpaceClosureId)
{
statistics_.setLocalLoopClosureId(localSpaceClosureId);
}
statistics_.setLocalLoopClosureId(localSpaceClosureId);
if(localSpaceNearestId)
{
int d1 = abs(signature->id() - localSpaceNearestId);
int d2 = abs(localSpaceNearestId - _memory->getLastGlobalLoopClosureChildId());
int d3 = abs(signature->id() - _memory->getLastGlobalLoopClosureParentId());
int d2 = abs(localSpaceNearestId - _lastLocalLoopClosureChildId);
int d3 = abs(signature->id() - _lastLocalLoopClosureParentId);
int d = d1<=d2?d1:d2;
d = d <= d3?d:d3;
statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
}
statistics_.addStatistic(Statistics::kLocalLoopSpace_last_parent(), _lastLocalLoopClosureParentId);
statistics_.addStatistic(Statistics::kLocalLoopSpace_last_child(), _lastLocalLoopClosureChildId);
if(_loopClosureHypothesis.first || localSpaceClosureId)
{
UASSERT(uContains(sLoop->getLinks(), signature->id()));
@@ -1919,8 +1934,8 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
// Only locations in Working Memory with ID not too far from the last loop closure child id
bool diffIdOk = maxDiffID == 0 ||
abs(fromId - iter->first) <= maxDiffID ||
(abs(iter->first - _memory->getLastGlobalLoopClosureChildId()) <= maxDiffID &&
abs(fromId - _memory->getLastGlobalLoopClosureParentId()) <= maxDiffID);
(abs(iter->first - _lastLocalLoopClosureChildId) <= maxDiffID &&
abs(fromId - _lastLocalLoopClosureParentId) <= maxDiffID);
if(stm.find(iter->first) == stm.end() && diffIdOk)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());

49
corelib/src/util3d.cpp
View File

@@ -827,7 +827,7 @@ cv::Mat disparityFromStereoImages(
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 0, 15);
cv::Mat disparity;
stereo(leftImage, rightImage, disparity, CV_16SC1);
cv::filterSpeckles(disparity, 0, 1000, 16);
@@ -2562,6 +2562,53 @@ pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::Indices
return ind;
}
cv::Mat decimate(const cv::Mat & image, int decimation)
{
UASSERT(decimation >= 1);
cv::Mat out;
if(!image.empty())
{
if(decimation > 1)
{
if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
{
UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
if(image.type() == CV_32FC1)
{
for(int j=0; j<out.rows; ++j)
{
for(int i=0; i<out.cols; ++i)
{
out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
}
}
}
else // CV_16UC1
{
for(int j=0; j<out.rows; ++j)
{
for(int i=0; i<out.cols; ++i)
{
out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
}
}
}
}
else
{
cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
}
}
else
{
out = image;
}
}
return out;
}
}
}