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

Updated version to 0.8.0

Browse Source 
Libraries are installed in lib directly with symbolic links, not in lib/rtabmap-0.8. Removed the need of RPATH in cmake.
Saving variance of each link in database (new field Link.variance). The variance is used to generate the constraint information matrices for TORO optimization.
ICP: computing variance instead of fitness.
ICP3: added correspondences ratio parameter
Added OdometryInfo class
Refactoring: renamed depth2d stuff to laserScan. rtabmap::Memory and rtabmap::Signature classes (no more distinct neighbor, loop closure or child loop closure links, only links with different types)
This commit is contained in:
Mathieu Labbe
2014-12-14 16:42:10 -05:00
parent 6acf374063
commit 744e2fb3c7
42 changed files with 1764 additions and 1460 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,18 +49,21 @@ public:
data_(image, seq)
{
}
CameraEvent() :
UEvent(kCodeNoMoreImages)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
UEvent(kCodeImageDepth),
data_(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
CameraEvent(const SensorData & data) :
UEvent(kCodeImageDepth),
data_(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
data_(data)
{
}

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

@@ -40,11 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Parameters.h"
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
namespace rtabmap {
class Signature;
class SMSignature;
class VWDictionary;
class VisualWord;
@@ -96,11 +96,10 @@ public:
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
void getPose(int signatureId, Transform & pose, int & mapId) const;
void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
void getLastNodeId(int & id) const;
@@ -131,11 +130,10 @@ private:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
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 loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const = 0;
virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;

10
corelib/include/rtabmap/core/DBReader.h
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UEventsSender.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
#include <opencv2/core/core.hpp>
@@ -53,14 +54,7 @@ public:
bool init(int startIndex=0);
void setFrameRate(float frameRate);
void getNextImage(cv::Mat & image,
cv::Mat & depth,
cv::Mat & depth2d,
float & fx, float & fy,
float & cx, float & cy,
Transform & localTransform,
Transform & pose,
int & seq);
SensorData getNextData();
protected:
virtual void mainLoopBegin();

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

@@ -39,14 +39,16 @@ public:
Link() :
from_(0),
to_(0),
type_(kUndef)
type_(kUndef),
variance_(1.0f)
{
}
Link(int from, int to, const Transform & transform, Type type) :
Link(int from, int to, Type type, const Transform & transform, float variance) :
from_(from),
to_(to),
transform_(transform),
type_(type)
type_(type),
variance_(variance)
{
}
@@ -56,12 +58,20 @@ public:
int to() const {return to_;}
const Transform & transform() const {return transform_;}
Type type() const {return type_;}
float variance() const {return variance_;}
void setFrom(int from) {from_ = from;}
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
void setVariance(float variance) {variance_ = variance;}
private:
int from_;
int to_;
Transform transform_;
Type type_;
float variance_;
};
}

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

@@ -80,8 +80,8 @@ public:
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
void updateNeighborLink(int fromId, int toId, const Transform & transform);
bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
std::map<int, int> getNeighborsId(int signatureId,
int margin,
int maxCheckedInDatabase = -1,
@@ -98,12 +98,9 @@ public:
void getPose(int locationId,
Transform & pose,
bool lookInDatabase = false) const;
std::map<int, Transform> getNeighborLinks(int signatureId,
bool ignoreNeighborByLoopClosure = false,
std::map<int, Link> getNeighborLinks(int signatureId,
bool lookInDatabase = false) const;
void getLoopClosureIds(int signatureId,
std::map<int, Transform> & loopClosureIds,
std::map<int, Transform> & childLoopClosureIds,
std::map<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
bool isRawDataKept() const {return _rawDataKept;}
float getSimilarityThreshold() const {return _similarityThreshold;}
@@ -154,19 +151,21 @@ public:
int getBowMinInliers() const {return _bowMinInliers;}
float getBowMaxDepth() const {return _bowMaxDepth;}
bool getBowForce2D() const {return _bowForce2D;}
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0) const;
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeScanMatchingTransform(
int newId,
int oldId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg = 0);
std::string * rejectedMsg = 0,
int * inliers = 0,
double * variance = 0);
private:
void preUpdate();
void addSignatureToStm(Signature * signature);
void addSignatureToStm(Signature * signature, float odomVariance);
void clear();
void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
@@ -244,12 +243,11 @@ private:
int _icpSamples;
float _icpMaxCorrespondenceDistance;
int _icpMaxIterations;
float _icpMaxFitness;
float _icpCorrespondenceRatio;
bool _icpPointToPlane;
int _icpPointToPlaneNormalNeighbors;
float _icp2MaxCorrespondenceDistance;
int _icp2MaxIterations;
float _icp2MaxFitness;
float _icp2CorrespondenceRatio;
float _icp2VoxelSize;

21
corelib/include/rtabmap/core/Odometry.h
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/OdometryInfo.h>
#include <opencv2/opencv.hpp>
@@ -56,7 +57,7 @@ class RTABMAP_EXP Odometry
{
public:
virtual ~Odometry() {}
Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
Transform process(const SensorData & data, OdometryInfo * info = 0);
virtual void reset(const Transform & initialPose = Transform::getIdentity());
bool isLargeEnoughTransform(const Transform & transform);
@@ -75,7 +76,7 @@ public:
float getAngularUpdate() const {return _angularUpdate;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
private:
int _maxFeatures;
@@ -110,7 +111,7 @@ public:
const Memory * getMemory() const {return _memory;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
private:
//Parameters
@@ -133,10 +134,10 @@ public:
const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
Transform computeTransformMono(const SensorData & image, int * quality, int * features);
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
Transform computeTransformStereo(const SensorData & image, OdometryInfo * info);
Transform computeTransformRGBD(const SensorData & image, OdometryInfo * info);
Transform computeTransformMono(const SensorData & image, OdometryInfo * info);
private:
//Parameters:
int flowWinSize_;
@@ -170,13 +171,13 @@ public:
int samples = 0,
float maxCorrespondenceDistance = 0.05f,
int maxIterations = 30,
float maxFitness = 0.01f,
float correspondenceRatio = 0.7f,
bool pointToPlane = true,
const ParametersMap & odometryParameter = rtabmap::ParametersMap());
virtual void reset(const Transform & initialPose = Transform::getIdentity());
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
private:
int _decimation;
@@ -184,7 +185,7 @@ private:
float _samples;
float _maxCorrespondenceDistance;
int _maxIterations;
float _maxFitness;
float _correspondenceRatio;
bool _pointToPlane;
pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane

18
corelib/include/rtabmap/core/OdometryEvent.h
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/OdometryInfo.h"
namespace rtabmap {
@@ -37,29 +38,20 @@ class OdometryEvent : public UEvent
{
public:
OdometryEvent(
const SensorData & data, int quality = -1, float time = 0.0f, int features = 0, int localMapSize = 0) :
const SensorData & data, const OdometryInfo & info = OdometryInfo()) :
_data(data),
_quality(quality),
_time(time),
_features(features),
_localMapSize(localMapSize)
_info(info)
{}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
bool isValid() const {return !_data.pose().isNull();}
const SensorData & data() const {return _data;}
int quality() const {return _quality;}
float time() const {return _time;} // seconds
int features() const {return _features;}
int localMapSize() const {return _localMapSize;}
const OdometryInfo & info() const {return _info;}
private:
SensorData _data;
int _quality;
float _time; // seconds
int _features;
int _localMapSize;
OdometryInfo _info;
};
class OdometryResetEvent : public UEvent

56
corelib/include/rtabmap/core/OdometryInfo.h Normal file
View File

@@ -0,0 +1,56 @@
/*
Copyright (c) 2010-2014, 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 ODOMETRYINFO_H_
#define ODOMETRYINFO_H_
namespace rtabmap {
class OdometryInfo
{
public:
OdometryInfo() :
lost(true),
matches(-1),
inliers(-1),
variance(-1),
features(-1),
localMapSize(-1),
time(0.0f)
{}
bool lost;
int matches;
int inliers;
float variance;
int features;
int localMapSize;
float time;
};
}
#endif /* ODOMETRYINFO_H_ */

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

@@ -283,6 +283,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations");
RTABMAP_PARAM(RGBD, ToroIgnoreVariance, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
// Local loop closure detection
@@ -344,13 +345,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "ICP 3D: Max iterations.");
RTABMAP_PARAM(LccIcp3, MaxFitness, float, 1.0, "ICP 3D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "ICP 3D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "ICP 3D: Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "ICP 3D: Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "ICP 2D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");
RTABMAP_PARAM(LccIcp2, MaxFitness, float, 1.0, "ICP 2D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");

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

@@ -157,6 +157,7 @@ private:
float _localDetectMaxNeighbors;
int _localDetectMaxDiffID;
int _toroIterations;
bool _toroIgnoreVariance;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;

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

@@ -52,20 +52,22 @@ public:
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
// Metric constructor + 2d depth
SensorData(const cv::Mat & image,
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
const cv::Mat & depth2d,
float fx,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
virtual ~SensorData() {}
@@ -80,11 +82,11 @@ public:
void setId(int id) {_id = id;}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}
void setPose(const Transform & pose, float variance) {_pose = pose; _poseVariance=variance;}
cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
const cv::Mat & depth2d() const {return _depth2d;}
const cv::Mat & laserScan() const {return _laserScan;}
float fx() const {return _fx;}
float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
float cx() const {return _cx;}
@@ -93,6 +95,7 @@ public:
float fyOrBaseline() const {return _fyOrBaseline;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
float poseVariance() const {return _poseVariance;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
@@ -108,13 +111,14 @@ private:
// Metric stuff
cv::Mat _depthOrRightImage;
cv::Mat _depth2d;
cv::Mat _laserScan;
float _fx;
float _fyOrBaseline;
float _cx;
float _cy;
Transform _pose;
Transform _localTransform;
float _poseVariance;
// features
std::vector<cv::KeyPoint> _keypoints;

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

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Link.h>
namespace rtabmap
{
@@ -56,7 +57,7 @@ public:
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
const cv::Mat & depth2D = cv::Mat(),
const cv::Mat & laserScan = cv::Mat(),
const cv::Mat & image = cv::Mat(),
const cv::Mat & depth = cv::Mat(),
float fx = 0.0f,
@@ -75,34 +76,27 @@ public:
int id() const {return _id;}
int mapId() const {return _mapId;}
void addNeighbors(const std::map<int, Transform> & neighbors);
void addNeighbor(int neighbor, const Transform & transform = Transform());
void removeNeighbor(int neighborId);
void removeNeighbors();
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
int getWeight() const {return _weight;}
bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
void changeLoopClosureId(int idFrom, int idTo);
void addLinks(const std::list<Link> & links);
void addLinks(const std::map<int, Link> & links);
void addLink(const Link & link);
void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
bool hasLink(int idTo) const;
void changeLinkIds(int idFrom, int idTo);
void removeLinks();
void removeLink(int idTo);
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
void changeNeighborIds(int idFrom, int idTo);
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
int getWeight() const {return _weight;}
const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
const std::map<int, Link> & getLinks() const {return _links;}
bool isSaved() const {return _saved;}
bool isModified() const {return _modified || _neighborsModified;}
bool isNeighborsModified() const {return _neighborsModified;}
bool isModified() const {return _modified || _linksModified;}
bool isLinksModified() const {return _linksModified;}
//visual words stuff
void removeAllWords();
@@ -121,12 +115,12 @@ public:
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
void setDepth2DCompressed(const cv::Mat & bytes) {_depth2DCompressed = bytes;}
void setLaserScanCompressed(const cv::Mat & bytes) {_laserScanCompressed = bytes;}
void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
const cv::Mat & getDepth2DCompressed() const {return _depth2DCompressed;}
const cv::Mat & getLaserScanCompressed() const {return _laserScanCompressed;}
float getDepthFx() const {return _fx;}
float getDepthFy() const {return _fy;}
float getDepthCx() const {return _cx;}
@@ -135,24 +129,22 @@ public:
const Transform & getLocalTransform() const {return _localTransform;}
void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
const cv::Mat & getDepthRaw() const {return _depthRaw;}
void setDepth2DRaw(const cv::Mat & depth2D) {_depth2DRaw = depth2D;}
const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
SensorData toSensorData();
void uncompressData();
void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw);
void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const;
void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw);
void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const;
private:
int _id;
int _mapId;
std::map<int, Transform> _neighbors; // id, transform
std::map<int, Link> _links; // id, transform
int _weight;
std::map<int, Transform> _loopClosureIds; // id, transform
std::map<int, Transform> _childLoopClosureIds; // id, transform
bool _saved; // If it's saved to bd
bool _modified;
bool _neighborsModified; // Optimization when updating signatures in database
bool _linksModified; // Optimization when updating signatures in database
// Contains all words (Some can be duplicates -> if a word appears 2
// times in the signature, it will be 2 times in this list)
@@ -163,7 +155,7 @@ private:
cv::Mat _imageCompressed; // compressed image
cv::Mat _depthCompressed; // compressed image
cv::Mat _depth2DCompressed; // compressed data
cv::Mat _laserScanCompressed; // compressed data
float _fx;
float _fy;
float _cx;
@@ -174,7 +166,7 @@ private:
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _depth2DRaw; // CV_32FC2
cv::Mat _laserScanRaw; // CV_32FC2
};
} // namespace rtabmap

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

@@ -232,8 +232,8 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type = CV_32FC1);
cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
@@ -298,31 +298,35 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
bool refineModel = false,
double refineModelSigma = 3.0,
int refineModelIterations = 10,
std::vector<int> * inliers = 0);
std::vector<int> * inliers = 0,
double * variance = 0);
Transform RTABMAP_EXP icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -463,6 +467,7 @@ void RTABMAP_EXP optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
void RTABMAP_EXP optimizeTOROGraph(
@@ -471,6 +476,7 @@ void RTABMAP_EXP optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
bool RTABMAP_EXP saveTOROGraph(

16
corelib/src/DBDriver.cpp
View File

@@ -406,7 +406,7 @@ void DBDriver::getNodeData(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -414,7 +414,7 @@ void DBDriver::getNodeData(
Transform & localTransform) const
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, depth2dCompressed, fx, fy, cx, cy, localTransform);
this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
_dbSafeAccessMutex.unlock();
}
@@ -435,10 +435,10 @@ void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
}
//TODO Check also in the trash ?
void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
{
_dbSafeAccessMutex.lock();
this->loadNeighborsQuery(signatureId, neighbors);
this->loadLinksQuery(signatureId, links, type);
_dbSafeAccessMutex.unlock();
}
@@ -450,14 +450,6 @@ void DBDriver::getWeight(int signatureId, int & weight) const
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
{
_dbSafeAccessMutex.lock();
this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
{

255
corelib/src/DBDriverSqlite3.cpp
View File

@@ -555,11 +555,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
cv::Mat depth2dCompressed;
//Create the laserScan
if(dataSize>4 && data)
{
(*iter)->setDepth2DCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
}
}
@@ -584,7 +583,7 @@ void DBDriverSqlite3::getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -681,7 +680,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth2d
if(dataSize>4 && data)
{
depth2dCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
@@ -820,7 +819,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
<< "FROM Node "
<< "LEFT OUTER JOIN Link "
<< "ON id = from_id "
<< "WHERE type!=1 "
<< "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
<< "ORDER BY id";
}
@@ -840,7 +839,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
}
}
@@ -962,72 +961,6 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
}
}
void DBDriverSqlite3::loadLoopClosuresQuery(int nodeId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
{
loopIds.clear();
childIds.clear();
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT to_id, type, transform FROM Link WHERE from_id = "
<< nodeId
<< " AND type > 0"
<< ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int toId = 0;
int type;
const void * data = 0;
int dataSize = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(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);
}
if(nodeId == toId)
{
UERROR("Loop links cannot be auto-reference links (node=%d)", toId);
}
else if(type == 1)
{
UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 1);
//loop id
loopIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(type == 2)
{
UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 2);
//loop id
childIds.insert(std::pair<int, Transform>(toId, transform));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
{
@@ -1413,7 +1346,10 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
}
}
void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::map<int, Link> & neighbors,
Link::Type typeIn) const
{
neighbors.clear();
if(_ppDb)
@@ -1424,15 +1360,38 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT to_id, transform FROM Link "
<< "WHERE from_id = " << signatureId
<< " AND type = 0"
<< " ORDER BY to_id";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, transform, variance FROM Link ";
}
else
{
query << "SELECT to_id, type, transform FROM Link ";
}
query << "WHERE from_id = " << signatureId;
if(typeIn != Link::kUndef)
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << " AND type = " << typeIn;
}
else if(typeIn == Link::kNeighbor)
{
query << " AND type = 0";
}
else if(typeIn > Link::kNeighbor)
{
query << " AND type > 0";
}
}
query << " ORDER BY to_id";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int toId = -1;
int type = Link::kUndef;
float variance = 1.0f;
const void * data = 0;
int dataSize = 0;
@@ -1443,6 +1402,7 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
int index = 0;
toId = sqlite3_column_int(ppStmt, index++);
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -1453,7 +1413,17 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
memcpy(transform.data(), data, dataSize);
}
neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
}
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, variance)));
}
rc = sqlite3_step(ppStmt);
}
@@ -1481,9 +1451,18 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
std::stringstream query;
int totalLinksLoaded = 0;
query << "SELECT to_id, type, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
query << "SELECT to_id, type, variance, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else
{
query << "SELECT to_id, type, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1496,9 +1475,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
int toId = -1;
int linkType = -1;
std::map<int, Transform> neighbors;
std::map<int, Transform> loopIds;
std::map<int, Transform> childIds;
float variance = 1.0f;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
@@ -1510,34 +1488,33 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
toId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
variance = sqlite3_column_double(ppStmt, index++);
}
//transform
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
Transform transform;
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
UASSERT((unsigned int)dataSize == transform.size()*sizeof(float) && data);
memcpy(transform.data(), data, dataSize);
if(linkType >= 0 && linkType != Link::kUndef)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
}
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, variance));
}
}
else
{
UFATAL("");
}
if(linkType == 1)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 1);
loopIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(linkType == 2)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 2);
childIds.insert(std::pair<int, Transform>(toId, transform));
}
else if(linkType == 0)
{
UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 0);
neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
linkType, (*iter)->id(), toId);
}
++totalLinksLoaded;
@@ -1546,14 +1523,12 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// add links
(*iter)->addNeighbors(neighbors);
(*iter)->setLoopClosureIds(loopIds);
(*iter)->setChildLoopClosureIds(childIds);
(*iter)->addLinks(links);
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", timer.ticks(), (*iter)->id(), neighbors.size(), loopIds.size(), childIds.size());
UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
}
// Finalize (delete) the statement
@@ -1610,7 +1585,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isNeighborsModified())
if((*j)->isLinksModified())
{
rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1632,24 +1607,13 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
{
if((*j)->isNeighborsModified())
if((*j)->isLinksModified())
{
// Save neighbor links
const std::map<int, Transform> & neighbors = (*j)->getNeighbors();
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
// Save links
const std::map<int, Link> & links = (*j)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 0, i->second);
}
// save loop closure links
const std::map<int, Transform> & loopIds = (*j)->getLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 1, i->second);
}
const std::map<int, Transform> & childIds = (*j)->getChildLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
{
stepLink(ppStmt, (*j)->id(), i->first, 2, i->second);
stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
}
}
}
@@ -1752,22 +1716,11 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save neighbor links
const std::map<int, Transform> & neighbors = (*jter)->getNeighbors();
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
// Save links
const std::map<int, Link> & links = (*jter)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 0, i->second);
}
// save loop closure links
const std::map<int, Transform> & loopIds = (*jter)->getLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 1, i->second);
}
const std::map<int, Transform> & childIds = (*jter)->getChildLoopClosureIds();
for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
{
stepLink(ppStmt, (*jter)->id(), i->first, 2, i->second);
stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
}
}
// Finalize (delete) the statement
@@ -1833,9 +1786,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getDepth2DCompressed().empty())
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getDepth2DCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
}
}
// Finalize (delete) the statement
@@ -2055,9 +2008,16 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepLink() const
{
return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
}
else
{
return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
}
}
void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const
void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
{
if(!ppStmt)
{
@@ -2072,6 +2032,13 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, type);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, variance);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, transform.data(), transform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

10
corelib/src/DBDriverSqlite3.h
View File

@@ -68,18 +68,14 @@ private:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
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 loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
virtual void loadLoopClosuresQuery(
int signatureId,
std::map<int, Transform> & loopIds,
std::map<int, Transform> & childIds) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery(
int signatureId,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
cv::Mat & laserScanCompressed,
float & fx,
float & fy,
float & cx,
@@ -113,7 +109,7 @@ private:
float cx,
float cy,
const Transform & localTransform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) 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 pcl::PointXYZ & pt) const;

89
corelib/src/DBReader.cpp
View File

@@ -131,36 +131,24 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
cv::Mat image, depth, depth2d;
float fx,fy,cx,cy;
Transform localTransform, pose;
int seq = 0;
this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
if(!image.empty())
SensorData data = this->getNextData();
if(data.isValid())
{
if(depth.empty())
if(!_odometryIgnored)
{
this->post(new CameraEvent(image));
if(data.pose().isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
this->post(new OdometryEvent(data));
}
else
{
if(!_odometryIgnored)
{
SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
this->post(new OdometryEvent(data));
if(pose.isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
}
else
{
// without odometry
this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
}
this->post(new CameraEvent(data));
}
}
else if(!this->isKilled())
{
@@ -171,18 +159,9 @@ void DBReader::mainLoop()
}
void DBReader::getNextImage(
cv::Mat & image,
cv::Mat & depth,
cv::Mat & depth2d,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform,
Transform & pose,
int & seq)
SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
@@ -209,11 +188,24 @@ void DBReader::getNextImage(
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat depth2dBytes;
cv::Mat laserScanBytes;
int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId);
seq = *_currentId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float variance = 1.0f;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
if(!_odometryIgnored)
{
_dbDriver->getPose(*_currentId, pose, mapId);
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
variance = links.begin()->second.variance();
}
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
@@ -222,22 +214,29 @@ void DBReader::getNextImage(
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(depth2dBytes, false);
util3d::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
image = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
depth2d = ctDepth2D.getUncompressedData();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
variance,
seq);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
} /* namespace rtabmap */

2
corelib/src/Features2d.cpp
View File

@@ -324,7 +324,7 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
if(RTABMAP_NONFREE == 0 &&
(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
{
UERROR("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
type = Feature2D::kFeatureOrb;
}
Feature2D * feature2D = 0;

861
corelib/src/Memory.cpp
View File

File diff suppressed because it is too large Load Diff

162
corelib/src/Odometry.cpp
View File

@@ -118,14 +118,22 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
fabs(yaw) > _angularUpdate;
}
Transform Odometry::process(SensorData & data, int * quality, int * features, int * localMapSize)
Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
UTimer time;
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
Transform t = this->computeTransform(data, quality, features, localMapSize);
Transform t = this->computeTransform(data, info);
if(info)
{
info->time = time.elapsed();
info->lost = t.isNull();
}
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
@@ -134,14 +142,10 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
_pose *= Transform(x,y,0,0,0,yaw);
}
else
{
_pose *= t;
t = Transform(x,y,0, 0,0,yaw);
}
return _pose;
return _pose *= t;
}
else if(_resetCurrentCount > 0)
{
@@ -231,11 +235,14 @@ void OdometryBOW::reset(const Transform & initialPose)
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
Transform OdometryBOW::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
@@ -276,10 +283,9 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
correspondences = (int)inliers1->size();
if((int)inliers1->size() >= this->getMinInliers())
{
correspondences = (int)inliers1->size();
// transform new words in local map referential
//inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, this->getPose());
@@ -291,7 +297,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
inliers = (int)inliersV.size();
if(!transform.isNull())
@@ -362,11 +369,6 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
transform = transform * icpT;
*/
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
transform.setNull();
@@ -469,13 +471,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
_memory->emptyTrash();
}
if(features)
if(info)
{
*features = nFeatures;
}
if(localMapSize)
{
*localMapSize = (int)localMap_.size();
info->variance = variance;
info->inliers = inliers;
info->matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
}
UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
@@ -547,32 +549,30 @@ void OdometryOpticalFlow::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
int * quality,
int * features,
int * localMapSize)
OdometryInfo * info)
{
UDEBUG("");
if(!data.rightImage().empty())
{
//stereo
return computeTransformStereo(data, quality, features);
return computeTransformStereo(data, info);
}
else
{
//rgbd
return computeTransformRGBD(data, quality, features);
return computeTransformRGBD(data, info);
}
}
Transform OdometryOpticalFlow::computeTransformStereo(
const SensorData & data,
int * quality,
int * features)
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
@@ -747,15 +747,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
output.setNull();
@@ -841,6 +837,14 @@ Transform OdometryOpticalFlow::computeTransformStereo(
output.setNull();
}
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
inliers,
@@ -853,12 +857,12 @@ Transform OdometryOpticalFlow::computeTransformStereo(
Transform OdometryOpticalFlow::computeTransformRGBD(
const SensorData & data,
int * quality,
int * features)
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = -1;
int inliers = 0;
int correspondences = 0;
@@ -967,15 +971,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(quality)
{
*quality = inliers;
}
if(inliers < this->getMinInliers())
{
output.setNull();
@@ -1097,6 +1097,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
output = Transform::getIdentity();
}
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
inliers,
@@ -1112,7 +1120,7 @@ OdometryICP::OdometryICP(int decimation,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
float maxFitness,
float correspondenceRatio,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
@@ -1121,7 +1129,7 @@ OdometryICP::OdometryICP(int decimation,
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_maxFitness(maxFitness),
_correspondenceRatio(correspondenceRatio),
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
@@ -1136,13 +1144,13 @@ void OdometryICP::reset(const Transform & initialPose)
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double fitness = 0;
double variance = -1;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
@@ -1177,27 +1185,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
//pcl::io::savePCDFile("old.pcd", *_previousCloud);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
@@ -1211,27 +1220,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
//pcl::io::savePCDFile("old.pcd", *_previousCloudNormal);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
@@ -1246,10 +1256,15 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
UERROR("Depth is empty?!?");
}
UINFO("Odom update time = %fs hasConverged=%s fitness=%f cloud=%d",
if(info)
{
info->variance = variance;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
fitness,
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
@@ -1316,13 +1331,10 @@ void OdometryThread::mainLoop()
getData(data);
if(data.isValid())
{
int quality = -1;
int features = -1;
int localMapSize = -1;
UTimer time;
Transform pose = _odometry->process(data, &quality, &features, &localMapSize);
data.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, quality, time.elapsed(), features, localMapSize));
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
data.setPose(pose, info.variance); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, info));
}
}

98
corelib/src/Rtabmap.cpp
View File

@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_toroIterations(Parameters::defaultRGBDToroIterations()),
_toroIgnoreVariance(Parameters::defaultRGBDToroIgnoreVariance()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -358,6 +359,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), _toroIgnoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -831,11 +833,11 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::map<int, Transform> & neighbors = signature->getNeighbors();
if(neighbors.size() == 1)
const std::map<int, Link> & links = signature->getLinks();
if(links.size() == 1)
{
float x,y,z, roll,pitch,yaw;
neighbors.begin()->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(x) < _rgbdLinearUpdate &&
fabs(y) < _rgbdLinearUpdate &&
fabs(z) < _rgbdLinearUpdate &&
@@ -857,26 +859,27 @@ bool Rtabmap::process(const SensorData & data)
// Scan matching
//============================================================
if(_poseScanMatching &&
signature->getNeighbors().size() == 1 &&
!signature->getDepth2DCompressed().empty() &&
signature->getLinks().size() == 1 &&
!signature->getLaserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
int oldId = signature->getNeighbors().begin()->first;
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
std::string rejectedMsg;
Transform guess = signature->getNeighbors().begin()->second;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg);
Transform guess = signature->getLinks().begin()->second.transform();
double variance = -1.0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
if(!t.isNull())
{
scanMatchingSuccess = true;
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getNeighbors().at(oldId).prettyPrint().c_str(),
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateNeighborLink(signature->id(), oldId, t);
_memory->updateNeighborLink(signature->id(), oldId, t, variance);
}
else
{
@@ -886,9 +889,9 @@ bool Rtabmap::process(const SensorData & data)
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
if(signature->getNeighbors().size() == 1)
if(signature->getLinks().size() == 1)
{
_constraints.insert(std::make_pair(signature->id(), Link(signature->id(), signature->getNeighbors().begin()->first, signature->getNeighbors().begin()->second, Link::kNeighbor)));
_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
}
//============================================================
@@ -902,15 +905,17 @@ bool Rtabmap::process(const SensorData & data)
for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
{
if(*iter != signature->id() &&
signature->getNeighbors().find(*iter) == signature->getNeighbors().end() &&
signature->getLinks().find(*iter) == signature->getLinks().end() &&
_memory->getSignature(*iter)->mapId() == signature->mapId())
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg);
double variance = -1.0;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg);
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -932,7 +937,7 @@ bool Rtabmap::process(const SensorData & data)
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, false))
if(_memory->addLoopClosureLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1251,6 +1256,7 @@ bool Rtabmap::process(const SensorData & data)
{
//Compute transform if metric data are present
Transform transform;
double variance = -1;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
@@ -1298,7 +1304,7 @@ bool Rtabmap::process(const SensorData & data)
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers);
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
@@ -1306,16 +1312,16 @@ bool Rtabmap::process(const SensorData & data)
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
Transform icpTransform = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1339,7 +1345,7 @@ bool Rtabmap::process(const SensorData & data)
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, true);
rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, Link::kGlobalClosure, variance);
}
if(rejectedHypothesis)
@@ -1362,7 +1368,7 @@ bool Rtabmap::process(const SensorData & data)
int localSpaceNearestId = 0;
if(_lcHypothesisId == 0 &&
_localLoopClosureDetectionSpace &&
!signature->getDepth2DCompressed().empty())
!signature->getLaserScanCompressed().empty())
{
if(_toroIterations == 0)
{
@@ -1386,10 +1392,11 @@ bool Rtabmap::process(const SensorData & data)
//The nearest will be the reference for a loop closure transform
if(poses.size() &&
localSpaceNearestId &&
signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
{
double variance = 1.0;
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg, 0, &variance);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
@@ -1397,7 +1404,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1531,9 +1538,9 @@ bool Rtabmap::process(const SensorData & data)
}
if(_lcHypothesisId || localSpaceClosureId)
{
UASSERT(uContains(sLoop->getLoopClosureIds(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLoopClosureIds().at(signature->id()).prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
}
if(!_rgbdSlamMode)
@@ -1602,10 +1609,9 @@ bool Rtabmap::process(const SensorData & data)
// global loop closure detection before starting the new map,
// otherwise it deletes the current node.
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
signature->getChildLoopClosureIds().size() == 0 && // no loop closure
signature->getNeighbors().size() == 0 && // no neighbors, alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
_memory->isIncremental() && // only in mapping mode
signature->getLinks().size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
{
_memory->deleteLocation(signature->id());
}
@@ -1751,9 +1757,9 @@ bool Rtabmap::process(const SensorData & data)
return true;
}
bool Rtabmap::process(const cv::Mat & sensorData, int id)
bool Rtabmap::process(const cv::Mat & image, int id)
{
return this->process(SensorData(sensorData, id));
return this->process(SensorData(image, id));
}
// SETTERS
@@ -1908,7 +1914,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(fromS->getNeighbors().find(ids[ind[i]]) == fromS->getNeighbors().end() && // can't be a neighbor
if(fromS->getLinks().find(ids[ind[i]]) == fromS->getLinks().end() && // can't be a neighbor
(minDistance == -1 || minDistance > dist[i]))
{
nearestId = ids[ind[i]];
@@ -2001,7 +2007,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true);
util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
}
}
}
@@ -2030,7 +2036,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
UDEBUG("values.size=%d", values.size());
float mean = uMean(values);
float stdDev = uStdDev(values, mean);
float stdDev = std::sqrt(uVariance(values, mean));
//Adjust likelihood with mean and standard deviation (see Angeli phd)
@@ -2132,8 +2138,15 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
// Get data
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
//remove virtual signature
@@ -2151,7 +2164,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
if(data.id() != Memory::kIdInvalid)
{
signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
}
@@ -2185,6 +2197,11 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
}
}
else
{
@@ -2197,11 +2214,6 @@ void Rtabmap::getGraph(
{
ids = _memory->getAllSignatureIds(); // STM + WM + LTM
}
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
{

25
corelib/src/SensorData.cpp
View File

@@ -42,7 +42,8 @@ SensorData::SensorData() :
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
{
}
@@ -54,7 +55,8 @@ SensorData::SensorData(const cv::Mat & image,
_fyOrBaseline(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
_localTransform(Transform::getIdentity()),
_poseVariance(1.0f)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -67,8 +69,9 @@ SensorData::SensorData(const cv::Mat & image,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id) :
_image(image),
_id(id),
@@ -78,7 +81,8 @@ SensorData::SensorData(const cv::Mat & image,
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
_localTransform(localTransform),
_poseVariance(poseVariance)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -90,27 +94,30 @@ SensorData::SensorData(const cv::Mat & image,
}
// Metric constructor + 2d depth
SensorData::SensorData(const cv::Mat & image,
SensorData::SensorData(const cv::Mat & laserScan,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
const cv::Mat & depth2d,
float fx,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id) :
_image(image),
_id(id),
_depthOrRightImage(depthOrRightImage),
_depth2d(depth2d),
_laserScan(laserScan),
_fx(fx),
_fyOrBaseline(fyOrBaseline),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
_localTransform(localTransform),
_poseVariance(poseVariance)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter

165
corelib/src/Signature.cpp
View File

@@ -42,7 +42,7 @@ Signature::Signature() :
_weight(-1),
_saved(false),
_modified(true),
_neighborsModified(true),
_linksModified(true),
_enabled(false),
_fx(0.0f),
_fy(0.0f),
@@ -57,7 +57,7 @@ Signature::Signature(
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const cv::Mat & depth2DCompressed, // in base_link frame
const cv::Mat & laserScanCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
float fx,
@@ -70,12 +70,12 @@ Signature::Signature(
_weight(0),
_saved(false),
_modified(true),
_neighborsModified(true),
_linksModified(true),
_words(words),
_enabled(false),
_imageCompressed(imageCompressed),
_depthCompressed(depthCompressed),
_depth2DCompressed(depth2DCompressed),
_laserScanCompressed(laserScanCompressed),
_fx(fx),
_fy(fy),
_cx(cx),
@@ -91,80 +91,64 @@ Signature::~Signature()
//UDEBUG("id=%d", _id);
}
void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
void Signature::addLinks(const std::list<Link> & links)
{
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
this->addNeighbor(i->first, i->second);
addLink(*iter);
}
}
void Signature::addLinks(const std::map<int, Link> & links)
{
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
addLink(iter->second);
}
}
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
UASSERT(link.from() == this->id());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true;
}
bool Signature::hasLink(int idTo) const
{
return _links.find(idTo) != _links.end();
}
void Signature::changeLinkIds(int idFrom, int idTo)
{
std::map<int, Link>::iterator iter = _links.find(idFrom);
if(iter != _links.end())
{
Link link = iter->second;
_links.erase(iter);
link.setTo(idTo);
_links.insert(std::make_pair(idTo, link));
_linksModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
}
void Signature::addNeighbor(int neighbor, const Transform & transform)
void Signature::removeLinks()
{
UDEBUG("Add neighbor %d to %d", neighbor, this->id());
_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
_neighborsModified = true;
if(_links.size())
_linksModified = true;
_links.clear();
}
void Signature::removeNeighbor(int neighborId)
void Signature::removeLink(int idTo)
{
int count = (int)_neighbors.erase(neighborId);
int count = (int)_links.erase(idTo);
if(count)
{
_neighborsModified = true;
_linksModified = true;
}
}
void Signature::removeNeighbors()
{
if(_neighbors.size())
_neighborsModified = true;
_neighbors.clear();
}
void Signature::changeNeighborIds(int idFrom, int idTo)
{
std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
if(iter != _neighbors.end())
{
Transform t = iter->second;
_neighbors.erase(iter);
_neighbors.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
{
if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
{
if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::changeLoopClosureId(int idFrom, int idTo)
{
std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
if(iter != _loopClosureIds.end())
{
Transform t = iter->second;
_loopClosureIds.erase(iter);
_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
}
float Signature::compareTo(const Signature & s) const
{
float similarity = 0.0f;
@@ -230,26 +214,43 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
SensorData Signature::toSensorData()
{
this->uncompressData();
return SensorData(_imageRaw,
float variance = 1.0f;
if(_links.size())
{
for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
{
if(iter->second.kNeighbor)
{
//Assume the first neighbor to be the backward neighbor link
if(iter->second.to() < iter->second.from())
{
variance = iter->second.variance();
break;
}
}
}
}
return SensorData(_laserScanRaw,
_imageRaw,
_depthRaw,
_depth2DRaw,
_fx,
_fy,
_cx,
_cy,
_pose,
_localTransform,
_pose,
variance,
_id);
}
void Signature::uncompressData()
{
uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
}
void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw)
void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
{
uncompressDataConst(imageRaw, depthRaw, depth2DRaw);
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
@@ -258,13 +259,13 @@ void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat *
{
_depthRaw = *depthRaw;
}
if(depth2DRaw && !depth2DRaw->empty() && _depth2DRaw.empty())
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
{
_depth2DRaw = *depth2DRaw;
_laserScanRaw = *laserScanRaw;
}
}
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const
void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
{
if(imageRaw)
{
@@ -274,17 +275,17 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
*depthRaw = _depthRaw;
}
if(depth2DRaw)
if(laserScanRaw)
{
*depth2DRaw = _depth2DRaw;
*laserScanRaw = _laserScanRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(depth2DRaw && depth2DRaw->empty()))
(laserScanRaw && laserScanRaw->empty()))
{
util3d::CompressionThread ctImage(_imageCompressed, true);
util3d::CompressionThread ctDepth(_depthCompressed, true);
util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
@@ -293,13 +294,13 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
ctDepth.start();
}
if(depth2DRaw && depth2DRaw->empty())
if(laserScanRaw && laserScanRaw->empty())
{
ctDepth2D.start();
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctDepth2D.join();
ctLaserScan.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
@@ -308,9 +309,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
{
*depthRaw = ctDepth.getUncompressedData();
}
if(depth2DRaw && depth2DRaw->empty())
if(laserScanRaw && laserScanRaw->empty())
{
*depth2DRaw = ctDepth2D.getUncompressedData();
*laserScanRaw = ctLaserScan.getUncompressedData();
}
}
}

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

@@ -46,6 +46,7 @@ CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
variance FLOAT NOT NULL,
transform BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)

275
corelib/src/util3d.cpp
View File

@@ -1070,27 +1070,27 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity,
return depth;
}
cv::Mat depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
{
cv::Mat depth2d(1, (int)cloud.size(), CV_32FC2);
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
for(unsigned int i=0; i<cloud.size(); ++i)
{
depth2d.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
depth2d.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
}
return depth2d;
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr depth2DToPointCloud(const cv::Mat & depth2D)
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
{
UASSERT(depth2D.empty() || depth2D.type() == CV_32FC2);
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(depth2D.cols);
for(int i=0; i<depth2D.cols; ++i)
output->resize(laserScan.cols);
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i).x = depth2D.at<cv::Vec2f>(i)[0];
output->at(i).y = depth2D.at<cv::Vec2f>(i)[1];
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
return output;
}
@@ -1495,12 +1495,17 @@ Transform transformFromXYZCorrespondences(
bool refineModel,
double refineModelSigma,
int refineModelIterations,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
double * varianceOut)
{
//NOTE: this method is a mix of two methods:
// - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
// - refineModel() in pcl/sample_consensus/sac.h
if(varianceOut)
{
*varianceOut = 1.0f;
}
Transform transform;
if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
{
@@ -1626,6 +1631,10 @@ Transform transformFromXYZCorrespondences(
{
*inliersOut = inliers;
}
if(varianceOut)
{
*varianceOut = model->computeVariance();
}
// get best transformation
Eigen::Matrix4f bestTransformation;
@@ -1661,8 +1670,9 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1677,13 +1687,65 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -1694,8 +1756,9 @@ Transform icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
@@ -1714,13 +1777,65 @@ Transform icpPointToPlane(
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointNormal> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -1730,8 +1845,9 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore)
bool * hasConvergedOut,
double * variance,
int * inliers)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
// Set the input source and target
@@ -1750,13 +1866,65 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
//icp.setTransformationEpsilon (transformationEpsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
// Perform the alignment
pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
icp.align (cloud_source_registered);
fitnessScore = icp.getFitnessScore();
hasConverged = icp.hasConverged();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
icp.align (*cloud_source_registered);
bool hasConverged = icp.hasConverged();
// compute variance
if((inliers || variance) && hasConverged)
{
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
est->setInputTarget(cloud_target);
est->setInputSource(cloud_source_registered);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(variance)
{
if(correspondences.size()>=3)
{
std::vector<double> distances(correspondences.size());
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
}
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
*variance = (2.1981 * median_error_sqr);
}
else
{
hasConverged = false;
*variance = -1.0;
}
}
if(inliers)
{
*inliers = correspondences.size();
}
}
else
{
if(inliers)
{
*inliers = 0;
}
if(variance)
{
*variance = -1;
}
}
if(hasConvergedOut)
{
*hasConvergedOut = hasConverged;
}
return transformFromEigen4f(icp.getFinalTransformation());
}
@@ -2145,6 +2313,7 @@ void optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes)
{
optimizedPoses.clear();
@@ -2190,17 +2359,26 @@ void optimizeTOROGraph(
{
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
}
}
std::map<int, rtabmap::Transform> optimizedPosesToro;
// Optimize!
if(posesToro.size() && edgeConstraintsToro.size())
{
std::list<std::map<int, rtabmap::Transform> > graphesToro;
rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro);
// Optimize!
rtabmap::util3d::optimizeTOROGraph(
posesToro,
edgeConstraintsToro,
optimizedPosesToro,
toroIterations,
toroInitialGuess,
ignoreCovariance,
&graphesToro);
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
@@ -2242,6 +2420,7 @@ void optimizeTOROGraph(
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
{
UASSERT(toroIterations>0);
@@ -2274,13 +2453,21 @@ void optimizeTOROGraph(
float x,y,z, roll,pitch,yaw;
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix m;
m=DMatrix<double>::I(6);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!ignoreCovariance && iter->second.variance()>0)
{
inf[0][0] = 1.0f/iter->second.variance(); // x
inf[1][1] = 1.0f/iter->second.variance(); // y
inf[2][2] = 1.0f/iter->second.variance(); // z
inf[3][3] = 1.0f/iter->second.variance(); // roll
inf[4][4] = 1.0f/iter->second.variance(); // pitch
inf[5][5] = 1.0f/iter->second.variance(); // yaw
}
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg.addEdge(v1, v2,t ,m))
if (!pg.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
return;
@@ -2380,7 +2567,7 @@ bool saveTOROGraph(
{
float x,y,z, yaw,pitch,roll;
pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1\n",
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
iter->first,
iter->second.to(),
x,
@@ -2388,7 +2575,13 @@ bool saveTOROGraph(
z,
roll,
pitch,
yaw);
yaw,
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance(),
1.0f/iter->second.variance());
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);