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

Update 0.15.4.

Browse Source 
Parameters:
-Added "GridGlobal/MaxNodes=0", "Rtabmap/PublishRAMUsage=false", "Mem/DepthAsMask=true", "Kp/FlannRebalancingFactor=2.0", "Vis/DepthAsMask=true".
-Modified "Kp/DetectorStrategy=6", "Kp/MaxFeatures=500",  "Mem/UseOdomFeatures=true", "GFTT/QualityLevel=0.001", "GFTT/MinDistance=3", "RGBD/OptimizeMaxError=1", "RGBD/ProximityPathFilteringRadius=1", "Odom/GuessMotion=true", "Odom/VisKeyFrameThr=150", "OdomF2M/BundleAdjustment=1", "Vis/Iterations=300" if built with g2o, "OdomF2M/BundleAdjustmentMaxFrames=10", "OdomFovis/MinFeaturesForEstimate=20", "OdomORBSLAM2/MapSize=3000", "Reg/RepeatOnce=true", "Vis/PnPRefineIterations=0" if built with g2o, "Vis/CorGuessMatchToProjection=true", "Vis/BundleAdjustment=1" if built with g2o, "Icp/MaxCorrespondenceDistance=0.1", "Icp/PointToPlaneK=5", "Icp/PointToPlaneRadius=1", "Icp/PM=true" if built with libpointmatcher, "Stereo/MaxLevel=5", "Stereo/MinDisparity=0.5".

BayesFilter: optimized prediction matrix update. Use of new argument "ignoreLocalSpaceLoopIds" of Memory::getNeighborsId() to ignore loop closure link by space in prediction update.
CameraThread: Added stereo exposure compensation option.
CameraRGB: Added forceGroundNormalsUp option and added support of ground truth from EuRoC dataset.
Statistics: Added "Memory/RAM_usage/MB".
Transform: Added clone() method to do deep copy.
Graph::importPoses(): EuRoC format support (9).
Rtabmap: Local visual loop closures are now identified as GlobalClosure link type.
OccupancyGrid/OctoMap: updated how cache is used (old node retrieved can be re-added to map without re-assembling the whole map).
OdometryF2F: when using ICP, increasing correspondence distance for first two frames. If Vis/CorType=1 and registration fails, second guess without motion is done with Vis/CorType=0.
OdometryF2M/RegVis: updated how features are removed from the map, using new projectedIDs filled in RegistrationInfo by RegistrationVis.
OdometryORBSLAM2: Maximum size of the feature map can be set with "OdomORBSLAM2/MapSize" parameter.
CloudViewer: fixed opengl camera drifting in follow mode.
DatabaseViewer: Added optimization scale option. ConstraintsView: hide loop closure links if type is ignored in gui parameters.
MainWindow: Support of "GridGlobal/MaxNodes" parameters when updating the maps.
UPlot: don't show ellipses when not in graphics view mode, updated how "random" colors are attributed to curves
Added rtabmap-euroc_dataset tool. Updated rtabmap-kitti_dataset and rtabmap-rgbd_dataset tools.
Added rtabmap-reprocess tool.
This commit is contained in:
matlabbe
2018-02-01 22:17:46 -05:00
parent 9f80f4ac42
commit 977d21eed5
76 changed files with 3844 additions and 1307 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/BayesFilter.cpp
+85 -67
View File
@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
#include <set>
#include <unordered_set>
#include "rtabmap/utilite/UtiLite.h"
@@ -125,6 +127,7 @@ void BayesFilter::reset()
{
_posterior.clear();
_prediction = cv::Mat();
_neighborsIndex.clear();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
@@ -219,7 +222,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
return _posterior;
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
{
if(!_fullPredictionUpdate && !_prediction.empty())
{
@@ -236,13 +239,17 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
UTimer timerGlobal;
timerGlobal.start();
std::map<int, int> idToIndexMap;
std::unordered_map<int,int> idToIndexMap;
idToIndexMap.reserve(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
if(ids[i]>0)
{
idToIndexMap[ids[i]] = i;
}
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
@@ -260,7 +267,13 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true);
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true);
if(!_fullPredictionUpdate)
{
uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors));
}
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -271,7 +284,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
}
else
{
if(iter->second == 0)
if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
{
idsLoopMargin.push_back(iter->first);
}
@@ -288,10 +301,16 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
if(!_fullPredictionUpdate)
{
uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
}
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap);
int index = idToIndexMap.at(*iter);
sum += this->addNeighborProb(prediction, index, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0);
this->normalize(prediction, index, sum, ids[0]<0);
}
}
else
@@ -405,7 +424,7 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde
cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const
const std::vector<int> & newIds)
{
UTimer timer;
UDEBUG("");
@@ -417,34 +436,32 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
oldIds.size() == (unsigned int)oldPrediction.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
UDEBUG("time creating prediction = %fs", timer.restart());
// Create id to index maps
std::map<int, int> oldIdToIndexMap;
std::map<int, int> newIdToIndexMap;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
UDEBUG("time creating old ids set = %fs", timer.restart());
std::unordered_map<int,int> newIdToIndexMap;
newIdToIndexMap.reserve(newIds.size());
for(unsigned int i=0; i<newIds.size(); ++i)
{
if(i<oldIds.size())
if(newIds[i]>0)
{
UASSERT(oldIds[i]);
oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
}
if(i<newIds.size())
{
UASSERT(newIds[i]);
newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
newIdToIndexMap[newIds[i]] = i;
}
}
UDEBUG("time creating id-index maps = %fs", timer.restart());
UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(!uContains(newIdToIndexMap, oldIds[i]))
if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
{
removedIds.insert(removedIds.end(), oldIds[i]);
_neighborsIndex.erase(oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
@@ -476,16 +493,31 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
}
}
if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true);
if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
if(jter != _neighborsIndex.end())
{
uInsert(jter->second, std::make_pair(newIds[i], iter->second));
}
}
_neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
}
const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0);
++added;
int count = 0;
for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(uContains(oldIdToIndexMap, iter->first) &&
if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
@@ -497,51 +529,35 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
}
UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
UTimer t1;
double e0=0,e1=0, e2=0, e3=0, e4=0;
// update modified/added ids
int modified = 0;
std::set<int> idsDone;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
if(idsDone.find(*iter) == idsDone.end() && *iter > 0)
int id = *iter;
if(id > 0 && id<(int)newIdToIndexMap.size())
{
std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
int index = newIdToIndexMap.at(id);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end();)
if(index > 0)
{
if(memory->isInSTM(jter->first))
{
neighbors.erase(jter++);
}
else
{
if(jter->second == 0)
{
idsLoopMargin.push_back(jter->first);
}
++jter;
}
}
e0 = t1.ticks();
std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
const std::map<int, int> & neighbors = kter->second;
e1+=t1.ticks();
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
UFATAL("No 0 margin neighbor for signature %d !?!?", *iter);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
int index = newIdToIndexMap.at(*iter);
float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
idsDone.insert(*iter);
e3+=t1.ticks();
this->normalize(prediction, index, sum, newIds[0]<0);
++modified;
e4+=t1.ticks();
}
}
}
UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart());
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
@@ -624,20 +640,22 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
_posterior = newPosterior;
}
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::unordered_map<int, int> & idToIndex) const
{
UASSERT((unsigned int)prediction.cols == idToIndexMap.size() &&
(unsigned int)prediction.rows == idToIndexMap.size() &&
col < (unsigned int)prediction.cols &&
UASSERT(col < (unsigned int)prediction.cols &&
col < (unsigned int)prediction.rows);
float sum=0;
float sum=0.0f;
float * dataPtr = (float*)prediction.data;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
int index = uValue(idToIndexMap, iter->first, -1);
if(index >= 0)
if(iter->first>=0)
{
sum += ((float*)prediction.data)[col + index*prediction.cols] = _predictionLC[iter->second+1];
std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
if(jter != idToIndex.end())
{
sum += dataPtr[col + jter->second*prediction.cols] = _predictionLC[iter->second+1];
}
}
}
return sum;
corelib/src/CameraRGB.cpp
+46 -11
View File
@@ -71,6 +71,7 @@ CameraImages::CameraImages() :
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -102,6 +103,7 @@ CameraImages::CameraImages(const std::string & path,
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -242,15 +244,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
const std::list<std::string> & filenames = _dir->getFileNames();
for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
{
// format is text_12234456.12334_text.png
// format is text_1223445645.12334_text.png or text_122344564512334_text.png
// If no decimals, 10 first number are the seconds
std::list<std::string> list = uSplit(*iter, '.');
if(list.size() == 3)
if(list.size() == 3 || list.size() == 2)
{
list.pop_back(); // remove extension
std::string decimals = uSplitNumChar(list.back()).front();
list.pop_back();
std::string sec = uSplitNumChar(list.back()).back();
double stamp = uStr2Double(sec + "." + decimals);
double stamp = 0.0;
if(list.size() == 1)
{
std::list<std::string> numberList = uSplitNumChar(list.front());
for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
{
if(uIsNumber(*iter))
{
std::string decimals;
std::string sec;
if(iter->length()>10)
{
decimals = iter->substr(10, iter->size()-10);
sec = iter->substr(0, 10);
}
else
{
sec = *iter;
}
stamp = uStr2Double(sec + "." + decimals);
break;
}
}
}
else
{
std::string decimals = uSplitNumChar(list.back()).front();
list.pop_back();
std::string sec = uSplitNumChar(list.back()).back();
stamp = uStr2Double(sec + "." + decimals);
}
if(stamp > 0.0)
{
_stamps.push_back(stamp);
@@ -338,19 +368,19 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
UERROR("Cannot read pose file \"%s\".", filePath.c_str());
return false;
}
else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
else if((format != 1 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
{
UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
"the pose file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), filePath.c_str());
return false;
}
else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
else if((format == 1 || format == 5 || format == 6 || format == 7 || format == 9) && inOutStamps.size() == 0)
{
UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
return false;
}
else if(format == 1 || format == 5 || format == 6 || format == 7)
else if(format == 1 || format == 5 || format == 6 || format == 7 || format == 9)
{
UDEBUG("");
//Match ground truth values with images
@@ -384,7 +414,6 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
{
warned = true;
if(!warned)
{
UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
@@ -393,6 +422,7 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
stampEnd,
maxTimeDiff);
}
warned=true;
}
else
{
@@ -744,7 +774,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
if(_scanForceGroundNormalsUp)
{
util3d::adjustNormalsToViewPoint(cloudNormals, Eigen::Vector3f(0,0,0), _scanForceGroundNormalsUp);
}
scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
UDEBUG("Normals computed (k=%d radius=%f)", _scanNormalsK, _scanNormalsRadius);
}
else
{
corelib/src/CameraThread.cpp
+31 -2
View File
@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -49,6 +49,7 @@ namespace rtabmap
CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_camera(camera),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_stereoToDepth(false),
@@ -269,6 +270,33 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
if(info) info->timeMirroring = timer.ticks();
}
if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
{
#if CV_MAJOR_VERSION < 3
UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.")
#else
UDEBUG("");
UTimer timer;
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
cv::Mat img = data.imageRaw().clone();
compensator->apply(0, cv::Point(0,0), img, masks[0]);
data.setImageRaw(img);
img = data.rightRaw().clone();
compensator->apply(1, cv::Point(0,0), img, masks[1]);
data.setDepthOrRightRaw(img);
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
if(info) info->timeStereoExposureCompensation = timer.ticks();
#endif
}
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
@@ -284,7 +312,8 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().localTransform(),
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().imageSize());
data.setCameraModel(model);
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
corelib/src/DBDriver.cpp
+1 -1
View File
@@ -1070,7 +1070,7 @@ cv::Mat DBDriver::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
#else
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
#endif
corelib/src/FlannIndex.cpp
+19 -10
View File
@@ -38,7 +38,8 @@ FlannIndex::FlannIndex():
featuresType_(0),
featuresDim_(0),
isLSH_(false),
useDistanceL1_(false)
useDistanceL1_(false),
rebalancingFactor_(2.0f)
{
}
FlannIndex::~FlannIndex()
@@ -134,7 +135,8 @@ unsigned int FlannIndex::memoryUsed() const
void FlannIndex::buildLinearIndex(
const cv::Mat & features,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -142,6 +144,7 @@ void FlannIndex::buildLinearIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::LinearIndexParams params;
@@ -180,7 +183,8 @@ void FlannIndex::buildLinearIndex(
void FlannIndex::buildKDTreeIndex(
const cv::Mat & features,
int trees,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -188,6 +192,7 @@ void FlannIndex::buildKDTreeIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeIndexParams params(trees);
@@ -227,7 +232,8 @@ void FlannIndex::buildKDTreeSingleIndex(
const cv::Mat & features,
int leafMaxSize,
bool reorder,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -235,6 +241,7 @@ void FlannIndex::buildKDTreeSingleIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
@@ -274,7 +281,8 @@ void FlannIndex::buildLSHIndex(
const cv::Mat & features,
unsigned int table_number,
unsigned int key_size,
unsigned int multi_probe_level)
unsigned int multi_probe_level,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -282,6 +290,7 @@ void FlannIndex::buildLSHIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = true;
rebalancingFactor_ = rebalancingFactor;
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
@@ -315,8 +324,8 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
// Rebuild index if it is now X times in size
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -333,7 +342,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -347,7 +356,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -361,7 +370,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
corelib/src/Graph.cpp
+76 -9
View File
@@ -161,10 +161,10 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1
std::map<int, double> * stamps) // optional for format 1 and 9
{
UDEBUG("%s format=%d", filePath.c_str(), format);
if(format==3) // TORO
@@ -202,12 +202,50 @@ bool importPoses(
std::string str;
std::getline(file, str);
if(str.size() && str.at(str.size()-1) == '\r')
{
str = str.substr(0, str.size()-1);
}
if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
{
continue;
}
if(format == 8) // Karlsruhe format
if(format == 9) // EuRoC format
{
std::list<std::string> strList = uSplit(str, ',');
if(strList.size() == 17)
{
double stamp = uStr2Double(strList.front())/1000000000.0;
strList.pop_front();
std::vector<std::string> v = uListToVector(strList);
Transform pose(uStr2Float(v[0]), uStr2Float(v[1]), uStr2Float(v[2]), // x y z
uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[3])); // qw qx qy qz -> qx qy qz qw
if(pose.isNull())
{
UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
}
else
{
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
// we need to rotate from IMU frame to world frame
Transform t( 0, 0, 1, 0,
0, -1, 0, 0,
1, 0, 0, 0);
pose = pose * t;
poses.insert(std::make_pair(id, pose));
}
}
else
{
UERROR("Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)", str.c_str());
}
}
else if(format == 8) // Karlsruhe format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 10)
@@ -364,7 +402,7 @@ bool importPoses(
}
else
{
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y)", str.c_str());
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
}
}
else if(format == 1) // rgbd-slam format
@@ -911,6 +949,20 @@ std::multimap<int, int>::const_iterator findLink(
return links.end();
}
std::multimap<int, Link> filterDuplicateLinks(
const std::multimap<int, Link> & links)
{
std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(graph::findLink(output, iter->second.from(), iter->second.to(), true) == output.end())
{
output.insert(*iter);
}
}
return output;
}
std::multimap<int, Link> filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType)
@@ -1820,6 +1872,20 @@ int findNearestNode(
const rtabmap::Transform & targetPose)
{
int id = 0;
std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
if(nearestNodes.size())
{
id = nearestNodes[0];
}
return id;
}
std::vector<int> findNearestNodes(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose,
int k)
{
std::vector<int> nearestIds;
if(nodes.size() && !targetPose.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -1837,14 +1903,15 @@ int findNearestNode(
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
kdTree->nearestKSearch(pt, 1, ind, dist);
if(ind.size() && dist.size() && ind[0] >= 0)
kdTree->nearestKSearch(pt, k, ind, dist);
nearestIds.resize(ind.size());
for(unsigned int i=0; i<ind.size(); ++i)
{
//UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
id = ids[ind[0]];
nearestIds[i] = ids[ind[i]];
}
}
return id;
return nearestIds;
}
// return <id, sqrd distance>, excluding query
corelib/src/Link.cpp
+2 -2
View File
@@ -65,13 +65,13 @@ Link::Link(int from,
double Link::rotVariance() const
{
double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
double min = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
UASSERT(min > 0.0);
return 1.0/min;
}
double Link::transVariance() const
{
double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
double min = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
UASSERT(min > 0.0);
return 1.0/min;
}
corelib/src/Memory.cpp
+100 -65
View File
@@ -84,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
_depthAsMask(Parameters::defaultMemDepthAsMask()),
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
@@ -98,6 +99,7 @@ Memory::Memory(const ParametersMap & parameters) :
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visCorType(Parameters::defaultVisCorType()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -428,38 +430,49 @@ Memory::~Memory()
void Memory::parseParameters(const ParametersMap & parameters)
{
uInsert(parameters_, parameters);
ParametersMap params = parameters;
UDEBUG("");
ParametersMap::const_iterator iter;
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(parameters, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(params, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
Parameters::parse(params, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(params, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(params, Parameters::kMemCompressionParallelized(), _compressionParallelized);
Parameters::parse(params, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kVisCorType(), _visCorType);
if(_visCorType != 0)
{
UWARN("%s is not 0 (Features Matching), the only approach supported for loop closure transformation estimation. Setting to 0...",
Parameters::kVisCorType().c_str());
_visCorType = 0;
uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
}
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -477,23 +490,23 @@ void Memory::parseParameters(const ParametersMap & parameters)
if(_dbDriver)
{
_dbDriver->parseParameters(parameters);
_dbDriver->parseParameters(params);
}
// Keypoint stuff
if(_vwd)
{
_vwd->parseParameters(parameters);
_vwd->parseParameters(params);
}
Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(params, Parameters::kKpBadSignRatio(), _badSignRatio);
//Keypoint detector
UASSERT(_feature2D != 0);
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
if((iter=params.find(Parameters::kKpDetectorStrategy())) != params.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
@@ -517,11 +530,11 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
else if(_feature2D)
{
_feature2D->parseParameters(parameters);
_feature2D->parseParameters(params);
}
Registration::Type regStrategy = Registration::kTypeUndef;
if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
if((iter=params.find(Parameters::kRegStrategy())) != params.end())
{
regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
}
@@ -538,23 +551,23 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
else if(_registrationPipeline)
{
_registrationPipeline->parseParameters(parameters);
_registrationPipeline->parseParameters(params);
}
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
_registrationIcp->parseParameters(params);
}
if(_occupancy)
{
_occupancy->parseParameters(parameters);
_occupancy->parseParameters(params);
}
// do this after all parameters are parsed
// do this after all params are parsed
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
iter = params.find(Parameters::kMemIncrementalMemory());
if(iter != params.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
@@ -578,6 +591,24 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
_incrementalMemory = value;
}
if(_useOdometryFeatures)
{
int visFeatureType = Parameters::defaultVisFeatureType();
int kpDetectorStrategy = Parameters::defaultKpDetectorStrategy();
Parameters::parse(parameters_, Parameters::kVisFeatureType(), visFeatureType);
Parameters::parse(parameters_, Parameters::kKpDetectorStrategy(), kpDetectorStrategy);
if(visFeatureType != kpDetectorStrategy)
{
UWARN("%s is enabled, but %s and %s parameters are not the same! Disabling %s...",
Parameters::kMemUseOdomFeatures().c_str(),
Parameters::kVisFeatureType().c_str(),
Parameters::kKpDetectorStrategy().c_str(),
Parameters::kMemUseOdomFeatures().c_str());
_useOdometryFeatures = false;
uInsert(parameters_, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
}
}
}
void Memory::preUpdate()
@@ -1065,6 +1096,7 @@ std::map<int, int> Memory::getNeighborsId(
bool incrementMarginOnLoop, // default false
bool ignoreLoopIds, // default false
bool ignoreIntermediateNodes, // default false
bool ignoreLocalSpaceLoopIds, // default false, ignored if ignoreLoopIds=true
const std::set<int> & nodesSet,
double * dbAccessTime
) const
@@ -1150,7 +1182,7 @@ std::map<int, int> Memory::getNeighborsId(
nextMargin.insert(iter->first);
}
}
else if(!ignoreLoopIds)
else if(!ignoreLoopIds && (!ignoreLocalSpaceLoopIds || iter->second.type()!=Link::kLocalSpaceClosure))
{
if(incrementMarginOnLoop)
{
@@ -1709,7 +1741,7 @@ cv::Mat Memory::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
#else
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
#endif
@@ -2277,13 +2309,13 @@ Transform Memory::computeTransform(
// make sure we have all data needed
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
{
fromS.sensorData() = getNodeData(fromS.id());
}
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
{
@@ -2292,13 +2324,13 @@ Transform Memory::computeTransform(
// uncompress only what we need
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
fromS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
toS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
@@ -2331,11 +2363,14 @@ Transform Memory::computeTransform(
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
}
if(guess.isNull() && !_registrationPipeline->isImageRequired())
if(guess.isNull() && (!_registrationPipeline->isImageRequired() || _visCorType==1))
{
UDEBUG("");
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
// make sure feature matching is used instead of optical flow to compute the guess
ParametersMap parameters = parameters_;
uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
RegistrationVis regVis(parameters);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
if(!guess.isNull())
{
@@ -3399,7 +3434,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
}
if(_parallelized)
if(_parallelized && !isIntermediateNode)
{
UDEBUG("Start dictionary update thread");
preUpdateThread.start();
@@ -3447,7 +3482,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
cv::Mat depthMask;
if(!decimatedData.depthRaw().empty())
if(!decimatedData.depthRaw().empty() && _depthAsMask)
{
if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3607,20 +3642,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("Joining dictionary update thread... thread finished!");
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
if(_parallelized)
{
UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
else
{
UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
std::list<int> wordIds;
if(descriptors.rows)
{
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
if(_parallelized)
{
UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
else
{
UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
// In case the number of features we want to do quantization is lower
// than extracted ones (that would be used for transform estimation)
std::vector<bool> inliers;
corelib/src/OccupancyGrid.cpp
+121 -30
View File
@@ -70,7 +70,10 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
erode_(Parameters::defaultGridGlobalEroded()),
footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
xMin_(0.0f),
yMin_(0.0f)
yMin_(0.0f),
cloudAssembling_(false),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>)
{
this->parseParameters(parameters);
}
@@ -198,6 +201,16 @@ void OccupancyGrid::setCellSize(float cellSize)
}
}
void OccupancyGrid::setCloudAssembling(bool enabled)
{
cloudAssembling_ = enabled;
if(!cloudAssembling_)
{
assembledGround_->clear();
assembledObstacles_->clear();
}
}
void OccupancyGrid::createLocalMap(
const Signature & node,
cv::Mat & ground,
@@ -386,9 +399,11 @@ void OccupancyGrid::clear()
xMin_ = 0.0f;
yMin_ = 0.0f;
addedNodes_.clear();
assembledGround_->clear();
assembledObstacles_->clear();
}
const cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
{
xMin = xMin_;
yMin = yMin_;
@@ -470,6 +485,9 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
bool assembledGroundUpdated = false;
bool assembledObstaclesUpdated = false;
if(graphOptimized || graphChanged)
{
if(graphChanged)
@@ -481,6 +499,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
UINFO("Graph optimized!");
}
if(cloudAssembling_)
{
assembledGround_->clear();
assembledObstacles_->clear();
}
if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
{
// 1) recreate all local maps
@@ -489,27 +513,32 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
{
if(iter->second.first)
if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
if(iter->second.first)
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
for(int y=0; y<map_.rows; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
for(int x=0; x<map_.cols; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
if(tmpIndices.find(nodeId)!=tmpIndices.end())
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
UASSERT(tter != transforms.end());
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
@@ -547,9 +576,26 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
}
else if(nodeId > 0)
{
UERROR("Cell referred b node %d is unknown!?", nodeId);
}
}
}
//verify if all cells were added
for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
{
std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
(iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
jter = occupiedLocalMaps.find(iter->first);
UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
(iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
}
@@ -575,25 +621,29 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
std::list<std::pair<int, Transform> > poses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
// add old poses that were not in the current map (they were just retrieved from LTM)
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(0); iter!=posesIn.end(); ++iter)
{
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
poses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
{
if(iter->first < 0)
{
if(iter->first < 0)
{
poses.push_back(*iter);
}
else
{
break;
}
poses.push_back(*iter);
}
else
{
break;
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -662,6 +712,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
maxY = vo[1];
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
if(cloudAssembling_)
{
*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first, iter->second, 0, 255, 0);
assembledGroundUpdated = true;
}
}
//obstacles
@@ -698,6 +754,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
maxY = vo[1];
}
uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
if(cloudAssembling_)
{
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 255, 0, 0);
assembledObstaclesUpdated = true;
}
}
}
}
@@ -770,6 +832,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
UASSERT(deltaX>=0 && deltaY>=0);
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -802,8 +866,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
float * ptf = iter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
@@ -912,8 +976,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
float * ptf = jter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -1091,10 +1155,37 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
if(!fullUpdate_)
if(cloudAssembling_)
{
if(assembledGroundUpdated && assembledGround_->size() > 1)
{
assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
}
if(assembledObstaclesUpdated && assembledGround_->size() > 1)
{
assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
}
}
if(!fullUpdate_ && !cloudAssembling_)
{
cache_.clear();
}
else
{
//clear only negative ids
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
{
if(iter->first < 0)
{
cache_.erase(iter++);
}
else
{
break;
}
}
}
UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
}
corelib/src/OctoMap.cpp
+58 -18
View File
@@ -35,13 +35,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(voxelSize)),
OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
hasColor_(false),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate())
{
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
UASSERT(cellSize>0.0f);
octree_ = new octomap::ColorOcTree(cellSize);
octree_->setOccupancyThres(occupancyThr);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
}
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(cellSize)),
hasColor_(false),
fullUpdate_(fullUpdate)
{
octree_->setOccupancyThres(occupancyThr);
UASSERT(voxelSize>0.0f);
UASSERT(cellSize>0.0f);
}
OctoMap::~OctoMap()
@@ -174,7 +187,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
// Note: normal if old nodes were transfered to LTM
//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
@@ -191,25 +205,29 @@ void OctoMap::update(const std::map<int, Transform> & poses)
// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
//
std::list<std::pair<int, Transform> > orderedPoses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
// add old poses that were not in the current map (they were just retrieved from LTM)
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(0); iter!=poses.end(); ++iter)
{
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
orderedPoses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first < 0)
{
if(iter->first < 0)
{
orderedPoses.push_back(*iter);
}
else
{
break;
}
orderedPoses.push_back(*iter);
}
else
{
break;
}
}
@@ -259,7 +277,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
if ((iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
@@ -267,6 +286,16 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
@@ -309,7 +338,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
if ((iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
@@ -317,6 +347,16 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
corelib/src/Odometry.cpp
+8
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UProcessInfo.h"
#include "rtabmap/core/ParticleFilter.h"
#include "rtabmap/core/util2d.h"
@@ -99,6 +100,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_imageDecimation(Parameters::defaultOdomImageDecimation()),
_alignWithGround(Parameters::defaultOdomAlignWithGround()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0),
previousStamp_(0),
@@ -125,6 +127,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
if(_imageDecimation == 0)
{
_imageDecimation = 1;
@@ -398,6 +402,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
info->stamp = data.stamp();
info->interval = dt;
info->transform = t;
if(_publishRAMUsage)
{
info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
}
if(!data.groundTruth().isNull())
{
corelib/src/OdometryF2F.cpp
+46
View File
@@ -49,6 +49,8 @@ OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
UASSERT(visKeyFrameThr_>=0);
UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
parameters_ = parameters;
}
OdometryF2F::~OdometryF2F()
@@ -96,6 +98,24 @@ Transform OdometryF2F::computeTransform(
Signature newFrame(data);
if(refFrame_.sensorData().isValid())
{
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!registrationPipeline_->isImageRequired() &&
registrationPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
registrationPipeline_->parseParameters(params);
}
Signature tmpRefFrame = refFrame_;
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
@@ -104,6 +124,15 @@ Transform OdometryF2F::computeTransform(
!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
registrationPipeline_->parseParameters(params);
}
if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
{
tmpRefFrame = refFrame_;
@@ -112,12 +141,29 @@ Transform OdometryF2F::computeTransform(
newFrame.setWords3(std::multimap<int, cv::Point3f>());
newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
// If optical flow is used, switch temporary to feature matching
int visCorTypeBackup = Parameters::defaultVisCorType();
Parameters::parse(parameters_, Parameters::kVisCorType(), visCorTypeBackup);
if(visCorTypeBackup == 1)
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kVisCorType(), "0"));
registrationPipeline_->parseParameters(params);
}
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
Transform(), // null guess
&regInfo);
if(visCorTypeBackup == 1)
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kVisCorType(), "1"));
registrationPipeline_->parseParameters(params);
}
if(output.isNull())
{
UWARN("Trial with no guess still fail.");
corelib/src/OdometryF2M.cpp
+299 -226
View File
@@ -167,6 +167,7 @@ void OdometryF2M::reset(const Transform & initialPose)
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
}
// return not null transform if odometry is correctly computed
@@ -209,250 +210,275 @@ Transform OdometryF2M::computeTransform(
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().empty()) &&
lastFrame_->sensorData().isValid())
{
Signature tmpMap = *map_;
Signature tmpMap;
Transform transform;
UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
// bundle adjustment stuff if used
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, StereoCameraModel> bundleStereoModels;
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
// special case for ICP-only odom, set guess to identity if we just started or reset
!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
regPipeline_->parseParameters(params);
}
if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
for(int guessIteration=0;
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
++guessIteration)
{
tmpMap = *map_;
// reset matches, but keep already extracted features in lastFrame_->sensorData()
lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
points3DMap.clear();
bundlePoses.clear();
bundleLinks.clear();
bundleModels.clear();
bundleStereoModels.clear();
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
if(guessIteration == 1)
{
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
}
transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
Transform(), // null guess
// special case for ICP-only odom, set guess to identity if we just started or reset
guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
regPipeline_->parseParameters(params);
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
bundleAdjustment_ = 0;
}
else
{
UASSERT(bundlePoses_.size());
UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
UASSERT(bundlePoses_.size() == bundleModels_.size());
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
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();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references
std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
std::map<int, cv::Point3f> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
references.insert(*jter);
++totalBundleWordReferencesUsed;
}
}
//make sure the last reference is here
if(refIter->second.size() > 1)
{
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
//{
// UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
//}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
totalBundleOutliers = (int)sbaOutliers.size();
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
{
if(!bundlePoses.rbegin()->second.isNull())
{
if(sbaOutliers.size())
{
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}
if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
{
regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
transform.setNull();
}
else
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Local bundle adjustment failed! transform is not refined.");
}
}
}
if(!transform.isNull())
{
// make it incremental
transform = this->getPose().inverse() * transform;
}
}
if(transform.isNull())
{
UWARN("Trial with no guess still fail.");
}
else
{
UWARN("Trial with no guess succeeded.");
}
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
UDEBUG("Registration time = %fs", regInfo.totalTime);
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, StereoCameraModel> bundleStereoModels;
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
if(guessIteration == 1)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
bundleAdjustment_ = 0;
UWARN("Trial with no guess still fail.");
}
if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UASSERT(bundlePoses_.size());
UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
UASSERT(bundlePoses_.size() == bundleModels_.size());
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
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();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references
std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
std::map<int, cv::Point3f> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
references.insert(*jter);
++totalBundleWordReferencesUsed;
}
}
//make sure the last reference is here
if(refIter->second.size() > 1)
{
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
//{
// UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
//}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
totalBundleOutliers = (int)sbaOutliers.size();
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
{
if(!bundlePoses.rbegin()->second.isNull())
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
if(sbaOutliers.size())
{
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Local bundle adjustment failed! transform is not refined.");
}
UWARN("Unknown registration error");
}
}
// make it incremental
transform = this->getPose().inverse() * transform;
}
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Unknown registration error");
else if(guessIteration == 1)
{
UWARN("Trial with no guess succeeded!");
}
}
if(!transform.isNull())
@@ -579,6 +605,8 @@ Transform OdometryF2M::computeTransform(
}
UDEBUG("newIds=%d", (int)newIds.size());
int lastFrameOldestNewId = lastFrameOldestNewId_;
lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
iter!=newIds.rend();
++iter)
@@ -610,6 +638,10 @@ Transform OdometryF2M::computeTransform(
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));
if(lastFrameOldestNewId_ > iter->second.first)
{
lastFrameOldestNewId_ = iter->second.first;
}
++added;
}
}
@@ -617,14 +649,55 @@ Transform OdometryF2M::computeTransform(
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features with their aliases
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
// remove oldest outliers first
std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
std::vector<int> ids = regInfo.matchesIDs;
if(regInfo.projectedIDs.size())
{
ids.resize(ids.size() + regInfo.projectedIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
{
if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
{
ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
}
}
ids.resize(regInfo.matchesIDs.size()+oi);
UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
}
for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
{
int id = ids.at(i);
if(inliers.find(id) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(id);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterFrame->first) -= 1;
}
}
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(id);
mapDescriptors.erase(id);
mapWords.erase(id);
++removed;
}
}
// remove oldest first
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())
if(inliers.find(iter->first) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
corelib/src/OdometryFovis.cpp
+6 -1
View File
@@ -143,7 +143,12 @@ Transform OdometryFovis::computeTransform(
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection())))
{
UERROR("Invalid camera model!");
UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo camera=%d (reproj=%d|%d)",
(int)data.cameraModels().size(),
data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
data.stereoCameraModel().isValidForProjection()?1:0,
data.stereoCameraModel().left().isValidForReprojection()?1:0,
data.stereoCameraModel().right().isValidForReprojection()?1:0);
return t;
}
corelib/src/OdometryORBSLAM2.cpp
+66 -7
View File
@@ -46,11 +46,14 @@ class Tracker: public Tracking
{
public:
Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor)
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor),
maxFeatureMapSize_(maxFeatureMapSize)
{
}
private:
long unsigned int maxFeatureMapSize_;
protected:
void Track()
@@ -248,7 +251,60 @@ protected:
// Check if we need to insert a new keyframe
if(NeedNewKeyFrame())
{
CreateNewKeyFrame();
}
if(maxFeatureMapSize_ > 0)
{
//limit size of the feature map, keep last X recent ones
if(mpMap->KeyFramesInMap()>1 && mpMap->MapPointsInMap()>maxFeatureMapSize_)
{
std::vector<KeyFrame*> kfs = mpMap->GetAllKeyFrames();
std::map<long unsigned int, KeyFrame*> kfsSorted;
for(unsigned int i=1; i<kfs.size(); ++i)
{
kfsSorted.insert(std::make_pair(kfs[i]->mnId, kfs[i]));
}
KeyFrame * lastFrame = kfsSorted.rbegin()->second;
std::vector<MapPoint*> mapPoints = mpMap->GetAllMapPoints();
std::map<long unsigned int, MapPoint*> mapPointsSorted;
for(unsigned int i=0; i<mapPoints.size(); ++i)
{
mapPointsSorted.insert(std::make_pair(mapPoints[i]->mnId, mapPoints[i]));
}
for(std::map<long unsigned int, MapPoint*>::iterator iter=mapPointsSorted.begin();
iter != mapPointsSorted.end() && mpMap->MapPointsInMap()>maxFeatureMapSize_;
++iter)
{
if(!iter->second->IsInKeyFrame(lastFrame))
{
// FIXME: Memory leak: ORB_SLAM2 doesn't delete after removing from the map...
// Not sure when it is safe to delete it, as if I delete just
// after setting the bad flag, the app crashes.
iter->second->SetBadFlag();
}
}
// remove kfs without observations
for(std::map<long unsigned int, KeyFrame*>::iterator iter=kfsSorted.begin();
iter != kfsSorted.end();
++iter)
{
if(iter->second!=lastFrame && iter->second->GetMapPoints().size()==0)
{
// FIXME: Memory leak: ORB_SLAM2 doesn't delete after removing from the map...
// Not sure when it is safe to delete it, as if I delete just
// after setting the bad flag, the app crashes.
iter->second->SetBadFlag();
}
else
{
break;
}
}
}
}
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
@@ -466,7 +522,7 @@ public:
if(CheckFinish())
break;
usleep(30000);
usleep(1000000); // 1 sec
}
SetFinish();
@@ -639,6 +695,9 @@ public:
ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
ofs << std::endl;
int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAM2MapSize();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MapSize(), maxFeatureMapSize);
ofs.close();
//Create KeyFrame Database
@@ -649,7 +708,7 @@ public:
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD);
mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD, maxFeatureMapSize);
//Initialize the Local Mapping thread and launch
mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false);
@@ -891,9 +950,9 @@ Transform OdometryORBSLAM2::computeTransform(
covariance.at<double>(0,0) = linearVar;
covariance.at<double>(1,1) = linearVar;
covariance.at<double>(2,2) = linearVar;
covariance.at<double>(3,3) = 0.01;
covariance.at<double>(4,4) = 0.01;
covariance.at<double>(5,5) = 0.01;
covariance.at<double>(3,3) = 0.0001;
covariance.at<double>(4,4) = 0.0001;
covariance.at<double>(5,5) = 0.0001;
}
}
corelib/src/OdometryViso2.cpp
+4 -4
View File
@@ -163,10 +163,10 @@ Transform OdometryViso2::computeTransform(
if(viso2_ == 0)
{
VisualOdometryStereo::parameters params;
params.base = data.stereoCameraModel().baseline();
params.calib.cu = data.stereoCameraModel().left().cx();
params.calib.cv = data.stereoCameraModel().left().cy();
params.calib.f = data.stereoCameraModel().left().fx();
params.base = params.match.base = data.stereoCameraModel().baseline();
params.calib.cu = params.match.cu = data.stereoCameraModel().left().cx();
params.calib.cv = params.match.cv = data.stereoCameraModel().left().cy();
params.calib.f = params.match.f = data.stereoCameraModel().left().fx();
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);
corelib/src/OptimizerG2O.cpp
+1 -1
View File
@@ -965,7 +965,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
if(baseline > 0.0)
{
UWARN("Stereo camera model detected but current "
UDEBUG("Stereo camera model detected but current "
"observation (pt=%d to cam=%d) has null depth (%f m), adding "
"mono observation instead.",
vpt3d->id()-stepVertexId, camId, depth);
corelib/src/Parameters.cpp
+2 -1
View File
@@ -172,7 +172,8 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
(icp && group.compare("Icp") == 0) ||
(vis && Parameters::isFeatureParameter(iter->first)) ||
group.compare("Reg") == 0 ||
(vis && group.compare("Vis") == 0))
(vis && group.compare("Vis") == 0) ||
iter->first.compare(kRtabmapPublishRAMUsage())==0)
{
if(stereo)
{
corelib/src/Recovery.cpp
+6 -1
View File
@@ -155,7 +155,12 @@ bool databaseRecovery(
}
else
{
if(!rtabmap.process(data, info.odomPose, info.odomCovariance))
if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
{
status = uFormat("High variance detected, triggering a new map...");
rtabmap.triggerNewMap();
}
if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity))
{
status = uFormat("Failed processing node %d.", data.id());
}
corelib/src/Registration.cpp
+11 -1
View File
@@ -119,6 +119,16 @@ bool Registration::isUserDataRequired() const
return val;
}
bool Registration::canUseGuess() const
{
bool val = canUseGuessImpl();
if(!val && child_)
{
val = child_->canUseGuess();
}
return val;
}
int Registration::getMinVisualCorrespondences() const
{
int min = this->getMinVisualCorrespondencesImpl();
@@ -197,7 +207,7 @@ Transform Registration::computeTransformationMod(
}
Transform t = computeTransformationImpl(from, to, guess, info);
if(repeatOnce_ && guess.isNull() && !t.isNull())
if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
{
// redo with guess to get a more accurate transform
t = computeTransformationImpl(from, to, t, info);
corelib/src/RegistrationVis.cpp
+47 -27
View File
@@ -67,7 +67,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_nndr(Parameters::defaultVisCorNNDR()),
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment())
_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -110,6 +111,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
uInsert(_bundleParameters, parameters);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -229,6 +231,7 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.sensorData().imageRaw().rows);
std::string msg;
info.projectedIDs.clear();
////////////////////
// Find correspondences
@@ -288,7 +291,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!fromSignature.sensorData().depthRaw().empty())
if(!fromSignature.sensorData().depthRaw().empty() && _depthAsMask)
{
if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
@@ -496,7 +499,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!toSignature.sensorData().depthRaw().empty())
if(!toSignature.sensorData().depthRaw().empty() && _depthAsMask)
{
if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
@@ -604,14 +607,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).",
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.getWords3().size());
}
else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsfrom).",
"is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.sensorData().keypoints3D().size());
}
@@ -674,14 +677,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
{
UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).",
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
(int)kptsTo.size(),
(int)toSignature.getWords3().size());
}
else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
{
UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo).",
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
(int)kptsTo.size(),
(int)toSignature.sensorData().keypoints3D().size());
}
@@ -961,6 +964,12 @@ Transform RegistrationVis::computeTransformationImpl(
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
{
int matchedIndexFrom = projectedIndexToDescIndex[i];
if(indices[i].size())
{
info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
}
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
{
int matchedIndexTo = -1;
@@ -973,6 +982,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
descriptors.resize(indices[i].size());
}
std::list<int> indicesToIgnoretmp;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octave = kptsTo[indices[i].at(j)].octave;
@@ -981,10 +991,7 @@ Transform RegistrationVis::computeTransformationImpl(
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
if(dists[i].at(j) < radius)
{
indicesToIgnore.insert(indices[i].at(j));
}
indicesToIgnoretmp.push_back(indices[i].at(j));
}
}
bruteForceDescCopy += bruteForceTimer.ticks();
@@ -999,6 +1006,8 @@ Transform RegistrationVis::computeTransformationImpl(
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
indicesToIgnore.insert(indicesToIgnore.begin(), indicesToIgnore.end());
}
}
else if(oi == 1)
@@ -1186,6 +1195,8 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
int inliersCount = 0;
int matchesCount = 0;
info.inliersIDs.clear();
info.matchesIDs.clear();
if(toSignature.getWords().size())
{
Transform transforms[2];
@@ -1565,22 +1576,6 @@ Transform RegistrationVis::computeTransformationImpl(
!optimizedPoses.rbegin()->second.isNull())
{
UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
transforms[0] = optimizedPoses.rbegin()->second;
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
Transform invT = transforms[0].inverse();
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
cpyWordsFrom3.find(iter->first)->second = iter->second;
if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
{
cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
}
}
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
if(sbaOutliers.size())
{
@@ -1597,6 +1592,31 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
allInliers = newInliers;
}
if((int)allInliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
transforms[0].setNull();
}
else
{
transforms[0] = optimizedPoses.rbegin()->second;
}
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
Transform invT = transforms[0].inverse();
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
cpyWordsFrom3.find(iter->first)->second = iter->second;
if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
{
cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
}
}
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
}
}
corelib/src/Rtabmap.cpp
+19 -10
View File
@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UProcessInfo.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/crop_box.h>
@@ -79,6 +80,7 @@ Rtabmap::Rtabmap() :
_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
@@ -401,6 +403,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
@@ -1506,6 +1509,7 @@ bool Rtabmap::process(
true,
true,
false,
true,
std::set<int>(),
&timeGetNeighborsTimeDb);
ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
@@ -1561,6 +1565,7 @@ bool Rtabmap::process(
true,
false,
false,
false,
std::set<int>(),
&timeGetNeighborsSpaceDb);
ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
@@ -1975,7 +1980,7 @@ bool Rtabmap::process(
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.covariance.inv()));
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(loopClosureVisualInliers == 0)
@@ -2246,14 +2251,14 @@ bool Rtabmap::process(
{
UWARN("Graph optimization failed! Rejecting last loop closures added.");
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedHypothesis = true;
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedHypothesis = true;
}
else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
_optimizationMaxLinearError > 0.0f &&
@@ -2438,6 +2443,10 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
if(_publishRAMUsage)
{
statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
}
if(_publishLikelihood || _publishPdf)
{
@@ -3068,7 +3077,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
std::map<int, Transform> path;
// select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, nodesSet);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
corelib/src/Stereo.cpp
+4 -3
View File
@@ -151,9 +151,10 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed, images are not calibrated, "
"the background is too far (no disparity between the images) or "
"maximum disparity may be too small (%d).",
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
countFlowRejected+countDisparityRejected,
(int)status.size(),
this->maxDisparity());
corelib/src/Transform.cpp
+5
View File
@@ -92,6 +92,11 @@ Transform::Transform(float x, float y, float theta)
*this = fromEigen3f(t);
}
Transform Transform::clone() const
{
return Transform(data_.clone());
}
bool Transform::isNull() const
{
return (data_.empty() ||
corelib/src/VWDictionary.cpp
+8 -6
View File
@@ -64,6 +64,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_totalActiveReferences(0),
_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
_nndrRatio(Parameters::defaultKpNndrRatio()),
_dictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
@@ -88,6 +89,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
@@ -363,15 +365,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_);
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_);
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2);
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
break;
default:
UFATAL("Not supposed to be here!");
@@ -486,15 +488,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_);
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_);
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2);
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
break;
default:
break;
corelib/src/util2d.cpp
+1 -1
View File
@@ -2019,7 +2019,7 @@ cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
fusion.convertTo(rgb8, CV_8UC3, 255.0);
fusion = rgb8;
#else
UWARN("Exposure fusion is only avaiable when rtabmap is built with OpenCV3.");
UWARN("Exposure fusion is only available when rtabmap is built with OpenCV3.");
if (images.size())
{
fusion = images[0].clone();
corelib/src/util3d.cpp
+20
View File
@@ -240,6 +240,26 @@ pcl::PointXYZ projectDepthTo3D(
return pt;
}
Eigen::Vector3f projectDepthTo3DRay(
const cv::Size & imageSize,
float x, float y,
float cx, float cy,
float fx, float fy)
{
Eigen::Vector3f ray;
// Use correct principal point from calibration
cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f; //cameraInfo.K.at(2)
cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f; //cameraInfo.K.at(5)
// Fill in XYZ
ray[0] = (x - cx) / fx;
ray[1] = (y - cy) / fy;
ray[2] = 1.0f;
return ray;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
corelib/src/util3d_motion_estimation.cpp
+1 -1
View File
@@ -156,7 +156,7 @@ Transform estimateMotion3DTo2D(
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2)*10.0f;
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
}
}
corelib/src/util3d_surface.cpp
+53
View File
@@ -2629,6 +2629,59 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
return cloud_with_normals;
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//UWARN("Reverse %d of n=%f,%f,%f result=%f", i, cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z, result);
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
}
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
}
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,