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

0.18.3: added landmarks (graph optimization, localization, navigation)

Browse Source
This commit is contained in:
matlabbe
2018-12-07 18:29:41 -05:00
parent b771aa00e0
commit 200ec8e5db
35 changed files with 1509 additions and 709 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -167,13 +167,13 @@ public:
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void loadTags(int signatureId, std::map<int, TransformStamped> & tags) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
void getNodeIdByLabel(const std::string & label, int & id) const;
void getAllLabels(std::map<int, std::string> & labels) const;
@@ -260,16 +260,16 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;

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

@@ -131,16 +131,16 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
@@ -152,7 +152,6 @@ private:
std::string queryStepSensorData() const;
std::string queryStepLinkUpdate() const;
std::string queryStepLink() const;
std::string queryStepTag() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepOccupancyGridUpdate() const;
@@ -162,7 +161,6 @@ private:
void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepTag(sqlite3_stmt * ppStmt, int nodeId, int tagId, const TransformStamped & pose) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -175,7 +173,6 @@ private:
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
void loadTagsQuery(std::list<Signature *> & signatures) const;
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
protected:

76
corelib/include/rtabmap/core/Landmark.h Normal file
View File

@@ -0,0 +1,76 @@
/*
Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap {
class Landmark
{
public:
Landmark() :
id_(0)
{}
Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
id_(id),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(5,5)).c_str());
}
virtual ~Landmark() {}
const int & id() const {return id_;}
const Transform & pose() const {return pose_;}
const cv::Mat & covariance() const {return covariance_;}
private:
int id_;
Transform pose_;
cv::Mat covariance_;
};
typedef std::map<int, Landmark> Landmarks;
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

8
corelib/include/rtabmap/core/Link.h
View File

@@ -46,7 +46,11 @@ public:
kUserClosure,
kVirtualClosure,
kNeighborMerged,
kPosePrior,
kPosePrior, // Absolute pose in /world frame, From == To
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To"
kEnd,
kAllWithLandmarks = 98,
kAllWithoutLandmarks = 99,
kUndef = 99};
Link();
Link(int from,
@@ -56,7 +60,7 @@ public:
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
const cv::Mat & userData = cv::Mat());
bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
int from() const {return from_;}
int to() const {return to_;}

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

@@ -150,17 +150,21 @@ public:
bool lookInDatabase = false) const;
std::map<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLinks(int signatureId,
bool lookInDatabase = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
std::map<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
bool lookInDatabase = false,
bool withLandmarks = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
bool isBinDataKept() const {return _binDataKept;}
float getSimilarityThreshold() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
int getLastSignatureId() const;
const Signature * getLastWorkingSignature() const;
std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
const std::map<int, std::string> & getAllLabels() const {return _labels;}
const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
const std::map<int, std::set<int> > & getLandmarksInvertedIndex() const {return _landmarksInvertedIndex;}
bool allNodesInWM() const {return _allNodesInWM;}
/**
@@ -229,7 +233,8 @@ public:
const std::set<int> & ids,
std::map<int, Transform> & poses,
std::multimap<int, Link> & links,
bool lookInDatabase = false);
bool lookInDatabase = false,
bool landmarksAdded = false);
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);
@@ -329,6 +334,8 @@ private:
std::map<int, double> _workingMem; // id,age
std::map<int, Transform> _groundTruths;
std::map<int, std::string> _labels;
std::map<int, std::set<int> > _landmarksIndex; // <nodeId, landmarkIds>
std::map<int, std::set<int> > _landmarksInvertedIndex; // <landmarkId, nodeIds>
//Keypoint stuff
VWDictionary * _vwd;

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

@@ -89,6 +89,7 @@ public:
double epsilon() const {return epsilon_;}
bool isRobust() const {return robust_;}
bool priorsIgnored() const {return priorsIgnored_;}
bool landmarksIgnored() const {return landmarksIgnored_;}
// setters
void setIterations(int iterations) {iterations_ = iterations;}
@@ -97,6 +98,7 @@ public:
void setEpsilon(double epsilon) {epsilon_ = epsilon;}
void setRobust(bool enabled) {robust_ = enabled;}
void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
virtual void parseParameters(const ParametersMap & parameters);
@@ -172,7 +174,8 @@ protected:
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust(),
bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored());
bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(),
bool landmarksIgnored = Parameters::defaultOptimizerLandmarksIgnored());
Optimizer(const ParametersMap & parameters);
private:
@@ -182,6 +185,7 @@ private:
double epsilon_;
bool robust_;
bool priorsIgnored_;
bool landmarksIgnored_;
};
} /* namespace rtabmap */

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

@@ -385,6 +385,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
#ifdef RTABMAP_ORB_SLAM2
RTABMAP_PARAM(g2o, Solver, int, 3, "0=csparse 1=pcg 2=cholmod 3=Eigen");

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

@@ -189,7 +189,7 @@ public:
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
std::map<int, std::map<int, Transform> > getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth = 0) const;
std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;

8
corelib/include/rtabmap/core/SensorData.h
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/IMU.h>
#include <rtabmap/core/GPS.h>
#include <rtabmap/core/EnvSensor.h>
#include <rtabmap/core/Landmark.h>
namespace rtabmap
{
@@ -246,6 +247,9 @@ public:
void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
const EnvSensors & envSensors() const {return _envSensors;}
void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
const Landmarks & landmarks() const {return _landmarks;}
long getMemoryUsed() const; // Return memory usage in Bytes
void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
@@ -283,8 +287,8 @@ private:
// environmental sensors
EnvSensors _envSensors;
// tags
std::map<int, TransformStamped> _tags;
// landmarks
Landmarks _landmarks;
// features
std::vector<cv::KeyPoint> _keypoints;

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

@@ -90,9 +90,8 @@ public:
void removeLink(int idTo);
void removeVirtualLinks();
void setTags(const std::map<int, TransformStamped> & tags) {_tags = tags;}
void addTag(int id, const TransformStamped & pose) {_tags.insert(std::make_pair(id, pose));}
const std::map<int, TransformStamped> & getTags() const {return _tags;}
void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
const std::map<int, Link> & getLandmarks() const {return _landmarks;}
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -145,7 +144,7 @@ private:
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
std::map<int, TransformStamped> _tags;
std::map<int, Link> _landmarks;
int _weight;
std::string _label;
bool _saved; // If it's saved to bd

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

@@ -69,6 +69,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Optimization_iterations, );
RTABMAP_STATS(Loop, Linear_variance,);
RTABMAP_STATS(Loop, Angular_variance,);
RTABMAP_STATS(Loop, Landmark_detected,);
RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
RTABMAP_STATS(Proximity, Time_detections,);
RTABMAP_STATS(Proximity, Space_last_detection_id,);

76
corelib/src/DBDriver.cpp
View File

@@ -775,11 +775,15 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
nIter!=s->getLinks().end();
++nIter)
{
if(type == Link::kUndef || nIter->second.type() == type)
if(type == Link::kAllWithoutLandmarks || type == Link::kAllWithLandmarks || nIter->second.type() == type)
{
links.insert(*nIter);
}
}
if(type == Link::kLandmark || type == Link::kAllWithLandmarks)
{
uInsert(links, s->getLandmarks());
}
found = true;
}
_trashesMutex.unlock();
@@ -792,33 +796,6 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
}
}
void DBDriver::loadTags(int signatureId, std::map<int, TransformStamped> & tags) const
{
bool found = false;
// look in the trash
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
UASSERT(s != 0);
for(std::map<int, TransformStamped>::const_iterator nIter = s->getTags().begin();
nIter!=s->getTags().end();
++nIter)
{
tags.insert(*nIter);
}
found = true;
}
_trashesMutex.unlock();
if(!found)
{
_dbSafeAccessMutex.lock();
this->loadTagsQuery(signatureId, tags);
_dbSafeAccessMutex.unlock();
}
}
void DBDriver::getWeight(int signatureId, int & weight) const
{
bool found = false;
@@ -878,10 +855,10 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren, bool igno
_dbSafeAccessMutex.unlock();
}
void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks) const
void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
{
_dbSafeAccessMutex.lock();
this->getAllLinksQuery(links, ignoreNullLinks);
this->getAllLinksQuery(links, ignoreNullLinks, withLandmarks);
_dbSafeAccessMutex.unlock();
// look in the trash
@@ -900,6 +877,18 @@ void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLink
links.insert(std::make_pair(iter->first, jter->second));
}
}
if(withLandmarks)
{
for(std::map<int, Link>::const_iterator jter=iter->second->getLandmarks().begin();
jter!=iter->second->getLandmarks().end();
++jter)
{
if(!ignoreNullLinks || jter->second.isValid())
{
links.insert(std::make_pair(iter->first, jter->second));
}
}
}
}
}
_trashesMutex.unlock();
@@ -964,6 +953,33 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
}
}
void DBDriver::getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const
{
if(landmarkId < 0)
{
// look in the trash
_trashesMutex.lock();
for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
{
std::map<int, Link>::const_iterator kter = sIter->second->getLandmarks().find(landmarkId);
if(kter != sIter->second->getLandmarks().end())
{
nodes.insert(std::make_pair(sIter->second->id(), kter->second));
}
}
_trashesMutex.unlock();
// then look in the database
_dbSafeAccessMutex.lock();
this->getNodesObservingLandmarkQuery(landmarkId, nodes);
_dbSafeAccessMutex.unlock();
}
else
{
UWARN("Can't search with an empty label!");
}
}
void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
{
if(!label.empty())

335
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2375,7 +2375,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
}
}
void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const
void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const
{
links.clear();
if(_ppDb)
@@ -2386,7 +2386,11 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(uStrNumCmp(_version, "0.18.3") >= 0 && !withLandmarks)
{
query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link WHERE type!=" << Link::kLandmark << " ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
}
@@ -2595,6 +2599,86 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
}
}
void DBDriverSqlite3::getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const
{
if(_ppDb && landmarkId < 0 && uStrNumCmp(_version, "0.18.3") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT from_id, type, information_matrix, transform, user_data FROM Link WHERE to_id=" << landmarkId <<"";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
int fromId = -1;
int linkType = -1;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
fromId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
cv::Mat userDataCompressed;
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
const void * data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
//transform
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
}
else if(dataSize)
{
UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, landmarkId);
}
if(linkType >= 0 && linkType != Link::kUndef)
{
nodes.insert(std::make_pair(fromId, Link(fromId, landmarkId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed)));
}
else
{
UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
linkType, fromId, landmarkId);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
}
}
void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
{
if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
@@ -3063,9 +3147,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
this->loadLinksQuery(nodes);
ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
this->loadTagsQuery(nodes);
ULOGGER_DEBUG("Time load tags=%fs", timer.ticks());
for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
(*iter)->setModified(false);
@@ -3490,10 +3571,10 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::map<int, Link> & neighbors,
std::map<int, Link> & links,
Link::Type typeIn) const
{
neighbors.clear();
links.clear();
if(_ppDb)
{
UTimer timer;
@@ -3538,6 +3619,11 @@ void DBDriverSqlite3::loadLinksQuery(
query << " AND type > 0";
}
}
if(uStrNumCmp(_version, "0.18.3") >= 0 && (typeIn != Link::kAllWithLandmarks && typeIn != Link::kLandmark))
{
query << " AND type != " << Link::kLandmark;
}
query << " ORDER BY to_id";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -3609,19 +3695,19 @@ void DBDriverSqlite3::loadLinksQuery(
}
}
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
}
else
{
// neighbor is 0, loop closures are 1 and 2 (child)
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
// neighbor is 0, loop closures are 1
links.insert(links.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
}
rc = sqlite3_step(ppStmt);
@@ -3633,9 +3719,9 @@ void DBDriverSqlite3::loadLinksQuery(
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(neighbors.size() == 0)
if(links.size() == 0)
{
//UERROR("No neighbors loaded from signature %d", signatureId);
//UERROR("No links loaded from signature %d", signatureId);
}
}
}
@@ -3765,13 +3851,20 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
if(linkType >= 0 && linkType != Link::kUndef)
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
if(linkType == Link::kLandmark)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
(*iter)->addLandmark(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
else
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
}
}
}
else
@@ -3799,133 +3892,6 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
}
}
void DBDriverSqlite3::loadTagsQuery(
int signatureId,
std::map<int, TransformStamped> & tags) const
{
tags.clear();
if(_ppDb && uStrNumCmp(_version, "0.18.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT tag_id, stamp, transform FROM Tag WHERE node_id = " << signatureId;
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int tagId = 0;
double stamp = 0.0;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
tagId = sqlite3_column_int(ppStmt, index++);
stamp = sqlite3_column_double(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
tags.insert(tags.end(), std::make_pair(tagId, TransformStamped(transform, stamp)));
}
else if(dataSize)
{
UERROR("Error while loading tag %d transform node %d! Ignoring it...", tagId, signatureId);
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::loadTagsQuery(std::list<Signature *> & signatures) const
{
if(_ppDb && uStrNumCmp(_version, "0.18.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT tag_id, stamp, transform FROM Tag WHERE node_id = ?";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int tagId = 0;
double stamp = 0.0;
std::map<int, TransformStamped> tags;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
tagId = sqlite3_column_int(ppStmt, index++);
stamp = sqlite3_column_double(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
tags.insert(tags.end(), std::make_pair(tagId, TransformStamped(transform, stamp)));
}
else if(dataSize)
{
UERROR("Error while loading tag %d transform node %d! Ignoring it...", tagId, (*iter)->id());
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// add tags
(*iter)->setTags(tags);
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), tags.size());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
{
UDEBUG("nodes = %d", nodes.size());
@@ -4004,7 +3970,14 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
// Update links part1
query = "DELETE FROM Link WHERE from_id=?;";
if(uStrNumCmp(_version, "0.18.3") >= 0)
{
query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
}
else
{
query = uFormat("DELETE FROM Link WHERE from_id=?;");
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
@@ -4125,7 +4098,6 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
bool hasTags = false;
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
_memoryUsedEstimate += (*i)->getMemoryUsed();
@@ -4134,11 +4106,6 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
if((*i)->getTags().size())
{
hasTags = true;
}
stepNode(ppStmt, *i);
}
// Finalize (delete) the statement
@@ -4159,6 +4126,15 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
{
stepLink(ppStmt, i->second);
}
if(uStrNumCmp(_version, "0.18.3") >= 0)
{
// Save landmarks
const std::map<int, Link> & links = (*jter)->getLandmarks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
@@ -4204,28 +4180,6 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
if(hasTags && uStrNumCmp(_version, "0.18.0") >= 0)
{
// Create new entries in table Tag
query = queryStepTag();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save tags
const std::map<int, TransformStamped> & tags = (*jter)->getTags();
for(std::map<int, TransformStamped>::const_iterator i=tags.begin(); i!=tags.end(); ++i)
{
stepTag(ppStmt, (*jter)->id(), i->first, i->second);
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
// Add SensorData
@@ -6050,41 +6004,6 @@ void DBDriverSqlite3::stepLink(
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepTag() const
{
UASSERT(uStrNumCmp(_version, "0.18.0") >= 0);
return "INSERT INTO Tag(node_id, tag_id, stamp, transform) VALUES(?,?,?,?);";
}
void DBDriverSqlite3::stepTag(
sqlite3_stmt * ppStmt,
int nodeId,
int tagId,
const TransformStamped & pose) const
{
if(!ppStmt)
{
UFATAL("");
}
UDEBUG("Save tag %d of node %d", tagId, nodeId);
int rc = SQLITE_OK;
int index = 1;
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, tagId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, pose.stamp());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, pose.transform().data(), pose.transform().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepWordsChanged() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)

40
corelib/src/Graph.cpp
View File

@@ -841,26 +841,20 @@ void computeMaxGraphErrors(
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
// ignore links with high variance, priors and landmarks
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
UASSERT(iter->second.transVariance()>0.0);
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
@@ -873,6 +867,15 @@ void computeMaxGraphErrors(
*maxLinearErrorLink = &iter->second;
}
}
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
UASSERT(iter->second.rotVariance()>0.0);
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
@@ -1780,7 +1783,7 @@ std::list<std::pair<int, Transform> > computePath(
{
UASSERT(memory!=0);
UASSERT(fromId>=0);
UASSERT(toId>=0);
UASSERT(toId!=0);
std::list<std::pair<int, Transform> > path;
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
fromId,
@@ -1795,7 +1798,14 @@ std::list<std::pair<int, Transform> > computePath(
{
// Faster to load all links in one query
UTimer t;
allLinks = memory->getAllLinks(lookInDatabase);
allLinks = memory->getAllLinks(lookInDatabase, true, true);
for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.to() < 0)
{
allLinks.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
}
}
UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
}
@@ -1846,7 +1856,7 @@ std::list<std::pair<int, Transform> > computePath(
std::map<int, Link> links;
if(allLinks.size() == 0)
{
links = memory->getLinks(currentNode->id(), lookInDatabase);
links = memory->getLinks(currentNode->id(), lookInDatabase, true);
}
else
{
@@ -1928,7 +1938,7 @@ std::list<std::pair<int, Transform> > computePath(
if(ULogger::level() == ULogger::kDebug)
{
std::stringstream stream;
std::vector<int> linkTypes(Link::kUndef, 0);
std::vector<int> linkTypes(Link::kEnd, 0);
std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
float length = 0.0f;
for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
@@ -1953,7 +1963,7 @@ std::list<std::pair<int, Transform> > computePath(
//float cost = angle ;
//UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kUndef);
UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kEnd);
++linkTypes[jter->second.type()];
stream << "[" << jter->second.type() << "]";
length += jter->second.transform().getNorm();

364
corelib/src/Memory.cpp
View File

@@ -242,6 +242,39 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
if(!(*iter)->getGroundTruthPose().isNull()) {
_groundTruths.insert(std::make_pair((*iter)->id(), (*iter)->getGroundTruthPose()));
}
if(!(*iter)->getLandmarks().empty())
{
// Update landmark indexes
for(std::map<int, Link>::const_iterator jter = (*iter)->getLandmarks().begin(); jter!=(*iter)->getLandmarks().end(); ++jter)
{
int landmarkId = jter->first;
UASSERT(landmarkId < 0);
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find((*iter)->id());
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair((*iter)->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert((*iter)->id());
}
else
{
std::set<int> tmp;
tmp.insert((*iter)->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
}
}
}
}
else
{
@@ -1182,32 +1215,70 @@ std::map<int, Link> Memory::getLoopClosureLinks(
std::map<int, Link> Memory::getLinks(
int signatureId,
bool lookInDatabase) const
bool lookInDatabase,
bool withLandmarks) const
{
std::map<int, Link> links;
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
if(s)
if(signatureId > 0)
{
links = s->getLinks();
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
if(s)
{
links = s->getLinks();
if(withLandmarks)
{
uInsert(links, s->getLandmarks());
}
}
else if(lookInDatabase && _dbDriver)
{
_dbDriver->loadLinks(signatureId, links, withLandmarks?Link::kAllWithLandmarks:Link::kAllWithoutLandmarks);
}
else
{
UWARN("Cannot find signature %d in memory", signatureId);
}
}
else if(lookInDatabase && _dbDriver)
else if(signatureId < 0) //landmark
{
_dbDriver->loadLinks(signatureId, links, Link::kUndef);
}
else
{
UWARN("Cannot find signature %d in memory", signatureId);
int landmarkId = signatureId;
std::map<int, std::set<int> >::const_iterator iter = _landmarksInvertedIndex.find(landmarkId);
if(iter != _landmarksInvertedIndex.end())
{
for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
const Signature * s = getSignature(*jter);
if(s)
{
std::map<int, Link>::const_iterator kter = s->getLandmarks().find(landmarkId);
if(kter != s->getLandmarks().end())
{
// should be from landmark to node
links.insert(std::make_pair(s->id(), kter->second.inverse()));
}
}
}
}
if(_dbDriver && lookInDatabase)
{
std::map<int, Link> nodes;
_dbDriver->getNodesObservingLandmark(landmarkId, nodes);
for(std::map<int, Link>::iterator kter=nodes.begin(); kter!=nodes.end(); ++kter)
{
links.insert(std::make_pair(kter->first, kter->second.inverse()));
}
}
}
return links;
}
std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks, bool withLandmarks) const
{
std::multimap<int, Link> links;
if(lookInDatabase && _dbDriver)
{
_dbDriver->getAllLinks(links, ignoreNullLinks);
_dbDriver->getAllLinks(links, ignoreNullLinks, withLandmarks);
}
for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
@@ -1222,6 +1293,18 @@ std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNul
links.insert(std::make_pair(iter->first, jter->second));
}
}
if(withLandmarks)
{
for(std::map<int, Link>::const_iterator jter=iter->second->getLandmarks().begin();
jter!=iter->second->getLandmarks().end();
++jter)
{
if(!ignoreNullLinks || jter->second.isValid())
{
links.insert(std::make_pair(iter->first, jter->second));
}
}
}
}
return links;
@@ -1275,7 +1358,9 @@ std::map<int, int> Memory::getNeighborsId(
// Look up in STM/WM if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::map<int, Link> tmpLinks;
std::map<int, Link> tmpLandmarks;
const std::map<int, Link> * links = &tmpLinks;
const std::map<int, Link> * landmarks = &tmpLandmarks;
if(s)
{
if(!ignoreIntermediateNodes || s->getWeight() != -1)
@@ -1288,6 +1373,10 @@ std::map<int, int> Memory::getNeighborsId(
}
links = &s->getLinks();
if(!ignoreLoopIds)
{
landmarks = &s->getLandmarks();
}
}
else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
{
@@ -1295,7 +1384,22 @@ std::map<int, int> Memory::getNeighborsId(
ids.insert(std::pair<int, int>(*jter, m));
UTimer timer;
_dbDriver->loadLinks(*jter, tmpLinks);
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
if(!ignoreLoopIds)
{
for(std::map<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
{
if(kter->first < 0)
{
tmpLandmarks.insert(*kter);
tmpLinks.erase(kter++);
}
else
{
++kter;
}
}
}
if(dbAccessTime)
{
*dbAccessTime += timer.getElapsedTime();
@@ -1340,6 +1444,32 @@ std::map<int, int> Memory::getNeighborsId(
}
}
}
// landmarks
for(std::map<int, Link>::const_iterator iter=landmarks->begin(); iter!=landmarks->end(); ++iter)
{
const std::map<int, std::set<int> >::const_iterator kter = _landmarksInvertedIndex.find(iter->first);
if(kter != _landmarksInvertedIndex.end())
{
for(std::set<int>::const_iterator nter=kter->second.begin(); nter!=kter->second.end(); ++nter)
{
if( !uContains(ids, *nter) && ignoredIds.find(*nter) == ignoredIds.end())
{
if(incrementMarginOnLoop)
{
nextMargin.insert(*nter);
}
else
{
if(currentMargin.insert(*nter).second)
{
curentMarginList.push_back(*nter);
}
}
}
}
}
}
}
}
++m;
@@ -1600,6 +1730,8 @@ void Memory::clear()
_odomMaxInf.clear();
_groundTruths.clear();
_labels.clear();
_landmarksIndex.clear();
_landmarksInvertedIndex.clear();
_allNodesInWM = true;
if(_dbDriver)
@@ -2130,6 +2262,33 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
UDEBUG("id=%d", s?s->id():0);
if(s)
{
// Cleanup landmark indexes
if(!s->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=s->getLandmarks().begin(); iter!=s->getLandmarks().end(); ++iter)
{
int landmarkId = iter->first;
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(s->id());
if(nter!=_landmarksIndex.end())
{
nter->second.erase(landmarkId);
if(nter->second.empty())
{
_landmarksIndex.erase(nter);
}
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.erase(s->id());
if(nter->second.empty())
{
_landmarksInvertedIndex.erase(nter);
}
}
}
}
// it is a bad signature (not saved), remove links!
if(keepLinkedToGraph && (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
{
@@ -2145,7 +2304,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
if(iter->second.from() != iter->second.to() && iter->first > 0)
{
Signature * sTo = this->_getSignature(iter->first);
// neighbor to s
@@ -2261,6 +2420,36 @@ const Signature * Memory::getLastWorkingSignature() const
return _lastSignature;
}
std::map<int, Link> Memory::getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const
{
UDEBUG("landmarkId=%d", landmarkId);
std::map<int, Link> nodes;
if(landmarkId < 0)
{
std::map<int, std::set<int> >::const_iterator iter = _landmarksInvertedIndex.find(landmarkId);
if(iter != _landmarksInvertedIndex.end())
{
for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
const Signature * s = getSignature(*jter);
if(s)
{
std::map<int, Link>::const_iterator kter = s->getLandmarks().find(landmarkId);
if(kter != s->getLandmarks().end())
{
nodes.insert(std::make_pair(s->id(), kter->second));
}
}
}
}
if(_dbDriver && lookInDatabase)
{
_dbDriver->getNodesObservingLandmark(landmarkId, nodes);
}
}
return nodes;
}
int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
{
UDEBUG("label=%s", label.c_str());
@@ -2807,7 +2996,7 @@ Transform Memory::computeIcpTransformMulti(
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
Signature * s = _getSignature(iter->first);
UASSERT(s != 0);
UASSERT_MSG(s != 0, uFormat("id=%d", iter->first).c_str());
//if image is already here, scan should be or it is null
if(s->sensorData().imageCompressed().empty() &&
s->sensorData().laserScanCompressed().isEmpty())
@@ -3598,7 +3787,7 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
{
UDEBUG("nodeId=%d", nodeId);
//UDEBUG("nodeId=%d", nodeId);
SensorData r;
Signature * s = this->_getSignature(nodeId);
if(s && !s->sensorData().imageCompressed().empty())
@@ -3624,7 +3813,7 @@ void Memory::getNodeWords(int nodeId,
std::multimap<int, cv::Point3f> & words3,
std::multimap<int, cv::Mat> & wordsDescriptors)
{
UDEBUG("nodeId=%d", nodeId);
//UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
if(s)
{
@@ -3662,7 +3851,7 @@ void Memory::getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel)
{
UDEBUG("nodeId=%d", nodeId);
//UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
if(s)
{
@@ -3679,7 +3868,7 @@ void Memory::getNodeCalibration(int nodeId,
SensorData Memory::getSignatureDataConst(int locationId,
bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("");
//UDEBUG("");
SensorData r;
const Signature * s = this->getSignature(locationId);
if(s && (!s->sensorData().imageCompressed().empty() ||
@@ -4816,6 +5005,45 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
}
//landmarks
for(Landmarks::const_iterator iter = data.landmarks().begin(); iter!=data.landmarks().end(); ++iter)
{
if(iter->second.id() > 0)
{
int landmarkId = -iter->first;
Link landmark(s->id(), landmarkId, Link::kLandmark, iter->second.pose(), iter->second.covariance().inv());
s->addLandmark(landmark);
// Update landmark indexes
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(s->id());
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair(s->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert(s->id());
}
else
{
std::set<int> tmp;
tmp.insert(s->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
}
}
else
{
UERROR("Invalid landmark received! IDs should be > 0 (it is %d). Ignoring this landmark.", iter->second.id());
}
}
return s;
}
@@ -5009,6 +5237,39 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
std::list<int> idsLoaded;
for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
{
if(!(*i)->getLandmarks().empty())
{
// Update landmark indexes
for(std::map<int, Link>::const_iterator iter = (*i)->getLandmarks().begin(); iter!=(*i)->getLandmarks().end(); ++iter)
{
int landmarkId = iter->first;
UASSERT(landmarkId < 0);
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find((*i)->id());
if(nter!=_landmarksIndex.end())
{
nter->second.insert(landmarkId);
}
else
{
std::set<int> tmp;
tmp.insert(landmarkId);
_landmarksIndex.insert(std::make_pair((*i)->id(), tmp));
}
nter = _landmarksInvertedIndex.find(landmarkId);
if(nter!=_landmarksInvertedIndex.end())
{
nter->second.insert((*i)->id());
}
else
{
std::set<int> tmp;
tmp.insert((*i)->id());
_landmarksInvertedIndex.insert(std::make_pair(landmarkId, tmp));
}
}
}
idsLoaded.push_back((*i)->id());
//append to working memory
this->addSignatureToWmFromLTM(*i);
@@ -5024,7 +5285,8 @@ void Memory::getMetricConstraints(
const std::set<int> & ids,
std::map<int, Transform> & poses,
std::multimap<int, Link> & links,
bool lookInDatabase)
bool lookInDatabase,
bool landmarksAdded)
{
UDEBUG("");
for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
@@ -5040,50 +5302,64 @@ void Memory::getMetricConstraints(
{
if(uContains(poses, *iter))
{
std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase);
std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{
if( jter->second.isValid() &&
uContains(poses, jter->first) &&
graph::findLink(links, *iter, jter->first) == links.end())
graph::findLink(links, *iter, jter->first) == links.end() &&
(uContains(poses, jter->first) || (landmarksAdded && jter->second.type() == Link::kLandmark)))
{
if(!lookInDatabase &&
(jter->second.type() == Link::kNeighbor ||
jter->second.type() == Link::kNeighborMerged))
{
Link link = jter->second;
const Signature * s = this->getSignature(jter->first);
UASSERT(s!=0);
while(s && s->getWeight() == -1)
if(s->getWeight() == -1)
{
// skip to next neighbor, well we assume that bad signatures
// are only linked by max 2 neighbor links.
std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
UASSERT(n.size() <= 2);
std::map<int, Link>::iterator uter = n.upper_bound(s->id());
if(uter != n.end())
Link link = jter->second;
while(s && s->getWeight() == -1)
{
const Signature * s2 = this->getSignature(uter->first);
if(s2)
// skip to next neighbor, well we assume that bad signatures
// are only linked by max 2 neighbor links.
std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
UASSERT(n.size() <= 2);
std::map<int, Link>::iterator uter = n.upper_bound(s->id());
if(uter != n.end())
{
link = link.merge(uter->second, uter->second.type());
poses.erase(s->id());
s = s2;
const Signature * s2 = this->getSignature(uter->first);
if(s2)
{
link = link.merge(uter->second, uter->second.type());
poses.erase(s->id());
s = s2;
}
}
else
{
break;
}
}
else
{
break;
}
links.insert(std::make_pair(*iter, link));
}
else
{
links.insert(std::make_pair(*iter, jter->second));
}
links.insert(std::make_pair(*iter, link));
}
else
else if(jter->second.type() != Link::kLandmark)
{
links.insert(std::make_pair(*iter, jter->second));
}
else if(landmarksAdded)
{
if(!uContains(poses, jter->first))
{
poses.insert(std::make_pair(jter->first, poses.at(*iter) * jter->second.transform()));
}
links.insert(std::make_pair(jter->first, jter->second.inverse()));
}
}
}
}

26
corelib/src/OccupancyGrid.cpp
View File

@@ -251,7 +251,7 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
xMin_ = xMin;
yMin_ = yMin;
cellSize_ = cellSize;
addedNodes_ = poses;
addedNodes_.insert(poses.lower_bound(1), poses.end());
}
}
@@ -679,7 +679,12 @@ void OccupancyGrid::addToCache(
const cv::Mat & empty)
{
UDEBUG("nodeId=%d", nodeId);
uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
return;
}
uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
}
void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
@@ -699,7 +704,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false).
bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
bool graphChanged = !addedNodes_.empty(); // If the new map doesn't have any node from the previous map
std::map<int, Transform> transforms;
float updateErrorSqrd = updateError_*updateError_;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -884,7 +889,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
UDEBUG("Last id = %d", lastId);
// 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.lower_bound(1); iter!=posesIn.end(); ++iter)
{
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
@@ -893,17 +898,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
// insert zero after
if(posesIn.find(0) != posesIn.end())
{
if(iter->first < 0)
{
poses.push_back(*iter);
}
else
{
break;
}
poses.push_back(std::make_pair(-1, posesIn.at(0)));
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

37
corelib/src/OctoMap.cpp
View File

@@ -351,9 +351,14 @@ void OctoMap::addToCache(int nodeId,
const pcl::PointXYZ & viewPoint)
{
UDEBUG("nodeId=%d", nodeId);
cacheClouds_.erase(nodeId);
cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
return;
}
cacheClouds_.erase(nodeId==0?-1:nodeId);
cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles)));
uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
}
void OctoMap::addToCache(int nodeId,
const cv::Mat & ground,
@@ -361,12 +366,17 @@ void OctoMap::addToCache(int nodeId,
const cv::Mat & empty,
const cv::Point3f & viewPoint)
{
UDEBUG("nodeId=%d", nodeId);
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
return;
}
UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
UDEBUG("nodeId=%d", nodeId);
uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
}
void OctoMap::update(const std::map<int, Transform> & poses)
@@ -519,7 +529,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("Last id = %d", lastId);
// 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.lower_bound(1); iter!=poses.end(); ++iter)
{
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
@@ -527,17 +537,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
// insert zero after
if(poses.find(0) != poses.end())
{
if(iter->first < 0)
{
orderedPoses.push_back(*iter);
}
else
{
break;
}
orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
}
UDEBUG("orderedPoses = %d", (int)orderedPoses.size());

17
corelib/src/Optimizer.cpp
View File

@@ -229,13 +229,14 @@ void Optimizer::getConnectedGraph(
}
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) :
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust),
priorsIgnored_(priorsIgnored)
priorsIgnored_(priorsIgnored),
landmarksIgnored_(landmarksIgnored)
{
}
@@ -245,7 +246,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust()),
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored())
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored())
{
parseParameters(parameters);
}
@@ -258,6 +260,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
}
std::map<int, Transform> Optimizer::optimizeIncremental(
@@ -371,7 +374,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
std::map<int, Transform> Optimizer::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
@@ -380,7 +383,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
{
UDEBUG("");
std::map<int, CameraModel> models;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
std::map<int, Transform> poses;
for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
{
// Get camera model
CameraModel model;
@@ -418,6 +422,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
UASSERT(model.isValidForProjection());
models.insert(std::make_pair(iter->first, model));
poses.insert(*iter);
}
// compute correspondences
@@ -484,7 +489,7 @@ void Optimizer::computeBACorrespondences(
int wordCount = 0;
int edgeWithWordsAdded = 0;
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())

215
corelib/src/Rtabmap.cpp
View File

@@ -316,7 +316,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
Transform lastPose;
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
if(_optimizedPoses.size())
if(!_optimizedPoses.empty())
{
if(!_savedLocalizationIgnored)
{
@@ -663,15 +663,7 @@ bool Rtabmap::getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, floa
*/
Transform Rtabmap::getPose(int locationId) const
{
if(_memory)
{
const Signature * s = _memory->getSignature(locationId);
if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
{
return _optimizedPoses.at(s->id());
}
}
return Transform();
return uValue(_optimizedPoses, locationId, Transform());
}
void Rtabmap::setInitialPose(const Transform & initialPose)
@@ -1020,7 +1012,8 @@ bool Rtabmap::process(
{
// Localization mode, set map->odom so that odom is moved back to last saved localization
_mapCorrection = _lastLocalizationPose * odomPose.inverse();
_lastLocalizationNodeId = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
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",
_mapCorrection.prettyPrint().c_str(),
_lastLocalizationNodeId,
@@ -1281,6 +1274,17 @@ bool Rtabmap::process(
UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
if(_memory->isIncremental() && signature->getWeight() >= 0)
{
for(std::map<int, Link>::const_iterator iter = signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
{
_optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
}
_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
}
}
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor)
@@ -1440,7 +1444,7 @@ bool Rtabmap::process(
{
//Search for latest node having GPS linked to current signature not too far.
std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
{
const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0);
@@ -1866,7 +1870,7 @@ bool Rtabmap::process(
// remove poses from STM
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
if(!_memory->isInSTM(iter->first))
if(iter->first > 0 && !_memory->isInSTM(iter->first))
{
poses.insert(*iter);
}
@@ -1898,32 +1902,35 @@ bool Rtabmap::process(
iter!=path.end();
++iter)
{
if(immunizedLocally >= maxLocalLocationsImmunized)
if(iter->first>0)
{
// set 20 to avoid this warning when starting mapping
if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
if(immunizedLocally >= maxLocalLocationsImmunized)
{
UWARN("Could not immunize the whole local path (%d) between "
"%d and %d (max location immunized=%d). You may want "
"to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
"to be able to immunize longer paths.",
(int)path.size(),
nearestId,
signature->id(),
maxLocalLocationsImmunized,
_localImmunizationRatio,
maxLocalLocationsImmunized,
(int)_memory->getWorkingMem().size());
// set 20 to avoid this warning when starting mapping
if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
{
UWARN("Could not immunize the whole local path (%d) between "
"%d and %d (max location immunized=%d). You may want "
"to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
"to be able to immunize longer paths.",
(int)path.size(),
nearestId,
signature->id(),
maxLocalLocationsImmunized,
_localImmunizationRatio,
maxLocalLocationsImmunized,
(int)_memory->getWorkingMem().size());
}
break;
}
break;
}
else if(!_memory->isInSTM(iter->first))
{
if(immunizedLocations.insert(iter->first).second)
else if(!_memory->isInSTM(iter->first))
{
++immunizedLocally;
if(immunizedLocations.insert(iter->first).second)
{
++immunizedLocally;
}
//UDEBUG("local node %d on path immunized=1", iter->first);
}
//UDEBUG("local node %d on path immunized=1", iter->first);
}
}
}
@@ -1935,7 +1942,7 @@ bool Rtabmap::process(
std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
// sort by distance
std::multimap<float, int> nearNodesByDist;
for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
{
nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
}
@@ -2086,6 +2093,27 @@ bool Rtabmap::process(
timeAddLoopClosureLink = timer.ticks();
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
//============================================================
// Landmark
//============================================================
int landmarkDetected = 0;
int landmarkDetectedNodeRef = 0;
if(!signature->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
{
if(uContains(_memory->getLandmarksInvertedIndex(), iter->first) &&
_memory->getLandmarksInvertedIndex().find(iter->first)->second.size()>1);
{
landmarkDetected = iter->first;
landmarkDetectedNodeRef = *_memory->getLandmarksInvertedIndex().find(iter->first)->second.begin();
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, landmarkDetectedNodeRef);
break;
}
}
}
int proximityDetectionsAddedVisually = 0;
int proximityDetectionsAddedByICPOnly = 0;
int lastProximitySpaceClosureId = 0;
@@ -2101,7 +2129,7 @@ bool Rtabmap::process(
{
UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
}
else if(_memory->isIncremental() || _loopClosureHypothesis.first == 0)
else if(_memory->isIncremental() || (_loopClosureHypothesis.first == 0 && landmarkDetected == 0))
{
// In localization mode, no need to check local loop
// closures if we are already localized by a global closure.
@@ -2130,7 +2158,7 @@ bool Rtabmap::process(
}
UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
std::map<int, Transform> nearestPoses;
for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
{
if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
{
@@ -2139,7 +2167,7 @@ bool Rtabmap::process(
}
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
// segment poses by paths, only one detection per path
// segment poses by paths, only one detection per path, landmarks are ignored
std::map<int, std::map<int, Transform> > nearestPathsNotSorted = getPaths(nearestPoses, _optimizedPoses.at(signature->id()), _proximityMaxGraphDepth);
UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
// sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
@@ -2257,8 +2285,9 @@ bool Rtabmap::process(
(_proximityMaxPaths <= 0 || localScanPathsChecked < _proximityMaxPaths);
++iter)
{
std::map<int, Transform> path = iter->second;
std::map<int, Transform> path = iter->second; // should contain only nodes (no landmarks)
UASSERT(path.size());
UASSERT(path.begin()->first > 0);
//find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
@@ -2286,6 +2315,7 @@ bool Rtabmap::process(
}
// Assemble scans in the path and do ICP only
std::map<int, Transform> filteredPath;
if(_proximityRawPosesUsed)
{
//optimize the path's poses locally
@@ -2294,16 +2324,21 @@ bool Rtabmap::process(
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
for(std::map<int, Transform>::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
filteredPath.insert(std::make_pair(jter->first, t * jter->second));
}
}
std::map<int, Transform> filteredPath = path;
if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
else
{
filteredPath = path;
}
if(filteredPath.size() > 2 && _proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
filteredPath = graph::radiusPosesFiltering(filteredPath, _proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredPath.insert(*path.find(nearestId));
}
@@ -2330,9 +2365,9 @@ bool Rtabmap::process(
{
std::stringstream stream;
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
for(std::map<int, Transform>::iterator iter=filteredPath.begin(); iter!=filteredPath.end(); ++iter)
{
if(iter != path.begin())
if(iter != filteredPath.begin())
{
stream << ";";
}
@@ -2417,6 +2452,7 @@ bool Rtabmap::process(
statistics_.reducedIds().size() ||
signature->hasLink(signature->id()) || // prior edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one
{
@@ -2425,6 +2461,18 @@ bool Rtabmap::process(
//used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
if(landmarkDetected!=0 && !_memory->isIncremental())
{
//Add fake link between current node and the node also observing the same landmark
UASSERT(uContains(_optimizedPoses, landmarkDetectedNodeRef));
const Signature * s = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(s!=0);
UASSERT(uContains(s->getLandmarks(), landmarkDetected));
UASSERT(uContains(signature->getLandmarks(), landmarkDetected));
const Link & landmarkLink = s->getLandmarks().at(landmarkDetected);
const Link & landmarkLink2 = signature->getLandmarks().at(landmarkDetected);
localizationLinks.insert(std::make_pair(s->id(), landmarkLink2.merge(landmarkLink.inverse(), Link::kLandmark)));
}
// Note that in localization mode, we don't re-optimize the graph
// if:
@@ -2673,6 +2721,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodeRef);
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
@@ -2692,6 +2742,7 @@ bool Rtabmap::process(
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
statistics_.setLocalizationCovariance(localizationCovariance);
statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
// timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -2787,7 +2838,8 @@ bool Rtabmap::process(
{
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
(landmarkDetected == 0 || rejectedHypothesis) && // if we re not seeing a landmark from a previous map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -2917,7 +2969,7 @@ bool Rtabmap::process(
std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
{
if(!uContains(ids, iter->first))
if(iter->first > 0 && !uContains(ids, iter->first))
{
UDEBUG("Removed %d from local map", iter->first);
UASSERT(iter->first != _lastLocalizationNodeId);
@@ -2930,7 +2982,7 @@ bool Rtabmap::process(
}
for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
{
if(!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to()))
if(iter->first > 0 && (!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to())))
{
_constraints.erase(iter++);
}
@@ -3482,14 +3534,20 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
return poses;
}
std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth) const
std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, Transform> & posesIn, const Transform & target, int maxGraphDepth) const
{
std::map<int, std::map<int, Transform> > paths;
if(_memory && poses.size() && !target.isNull())
std::set<int> nodesSet;
std::map<int, Transform> poses;
for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
{
nodesSet.insert(iter->first);
poses.insert(*iter);
}
if(_memory && nodesSet.size() && !target.isNull())
{
double e0=0,e1=0,e2=0,e3=0,e4=0;
UTimer t;
std::set<int> nodesSet = uKeysSet(poses);
e0 = t.ticks();
// Segment poses connected only by neighbor links
while(poses.size())
@@ -3619,7 +3677,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
std::map<int, Transform> poses, posesOut;
std::multimap<int, Link> edgeConstraints, linksOut;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, true);
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
// Apply guess poses (if some)
@@ -3635,6 +3693,8 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
}
bool hasLandmarks = poses.begin()->first < 0;
// The constraints must be all already connected! Only check in debug
if(ULogger::level() == ULogger::kDebug)
{
@@ -3674,6 +3734,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
}
}
UDEBUG("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks);
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
}
@@ -3691,7 +3752,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
else
{
if(poses.size() != guessPoses.size())
if(poses.size() != guessPoses.size() || hasLandmarks)
{
// recompute poses using only links (robust to multi-session)
std::map<int, Transform> posesOut;
@@ -3712,6 +3773,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
(int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
}
}
UINFO("Optimization time %f s", timer.ticks());
return optimizedPoses;
@@ -3969,7 +4031,7 @@ void Rtabmap::getGraph(
if(signatures)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
Transform odomPoseLocal;
int weight = -1;
@@ -4052,6 +4114,7 @@ int Rtabmap::detectMoreLoopClosures(
std::list<Link> loopClosuresAdded;
std::multimap<int, int> checkedLoopClosures;
std::map<int, Transform> posesWithoutLandmarks;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
@@ -4067,7 +4130,11 @@ int Rtabmap::detectMoreLoopClosures(
}
else
{
mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
if(iter->first > 0)
{
posesWithoutLandmarks.insert(*iter);
mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
}
++iter;
}
}
@@ -4078,7 +4145,7 @@ int Rtabmap::detectMoreLoopClosures(
n+1, iterations, clusterRadius, clusterAngle);
std::multimap<int, int> clusters = graph::radiusPosesClustering(
poses,
posesWithoutLandmarks,
clusterRadius,
clusterAngle);
@@ -4315,7 +4382,7 @@ int Rtabmap::refineLinks()
this->getGraph(poses, links, false, true, &signatures);
int i=0;
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!= links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter=links.lower_bound(1); iter!= links.end(); ++iter)
{
int from = iter->second.from();
int to = iter->second.to();
@@ -4385,9 +4452,17 @@ void Rtabmap::clearPath(int status)
// return true if path is updated
bool Rtabmap::computePath(int targetNode, bool global)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
this->clearPath(0);
if(targetNode>0)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
}
else
{
UINFO("Planning a path to landmark %d (global=%d)", -targetNode, global?1:0);
}
if(!_rgbdSlamMode)
{
UWARN("A path can only be computed in RGBD-SLAM mode");
@@ -4396,6 +4471,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
UTimer totalTimer;
UTimer timer;
Transform transformToLandmark = Transform::getIdentity();
// No need to optimize the graph
if(_memory)
@@ -4436,8 +4512,17 @@ bool Rtabmap::computePath(int targetNode, bool global)
int oi = 0;
for(std::list<std::pair<int, Transform> >::iterator iter=path.begin(); iter!=path.end();++iter)
{
_path[oi].first = iter->first;
_path[oi++].second = t * iter->second;
if(iter->first > 0)
{
// just keep nodes in the path
_path[oi].first = iter->first;
_path[oi++].second = t * iter->second;
}
}
_path.resize(oi);
if(!_path.empty() && !path.empty() && path.rbegin()->first < 0)
{
transformToLandmark = _path.back().second.inverse() * t * path.rbegin()->second;
}
}
else if(currentNode == 0)
@@ -4493,6 +4578,8 @@ bool Rtabmap::computePath(int targetNode, bool global)
}
setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
}
_pathTransformToGoal = transformToLandmark;
updateGoalIndex();
return _path.size() || _pathStatus > 0;
}
@@ -4502,14 +4589,14 @@ bool Rtabmap::computePath(int targetNode, bool global)
bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
{
this->clearPath(0);
UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
if(tolerance < 0.0f)
{
tolerance = _localRadius;
}
UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str());
this->clearPath(0);
std::list<std::pair<int, Transform> > pathPoses;
if(!_rgbdSlamMode)

2
corelib/src/VWDictionary.cpp
View File

@@ -1291,7 +1291,7 @@ std::vector<int> VWDictionary::getUnusedWordIds() const
void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
{
UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size());
//UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size());
for(unsigned int i=0; i<words.size(); ++i)
{
_visualWords.erase(words[i]->id());

4
corelib/src/optimizer/OptimizerCVSBA.cpp
View File

@@ -55,7 +55,7 @@ bool OptimizerCVSBA::available()
std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
@@ -66,6 +66,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
// run sba optimization
cvsba::Sba sba;
std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
// change params if desired
cvsba::Sba::Params params ;
params.type = cvsba::Sba::MOTIONSTRUCTURE;

516
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -189,7 +189,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
#endif
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0 && poses.rbegin()->first > 0)
{
// Apply g2o optimization
@@ -313,43 +313,74 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
int landmarkVertexOffset = poses.rbegin()->first+1;
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
g2o::HyperGraph::Vertex * vertex = 0;
int id = iter->first;
if(isSlam2d())
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(iter->first == rootId)
if(id > 0)
{
v2->setFixed(true);
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId)
{
v2->setFixed(true);
}
vertex = v2;
}
else if(!landmarksIgnored())
{
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
id = landmarkVertexOffset - id;
}
else
{
continue;
}
vertex = v2;
}
else
{
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(iter->first == rootId)
if(id > 0)
{
v3->setFixed(true);
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(id == rootId)
{
v3->setFixed(true);
}
vertex = v3;
}
else if(!landmarksIgnored())
{
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
id = landmarkVertexOffset - id;
}
else
{
continue;
}
vertex = v3;
}
vertex->setId(iter->first);
vertex->setId(id);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
UDEBUG("fill edges to g2o...");
#if defined(RTABMAP_VERTIGO)
int vertigoVertexId = poses.rbegin()->first+1;
int vertigoVertexId = landmarkVertexOffset - (poses.begin()->first<0?poses.begin()->first:0);
#endif
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
@@ -408,6 +439,72 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
}
else if(id1<0 || id2 < 0)
{
if(!landmarksIgnored())
{
//landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d())
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
id2 = landmarkVertexOffset - id2;
g2o::EdgeSE2PointXY* e = new g2o::EdgeSE2PointXY;
e->vertices()[0] = optimizer.vertex(id1);
e->vertices()[1] = optimizer.vertex(id2);
e->setMeasurement(Eigen::Vector2d(t.x(), t.y()));
e->setInformation(information);
e->setParameterId(0, PARAM_OFFSET);
edge = e;
}
else
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
id2 = landmarkVertexOffset - id2;
g2o::EdgeSE3PointXYZ* e = new g2o::EdgeSE3PointXYZ;
e->vertices()[0] = optimizer.vertex(id1);
e->vertices()[1] = optimizer.vertex(id2);
e->setMeasurement(Eigen::Vector3d(t.x(), t.y(), t.z()));
e->setInformation(information);
e->setParameterId(0, PARAM_OFFSET);
edge = e;
}
}
}
else
{
#if defined(RTABMAP_VERTIGO)
@@ -573,18 +670,38 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset - id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw);
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
}
@@ -592,16 +709,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset - id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw);
tmpPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
}
@@ -669,18 +806,38 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXY* v = (const g2o::VertexPointXY*)optimizer.vertex(landmarkVertexOffset-id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], iter->second.z(), roll, pitch, yaw);
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
@@ -723,16 +880,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
int id = iter->first;
if(id > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(id);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
else
else if(!landmarksIgnored())
{
UERROR("Vertex %d not found!?", iter->first);
const g2o::VertexPointXYZ* v = (const g2o::VertexPointXYZ*)optimizer.vertex(landmarkVertexOffset-id);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate()[0], v->estimate()[1], v->estimate()[2], roll, pitch, yaw);
optimizedPoses.insert(std::pair<int, Transform>(id, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", id).c_str());
}
else
{
UERROR("Vertex %d not found!?", id);
}
}
}
@@ -874,7 +1051,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
@@ -952,61 +1129,64 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
if(iter->first > 0)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
Transform camPose = iter->second * iterModel->second.localTransform();
Transform camPose = iter->second * iterModel->second.localTransform();
// Add node's pose
UASSERT(!camPose.isNull());
// Add node's pose
UASSERT(!camPose.isNull());
#ifdef RTABMAP_ORB_SLAM2
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
#else
g2o::VertexCam * vCam = new g2o::VertexCam();
g2o::VertexCam * vCam = new g2o::VertexCam();
#endif
Eigen::Affine3d a = camPose.toEigen3d();
Eigen::Affine3d a = camPose.toEigen3d();
#ifdef RTABMAP_ORB_SLAM2
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
#else
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
#endif
vCam->setId(iter->first);
vCam->setId(iter->first);
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
++iter;
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
UDEBUG("fill edges to g2o...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(uContains(poses, iter->second.from()) &&
if(iter->second.from() > 0 &&
iter->second.to() > 0 &&
uContains(poses, iter->second.from()) &&
uContains(poses, iter->second.to()))
{
// add edge
@@ -1283,46 +1463,49 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// update poses
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
#ifdef RTABMAP_ORB_SLAM2
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
#else
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
#endif
if(v)
if(iter->first > 0)
{
Transform t = Transform::fromEigen3d(v->estimate());
#ifdef RTABMAP_ORB_SLAM2
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
#else
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
#endif
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
#ifdef RTABMAP_ORB_SLAM2
t=t.inverse();
t=t.inverse();
#endif
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{
UERROR("Optimized pose %d is null!?!?", iter->first);
optimizedPoses.clear();
return optimizedPoses;
}
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{
UERROR("Optimized pose %d is null!?!?", iter->first);
optimizedPoses.clear();
return optimizedPoses;
}
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
}
else
{
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
}
else
{
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UERROR("Vertex (pose) %d not found!?", iter->first);
}
}
else
{
UERROR("Vertex (pose) %d not found!?", iter->first);
}
}
//update points3D
@@ -1352,7 +1535,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
}
else if(poses.size() > 1 && poses.size() != models.size())
else if(poses.size() > 1 && (poses.size() != models.size() && poses.begin()->first > 0))
{
UERROR("This method should be called with size of poses = size camera models!");
}
@@ -1410,37 +1593,98 @@ bool OptimizerG2O::saveGraph(
q.w());
}
int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if (isSlam2d())
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
if(iter->first > 0)
{
// VERTEX_SE2 id x y theta
fprintf(file, "VERTEX_SE2 %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.theta());
}
else if(!landmarksIgnored())
{
// VERTEX_XY id x y
fprintf(file, "VERTEX_XY %d %f %f\n",
iter->first,
iter->second.x(),
iter->second.y());
}
}
else
{
// VERTEX_SE3 id x y z qw qx qy qz
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
if(iter->first > 0)
{
// VERTEX_SE3 id x y z qw qx qy qz
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
}
else if(!landmarksIgnored())
{
// VERTEX_XYZ id x y z
fprintf(file, "VERTEX_XYZ %d %f %f %f\n",
landmarkOffset-iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z());
}
}
}
int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
int virtualVertexId = landmarkOffset - (poses.size()&&poses.rbegin()->first<0?poses.rbegin()->first:0);
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if (iter->second.type() == Link::kLandmark)
{
if (this->landmarksIgnored())
{
continue;
}
if(isSlam2d())
{
// EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22
fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(1, 1));
}
else
{
// EDGE_SE3_XYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2_XY %d %d %d %f %f %f %f %f %f %f %f %f\n",
iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(),
iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(),
PARAM_OFFSET,
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
iter->second.infMatrix().at<double>(0, 0),
iter->second.infMatrix().at<double>(0, 1),
iter->second.infMatrix().at<double>(0, 2),
iter->second.infMatrix().at<double>(1, 1),
iter->second.infMatrix().at<double>(1, 2),
iter->second.infMatrix().at<double>(2, 2));
}
continue;
}
std::string prefix = isSlam2d()? "EDGE_SE2" :"EDGE_SE3:QUAT";
std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to());

138
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
@@ -138,11 +139,26 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UASSERT(!iter->second.isNull());
if(isSlam2d())
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(iter->first > 0)
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
}
else if(!landmarksIgnored())
{
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
}
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
if(iter->first > 0)
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
}
else if(!landmarksIgnored())
{
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
}
}
}
@@ -195,6 +211,62 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else if(id1<0 || id2 < 0)
{
if(!landmarksIgnored())
{
//landmarks
UASSERT((id1 < 0 && id2 > 0) || (id1 > 0 && id2 < 0));
if(isSlam2d())
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
gtsam::Point2 landmark(t.x(), t.y());
gtsam::Pose2 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
}
else
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
Transform t;
if(id2 < 0)
{
t = iter->second.transform();
}
else
{
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
gtsam::Point3 landmark(t.x(), t.y(), t.z());
gtsam::Pose3 p;
graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
}
}
}
else
{
#ifdef RTABMAP_VERTIGO
@@ -318,20 +390,40 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(intermediateGraphes && i > 0)
{
float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses;
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
int key = (int)iter->key;
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
if(key > 0)
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw)));
}
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
if(key > 0)
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
}
}
}
}
@@ -385,30 +477,50 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
gtsam::Marginals marginals(graph, optimizer->values());
float x,y,z,roll,pitch,yaw;
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
int key = (int)iter->key;
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
if(key > 0)
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point2 p = iter->value.cast<gtsam::Point2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z,roll,pitch,yaw)));
}
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
if(key > 0)
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored())
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Point3 p = iter->value.cast<gtsam::Point3>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw)));
}
}
}
}
// compute marginals
try {
UDEBUG("Computing marginals...");
UTimer t;
gtsam::Marginals marginals(graph, optimizer->values());
gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);
if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
{
outputCovariance.at<double>(0,0) = info(0,0); // x-x

92
corelib/src/optimizer/OptimizerTORO.cpp
View File

@@ -77,23 +77,29 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
}
}
}
@@ -128,7 +134,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2)
if(id1 != id2 && id1 > 0 && id2 > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
@@ -140,7 +146,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
//else // not supporting pose prior and landmarks
}
}
else
@@ -160,7 +166,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2)
if(id1 != id2 && id1 > 0 && id2 > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
@@ -172,7 +178,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
//else // not supporting pose prior
//else // not supporting pose prior and landmarks
}
}
UDEBUG("buildMST... root=%d", rootId);
@@ -214,25 +220,31 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
intermediateGraphes->push_back(tmpPoses);
@@ -293,25 +305,31 @@ std::map<int, Transform> OptimizerTORO::optimize(
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
if(iter->first > 0)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}

9
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -53,7 +53,7 @@ CREATE TABLE Data (
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
type INTEGER NOT NULL, -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
transform BLOB, -- 3x4 float
user_data BLOB, -- compressed data (User data)
@@ -88,13 +88,6 @@ CREATE TABLE Feature (
);
--
CREATE TABLE Tag (
node_id INTEGER NOT NULL,
tag_id INTEGER NOT NULL,
stamp FLOAT NOT NULL,
transform BLOB NOT NULL, -- 3x4 float, /base_link -> /tag_frame
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE Info (
STM_size INTEGER,