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

Added RGBD/ProximityGlobalScanMap for proximity detection using the whole scan map in localization mode. Added Icp RMS statistics (CCCorelib). Updated how localization is corrected with gravity when available (simply use roll and pitch of current gravity constraint).

Browse Source
This commit is contained in:
matlabbe
2021-06-23 17:13:07 -04:00
parent dddf6378ae
commit 11e9837b82
18 changed files with 522 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/include/rtabmap/core/LaserScan.h
View File

@@ -136,8 +136,19 @@ public:
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
float & field(unsigned int pointIndex, unsigned int channelOffset);
void clear() {data_ = cv::Mat();}
/**
* Concatenate scan's data, localTransform is ignored.
*/
LaserScan & operator+=(const LaserScan &);
/**
* Concatenate scan's data, localTransform is ignored.
*/
LaserScan operator+(const LaserScan &);
private:
void init(const cv::Mat & data,
Format format,

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

@@ -242,6 +242,7 @@ public:
Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
Transform computeIcpTransformMulti(
int newId,
int oldId,

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

@@ -385,6 +385,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
RTABMAP_PARAM(RGBD, ProximityAngle, float, 45, "Maximum angle (degrees) for one-to-one proximity detection.");
RTABMAP_PARAM(RGBD, ProximityOdomGuess, bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
RTABMAP_PARAM(RGBD, ProximityGlobalScanMap, bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
// Graph optimization
#ifdef RTABMAP_GTSAM

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

@@ -37,6 +37,7 @@ public:
RegistrationInfo() :
totalTime(0.0),
inliers(0),
inliersRatio(0),
inliersMeanDistance(0.0f),
inliersDistribution(0.0f),
matches(0),
@@ -45,7 +46,8 @@ public:
icpRotation(0.0f),
icpStructuralComplexity(0.0f),
icpStructuralDistribution(0.0f),
icpCorrespondences(0)
icpCorrespondences(0),
icpRMS(0)
{
}
@@ -90,6 +92,7 @@ public:
float icpStructuralComplexity;
float icpStructuralDistribution;
int icpCorrespondences;
float icpRMS;
};
}

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

@@ -248,6 +248,8 @@ private:
void updateGoalIndex();
bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
void createGlobalScanMap();
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
@@ -306,6 +308,7 @@ private:
bool _loopCovLimited;
bool _loopGPS;
int _maxOdomCacheSize;
bool _createGlobalScanMap;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
@@ -341,6 +344,8 @@ private:
int _lastLocalizationNodeId; // for localization mode
std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
bool _currentSessionHasGPS;
LaserScan _globalScanMap;
std::map<int, Transform> _globalScanMapPoses;
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link

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

@@ -122,7 +122,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Proximity, Space_visual_paths_checked,);
RTABMAP_STATS(Proximity, Space_scan_paths_checked,);
RTABMAP_STATS(Proximity, Space_detections_added_visually,);
RTABMAP_STATS(Proximity, Space_detections_added_icp_only,);
RTABMAP_STATS(Proximity, Space_detections_added_icp_multi,);
RTABMAP_STATS(Proximity, Space_detections_added_icp_global,);
RTABMAP_STATS(NeighborLinkRefining, Accepted,);
RTABMAP_STATS(NeighborLinkRefining, Inliers,);

1
corelib/include/rtabmap/core/Transform.h
View File

@@ -103,6 +103,7 @@ public:
Transform rotation() const;
Transform translation() const;
Transform to3DoF() const;
Transform to4DoF() const;
cv::Mat rotationMatrix() const;
cv::Mat translationMatrix() const;

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

@@ -437,6 +437,12 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
const std::vector<int> & rawCameraIndices,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & viewpoints,
const LaserScan & rawScan,
const std::vector<int> & viewpointIds,
LaserScan & scan);
pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
template<typename pointT>

38
corelib/src/LaserScan.cpp
View File

@@ -366,4 +366,42 @@ LaserScan LaserScan::clone() const
return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
}
float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
{
UASSERT(pointIndex < data_.cols);
UASSERT(channelOffset < data_.channels());
return data_.ptr<float>(0, pointIndex)[channelOffset];
}
LaserScan & LaserScan::operator+=(const LaserScan & scan)
{
*this = *this+scan;
return *this;
}
LaserScan LaserScan::operator+(const LaserScan & scan)
{
UASSERT(this->empty() || scan.empty() || this->format() == scan.format());
LaserScan dest;
if(!scan.empty())
{
if(this->empty())
{
dest = LaserScan(scan.data().clone(), 0, 0, this->format());
}
else
{
cv::Mat destData(1, data_.cols + scan.data().cols, data_.type());
data_.copyTo(destData(cv::Range::all(), cv::Range(0,data_.cols)));
scan.data().copyTo(destData(cv::Range::all(), cv::Range(data_.cols, data_.cols+scan.data().cols)));
dest = LaserScan(destData, 0, 0, this->format());
}
}
else
{
dest = this->clone();
}
return dest;
}
}

23
corelib/src/Memory.cpp
View File

@@ -3017,6 +3017,19 @@ Transform Memory::computeTransform(
return transform;
}
Transform Memory::computeIcpTransform(
const Signature & fromS,
const Signature & toS,
Transform guess,
RegistrationInfo * info) const
{
UDEBUG("%d -> %d, Guess=%s", fromS.id(), toS.id(), guess.prettyPrint().c_str());
Signature tmpFrom = fromS;
Signature tmpTo = toS;
return _registrationIcpMulti->computeTransformation(tmpFrom.sensorData(), tmpTo.sensorData(), guess, info);
}
// compute transform fromId -> multiple toId
Transform Memory::computeIcpTransformMulti(
int fromId,
@@ -3042,7 +3055,7 @@ Transform Memory::computeIcpTransformMulti(
}
// make sure that all laser scans are loaded
std::list<Signature*> depthToLoad;
std::list<Signature*> scansToLoad;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
Signature * s = _getSignature(iter->first);
@@ -3051,12 +3064,12 @@ Transform Memory::computeIcpTransformMulti(
if(s->sensorData().imageCompressed().empty() &&
s->sensorData().laserScanCompressed().isEmpty())
{
depthToLoad.push_back(s);
scansToLoad.push_back(s);
}
}
if(depthToLoad.size() && _dbDriver)
if(scansToLoad.size() && _dbDriver)
{
_dbDriver->loadNodeData(depthToLoad, false, true, false, false);
_dbDriver->loadNodeData(scansToLoad, false, true, false, false);
}
Signature * fromS = _getSignature(fromId);
@@ -3081,7 +3094,6 @@ Transform Memory::computeIcpTransformMulti(
}
// Create a fake signature with all scans merged in oldId referential
SensorData assembledData;
Transform toPoseInv = poses.at(toId).inverse();
std::string msg;
int maxPoints = fromScan.size();
@@ -3203,6 +3215,7 @@ Transform Memory::computeIcpTransformMulti(
UDEBUG("assembledScan=%d points", assembledScan.size());
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
SensorData assembledData;
assembledData.setLaserScan(
LaserScan(assembledScan,
maxPoints,

1
corelib/src/RegistrationIcp.cpp
View File

@@ -701,6 +701,7 @@ Transform RegistrationIcp::computeTransformationImpl(
_outlierRatio,
_ccFilterOutFarthestPoints,
_ccMaxFinalRMS,
&info.icpRMS,
&msg);
hasConverged = !icpT.isNull();
}

274
corelib/src/Rtabmap.cpp
View File

@@ -34,6 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
@@ -130,6 +135,7 @@ Rtabmap::Rtabmap() :
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -333,14 +339,27 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
_memory->init(_databasePath, false, allParameters, true);
}
_optimizedPoses.clear();
_constraints.clear();
_globalScanMap.clear();
_globalScanMapPoses.clear();
// Parse all parameters
this->parseParameters(allParameters);
Transform lastPose;
_optimizedPoses.clear();
if(!_memory->isIncremental())
{
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
if(_optimizedPoses.empty() &&
_memory->getWorkingMem().size()>1 &&
_memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
{
cv::Mat cov;
this->optimizeCurrentMap(
_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
false, _optimizedPoses, cov, &_constraints);
}
if(!_optimizedPoses.empty())
{
if(_restartAtOrigin)
@@ -352,9 +371,12 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
if(_constraints.empty())
{
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
}
// Initialize Bayes' prediction matrix
UTimer time;
@@ -369,6 +391,9 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
}
_bayesFilter->computePosterior(_memory, likelihood);
UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
if(_createGlobalScanMap)
createGlobalScanMap();
}
else
{
@@ -419,6 +444,9 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_gpsGeocentricCache.clear();
_currentSessionHasGPS = false;
_globalScanMap.clear();
_globalScanMapPoses.clear();
flushStatisticLogs();
if(_foutFloat)
{
@@ -554,6 +582,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -590,9 +619,26 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
_graphOptimizer = Optimizer::create(optimizerType, parameters);
}
if(!_createGlobalScanMap)
{
_globalScanMap.clear();
_globalScanMapPoses.clear();
}
if(_memory)
{
_memory->parseParameters(parameters);
if(_memory->isIncremental() && !_globalScanMap.empty())
{
UWARN("Map is now incremental, clearing global scan map...");
_globalScanMap.clear();
_globalScanMapPoses.clear();
}
if(_createGlobalScanMap && !_memory->isIncremental() && _globalScanMap.empty() && !_optimizedPoses.empty())
{
this->createGlobalScanMap();
}
}
if(!_epipolarGeometry)
@@ -757,9 +803,9 @@ int Rtabmap::triggerNewMap()
if(!_memory->isIncremental())
{
_mapCorrection.setIdentity();
if(_restartAtOrigin)
{
_mapCorrection.setIdentity();
_lastLocalizationPose.setIdentity();
}
return mapId;
@@ -922,6 +968,8 @@ void Rtabmap::resetMemory()
_distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
_optimizeFromGraphEndChanged = false;
_globalScanMap.clear();
_globalScanMapPoses.clear();
this->clearPath(0);
if(_memory)
@@ -1117,6 +1165,14 @@ bool Rtabmap::process(
}
}
UDEBUG("incremental=%d odomPose=%s optimizedPoses=%d mapCorrection=%s lastLocalizationPose=%s lastLocalizationNodeId=%d",
_memory->isIncremental()?1:0,
odomPose.prettyPrint().c_str(),
(int)_optimizedPoses.size(),
_mapCorrection.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str(),
_lastLocalizationNodeId);
if(!_memory->isIncremental() &&
!odomPose.isNull() &&
_optimizedPoses.size() &&
@@ -1128,7 +1184,18 @@ bool Rtabmap::process(
if(!_optimizeFromGraphEnd)
{
//set map->odom so that odom is moved back to last saved localization
_mapCorrection = _lastLocalizationPose * odomPose.inverse();
if(_graphOptimizer->isSlam2d())
{
_mapCorrection = _lastLocalizationPose.to3DoF() * odomPose.to3DoF().inverse();
}
else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
{
_mapCorrection = _lastLocalizationPose.to4DoF() * odomPose.to4DoF().inverse();
}
else
{
_mapCorrection = _lastLocalizationPose * odomPose.inverse();
}
std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
@@ -1140,7 +1207,19 @@ bool Rtabmap::process(
else
{
//move optimized poses accordingly to last saved localization
Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
Transform mapCorrectionInv;
if(_graphOptimizer->isSlam2d())
{
mapCorrectionInv = odomPose.to3DoF() * _lastLocalizationPose.to3DoF().inverse();
}
else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
{
mapCorrectionInv = odomPose.to4DoF() * _lastLocalizationPose.to4DoF().inverse();
}
else
{
mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
}
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * iter->second;
@@ -2224,6 +2303,13 @@ bool Rtabmap::process(
// Immunize just retrieved signatures
immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
if(!signaturesRetrieved.empty() && !_globalScanMap.empty())
{
UWARN("Some signatures have been retrieved from memory management, clearing global scan map...");
_globalScanMap.clear();
_globalScanMapPoses.clear();
}
}
timeReactivations = timer.ticks();
ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
@@ -2263,7 +2349,8 @@ bool Rtabmap::process(
float loopClosureVisualInliersDistribution = 0;
int proximityDetectionsAddedVisually = 0;
int proximityDetectionsAddedByICPOnly = 0;
int proximityDetectionsAddedByICPMulti = 0;
int proximityDetectionsAddedByICPGlobal = 0;
int lastProximitySpaceClosureId = 0;
int proximitySpacePaths = 0;
int localVisualPathsChecked = 0;
@@ -2544,7 +2631,32 @@ bool Rtabmap::process(
{
++localScanPathsChecked;
RegistrationInfo info;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
Transform transform;
bool icpMulti = true;
if(_globalScanMap.empty())
{
transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
}
else
{
icpMulti = false;
// use pre-assembled scan map
SensorData assembledData;
assembledData.setId(nearestId);
assembledData.setLaserScan(
LaserScan(_globalScanMap,
signature->sensorData().laserScanCompressed().maxPoints(),
signature->sensorData().laserScanCompressed().rangeMax(),
_globalScanMapPoses.at(nearestId).inverse() * (signature->sensorData().laserScanCompressed().is2d()?Transform(0,0,signature->sensorData().laserScanCompressed().localTransform().z(),0,0,0):Transform::getIdentity())));
Signature nearestNode(assembledData);
Transform guess = filteredPath.at(nearestId).inverse() * filteredPath.at(signature->id());
transform = _memory->computeIcpTransform(nearestNode, *signature, guess, &info);
if(!transform.isNull())
{
transform = transform.inverse();
}
}
if(!transform.isNull())
{
UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
@@ -2575,7 +2687,14 @@ bool Rtabmap::process(
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
if(icpMulti)
{
++proximityDetectionsAddedByICPMulti;
}
else
{
++proximityDetectionsAddedByICPGlobal;
}
// no local loop closure added visually
if(proximityDetectionsAddedVisually == 0)
@@ -2587,6 +2706,10 @@ bool Rtabmap::process(
{
UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
}
if(!_globalScanMap.empty())
{
break;
}
}
}
}
@@ -2925,40 +3048,20 @@ bool Rtabmap::process(
else if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
Transform transform = localizationLinks.begin()->second.transform();
int loopId = localizationLinks.begin()->first;
if(loopId < 0)
{
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
}
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end())
if(iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
newPose = _optimizedPoses.at(loopId) * transform.inverse();
float tmp1,tmp2;
UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1);
newPose.getEulerAngles(tmp1, tmp2, yaw);
newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw);
UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
}
else if(iterGravityLoop!=loopS->getLinks().end() ||
iterGravitySign!=signature->getLinks().end())
else if(iterGravitySign!=signature->getLinks().end())
{
UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id());
}
}
_optimizedPoses.at(signature->id()) = newPose;
@@ -3225,7 +3328,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_multi(), proximityDetectionsAddedByICPMulti);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_global(), proximityDetectionsAddedByICPGlobal);
statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
statistics_.addStatistic(Statistics::kProximitySpace_visual_paths_checked(), localVisualPathsChecked);
statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked);
@@ -3582,6 +3686,13 @@ bool Rtabmap::process(
UDEBUG("Removed %d from local map", iter->first);
UASSERT(iter->first != _lastLocalizationNodeId);
_optimizedPoses.erase(iter++);
if(!_globalScanMap.empty())
{
UWARN("optimized poses have been modified, clearing global scan map...");
_globalScanMap.clear();
_globalScanMapPoses.clear();
}
}
else
{
@@ -3603,6 +3714,8 @@ bool Rtabmap::process(
}
else
{
if(!_optimizedPoses.empty())
UDEBUG("Optimized poses cleared!");
_optimizedPoses.clear();
_constraints.clear();
}
@@ -5862,4 +5975,87 @@ void Rtabmap::updateGoalIndex()
}
}
void Rtabmap::createGlobalScanMap()
{
UDEBUG("Creating global scan map (if scans are available)");
_globalScanMap.clear();
_globalScanMapPoses.clear();
std::vector<int> scanIndices;
std::map<int, Transform> scanViewpoints;
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
SensorData data = _memory->getNodeData(iter->first, false, true, false, false);
if(!data.laserScanCompressed().empty())
{
LaserScan scan;
data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0);
if(!scan.empty())
{
scan = util3d::transformLaserScan(scan, iter->second*scan.localTransform());
if(_globalScanMap.empty() || _globalScanMap.format() == scan.format())
{
_globalScanMap += scan;
_globalScanMapPoses.insert(*iter);
scanViewpoints.insert(std::make_pair(iter->first, iter->second * scan.localTransform()));
scanIndices.resize(_globalScanMap.size(), iter->first);
}
else
{
UWARN("Incompatible scan formats (%s vs %s), cannot create global scan map.",
_globalScanMap.formatName().c_str(),
scan.formatName().c_str());
_globalScanMap.clear();
_globalScanMapPoses.clear();
break;
}
}
}
}
if(_globalScanMap.size() > 3)
{
float voxelSize = 0.0f;
int normalK = 0;
float normalRadius = 0.0f;
Parameters::parse(_parameters, Parameters::kMemLaserScanVoxelSize(), voxelSize);
Parameters::parse(_parameters, Parameters::kMemLaserScanNormalK(), normalK);
Parameters::parse(_parameters, Parameters::kMemLaserScanNormalRadius(), normalRadius);
if(voxelSize > 0.0f)
{
LaserScan voxelScan = util3d::commonFiltering(_globalScanMap, 1, 0, 0, voxelSize, normalK, normalRadius);
if(voxelScan.hasNormals())
{
// adjust with point of views
util3d::adjustNormalsToViewPoints(
scanViewpoints,
_globalScanMap,
scanIndices,
voxelScan);
}
_globalScanMap = voxelScan;
}
UINFO("Global scan map has been assembled (size=%d points, %d poses) "
"for proximity detection (only in localization mode %s=false and with %s=false)",
(int)_globalScanMap.size(),
(int)_globalScanMapPoses.size(),
Parameters::kMemIncrementalMemory().c_str(),
Parameters::kRGBDProximityPathRawPosesUsed().c_str());
//for debugging...
if(!_globalScanMap.empty() && ULogger::level() == ULogger::kDebug)
{
UWARN("Saving rtabmap_global_scan_map.pcd (only saved when logger level is debug)");
pcl::PCLPointCloud2::Ptr cloud2 = util3d::laserScanToPointCloud2(_globalScanMap);
pcl::io::savePCDFile("rtabmap_global_scan_map.pcd", *cloud2);
}
}
if(!_globalScanMap.empty() && _globalScanMap.size()<100)
{
UWARN("Ignoring global scan map because it is too small (%d points).", (int)_globalScanMap.size());
_globalScanMap.clear();
_globalScanMapPoses.clear();
}
}
} // namespace rtabmap

7
corelib/src/Transform.cpp
View File

@@ -214,6 +214,13 @@ Transform Transform::to3DoF() const
return Transform(x,y,0, 0,0,yaw);
}
Transform Transform::to4DoF() const
{
float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,z, 0,0,yaw);
}
cv::Mat Transform::rotationMatrix() const
{
return data_.colRange(0, 3).clone();

10
corelib/src/icp/cccorelib.h
View File

@@ -43,6 +43,7 @@ rtabmap::Transform icpCC(
double finalOverlapRatio = 0.85,
bool filterOutFarthestPoints = false,
double maxFinalRMS = 0.2,
float * finalRMS = 0,
std::string * errorMsg = 0)
{
UDEBUG("maxIterations=%d", maxIterations);
@@ -119,6 +120,11 @@ rtabmap::Transform icpCC(
UDEBUG("CC Final error: %f . Finall Pointcount: %d", finalError, finalPointCount);
UDEBUG("CC ICP success Trans: %f %f %f", transform.T.x,transform.T.y,transform.T.z);
if(finalRMS)
{
*finalRMS = (float)finalError;
}
if(result != 1)
{
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as result %d !=1", result);
@@ -131,9 +137,9 @@ rtabmap::Transform icpCC(
icpTransformation.setNull();
return icpTransformation;
}
else if(finalPointCount <10)
else if(finalPointCount < 50)
{
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 10 ", finalPointCount);
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 50 ", finalPointCount);
UDEBUG(msg.c_str());
if(errorMsg)
{

49
corelib/src/util3d_surface.cpp
View File

@@ -3572,6 +3572,55 @@ void adjustNormalsToViewPoints(
}
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & viewpoints,
const LaserScan & rawScan,
const std::vector<int> & viewpointIds,
LaserScan & scan)
{
UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
rawTree->setInputCloud (rawCloud);
for(int i=0; i<scan.size(); ++i)
{
pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);
pcl::PointXYZ normal(point.normal_x, point.normal_y, point.normal_z);
if(pcl::isFinite(normal))
{
std::vector<int> indices;
std::vector<float> dist;
rawTree->nearestKSearch(pcl::PointXYZ(point.x, point.y, point.z), 1, indices, dist);
if(indices.size() && indices[0]>=0)
{
UASSERT_MSG(indices[0]<(int)viewpointIds.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)viewpointIds.size()).c_str());
UASSERT(uContains(viewpoints, viewpointIds[indices[0]]));
Transform p = viewpoints.at(viewpointIds[indices[0]]);
pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
Eigen::Vector3f v = viewpoint.getVector3fMap() - point.getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0)
{
//reverse normal
scan.field(i, scan.getNormalsOffset()) *= -1.0f;
scan.field(i, scan.getNormalsOffset()+1) *= -1.0f;
scan.field(i, scan.getNormalsOffset()+2) *= -1.0f;
}
}
else
{
UWARN("Not found camera viewpoint for point %d!?", i);
}
}
}
}
}
pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
{
pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);