diff --git a/CMakeLists.txt b/CMakeLists.txt index 3326210b88..af93f8779f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") # VERSION ####################### SET(RTABMAP_MAJOR_VERSION 0) -SET(RTABMAP_MINOR_VERSION 7) -SET(RTABMAP_PATCH_VERSION 4) +SET(RTABMAP_MINOR_VERSION 8) +SET(RTABMAP_PATCH_VERSION 0) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/bin/.gitignore b/bin/.gitignore index 70f7bd9151..710bcf4e1e 100644 --- a/bin/.gitignore +++ b/bin/.gitignore @@ -19,3 +19,9 @@ /librtabmap_cored.so /librtabmap_guid.so /librtabmap_utilited.so +/librtabmap_core.so.0.8 +/librtabmap_core.so.0.8.0 +/librtabmap_gui.so.0.8 +/librtabmap_gui.so.0.8.0 +/librtabmap_utilite.so.0.8 +/librtabmap_utilite.so.0.8.0 diff --git a/corelib/include/rtabmap/core/CameraEvent.h b/corelib/include/rtabmap/core/CameraEvent.h index f8a033eece..415c1cbd4d 100644 --- a/corelib/include/rtabmap/core/CameraEvent.h +++ b/corelib/include/rtabmap/core/CameraEvent.h @@ -49,18 +49,21 @@ class CameraEvent : 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) { } diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index f0be0d36ca..09f8173f1c 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -40,11 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Parameters.h" #include +#include namespace rtabmap { class Signature; -class SMSignature; class VWDictionary; class VisualWord; @@ -96,11 +96,10 @@ class RTABMAP_EXP DBDriver : public UThreadNode // Specific queries... void loadNodeData(std::list & 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 & neighbors) const; - void loadLoopClosures(int signatureId, std::map & loopIds, std::map & childIds) const; + void loadLinks(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; void getWeight(int signatureId, int & weight) const; void getAllNodeIds(std::set & ids, bool ignoreChildren = false) const; void getLastNodeId(int & id) const; @@ -131,11 +130,10 @@ class RTABMAP_EXP DBDriver : public UThreadNode virtual void loadLastNodesQuery(std::list & signatures) const = 0; virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures) const = 0; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const = 0; - virtual void loadNeighborsQuery(int signatureId, std::map & neighbors) const = 0; - virtual void loadLoopClosuresQuery(int signatureId, std::map & loopIds, std::map & childIds) const = 0; + virtual void loadLinksQuery(int signatureId, std::map & links, Link::Type type = Link::kUndef) const = 0; virtual void loadNodeDataQuery(std::list & 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 & ids, bool ignoreChildren) const = 0; diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index a7d95bb06c..f0df4aee16 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -53,14 +54,7 @@ class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender { 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(); diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 5005726e8f..dfefaf6403 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -39,14 +39,16 @@ class Link 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 @@ class Link 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_; }; } diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 10771158bd..63a045c02f 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -80,8 +80,8 @@ class RTABMAP_EXP Memory std::list cleanup(const std::list & ignoredIds = std::list()); 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 getNeighborsId(int signatureId, int margin, int maxCheckedInDatabase = -1, @@ -98,12 +98,9 @@ class RTABMAP_EXP Memory void getPose(int locationId, Transform & pose, bool lookInDatabase = false) const; - std::map getNeighborLinks(int signatureId, - bool ignoreNeighborByLoopClosure = false, + std::map getNeighborLinks(int signatureId, bool lookInDatabase = false) const; - void getLoopClosureIds(int signatureId, - std::map & loopClosureIds, - std::map & childLoopClosureIds, + std::map getLoopClosureLinks(int signatureId, bool lookInDatabase = false) const; bool isRawDataKept() const {return _rawDataKept;} float getSimilarityThreshold() const {return _similarityThreshold;} @@ -154,19 +151,21 @@ class RTABMAP_EXP Memory 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 & 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 * deletedWords = 0); @@ -244,12 +243,11 @@ class RTABMAP_EXP Memory int _icpSamples; float _icpMaxCorrespondenceDistance; int _icpMaxIterations; - float _icpMaxFitness; + float _icpCorrespondenceRatio; bool _icpPointToPlane; int _icpPointToPlaneNormalNeighbors; float _icp2MaxCorrespondenceDistance; int _icp2MaxIterations; - float _icp2MaxFitness; float _icp2CorrespondenceRatio; float _icp2VoxelSize; diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 10ed84e16e..b19172f3a6 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include @@ -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 @@ class RTABMAP_EXP Odometry 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 @@ class RTABMAP_EXP OdometryBOW : public Odometry 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 @@ class RTABMAP_EXP OdometryOpticalFlow : public Odometry const pcl::PointCloud::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 @@ class RTABMAP_EXP OdometryICP : public Odometry 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 @@ class RTABMAP_EXP OdometryICP : public Odometry float _samples; float _maxCorrespondenceDistance; int _maxIterations; - float _maxFitness; + float _correspondenceRatio; bool _pointToPlane; pcl::PointCloud::Ptr _previousCloudNormal; // for point ot plane diff --git a/corelib/include/rtabmap/core/OdometryEvent.h b/corelib/include/rtabmap/core/OdometryEvent.h index aa4900414b..68ae5cdaa5 100644 --- a/corelib/include/rtabmap/core/OdometryEvent.h +++ b/corelib/include/rtabmap/core/OdometryEvent.h @@ -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 diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h new file mode 100644 index 0000000000..cb40de031d --- /dev/null +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -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_ */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index c1e905f3a7..b899a327a7 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -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."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 651b2c569f..57fa73c4f2 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -157,6 +157,7 @@ class RTABMAP_EXP Rtabmap float _localDetectMaxNeighbors; int _localDetectMaxDiffID; int _toroIterations; + bool _toroIgnoreVariance; std::string _databasePath; bool _optimizeFromGraphEnd; bool _reextractLoopClosureFeatures; diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 4981f50948..ebd3d8981e 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -52,20 +52,22 @@ class RTABMAP_EXP SensorData 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 @@ class RTABMAP_EXP SensorData 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 @@ class RTABMAP_EXP SensorData 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 & keypoints, const cv::Mat & descriptors) { @@ -108,13 +111,14 @@ class RTABMAP_EXP SensorData // 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 _keypoints; diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index 720351965f..7ca327a2cf 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace rtabmap { @@ -56,7 +57,7 @@ class RTABMAP_EXP Signature const std::multimap & words, const std::multimap & 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 @@ class RTABMAP_EXP Signature int id() const {return _id;} int mapId() const {return _mapId;} - void addNeighbors(const std::map & 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;} + + void addLinks(const std::list & links); + void addLinks(const std::map & links); + void addLink(const Link & link); - bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();} - void setLoopClosureIds(const std::map & 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); + bool hasLink(int idTo) const; - void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;} - void setChildLoopClosureIds(const std::map & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;} - void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform()); + 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 & getNeighbors() const {return _neighbors;} - int getWeight() const {return _weight;} - const std::map & getLoopClosureIds() const {return _loopClosureIds;} - const std::map & getChildLoopClosureIds() const {return _childLoopClosureIds;} + const std::map & 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 @@ class RTABMAP_EXP Signature //metric stuff void setWords3(const std::multimap & 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 & 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 @@ class RTABMAP_EXP Signature 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 _neighbors; // id, transform + std::map _links; // id, transform int _weight; - std::map _loopClosureIds; // id, transform - std::map _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 @@ class RTABMAP_EXP Signature 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 @@ class RTABMAP_EXP Signature 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 diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index fa91535db2..17a3a6c4ab 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -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 & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D); +cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud); +pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); std::vector 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 * inliers = 0); + std::vector * inliers = 0, + double * variance = 0); Transform RTABMAP_EXP icp( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::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::ConstPtr & cloud_source, const pcl::PointCloud::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::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool & hasConverged, - double & fitnessScore); + bool * hasConverged = 0, + double * variance = 0, + int * inliers = 0); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, @@ -463,6 +467,7 @@ void RTABMAP_EXP optimizeTOROGraph( std::map & optimizedPoses, int toroIterations = 100, bool toroInitialGuess = true, + bool ignoreCovariance = false, std::list > * intermediateGraphes = 0); void RTABMAP_EXP optimizeTOROGraph( @@ -471,6 +476,7 @@ void RTABMAP_EXP optimizeTOROGraph( std::map & optimizedPoses, int toroIterations = 100, bool toroInitialGuess = true, + bool ignoreCovariance = false, std::list > * intermediateGraphes = 0); bool RTABMAP_EXP saveTOROGraph( diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 75c6170764..9826ffed8a 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -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 & neighbors) const +void DBDriver::loadLinks(int signatureId, std::map & 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 & loopIds, std::map & childIds) const -{ - _dbSafeAccessMutex.lock(); - this->loadLoopClosuresQuery(signatureId, loopIds, childIds); - _dbSafeAccessMutex.unlock(); -} - //TODO Check also in the trash ? void DBDriver::getAllNodeIds(std::set & ids, bool ignoreChildren) const { diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 905e105646..1df5ca6bfa 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -555,11 +555,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & 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 & 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 & 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 & loopIds, std::map & 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(toId, transform)); - } - else if(type == 2) - { - UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 2); - //loop id - childIds.insert(std::pair(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 & ids, std::list & nodes) const { @@ -1413,7 +1346,10 @@ void DBDriverSqlite3::loadWordsQuery(const std::set & wordIds, std::list & neighbors) const +void DBDriverSqlite3::loadLinksQuery( + int signatureId, + std::map & neighbors, + Link::Type typeIn) const { neighbors.clear(); if(_ppDb) @@ -1424,15 +1360,38 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map= 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(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 & 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 & signatures) const int toId = -1; int linkType = -1; - std::map neighbors; - std::map loopIds; - std::map childIds; + float variance = 1.0f; + std::list links; const void * data = 0; int dataSize = 0; @@ -1510,34 +1488,33 @@ void DBDriverSqlite3::loadLinksQuery(std::list & 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) - { - memcpy(transform.data(), data, dataSize); - } - else - { - UFATAL(""); - } + UASSERT((unsigned int)dataSize == transform.size()*sizeof(float) && data); + memcpy(transform.data(), data, dataSize); - if(linkType == 1) - { - UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 1); - loopIds.insert(std::pair(toId, transform)); - } - else if(linkType == 2) + if(linkType >= 0 && linkType != Link::kUndef) { - UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 2); - childIds.insert(std::pair(toId, transform)); + 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 if(linkType == 0) + else { - UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 0); - neighbors.insert(neighbors.end(), std::pair(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 & 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 & nodes) const UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); for(std::list::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 & nodes) const UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); for(std::list::const_iterator j=nodes.begin(); j!=nodes.end(); ++j) { - if((*j)->isNeighborsModified()) + if((*j)->isLinksModified()) { - // Save neighbor links - const std::map & neighbors = (*j)->getNeighbors(); - for(std::map::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i) - { - stepLink(ppStmt, (*j)->id(), i->first, 0, i->second); - } - // save loop closure links - const std::map & loopIds = (*j)->getLoopClosureIds(); - for(std::map::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i) + // Save links + const std::map & links = (*j)->getLinks(); + for(std::map::const_iterator i=links.begin(); i!=links.end(); ++i) { - stepLink(ppStmt, (*j)->id(), i->first, 1, i->second); - } - const std::map & childIds = (*j)->getChildLoopClosureIds(); - for(std::map::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 & signatures) const UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); for(std::list::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter) { - // Save neighbor links - const std::map & neighbors = (*jter)->getNeighbors(); - for(std::map::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i) + // Save links + const std::map & links = (*jter)->getLinks(); + for(std::map::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 & loopIds = (*jter)->getLoopClosureIds(); - for(std::map::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i) - { - stepLink(ppStmt, (*jter)->id(), i->first, 1, i->second); - } - const std::map & childIds = (*jter)->getChildLoopClosureIds(); - for(std::map::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 & signatures) const for(std::list::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()); diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index 8c0501148c..4ee8c8fbbc 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -68,18 +68,14 @@ class RTABMAP_EXP DBDriverSqlite3: public DBDriver { virtual void loadLastNodesQuery(std::list & signatures) const; virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures) const; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const; - virtual void loadNeighborsQuery(int signatureId, std::map & neighbors) const; - virtual void loadLoopClosuresQuery( - int signatureId, - std::map & loopIds, - std::map & childIds) const; + virtual void loadLinksQuery(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; virtual void loadNodeDataQuery(std::list & 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 @@ class RTABMAP_EXP DBDriverSqlite3: public DBDriver { 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; diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 5b8eadd6a6..38323e18ef 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -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 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 */ diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 2ed0f23d28..34d16c8874 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -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; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index ad63c2e6fc..3b2379676e 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -93,13 +93,12 @@ Memory::Memory(const ParametersMap & parameters) : _icpSamples(Parameters::defaultLccIcp3Samples()), _icpMaxCorrespondenceDistance(Parameters::defaultLccIcp3MaxCorrespondenceDistance()), _icpMaxIterations(Parameters::defaultLccIcp3Iterations()), - _icpMaxFitness(Parameters::defaultLccIcp3MaxFitness()), + _icpCorrespondenceRatio(Parameters::defaultLccIcp3CorrespondenceRatio()), _icpPointToPlane(Parameters::defaultLccIcp3PointToPlane()), _icpPointToPlaneNormalNeighbors(Parameters::defaultLccIcp3PointToPlaneNormalNeighbors()), _icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()), _icp2MaxIterations(Parameters::defaultLccIcp2Iterations()), - _icp2MaxFitness(Parameters::defaultLccIcp2MaxFitness()), _icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()), _icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize()), @@ -405,12 +404,11 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccIcp3Samples(), _icpSamples); Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), _icpMaxIterations); - Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), _icpMaxFitness); + Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), _icpCorrespondenceRatio); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), _icpPointToPlane); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors); Parameters::parse(parameters, Parameters::kLccIcp2MaxCorrespondenceDistance(), _icp2MaxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations); - Parameters::parse(parameters, Parameters::kLccIcp2MaxFitness(), _icp2MaxFitness); Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio); Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize); @@ -431,12 +429,11 @@ void Memory::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_icpSamples >= 0, uFormat("value=%d", _icpSamples).c_str()); UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str()); UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str()); - UASSERT_MSG(_icpMaxFitness > 0.0f, uFormat("value=%f", _icpMaxFitness).c_str()); + UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str()); UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str()); UASSERT_MSG(_icp2MaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icp2MaxCorrespondenceDistance).c_str()); UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str()); - UASSERT_MSG(_icp2MaxFitness > 0.0f, uFormat("value=%f", _icp2MaxFitness).c_str()); - UASSERT_MSG(_icp2CorrespondenceRatio >=0.0f && _icp2CorrespondenceRatio <=1.0f, uFormat("value=%f", _icp2MaxFitness).c_str()); + UASSERT_MSG(_icp2CorrespondenceRatio >=0.0f && _icp2CorrespondenceRatio <=1.0f, uFormat("value=%f", _icp2CorrespondenceRatio).c_str()); UASSERT_MSG(_icp2VoxelSize >= 0, uFormat("value=%d", _icp2VoxelSize).c_str()); // Keypoint stuff @@ -529,7 +526,7 @@ bool Memory::update(const SensorData & data, Statistics * stats) UDEBUG("time creating signature=%f ms", t); // It will be added to the short-term memory, no need to delete it... - this->addSignatureToStm(signature); + this->addSignatureToStm(signature, data.poseVariance()); _lastSignature = signature; @@ -618,7 +615,7 @@ void Memory::setRoi(const std::string & roi) } } -void Memory::addSignatureToStm(Signature * signature) +void Memory::addSignatureToStm(Signature * signature, float poseVariance) { UTimer timer; // add signature on top of the short-term memory @@ -635,13 +632,14 @@ void Memory::addSignatureToStm(Signature * signature) !_signatures.at(*_stMem.rbegin())->getPose().isNull()) { motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose(); - _signatures.at(*_stMem.rbegin())->addNeighbor(signature->id(), motionEstimate); + _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseVariance)); + signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseVariance)); } else { - _signatures.at(*_stMem.rbegin())->addNeighbor(signature->id()); + _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f)); + signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f)); } - signature->addNeighbor(*_stMem.rbegin(), motionEstimate.isNull()?Transform():motionEstimate.inverse()); UDEBUG("Min STM id = %d", *_stMem.begin()); } else @@ -718,71 +716,27 @@ void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) cons } } -std::map Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool lookInDatabase) const +std::map Memory::getNeighborLinks( + int signatureId, + bool lookInDatabase) const { - std::map links; - Signature * sTop = uValue(_signatures, signatureId, (Signature*)0); - if(sTop) + std::map links; + Signature * s = uValue(_signatures, signatureId, (Signature*)0); + if(s) { - std::list loops; - loops.push_back(sTop); - while(loops.size()) + const std::map & allLinks = s->getLinks(); + for(std::map::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter) { - Signature * s = *loops.begin(); - loops.pop_front(); - if(s) + if(iter->second.type() == Link::kNeighbor) { - const std::map & neighbors = s->getNeighbors(); - links.insert(neighbors.begin(), neighbors.end()); - if(!ignoreNeighborByLoopClosure) - { - const std::map & loopIds = s->getLoopClosureIds(); - for(std::map::const_iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter) - { - if(iter->first > 0 && _stMem.find(iter->first) == _stMem.end()) - { - loops.push_back(uValue(_signatures, iter->first, (Signature*)0)); - } - } - } - } - } - - if(!ignoreNeighborByLoopClosure) - { - // Check for child loop closure ids - const std::map & childTopIds = sTop->getChildLoopClosureIds(); - for(std::map::const_iterator iter = childTopIds.begin(); iter!=childTopIds.end(); ++iter) - { - if(iter->first > 0 && _stMem.find(iter->first) == _stMem.end()) - { - loops.push_back(uValue(_signatures, iter->first, (Signature*)0)); - } - } - while(loops.size()) - { - Signature * s = *loops.begin(); - loops.pop_front(); - if(s) - { - const std::map & neighbors = s->getNeighbors(); - links.insert(neighbors.begin(), neighbors.end()); - const std::map & childIds = s->getChildLoopClosureIds(); - for(std::map::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter) - { - if(iter->first > 0 && _stMem.find(iter->first) == _stMem.end()) - { - loops.push_back(uValue(_signatures, iter->first, (Signature*)0)); - } - } - } + links.insert(*iter); } } } else if(lookInDatabase && _dbDriver) { - std::map neighbors; - _dbDriver->loadNeighbors(signatureId, neighbors); + std::map neighbors; + _dbDriver->loadLinks(signatureId, neighbors, Link::kNeighbor); links.insert(neighbors.begin(), neighbors.end()); } else @@ -792,6 +746,42 @@ std::map Memory::getNeighborLinks(int signatureId, bool ignoreNe return links; } +std::map Memory::getLoopClosureLinks( + int signatureId, + bool lookInDatabase) const +{ + const Signature * s = this->getSignature(signatureId); + std::map loopClosures; + if(s) + { + const std::map & allLinks = s->getLinks(); + for(std::map::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter) + { + if(iter->second.type() > Link::kNeighbor && + iter->second.type() != Link::kUndef) + { + loopClosures.insert(*iter); + } + } + } + else if(lookInDatabase && _dbDriver) + { + _dbDriver->loadLinks(signatureId, loopClosures); + for(std::map::iterator iter=loopClosures.begin(); iter!=loopClosures.end();) + { + if(iter->second.type() == Link::kNeighbor) + { + loopClosures.erase(iter++); + } + else + { + ++iter; + } + } + } + return loopClosures; +} + // return map, including signatureId // maxCheckedInDatabase = -1 means no limit to check in database (default) // maxCheckedInDatabase = 0 means don't check in database @@ -844,100 +834,54 @@ std::map Memory::getNeighborsId(int signatureId, //UDEBUG("Added %d with margin %d", *jter, m); // Look up in STM/WM if all ids are here, if not... load them from the database const Signature * s = this->getSignature(*jter); - std::map tmpNeighborIds; - std::map tmpLoopClosureIds; - std::map tmpChildLoopClosureIds; - const std::map * neighborIds = &tmpNeighborIds; - const std::map * loopClosureIds = &tmpLoopClosureIds; - const std::map * childLoopClosureIds = &tmpChildLoopClosureIds; + std::map tmpLinks; + const std::map * links = &tmpLinks; if(s) { ids.insert(std::pair(*jter, m)); - neighborIds = &s->getNeighbors(); - - if(!ignoreLoopIds) - { - loopClosureIds = &s->getLoopClosureIds(); - childLoopClosureIds = &s->getChildLoopClosureIds(); - } + links = &s->getLinks(); } else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase)) { ids.insert(std::pair(*jter, m)); UTimer timer; - _dbDriver->loadNeighbors(*jter, tmpNeighborIds); - if(!ignoreLoopIds) - { - _dbDriver->loadLoopClosures(*jter, tmpLoopClosureIds, tmpChildLoopClosureIds); - } + _dbDriver->loadLinks(*jter, tmpLinks); if(dbAccessTime) { *dbAccessTime += timer.getElapsedTime(); } } - // Neighbor links - for(std::map::const_iterator iter=neighborIds->begin(); iter!=neighborIds->end(); ++iter) - { - if( (ignoreLoopIds || (loopClosureIds->find(iter->first) == loopClosureIds->end() && childLoopClosureIds->find(iter->first) == childLoopClosureIds->end())) && - !uContains(ids, iter->first) && - nextMargin.find(iter->first) == nextMargin.end()) - { - nextMargin.insert(iter->first); - } - } - - // Parent links - for(std::map::const_iterator iter=loopClosureIds->begin(); iter!=loopClosureIds->end(); ++iter) + // links + for(std::map::const_iterator iter=links->begin(); iter!=links->end(); ++iter) { - if( iter->first && !uContains(ids, iter->first)/* && isInNeighborLimits(iter->first, limits)*/) + if( !uContains(ids, iter->first)) { - if(incrementMarginOnLoop) + UASSERT(iter->second.type() != Link::kUndef); + if(iter->second.type() == Link::kNeighbor) { nextMargin.insert(iter->first); - //UDEBUG("next of %d + %d", *jter, iter->first); } - else + else if(!ignoreLoopIds) { - if(currentMargin.insert(iter->first).second) + if(incrementMarginOnLoop) { - const Signature * s = this->getSignature(iter->first); - if(!s) - { - // update db count because it's on current margin - ++nbLoadedFromDb; - } - curentMarginList.push_back(iter->first); - //UDEBUG("current of %d + %d", *jter, iter->first); + nextMargin.insert(iter->first); } - } - } - } - - //Child links - for(std::map::const_iterator iter=childLoopClosureIds->begin(); iter!=childLoopClosureIds->end(); ++iter) - { - if( iter->first && !uContains(ids, iter->first)/* && isInNeighborLimits(iter->first, limits)*/) - { - if(incrementMarginOnLoop) - { - nextMargin.insert(iter->first); - //UDEBUG("next of %d + %d", *jter, iter->first); - } - else - { - if(currentMargin.insert(iter->first).second) + else { - const Signature * s = this->getSignature(iter->first); - if(!s) + if(currentMargin.insert(iter->first).second) { - // update db count because it's on current margin - ++nbLoadedFromDb; + const Signature * s = this->getSignature(iter->first); + if(!s) + { + // update db count because it's on current margin + ++nbLoadedFromDb; + } + curentMarginList.push_back(iter->first); } - curentMarginList.push_back(iter->first); - //UDEBUG("current of %d + %d", *jter, iter->first); } } } @@ -949,22 +893,6 @@ std::map Memory::getNeighborsId(int signatureId, return ids; } -void Memory::getLoopClosureIds(int signatureId, std::map & loopClosureIds, std::map & childLoopClosureIds, bool lookInDatabase) const -{ - const Signature * s = this->getSignature(signatureId); - loopClosureIds.clear(); - childLoopClosureIds.clear(); - if(s) - { - loopClosureIds = s->getLoopClosureIds(); - childLoopClosureIds = s->getChildLoopClosureIds(); - } - else if(lookInDatabase && _dbDriver) - { - _dbDriver->loadLoopClosures(signatureId, loopClosureIds, childLoopClosureIds); - } -} - int Memory::getNextId() { return ++_idCount; @@ -1413,35 +1341,22 @@ std::list Memory::getRemovableSignatures(int count, const std::set< { // ignore recent memory } - else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasNeighbor(*memIter))) + else if(*memIter > 0 && ignoredIds.find(*memIter) == ignoredIds.end() && (!lastInSTM || !lastInSTM->hasLink(*memIter))) { Signature * s = this->_getSignature(*memIter); if(s) { - // Its loop closures must not be in STM to be removable, rehearsal issue + // Links must not be in STM to be removable, rehearsal issue bool foundInSTM = false; - for(std::map::const_iterator iter = s->getLoopClosureIds().begin(); iter!=s->getLoopClosureIds().end(); ++iter) + for(std::map::const_iterator iter = s->getLinks().begin(); iter!=s->getLinks().end(); ++iter) { if(_stMem.find(iter->first) != _stMem.end()) { - UDEBUG("Ignored %d because it has a parent (%d) in STM", s->id(), iter->first); + UDEBUG("Ignored %d because it has a link (%d) to STM", s->id(), iter->first); foundInSTM = true; break; } } - // Its neighbors must not be in STM to be removable, rehearsal issue - if(!foundInSTM) - { - for(std::set::iterator iter = _stMem.begin(); iter!=_stMem.end(); ++iter) - { - if(s->hasNeighbor(*iter)) - { - UDEBUG("Ignored %d because it has a neighbor (%d) in STM", s->id(), *iter); - foundInSTM = true; - break; - } - } - } if(!foundInSTM) { // less weighted signature priority to be transferred @@ -1473,13 +1388,9 @@ std::list Memory::getRemovableSignatures(int count, const std::set< { if(!recentWmImmunized) { - - UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d", + UDEBUG("weight=%d, id=%d", iter->first.weight, - iter->second->id(), - int(iter->second->getLoopClosureIds().size()), - iter->second->getLoopClosureIds().size()?iter->second->getLoopClosureIds().rbegin()->first:0, - iter->second->getChildLoopClosureIds().size()?iter->second->getChildLoopClosureIds().rbegin()->first:0); + iter->second->id()); removableSignatures.push_back(iter->second); addedSignatures.insert(iter->second->id()); @@ -1495,12 +1406,9 @@ std::list Memory::getRemovableSignatures(int count, const std::set< } else if(iter->second->id() < _lastGlobalLoopClosureParentId) { - UDEBUG("weight=%d, id=%d, lcCount=%d, lcId=%d, childId=%d", + UDEBUG("weight=%d, id=%d", iter->first.weight, - iter->second->id(), - int(iter->second->getLoopClosureIds().size()), - iter->second->getLoopClosureIds().size()?iter->second->getLoopClosureIds().rbegin()->first:0, - iter->second->getChildLoopClosureIds().size()?iter->second->getChildLoopClosureIds().rbegin()->first:0); + iter->second->id()); removableSignatures.push_back(iter->second); addedSignatures.insert(iter->second->id()); } @@ -1531,61 +1439,37 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list * de { UASSERT_MSG(this->isInSTM(s->id()), uFormat("Deleting location (%d) outside the STM is not implemented!", s->id()).c_str()); - const std::map & neighbors = s->getNeighbors(); - for(std::map::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) + const std::map & links = s->getLinks(); + for(std::map::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - Signature * n = this->_getSignature(iter->first); + Signature * sTo = this->_getSignature(iter->first); // neighbor to s - if(n) + if(sTo) { - if(iter->first > s->id() && (n->getNeighbors().size() > 2 || !n->hasNeighbor(s->id()))) + if(iter->first > s->id() && (sTo->getLinks().size() == 1 || !sTo->hasLink(s->id()))) { - UWARN("Neighbor %d of %d is newer, removing neighbor link may split the map!", + UWARN("Link %d of %d is newer, removing neighbor link may split the map!", iter->first, s->id()); } - n->removeNeighbor(s->id()); - if(s == _lastSignature) + // child + if(iter->second.type() > Link::kNeighbor && s->id() > sTo->id()) { - _lastSignature = n; + sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight } - } - else - { - UERROR("neighbor %d of %d not in WM/STM?!?", iter->first, s->id()); - } - } - s->removeNeighbors(); - std::map children = s->getChildLoopClosureIds(); - for(std::map::const_iterator iter=children.begin(); iter!=children.end(); ++iter) - { - Signature * child = _getSignature(iter->first); - if(child) - { - child->removeLoopClosureId(s->id()); - child->setWeight(child->getWeight() + s->getWeight()); // copy weight - } - else - { - UERROR("loop child %d of %d not in WM/STM?!?", iter->first, s->id()); - } - s->removeChildLoopClosureId(iter->first); - } - std::map parents = s->getLoopClosureIds(); - for(std::map::const_iterator iter=parents.begin(); iter!=parents.end(); ++iter) - { - Signature * p = _getSignature(iter->first); - if(p) - { - p->removeChildLoopClosureId(s->id()); + sTo->removeLink(s->id()); + if(s == _lastSignature) + { + _lastSignature = sTo; + } } else { - UERROR("loop parent %d of %d not in WM/STM?!?", iter->first, s->id()); + UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id()); } - s->removeLoopClosureId(iter->first); } + s->removeLinks(); // remove all links s->setWeight(0); } @@ -1669,21 +1553,22 @@ void Memory::rejectLoopClosure(int oldId, int newId) if(oldS && newS) { UDEBUG("removing loop closure from location %d", newS->id()); + oldS->removeLink(newS->id()); + oldS->setWeight(oldS->getWeight()+1); - const std::map & children = newS->getChildLoopClosureIds(); - for(std::map::const_iterator iter=children.begin(); iter!=children.end(); ++iter) + newS->removeLink(oldS->id()); + newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0); + + bool noChildrenAnymore = true; + for(std::map::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter) { - if(iter->first == oldId) + if(iter->second.type() > Link::kNeighbor && iter->first < newS->id()) { - oldS->removeLoopClosureId(newS->id()); - oldS->setWeight(oldS->getWeight()+1); - - newS->removeChildLoopClosureId(iter->first); - newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0); + noChildrenAnymore = false; break; } } - if(newS->getChildLoopClosureIds().size() == 0 && newId == _lastGlobalLoopClosureParentId) + if(noChildrenAnymore && newId == _lastGlobalLoopClosureParentId) { _lastGlobalLoopClosureParentId = 0; _lastGlobalLoopClosureChildId = 0; @@ -1703,7 +1588,12 @@ void Memory::rejectLoopClosure(int oldId, int newId) } // compute transform newId -> oldId -Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rejectedMsg, int * inliers) const +Transform Memory::computeVisualTransform( + int oldId, + int newId, + std::string * rejectedMsg, + int * inliers, + double * variance) const { const Signature * oldS = this->getSignature(oldId); const Signature * newS = this->getSignature(newId); @@ -1712,7 +1602,7 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej if(oldS && newId) { - return computeVisualTransform(*oldS, *newS, rejectedMsg, inliers); + return computeVisualTransform(*oldS, *newS, rejectedMsg, inliers, variance); } else { @@ -1727,7 +1617,12 @@ Transform Memory::computeVisualTransform(int oldId, int newId, std::string * rej } // compute transform newId -> oldId -Transform Memory::computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg, int * inliers) const +Transform Memory::computeVisualTransform( + const Signature & oldS, + const Signature & newS, + std::string * rejectedMsg, + int * inliers, + double * variance) const { Transform transform; std::string msg; @@ -1755,7 +1650,8 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature _bowInlierDistance, _bowIterations, true, 3.0, 10, - &inliersV); + &inliersV, + variance); inliersCount = inliersV.size(); if(!t.isNull() && inliersCount >= _bowMinInliers) { @@ -1805,7 +1701,14 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature } // compute transform newId -> oldId -Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg) +Transform Memory::computeIcpTransform( + int oldId, + int newId, + Transform guess, + bool icp3D, + std::string * rejectedMsg, + int * inliers, + double * variance) { Signature * oldS = this->_getSignature(oldId); Signature * newS = this->_getSignature(newId); @@ -1831,11 +1734,11 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo else { //Depth required, if not in RAM, load it from LTM - if(oldS->getDepth2DCompressed().empty() && added.find(oldS->id()) == added.end()) + if(oldS->getLaserScanCompressed().empty() && added.find(oldS->id()) == added.end()) { depthToLoad.push_back(oldS); } - if(newS->getDepth2DCompressed().empty() && added.find(newS->id()) == added.end()) + if(newS->getLaserScanCompressed().empty() && added.find(newS->id()) == added.end()) { depthToLoad.push_back(newS); } @@ -1863,7 +1766,7 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo newS->uncompressData(0, 0, &tmp2); } - t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg); + t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance); } else { @@ -1878,7 +1781,14 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo } // get transform from the new to old node -Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg) const +Transform Memory::computeIcpTransform( + const Signature & oldS, + const Signature & newS, + Transform guess, + bool icp3D, + std::string * rejectedMsg, + int * inliers, + double * variance) const { if(guess.isNull()) { @@ -1931,11 +1841,12 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & guess * newS.getLocalTransform()); // 3D - double fitness = 0; - bool hasConverged = false; - Transform icpT; if(newCloudXYZ->size() && oldCloudXYZ->size()) { + bool hasConverged = false; + Transform icpT; + int correspondences = 0; + float correspondencesRatio = -1.0f; if(_icpPointToPlane) { pcl::PointCloud::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors); @@ -1951,36 +1862,47 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & oldCloud, _icpMaxCorrespondenceDistance, _icpMaxIterations, - hasConverged, - fitness); + &hasConverged, + variance, + &correspondences); } } else { - transform = util3d::icp(newCloudXYZ, + icpT = util3d::icp(newCloudXYZ, oldCloudXYZ, _icpMaxCorrespondenceDistance, _icpMaxIterations, - hasConverged, - fitness); + &hasConverged, + variance, + &correspondences); } - //pcl::io::savePCDFile("old.pcd", *oldCloudXYZ); - //pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ); - //newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT); - //pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ); + // verify if there are enough correspondences + correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()); + + UDEBUG("hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", + hasConverged?"true":"false", + variance?*variance:-1, + correspondences, + (int)oldCloudXYZ->size(), + correspondencesRatio*100.0f); - UDEBUG("fitness=%f", fitness); + if(inliers) + { + *inliers = correspondences; + } - if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness)) + if(!icpT.isNull() && hasConverged && + correspondencesRatio >= _icpCorrespondenceRatio) { transform = icpT * guess; transform = transform.inverse(); } else { - msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)", - hasConverged?"true":"false", fitness, _icpMaxFitness); + msg = uFormat("Cannot compute transform (hasConverged=%s variance=%f correspondencesRatio=%f/%f)", + hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icpCorrespondenceRatio); UINFO(msg.c_str()); } } @@ -2010,11 +1932,11 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p); } - if(!oldS.getDepth2DRaw().empty() && !newS.getDepth2DRaw().empty()) + if(!oldS.getLaserScanRaw().empty() && !newS.getLaserScanRaw().empty()) { // 2D - pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldS.getDepth2DRaw()); - pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.getDepth2DRaw(), guess); + pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldS.getLaserScanRaw()); + pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.getLaserScanRaw(), guess); //voxelize if(_icp2VoxelSize > 0.0f) @@ -2023,38 +1945,36 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & newCloud = util3d::voxelize(newCloud, _icp2VoxelSize); } - double fitness = 0.0f; - bool hasConverged = false; - Transform icpT; - float correspondencesRatio = -1.0f; if(newCloud->size() && oldCloud->size()) { + Transform icpT; + bool hasConverged = false; + float correspondencesRatio = -1.0f; + int correspondences = 0; icpT = util3d::icp2D(newCloud, oldCloud, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - hasConverged, - fitness); - - //UWARN("saving ICP2D clouds!"); - //pcl::io::savePCDFile("lccold.pcd", *oldCloud); - //pcl::io::savePCDFile("lccnewguess.pcd", *newCloud); - newCloud = util3d::transformPointCloud(newCloud, icpT); - //pcl::io::savePCDFile("lccnewicp.pcd", *newCloud); + &hasConverged, + variance, + &correspondences); // verify if there are enough correspondences - int correspondences = util3d::getCorrespondencesCount(newCloud, oldCloud, _icp2MaxCorrespondenceDistance); correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()); - UDEBUG("hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)", + UDEBUG("hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", hasConverged?"true":"false", - fitness, + variance?*variance:-1, correspondences, (int)oldCloud->size(), correspondencesRatio*100.0f); + if(inliers) + { + *inliers = correspondences; + } + if(!icpT.isNull() && hasConverged && - (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) && correspondencesRatio >= _icp2CorrespondenceRatio) { transform = icpT * guess; @@ -2062,8 +1982,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature & } else { - msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f correspondencesRatio=%f/%f)", - hasConverged?"true":"false", fitness, _icpMaxFitness, correspondencesRatio, _icp2CorrespondenceRatio); + msg = uFormat("Cannot compute transform (hasConverged=%s variance=%f correspondencesRatio=%f/%f)", + hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icp2CorrespondenceRatio); UINFO(msg.c_str()); } } @@ -2094,7 +2014,9 @@ Transform Memory::computeScanMatchingTransform( int newId, int oldId, const std::map & poses, - std::string * rejectedMsg) + std::string * rejectedMsg, + int * inliers, + double * variance) { // make sure that all depth2D are loaded std::list depthToLoad; @@ -2102,7 +2024,7 @@ Transform Memory::computeScanMatchingTransform( { Signature * s = _getSignature(iter->first); UASSERT(s != 0); - if(s->getDepth2DCompressed().empty()) + if(s->getLaserScanCompressed().empty()) { depthToLoad.push_back(s); } @@ -2119,9 +2041,9 @@ Transform Memory::computeScanMatchingTransform( if(iter->first != newId) { const Signature * s = this->getSignature(iter->first); - if(!s->getDepth2DCompressed().empty()) + if(!s->getLaserScanCompressed().empty()) { - *assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getDepth2DCompressed()), iter->second); + *assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getLaserScanCompressed()), iter->second); } else { @@ -2140,7 +2062,7 @@ Transform Memory::computeScanMatchingTransform( const Signature * newS = getSignature(newId); pcl::PointCloud::Ptr newCloud; UASSERT(uContains(poses, newId)); - newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(newId)); + newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getLaserScanCompressed()), poses.at(newId)); //voxelize if(newCloud->size() && _icp2VoxelSize > 0.0f) @@ -2155,32 +2077,33 @@ Transform Memory::computeScanMatchingTransform( Transform transform; if(assembledOldClouds->size() && newCloud->size()) { - double fitness = 0.0f; + int correspondences = 0; bool hasConverged = false; Transform icpT = util3d::icp2D(newCloud, assembledOldClouds, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - hasConverged, - fitness); + &hasConverged, + variance, + &correspondences); UDEBUG("icpT=%s", icpT.prettyPrint().c_str()); - newCloud = util3d::transformPointCloud(newCloud, icpT); - //pcl::io::savePCDFile("newCorrected.pcd", *newCloud); - // verify if there enough correspondences - int correspondences = util3d::getCorrespondencesCount(newCloud, assembledOldClouds, _icp2MaxCorrespondenceDistance); float correspondencesRatio = float(correspondences)/float(newCloud->size()); - UDEBUG("fitness=%f, correspondences=%d/%d (%f%%)", - fitness, + UDEBUG("variance=%f, correspondences=%d/%d (%f%%)", + variance?*variance:-1, correspondences, (int)newCloud->size(), correspondencesRatio); + if(inliers) + { + *inliers = correspondences; + } + if(!icpT.isNull() && hasConverged && - (_icp2MaxFitness == 0 || fitness < _icp2MaxFitness) && correspondencesRatio >= _icp2CorrespondenceRatio) { transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); @@ -2190,9 +2113,9 @@ Transform Memory::computeScanMatchingTransform( } else { - msg = uFormat("Constraints failed... hasConverged=%s, fitness=%f, correspondences=%d/%d (%f%%)", + msg = uFormat("Constraints failed... hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", hasConverged?"true":"false", - fitness, + variance?*variance:-1, correspondences, (int)newCloud->size(), correspondencesRatio); @@ -2214,28 +2137,29 @@ Transform Memory::computeScanMatchingTransform( } // Transform from new to old -bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global) +bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance) { + UASSERT(type > Link::kNeighbor && type != Link::kUndef); + ULOGGER_INFO("old=%d, new=%d transform: %s", oldId, newId, transform.prettyPrint().c_str()); Signature * oldS = _getSignature(oldId); Signature * newS = _getSignature(newId); if(oldS && newS) { - _memoryChanged = true; - const std::map & oldLoopclosureIds = oldS->getLoopClosureIds(); - if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end()) + if(oldS->hasLink(newId)) { // do nothing, already merged - UDEBUG("already merged, old=%d, new=%d", oldId, newId); + UINFO("already linked! old=%d, new=%d", oldId, newId); return true; } + _memoryChanged = true; UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id()); - oldS->addLoopClosureId(newS->id(), transform.inverse()); - newS->addChildLoopClosureId(oldS->id(), transform); + oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance)); + newS->addLink(Link(newS->id(), oldS->id(), type, transform, variance)); - if(_incrementalMemory && global) + if(_incrementalMemory && type == Link::kGlobalClosure) { _lastGlobalLoopClosureParentId = newS->id(); _lastGlobalLoopClosureChildId = oldS->id(); @@ -2260,18 +2184,18 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor return false; } -void Memory::updateNeighborLink(int fromId, int toId, const Transform & transform) +void Memory::updateNeighborLink(int fromId, int toId, const Transform & transform, float variance) { Signature * fromS = this->_getSignature(fromId); Signature * toS = this->_getSignature(toId); - if(fromS->hasNeighbor(toId) && toS->hasNeighbor(fromId)) + if(fromS->hasLink(toId) && toS->hasLink(fromId)) { - fromS->removeNeighbor(toId); - toS->removeNeighbor(fromId); + fromS->removeLink(toId); + toS->removeLink(fromId); - fromS->addNeighbor(toId, transform); - toS->addNeighbor(fromId, transform.inverse()); + fromS->addLink(Link(fromId, toId, Link::kNeighbor, transform, variance)); + toS->addLink(Link(toId, fromId, Link::kNeighbor, transform.inverse(), variance)); } else { @@ -2368,16 +2292,33 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const { fprintf(foutTree, "%d %d", i->first, i->second->getWeight()); - const std::map & loopIds = i->second->getLoopClosureIds(); + std::map loopIds, childIds; + + for(std::map::const_iterator iter = i->second->getLinks().begin(); + iter!=i->second->getLinks().end(); + ++iter) + { + if(iter->second.type() > Link::kNeighbor) + { + if(iter->first < i->first) + { + childIds.insert(*iter); + } + else + { + loopIds.insert(*iter); + } + } + } + fprintf(foutTree, " %d", (int)loopIds.size()); - for(std::map::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j) + for(std::map::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j) { fprintf(foutTree, " %d", j->first); } - const std::map & childIds = i->second->getChildLoopClosureIds(); fprintf(foutTree, " %d", (int)childIds.size()); - for(std::map::const_iterator j=childIds.begin(); j!=childIds.end(); ++j) + for(std::map::const_iterator j=childIds.begin(); j!=childIds.end(); ++j) { fprintf(foutTree, " %d", j->first); } @@ -2393,7 +2334,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const void Memory::rehearsal(Signature * signature, Statistics * stats) { UTimer timer; - if(signature->getNeighbors().size() != 1) + if(signature->getLinks().size() != 1) { return; } @@ -2401,7 +2342,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) //============================================================ // Compare with the last //============================================================ - int id = signature->getNeighbors().begin()->first; + int id = signature->getLinks().begin()->first; UDEBUG("Comparing with last signature (%d)...", id); const Signature * sB = this->getSignature(id); if(!sB) @@ -2439,8 +2380,8 @@ bool Memory::rehearsalMerge(int oldId, int newId) Signature * newS = _getSignature(newId); if(oldS && newS && _incrementalMemory) { - const std::map & oldLoopclosureIds = oldS->getLoopClosureIds(); - if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end()) + std::map::const_iterator iter = oldS->getLinks().find(newS->id()); + if(iter != oldS->getLinks().end() && iter->second.type() > Link::kNeighbor) { // do nothing, already merged UWARN("already merged, old=%d, new=%d", oldId, newId); @@ -2450,13 +2391,42 @@ bool Memory::rehearsalMerge(int oldId, int newId) UDEBUG("Rehearsal merge %d and %d", oldS->id(), newS->id()); + //remove mutual links + oldS->removeLink(newId); + newS->removeLink(oldId); + if(_idUpdatedToNewOneRehearsal) { + // redirect neighbor links + const std::map & links = oldS->getLinks(); + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + { + Link link = iter->second; + link.setFrom(newS->id()); + + Signature * s = this->_getSignature(link.to()); + if(s) + { + // modify neighbor "from" + s->changeLinkIds(oldS->id(), newS->id()); + + newS->addLink(link); + } + else + { + UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); + } + } + + oldS->removeLinks(); // remove all links + oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location + + // Set old image to new signature + this->copyData(oldS, newS); + // update weight newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - oldS->addLoopClosureId(newS->id()); // to keep track of the merged location - if(_lastGlobalLoopClosureParentId == oldS->id()) { _lastGlobalLoopClosureParentId = newS->id(); @@ -2464,76 +2434,17 @@ bool Memory::rehearsalMerge(int oldId, int newId) } else { + newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location + // update weight oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - newS->addLoopClosureId(oldS->id()); // to keep track of the merged location - if(_lastSignature == newS) { _lastSignature = oldS; } } - if(_idUpdatedToNewOneRehearsal) - { - // redirect neighbor links - const std::map & neighbors = oldS->getNeighbors(); - for(std::map::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter) - { - int link = iter->first; - Transform t = iter->second; - if(link != newS->id() && link != oldS->id()) - { - Signature * s = this->_getSignature(link); - if(s) - { - // modify neighbor "from" - s->changeNeighborIds(oldS->id(), newS->id()); - - if(!newS->hasNeighbor(link)) - { - newS->addNeighbor(link, t); - } - } - else - { - UERROR("Didn't find neighbor %d of %d in RAM...", link, oldS->id()); - } - } - } - oldS->removeNeighbors(); - - // redirect child loop closure links - std::map childIds = oldS->getChildLoopClosureIds(); - for(std::map::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter) - { - if(iter->first == newS->id()) - { - UERROR(""); - } - newS->addChildLoopClosureId(iter->first, iter->second); - Signature * s = _getSignature(iter->first); - if(s) - { - UDEBUG("changed child Loop closure %d from %d to %d", iter->first, oldS->id(), newS->id()); - s->changeLoopClosureId(oldS->id(), newS->id()); - } - else - { - UERROR("A location (%d, child of %d) in WM/STM cannot be transferred if its loop closure id is in STM", iter->first, oldS->id()); - } - oldS->removeChildLoopClosureId(iter->first); - } - - // Set old image to new signature - this->copyData(oldS, newS); - } - - //remove mutual links - oldS->removeNeighbor(newId); - newS->removeNeighbor(oldId); - // remove location bool saveToDb = _keepRehearsedNodesInDb; moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, saveToDb); @@ -2601,7 +2512,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData) { std::list signatures; signatures.push_back(s); - _dbDriver->loadNodeData(signatures, !s->getPose().isNull()); + _dbDriver->loadNodeData(signatures, true); r = *s; } else @@ -2641,7 +2552,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData) s->uncompressData(); r.setImageRaw(s->getImageRaw()); r.setDepthRaw(s->getDepthRaw()); - r.setDepth2DRaw(s->getDepth2DRaw()); + r.setLaserScanRaw(s->getLaserScanRaw()); } else { @@ -2695,38 +2606,11 @@ void Memory::generateGraph(const std::string & fileName, std::set ids) if(_signatures.find(*i) == _signatures.end()) { int id = *i; - std::map loopIds; - std::map childIds; - _dbDriver->loadLoopClosures(id, loopIds, childIds); - - std::map neighbors; - _dbDriver->loadNeighbors(id, neighbors); + std::map links; + _dbDriver->loadLinks(id, links); int weight = 0; _dbDriver->getWeight(id, weight); - for(std::map::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter) - { - if(id!=iter->first) - { - int weightNeighbor = 0; - if(_signatures.find(iter->first) == _signatures.end()) - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - else - { - weightNeighbor = _signatures.find(iter->first)->second->getWeight(); - } - //UDEBUG("Add neighbor link from %d to %d", id, iter->first); - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", - id, - weight, - iter->first, - weightNeighbor); - } - } - - // loop closure links... - for(std::map::iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter) + for(std::map::iterator iter = links.begin(); iter!=links.end(); ++iter) { int weightNeighbor = 0; if(_signatures.find(iter->first) == _signatures.end()) @@ -2737,32 +2621,35 @@ void Memory::generateGraph(const std::string & fileName, std::set ids) { weightNeighbor = _signatures.find(iter->first)->second->getWeight(); } - //UDEBUG("Add loop link from %d to %d", id, iter->first); - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorG); - } - for(std::map::iterator iter = childIds.begin(); iter!=childIds.end(); ++iter) - { - int weightNeighbor = 0; - if(_signatures.find(iter->first) == _signatures.end()) + //UDEBUG("Add neighbor link from %d to %d", id, iter->first); + if(iter->second.type() == Link::kNeighbor) { - _dbDriver->getWeight(iter->first, weightNeighbor); + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", + id, + weight, + iter->first, + weightNeighbor); + } + else if(iter->first > id) + { + //loop + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorG); } else { - weightNeighbor = _signatures.find(iter->first)->second->getWeight(); + //child + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorP); } - //UDEBUG("Add child link from %d to %d", id, iter->first); - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorP); } } } @@ -2771,48 +2658,32 @@ void Memory::generateGraph(const std::string & fileName, std::set ids) if(ids.find(i->first) != ids.end()) { int id = i->second->id(); - const std::map & loopIds = i->second->getLoopClosureIds(); - //don't show children when _loopClosuresMerged is on - //if(!_loopClosuresMerged || (loopIds.size() == 0)) + const std::map & links = i->second->getLinks(); + int weight = i->second->getWeight(); + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { - const std::map & neighbors = i->second->getNeighbors(); - int weight = i->second->getWeight(); - for(std::map::const_iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter) + int weightNeighbor = 0; + const Signature * s = this->getSignature(iter->first); + if(s) { - if(id != iter->first) - { - int weightNeighbor = 0; - const Signature * s = this->getSignature(iter->first); - if(s) - { - weightNeighbor = s->getWeight(); - } - else - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - //UDEBUG("Add neighbor link from %d to %d", id, iter->first); - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\";\n", - id, - weight, - iter->first, - weightNeighbor); - } + weightNeighbor = s->getWeight(); } - - // loop closure link - for(std::map::const_iterator iter = loopIds.begin(); iter!=loopIds.end(); ++iter) + else + { + _dbDriver->getWeight(iter->first, weightNeighbor); + } + //UDEBUG("Add neighbor link from %d to %d", id, iter->first); + if(iter->second.type() == Link::kNeighbor) { - int weightNeighbor = 0; - if(_signatures.find(iter->first) == _signatures.end()) - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - else - { - weightNeighbor = _signatures.find(iter->first)->second->getWeight(); - } - //UDEBUG("Add loop link from %d to %d", id, iter->first); + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", + id, + weight, + iter->first, + weightNeighbor); + } + else if(iter->first > id) + { + //loop fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", id, weight, @@ -2820,21 +2691,9 @@ void Memory::generateGraph(const std::string & fileName, std::set ids) weightNeighbor, colorG); } - - // child loop closure link - const std::map & childIds = i->second->getChildLoopClosureIds(); - for(std::map::const_iterator iter = childIds.begin(); iter!=childIds.end(); ++iter) + else { - int weightNeighbor = 0; - if(_signatures.find(iter->first) == _signatures.end()) - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - else - { - weightNeighbor = _signatures.find(iter->first)->second->getWeight(); - } - //UDEBUG("Add child link from %d to %d", id, iter->first); + //child fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", id, weight, @@ -2977,14 +2836,14 @@ void Memory::copyData(const Signature * from, Signature * to) { cv::Mat image; cv::Mat depth; - cv::Mat depth2d; + cv::Mat laserScan; float fx, fy, cx, cy; Transform localTransform; - _dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform); + _dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform); to->setImageCompressed(image); to->setDepthCompressed(depth, fx, fy, cx, cy); - to->setDepth2DCompressed(depth2d); + to->setLaserScanCompressed(laserScan); to->setLocalTransform(localTransform); UDEBUG("Loaded image data from database"); @@ -2993,7 +2852,7 @@ void Memory::copyData(const Signature * from, Signature * to) { to->setImageCompressed(from->getImageCompressed()); to->setDepthCompressed(from->getDepthCompressed(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy()); - to->setDepth2DCompressed(from->getDepth2DCompressed()); + to->setLaserScanCompressed(from->getLaserScanCompressed()); to->setLocalTransform(from->getLocalTransform()); } @@ -3028,7 +2887,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3); UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1); UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1); - UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2); + UASSERT(data.laserScan().empty() || data.laserScan().type() == CV_32FC2); UASSERT(_feature2D != 0); PreUpdateThread preUpdateThread(_vwd); @@ -3448,7 +3307,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S } util3d::CompressionThread ctImage(data.image(), std::string(".jpg")); util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png")); - util3d::CompressionThread ctDepth2d(data.depth2d()); + util3d::CompressionThread ctDepth2d(data.laserScan()); ctImage.start(); ctDepth.start(); ctDepth2d.start(); @@ -3471,7 +3330,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S data.localTransform()); s->setImageRaw(data.image()); s->setDepthRaw(depthOrRightImage); - s->setDepth2DRaw(data.depth2d()); + s->setLaserScanRaw(data.laserScan()); } else { @@ -3480,7 +3339,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S words, words3D, data.pose(), - util3d::compressData2(data.depth2d())); + util3d::compressData2(data.laserScan())); } @@ -3694,6 +3553,7 @@ void Memory::getMetricConstraints( std::multimap & links, bool lookInDatabase) { + UDEBUG(""); for(unsigned int i=0; i neighbors = this->getNeighborLinks(ids[i], true, lookInDatabase); // only direct neighbors - for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) + std::map neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors + for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) { - if(!jter->second.isNull() && uContains(poses, jter->first)) + if(!jter->second.transform().isNull() && uContains(poses, jter->first)) { bool edgeAlreadyAdded = false; for(std::multimap::iterator iter = links.lower_bound(jter->first); @@ -3725,18 +3585,19 @@ void Memory::getMetricConstraints( } if(!edgeAlreadyAdded) { - links.insert(std::make_pair(ids[i], Link(ids[i], jter->first, jter->second, Link::kNeighbor))); + links.insert(std::make_pair(ids[i], jter->second)); } } } - std::map loops, children; - this->getLoopClosureIds(ids[i], loops, children, lookInDatabase); - for(std::map::iterator jter=children.begin(); jter!=children.end(); ++jter) + std::map loops = this->getLoopClosureLinks(ids[i], lookInDatabase); + for(std::map::iterator jter=loops.begin(); jter!=loops.end(); ++jter) { - if(!jter->second.isNull() && uContains(poses, jter->first)) + if(jter->first < ids[i] && + !jter->second.transform().isNull() && + uContains(poses, jter->first)) { - links.insert(std::make_pair(ids[i], Link(ids[i], jter->first, jter->second, Link::kGlobalClosure))); + links.insert(std::make_pair(ids[i],jter->second)); } } } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 8e7ddfe594..7e77d7f024 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -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(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) - { - *features = nFeatures; - } - if(localMapSize) + if(info) { - *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), _previousCloud(new pcl::PointCloud) @@ -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::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::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)); } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 2c0caeb11e..8bfb57c3db 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -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 & neighbors = signature->getNeighbors(); - if(neighbors.size() == 1) + const std::map & 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::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 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 & 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 & signatures, std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeys(ids), poses, constraints, global); } + + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first))); + } } + + // Get data std::set ids = _memory->getWorkingMem(); // STM + WM //remove virtual signature @@ -2151,7 +2164,6 @@ void Rtabmap::get3DMap(std::map & 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 ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeys(ids), poses, constraints, global); } + + for(std::map::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::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())) { diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index cc8e4ff553..d7851ecfe9 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -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 diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index a84bf03cdd..bc25123789 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -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 & words, const std::multimap & 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 & neighbors) +void Signature::addLinks(const std::list & links) { - for(std::map::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i) + for(std::list::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { - this->addNeighbor(i->first, i->second); + addLink(*iter); } } - -void Signature::addNeighbor(int neighbor, const Transform & transform) +void Signature::addLinks(const std::map & links) { - UDEBUG("Add neighbor %d to %d", neighbor, this->id()); - _neighbors.insert(std::pair(neighbor, transform)); - _neighborsModified = true; -} - -void Signature::removeNeighbor(int neighborId) -{ - int count = (int)_neighbors.erase(neighborId); - if(count) + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { - _neighborsModified = true; + addLink(iter->second); } } - -void Signature::removeNeighbors() +void Signature::addLink(const Link & link) { - if(_neighbors.size()) - _neighborsModified = true; - _neighbors.clear(); + UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type()); + UASSERT(link.from() == this->id()); + std::pair::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; } -void Signature::changeNeighborIds(int idFrom, int idTo) +bool Signature::hasLink(int idTo) const { - std::map::iterator iter = _neighbors.find(idFrom); - if(iter != _neighbors.end()) - { - Transform t = iter->second; - _neighbors.erase(iter); - _neighbors.insert(std::pair(idTo, t)); - _neighborsModified = true; - } - UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo); + return _links.find(idTo) != _links.end(); } -void Signature::addLoopClosureId(int loopClosureId, const Transform & transform) +void Signature::changeLinkIds(int idFrom, int idTo) { - if(loopClosureId && _loopClosureIds.insert(std::pair(loopClosureId, transform)).second) + std::map::iterator iter = _links.find(idFrom); + if(iter != _links.end()) { - _neighborsModified=true; + 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::addChildLoopClosureId(int childLoopClosureId, const Transform & transform) +void Signature::removeLinks() { - if(childLoopClosureId && _childLoopClosureIds.insert(std::pair(childLoopClosureId, transform)).second) - { - _neighborsModified=true; - } + if(_links.size()) + _linksModified = true; + _links.clear(); } -void Signature::changeLoopClosureId(int idFrom, int idTo) +void Signature::removeLink(int idTo) { - std::map::iterator iter = _loopClosureIds.find(idFrom); - if(iter != _loopClosureIds.end()) + int count = (int)_links.erase(idTo); + if(count) { - Transform t = iter->second; - _loopClosureIds.erase(iter); - _loopClosureIds.insert(std::pair(idTo, t)); - _neighborsModified = true; + _linksModified = 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::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(); } } } diff --git a/corelib/src/resources/DatabaseSchema.sql.in b/corelib/src/resources/DatabaseSchema.sql.in index 30e054c672..10469eee9d 100644 --- a/corelib/src/resources/DatabaseSchema.sql.in +++ b/corelib/src/resources/DatabaseSchema.sql.in @@ -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) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 968029458f..e6300c1d35 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1070,27 +1070,27 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity, return depth; } -cv::Mat depth2DFromPointCloud(const pcl::PointCloud & cloud) +cv::Mat laserScanFromPointCloud(const pcl::PointCloud & 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(i)[0] = cloud.at(i).x; - depth2d.at(i)[1] = cloud.at(i).y; + laserScan.at(i)[0] = cloud.at(i).x; + laserScan.at(i)[1] = cloud.at(i).y; } - return depth2d; + return laserScan; } -pcl::PointCloud::Ptr depth2DToPointCloud(const cv::Mat & depth2D) +pcl::PointCloud::Ptr laserScanToPointCloud(const cv::Mat & laserScan) { - UASSERT(depth2D.empty() || depth2D.type() == CV_32FC2); + UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2); pcl::PointCloud::Ptr output(new pcl::PointCloud); - output->resize(depth2D.cols); - for(int i=0; iresize(laserScan.cols); + for(int i=0; iat(i).x = depth2D.at(i)[0]; - output->at(i).y = depth2D.at(i)[1]; + output->at(i).x = laserScan.at(i)[0]; + output->at(i).y = laserScan.at(i)[1]; } return output; } @@ -1495,12 +1495,17 @@ Transform transformFromXYZCorrespondences( bool refineModel, double refineModelSigma, int refineModelIterations, - std::vector * inliersOut) + std::vector * 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::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool & hasConverged, - double & fitnessScore) + bool * hasConvergedOut, + double * variance, + int * inliers) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -1677,13 +1687,65 @@ Transform icp(const pcl::PointCloud::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 cloud_source_registered; - icp.align (cloud_source_registered); - fitnessScore = icp.getFitnessScore(); - hasConverged = icp.hasConverged(); + pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); + icp.align (*cloud_source_registered); + bool hasConverged = icp.hasConverged(); + + // compute variance + if((inliers || variance) && hasConverged) + { + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloud_target); + est->setInputSource(cloud_source_registered); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + if(variance) + { + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 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::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool & hasConverged, - double & fitnessScore) + bool * hasConvergedOut, + double * variance, + int * inliers) { pcl::IterativeClosestPoint 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 cloud_source_registered; - icp.align (cloud_source_registered); - fitnessScore = icp.getFitnessScore(); - hasConverged = icp.hasConverged(); + pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); + icp.align (*cloud_source_registered); + bool hasConverged = icp.hasConverged(); + + // compute variance + if((inliers || variance) && hasConverged) + { + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloud_target); + est->setInputSource(cloud_source_registered); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + if(variance) + { + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 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::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool & hasConverged, - double & fitnessScore) + bool * hasConvergedOut, + double * variance, + int * inliers) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -1750,13 +1866,65 @@ Transform icp2D(const pcl::PointCloud::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 cloud_source_registered; - icp.align (cloud_source_registered); - fitnessScore = icp.getFitnessScore(); - hasConverged = icp.hasConverged(); + pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); + icp.align (*cloud_source_registered); + bool hasConverged = icp.hasConverged(); + + // compute variance + if((inliers || variance) && hasConverged) + { + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloud_target); + est->setInputSource(cloud_source_registered); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + if(variance) + { + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 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 & optimizedPoses, int toroIterations, bool toroInitialGuess, + bool ignoreCovariance, std::list > * 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 optimizedPosesToro; - // Optimize! if(posesToro.size() && edgeConstraintsToro.size()) { std::list > graphesToro; - rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro); + + // Optimize! + rtabmap::util3d::optimizeTOROGraph( + posesToro, + edgeConstraintsToro, + optimizedPosesToro, + toroIterations, + toroInitialGuess, + ignoreCovariance, + &graphesToro); + for(std::map::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 & optimizedPoses, int toroIterations, bool toroInitialGuess, + bool ignoreCovariance, std::list > * 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::I(6); + AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix::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 >::Vertex* v1=pg.vertex(id1); AISNavigation::TreePoseGraph >::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); diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 511d879b8b..65a9a62db0 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -95,6 +95,7 @@ private slots: void addConstraint(); void resetConstraint(); void rejectConstraint(); + void updateConstraintView(); private: void updateIds(); @@ -120,9 +121,9 @@ private slots: std::multimap updateLinksWithModifications( const std::multimap & edgeConstraints); void updateLoopClosuresSlider(int from = 0, int to = 0); - void refineConstraint(int from, int to); - void refineConstraintVisually(int from, int to); - bool addConstraint(int from, int to, bool silent); + void refineConstraint(int from, int to, bool updateGraph); + void refineConstraintVisually(int from, int to, bool updateGraph); + bool addConstraint(int from, int to, bool silent, bool updateGraph); private: Ui_DatabaseViewer * ui_; diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 4a5c39cd98..e8a53b0024 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/SensorData.h" +#include "rtabmap/core/OdometryInfo.h" #include "rtabmap/gui/PreferencesDialog.h" #include @@ -148,7 +149,7 @@ private slots: void selectScreenCaptureFormat(bool checked); void takeScreenshot(); void updateElapsedTime(); - void processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize); + void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info); void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags); void applyPrefSettings(const rtabmap::ParametersMap & parameters); void processRtabmapEventInit(int status, const QString & info); @@ -179,7 +180,7 @@ private slots: signals: void statsReceived(const rtabmap::Statistics &); - void odometryReceived(const rtabmap::SensorData &, int, float, int, int); + void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &); void thresholdsChanged(int, int); void stateChanged(MainWindow::State); void rtabmapEventInitReceived(int status, const QString & info); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index a302cb6cee..057174089e 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -137,6 +137,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int))); connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int))); connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int))); + connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView())); + ui_->checkBox_showOptimized->setEnabled(false); ui_->horizontalSlider_iterations->setTracking(false); ui_->dockWidget_graphView->setEnabled(false); @@ -145,9 +147,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); + connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); ui_->constraintsViewer->setCameraLockZ(false); - ui_->constraintsViewer->updateCameraPosition(Transform::getIdentity()); + ui_->constraintsViewer->setCameraFree(); } DatabaseViewer::~DatabaseViewer() @@ -189,6 +192,7 @@ bool DatabaseViewer::openDatabase(const QString & path) linksRemoved_.clear(); scans_.clear(); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->checkBox_showOptimized->setEnabled(false); } std::string driverType = "sqlite3"; @@ -238,11 +242,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event) std::multimap::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to()); if(refinedIter != linksRefined_.end()) { - memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true); + memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance()); } else { - memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true); + memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance()); } } @@ -252,7 +256,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event) if(!containsLink(linksAdded_, iter->second.from(), iter->second.to())) { memory_->rejectLoopClosure(iter->second.to(), iter->second.from()); - memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true); + memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance()); } } @@ -553,62 +557,158 @@ void DatabaseViewer::view3DMap() { if(!ids_.size() || !memory_) { - QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty...")); + QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty...")); return; } + if(graphes_.empty()) + { + this->updateGraphView(); + if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1) + { + QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!")); + return; + } + } bool ok = false; - int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok); + QStringList items; + items.append("1"); + items.append("2"); + items.append("4"); + items.append("8"); + items.append("16"); + QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); if(ok) { - QStringList items; - items.append("1"); - items.append("2"); - items.append("4"); - items.append("8"); - items.append("16"); - QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); + int decimation = item.toInt(); + double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok); if(ok) { - int decimation = item.toInt(); - double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok); - if(ok) + const std::map & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + if(optimizedPoses.size() > 0) { - std::multimap links = updateLinksWithModifications(links_); - // - std::map depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), margin); - if(depthGraph.size() > 0) + rtabmap::DetailedProgressDialog progressDialog(this); + progressDialog.setMaximumSteps(optimizedPoses.size()); + progressDialog.show(); + + // create a window + QDialog * window = new QDialog(this, Qt::Window); + window->setModal(this->isModal()); + window->setWindowTitle(tr("3D Map")); + window->setMinimumWidth(800); + window->setMinimumHeight(600); + + rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(viewer); + viewer->setCameraLockZ(false); + window->setLayout(layout); + connect(window, SIGNAL(finished(int)), viewer, SLOT(clear())); + + window->show(); + + for(std::map::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { - rtabmap::DetailedProgressDialog progressDialog(this); - progressDialog.setMaximumSteps(depthGraph.size()+2); - progressDialog.show(); + rtabmap::Transform pose = iter->second; + if(!pose.isNull()) + { + Signature data = memory_->getSignatureData(iter->first, true); + pcl::PointCloud::Ptr cloud; + UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1); + UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1); + if(data.getDepthRaw().type() == CV_8UC1) + { + cv::Mat leftImg; + if(data.getImageRaw().channels() == 3) + { + cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY); + } + else + { + leftImg = data.getImageRaw(); + } + cloud = rtabmap::util3d::cloudFromDisparityRGB( + data.getImageRaw(), + util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()), + data.getDepthCx(), data.getDepthCy(), + data.getDepthFx(), data.getDepthFy(), + decimation); + } + else + { + cloud = rtabmap::util3d::cloudFromDepthRGB( + data.getImageRaw(), + data.getDepthRaw(), + data.getDepthCx(), data.getDepthCy(), + data.getDepthFx(), data.getDepthFy(), + decimation); + } - progressDialog.appendText("Graph optimization..."); - std::multimap links = updateLinksWithModifications(links_); - std::map optimizedPoses; - util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked()); - progressDialog.appendText("Graph optimization... done!"); - progressDialog.incrementStep(); + if(maxDepth) + { + cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth); + } - // create a window - QDialog * window = new QDialog(this, Qt::Window); - window->setModal(this->isModal()); - window->setWindowTitle(tr("3D Map")); - window->setMinimumWidth(800); - window->setMinimumHeight(600); + cloud = rtabmap::util3d::transformPointCloud(cloud, data.getLocalTransform()); - rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window); + QColor color = Qt::red; + int mapId = memory_->getMapId(iter->first); + if(mapId >= 0) + { + color = (Qt::GlobalColor)(mapId % 12 + 7 ); + } + viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color); - QVBoxLayout *layout = new QVBoxLayout(); - layout->addWidget(viewer); - viewer->setCameraLockZ(false); - window->setLayout(layout); - connect(window, SIGNAL(finished(int)), viewer, SLOT(clear())); + UINFO("Generated %d (%d points)", iter->first, cloud->size()); + progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size())); + progressDialog.incrementStep(); + QApplication::processEvents(); + } + } + progressDialog.setValue(progressDialog.maximumSteps()); + } + else + { + QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value())); + } + } + } +} - window->show(); +void DatabaseViewer::generate3DMap() +{ + if(!ids_.size() || !memory_) + { + QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty...")); + return; + } + bool ok = false; + QStringList items; + items.append("1"); + items.append("2"); + items.append("4"); + items.append("8"); + items.append("16"); + QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); + if(ok) + { + int decimation = item.toInt(); + double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok); + if(ok) + { + QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_); + if(!path.isEmpty()) + { + const std::map & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + if(optimizedPoses.size() > 0) + { + rtabmap::DetailedProgressDialog progressDialog; + progressDialog.setMaximumSteps((int)optimizedPoses.size()); + progressDialog.show(); - for(std::map::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + for(std::map::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { - rtabmap::Transform pose = iter->second; + const rtabmap::Transform & pose = iter->second; if(!pose.isNull()) { Signature data = memory_->getSignatureData(iter->first, true); @@ -648,23 +748,18 @@ void DatabaseViewer::view3DMap() cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth); } - cloud = rtabmap::util3d::transformPointCloud(cloud, data.getLocalTransform()); - - QColor color = Qt::red; - int mapId = memory_->getMapId(iter->first); - if(mapId >= 0) - { - color = (Qt::GlobalColor)(mapId % 12 + 7 ); - } - viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color); - - UINFO("Generated %d (%d points)", iter->first, cloud->size()); - progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size())); + cloud = rtabmap::util3d::transformPointCloud(cloud, pose*data.getLocalTransform()); + std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first); + pcl::io::savePCDFile(name, *cloud); + UINFO("Saved %s (%d points)", name.c_str(), cloud->size()); + progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size())); progressDialog.incrementStep(); QApplication::processEvents(); } } progressDialog.setValue(progressDialog.maximumSteps()); + + QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path)); } else { @@ -675,132 +770,9 @@ void DatabaseViewer::view3DMap() } } -void DatabaseViewer::generate3DMap() -{ - if(!ids_.size() || !memory_) - { - QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty...")); - return; - } - bool ok = false; - int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok); - - if(ok) - { - int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok); - if(ok) - { - QStringList items; - items.append("1"); - items.append("2"); - items.append("4"); - items.append("8"); - items.append("16"); - QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); - if(ok) - { - int decimation = item.toInt(); - double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok); - if(ok) - { - QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_); - if(!path.isEmpty()) - { - std::multimap links = updateLinksWithModifications(links_); - // - std::map depthGraph = util3d::generateDepthGraph(links, id, margin); - if(depthGraph.size() > 0) - { - rtabmap::DetailedProgressDialog progressDialog; - progressDialog.setMaximumSteps((int)depthGraph.size()+2); - progressDialog.show(); - - progressDialog.appendText("Graph generation..."); - std::map poses, optimizedPoses; - std::multimap edgeConstraints; - memory_->getMetricConstraints(uKeys(depthGraph), poses, edgeConstraints, true); - edgeConstraints = updateLinksWithModifications(edgeConstraints); - progressDialog.appendText("Graph generation... done!"); - progressDialog.incrementStep(); - - progressDialog.appendText("Graph optimization..."); - rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked()); - progressDialog.appendText("Graph optimization... done!"); - progressDialog.incrementStep(); - - for(std::map::iterator iter = depthGraph.begin(); iter!=depthGraph.end(); ++iter) - { - rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform()); - if(!pose.isNull()) - { - Signature data = memory_->getSignatureData(iter->first, true); - pcl::PointCloud::Ptr cloud; - UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1); - UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1); - if(data.getDepthRaw().type() == CV_8UC1) - { - cv::Mat leftImg; - if(data.getImageRaw().channels() == 3) - { - cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY); - } - else - { - leftImg = data.getImageRaw(); - } - cloud = rtabmap::util3d::cloudFromDisparityRGB( - data.getImageRaw(), - util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()), - data.getDepthCx(), data.getDepthCy(), - data.getDepthFx(), data.getDepthFy(), - decimation); - } - else - { - cloud = rtabmap::util3d::cloudFromDepthRGB( - data.getImageRaw(), - data.getDepthRaw(), - data.getDepthCx(), data.getDepthCy(), - data.getDepthFx(), data.getDepthFy(), - decimation); - } - - if(maxDepth) - { - cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth); - } - - cloud = rtabmap::util3d::transformPointCloud(cloud, pose*data.getLocalTransform()); - std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first); - pcl::io::savePCDFile(name, *cloud); - UINFO("Saved %s (%d points)", name.c_str(), cloud->size()); - progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size())); - progressDialog.incrementStep(); - QApplication::processEvents(); - } - } - progressDialog.setValue(progressDialog.maximumSteps()); - - QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(depthGraph.size()).arg(path)); - } - else - { - QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(id)); - } - } - } - } - } - } -} - void DatabaseViewer::detectMoreLoopClosures() { - std::map optimizedPoses; - - std::multimap links = updateLinksWithModifications(links_); - std::map depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value()); - util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked()); + const std::map & optimizedPoses = graphes_.back(); int iterations = ui_->doubleSpinBox_detectMore_iterations->value(); UASSERT(iterations > 0); @@ -825,7 +797,7 @@ void DatabaseViewer::detectMoreLoopClosures() if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) && addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end()) { - if(addConstraint(from, to, true)) + if(addConstraint(from, to, true, false)) { UINFO("Added new loop closure between %d and %d.", from, to); ++added; @@ -840,6 +812,10 @@ void DatabaseViewer::detectMoreLoopClosures() break; } } + if(added) + { + this->updateGraphView(); + } UINFO("Total added %d loop closures.", added); } @@ -855,12 +831,14 @@ void DatabaseViewer::refineAllNeighborLinks() { int from = neighborLinks_[i].from(); int to = neighborLinks_[i].to(); - this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to()); + this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), false); progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size())); progressDialog.incrementStep(); QApplication::processEvents(); } + this->updateGraphView(); + progressDialog.setValue(progressDialog.maximumSteps()); progressDialog.appendText("Refining links finished!"); } @@ -878,12 +856,14 @@ void DatabaseViewer::refineAllLoopClosureLinks() { int from = loopLinks_[i].from(); int to = loopLinks_[i].to(); - this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to()); + this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), false); progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size())); progressDialog.incrementStep(); QApplication::processEvents(); } + this->updateGraphView(); + progressDialog.setValue(progressDialog.maximumSteps()); progressDialog.appendText("Refining links finished!"); } @@ -901,12 +881,14 @@ void DatabaseViewer::refineVisuallyAllNeighborLinks() { int from = neighborLinks_[i].from(); int to = neighborLinks_[i].to(); - this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to()); + this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), false); progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size())); progressDialog.incrementStep(); QApplication::processEvents(); } + this->updateGraphView(); + progressDialog.setValue(progressDialog.maximumSteps()); progressDialog.appendText("Refining links finished!"); } @@ -924,12 +906,14 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks() { int from = loopLinks_[i].from(); int to = loopLinks_[i].to(); - this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to()); + this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), false); progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size())); progressDialog.incrementStep(); QApplication::processEvents(); } + this->updateGraphView(); + progressDialog.setValue(progressDialog.maximumSteps()); progressDialog.appendText("Refining links finished!"); } @@ -1026,26 +1010,24 @@ void DatabaseViewer::update(int value, } // loops - std::map parents; - std::map children; - memory_->getLoopClosureIds(id, parents, children, true); - if(parents.size()) + std::map loopClosures; + loopClosures = memory_->getLoopClosureLinks(id, true); + if(loopClosures.size()) { - QString str; - for(std::map::iterator iter=parents.begin(); iter!=parents.end(); ++iter) + QString strParents, strChildren; + for(std::map::iterator iter=loopClosures.begin(); iter!=loopClosures.end(); ++iter) { - str.append(QString("%1 ").arg(iter->first)); - } - labelParents->setText(str); - } - if(children.size()) - { - QString str; - for(std::map::iterator iter=children.begin(); iter!=children.end(); ++iter) - { - str.append(QString("%1 ").arg(iter->first)); + if(iter->first < id) + { + strChildren.append(QString("%1 ").arg(iter->first)); + } + else + { + strParents.append(QString("%1 ").arg(iter->first)); + } } - labelChildren->setText(str); + labelParents->setText(strParents); + labelChildren->setText(strChildren); } } @@ -1396,22 +1378,61 @@ void DatabaseViewer::sliderLoopValueChanged(int value) this->updateConstraintView(loopLinks_.at(value)); } -void DatabaseViewer::updateConstraintView(const rtabmap::Link & link, +// only called when ui_->checkBox_showOptimized state changed +void DatabaseViewer::updateConstraintView() +{ + this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), + pcl::PointCloud::Ptr(new pcl::PointCloud), + pcl::PointCloud::Ptr(new pcl::PointCloud), + false); +} + +void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn, const pcl::PointCloud::Ptr & cloudFrom, const pcl::PointCloud::Ptr & cloudTo, bool updateImageSliders) { - std::multimap::iterator iter = util3d::findLink(linksRefined_, link.from(), link.to()); - rtabmap::Transform t = link.transform(); + std::multimap::iterator iter = util3d::findLink(linksRefined_, linkIn.from(), linkIn.to()); + rtabmap::Link link = linkIn; if(iter != linksRefined_.end()) { - t = iter->second.transform(); + link = iter->second; } + rtabmap::Transform t = link.transform(); ui_->label_constraint->clear(); + ui_->label_constraint_opt->clear(); + ui_->checkBox_showOptimized->setEnabled(false); UASSERT(!t.isNull() && memory_); - ui_->label_constraint->setText(t.prettyPrint().c_str()); + ui_->label_constraint->setText(QString("%1 (%2=%3)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.variance()))); + if(link.type() == Link::kNeighbor && + graphes_.size() && + (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum()) + { + std::map & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + if(link.type() == Link::kNeighbor) + { + std::map::iterator iterFrom = graph.find(link.from()); + std::map::iterator iterTo = graph.find(link.to()); + if(iterFrom != graph.end() && iterTo != graph.end()) + { + ui_->checkBox_showOptimized->setEnabled(true); + Transform topt = iterFrom->second.inverse()*iterTo->second; + Transform delta = t.inverse()*topt; + Transform v1 = t.rotation()*Transform(1,0,0,0,0,0); + Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0); + float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0)); + a = (a *180.0f) / CV_PI; + ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((delta.getNorm()/t.getNorm())*100.0f).arg(a)); + + if(ui_->checkBox_showOptimized->isChecked()) + { + t = topt; + } + } + } + } if(updateImageSliders) { @@ -1587,8 +1608,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link, //cloud 2d pcl::PointCloud::Ptr scanA, scanB; - scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw()); - scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw()); + scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw()); + scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw()); scanB = rtabmap::util3d::transformPointCloud(scanB, t); if(scanA->size()) @@ -1611,6 +1632,11 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link, ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo); } } + + //update cordinate + ui_->constraintsViewer->updateCameraPosition(t); + ui_->constraintsViewer->clearTrajectory(); + ui_->constraintsViewer->render(); } @@ -1669,18 +1695,18 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) { if(memory_ && value >=0 && value < (int)graphes_.size()) { - if(scans_.size() == 0) + if(ui_->dockWidget_graphView->isVisible() && scans_.size() == 0) { //update scans UINFO("Update scans list..."); for(int i=0; igetSignatureData(ids_.at(i), false); - if(!data.getDepth2DCompressed().empty()) + if(!data.getLaserScanCompressed().empty()) { pcl::PointCloud::Ptr cloud; - cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2DCompressed()); - cloud = rtabmap::util3d::depth2DToPointCloud(depth2d); + cv::Mat laserScan = rtabmap::util3d::uncompressData(data.getLaserScanCompressed()); + cloud = rtabmap::util3d::laserScanToPointCloud(laserScan); scans_.insert(std::make_pair(ids_.at(i), cloud)); } } @@ -1722,7 +1748,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } void DatabaseViewer::updateGraphView() { - if(ui_->dockWidget_graphView->isVisible() && poses_.size()) + if(poses_.size()) { if(!uContains(poses_, ui_->spinBox_optimizationsFrom->value())) { @@ -1740,7 +1766,14 @@ void DatabaseViewer::updateGraphView() ui_->actionGenerate_TORO_graph_graph->setEnabled(true); std::multimap links = updateLinksWithModifications(links_); std::map depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0); - util3d::optimizeTOROGraph(depthGraph, poses_, links, finalPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphes_); + util3d::optimizeTOROGraph( + depthGraph, + poses_, + links, finalPoses, + ui_->spinBox_iterations->value(), + ui_->checkBox_initGuess->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + &graphes_); graphes_.push_back(finalPoses); } if(graphes_.size()) @@ -1792,10 +1825,10 @@ void DatabaseViewer::refineConstraint() { int from = ids_.at(ui_->horizontalSlider_A->value()); int to = ids_.at(ui_->horizontalSlider_B->value()); - refineConstraint(from, to); + refineConstraint(from, to, true); } -void DatabaseViewer::refineConstraint(int from, int to) +void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph) { if(from == to) { @@ -1809,10 +1842,29 @@ void DatabaseViewer::refineConstraint(int from, int to) UERROR("Not found link! (%d->%d)", from, to); return; } + Transform t = currentLink.transform(); + if(ui_->checkBox_showOptimized->isChecked() && + currentLink.type() == Link::kNeighbor && + graphes_.size() && + (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum()) + { + std::map & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + if(currentLink.type() == Link::kNeighbor) + { + std::map::iterator iterFrom = graph.find(currentLink.from()); + std::map::iterator iterTo = graph.find(currentLink.to()); + if(iterFrom != graph.end() && iterTo != graph.end()) + { + Transform topt = iterFrom->second.inverse()*iterTo->second; + t = topt; + } + } + } bool hasConverged = false; - double fitness = 0.0f; + double variance = -1.0; + int correspondences = 0; Transform transform; Signature dataFrom, dataTo; @@ -1824,14 +1876,14 @@ void DatabaseViewer::refineConstraint(int from, int to) if(ui_->checkBox_icp_2d->isChecked()) { //2D - cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2DCompressed()); - cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2DCompressed()); + cv::Mat oldLaserScan = util3d::uncompressData(dataFrom.getLaserScanCompressed()); + cv::Mat newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed()); - if(!oldDepth2D.empty() && !newDepth2D.empty()) + if(!oldLaserScan.empty() && !newLaserScan.empty()) { // 2D - pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D); - pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, currentLink.transform()); + pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldLaserScan); + pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newLaserScan, t); //voxelize if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f) @@ -1846,8 +1898,9 @@ void DatabaseViewer::refineConstraint(int from, int to) oldCloud, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - hasConverged, - fitness); + &hasConverged, + &variance, + &correspondences); } } } @@ -1911,7 +1964,7 @@ void DatabaseViewer::refineConstraint(int from, int to) { cloudB = util3d::voxelize(cloudB, ui_->doubleSpinBox_icp_voxel->value()); } - cloudB = util3d::transformPointCloud(cloudB, currentLink.transform() * dataTo.getLocalTransform()); + cloudB = util3d::transformPointCloud(cloudB, t * dataTo.getLocalTransform()); } else { @@ -1921,7 +1974,7 @@ void DatabaseViewer::refineConstraint(int from, int to) ui_->doubleSpinBox_icp_maxDepth->value(), ui_->doubleSpinBox_icp_voxel->value(), 0, // no sampling - currentLink.transform() * dataTo.getLocalTransform()); + t * dataTo.getLocalTransform()); } if(ui_->checkBox_icp_p2plane->isChecked()) @@ -1945,8 +1998,9 @@ void DatabaseViewer::refineConstraint(int from, int to) cloudANormals, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - hasConverged, - fitness); + &hasConverged, + &variance, + &correspondences); } else { @@ -1954,15 +2008,15 @@ void DatabaseViewer::refineConstraint(int from, int to) cloudA, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - hasConverged, - fitness); + &hasConverged, + &variance, + &correspondences); } } if(hasConverged && !transform.isNull()) { - ui_->label_fitness->setNum(fitness); - Link newLink(currentLink.from(), currentLink.to(), transform*currentLink.transform(), currentLink.type()); + Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance); bool updated = false; std::multimap::iterator iter = linksRefined_.find(currentLink.from()); @@ -1980,27 +2034,29 @@ void DatabaseViewer::refineConstraint(int from, int to) if(!updated) { linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + + if(updateGraph) + { + this->updateGraphView(); + } } + if(ui_->dockWidget_constraints->isVisible()) { cloudB = util3d::transformPointCloud(cloudB, transform); this->updateConstraintView(newLink, cloudA, cloudB); } } - else - { - ui_->label_fitness->setText("not converged"); - } } void DatabaseViewer::refineConstraintVisually() { int from = ids_.at(ui_->horizontalSlider_A->value()); int to = ids_.at(ui_->horizontalSlider_B->value()); - refineConstraintVisually(from, to); + refineConstraintVisually(from, to, true); } -void DatabaseViewer::refineConstraintVisually(int from, int to) +void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph) { if(from == to) { @@ -2017,6 +2073,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to) Transform t; std::string rejectedMsg; + double variance = -1.0; + int inliers = -1; if(ui_->checkBox_visual_recomputeFeatures->isChecked()) { // create a fake memory to regenerate features @@ -2048,7 +2106,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to) } - t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg); + t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } else { @@ -2058,12 +2116,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to) parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); memory_->parseParameters(parameters); - t = memory_->computeVisualTransform(to, from, &rejectedMsg); + t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } if(!t.isNull()) { - Link newLink(currentLink.from(), currentLink.to(), t, currentLink.type()); + Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance); bool updated = false; std::multimap::iterator iter = linksRefined_.find(currentLink.from()); @@ -2081,6 +2139,11 @@ void DatabaseViewer::refineConstraintVisually(int from, int to) if(!updated) { linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + + if(updateGraph) + { + this->updateGraphView(); + } } if(ui_->dockWidget_constraints->isVisible()) { @@ -2093,10 +2156,10 @@ void DatabaseViewer::addConstraint() { int from = ids_.at(ui_->horizontalSlider_A->value()); int to = ids_.at(ui_->horizontalSlider_B->value()); - addConstraint(from, to, false); + addConstraint(from, to, false, true); } -bool DatabaseViewer::addConstraint(int from, int to, bool silent) +bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGraph) { if(from < to) { @@ -2120,6 +2183,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent) Transform t; std::string rejectedMsg; + double variance = -1.0; + int inliers = -1; if(ui_->checkBox_visual_recomputeFeatures->isChecked()) { // create a fake memory to regenerate features @@ -2151,7 +2216,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent) } - t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg); + t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } else { @@ -2161,7 +2226,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent) parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); memory_->parseParameters(parameters); - t = memory_->computeVisualTransform(to, from, &rejectedMsg); + t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } if(t.isNull()) @@ -2184,7 +2249,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent) } // transform is valid, make a link - linksAdded_.insert(std::make_pair(from, Link(from, to, t, Link::kUserClosure))); + linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance))); updateSlider = true; } } @@ -2198,6 +2263,10 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent) if(updateSlider) { updateLoopClosuresSlider(from, to); + if(updateGraph) + { + this->updateGraphView(); + } } return updateSlider; } @@ -2224,6 +2293,7 @@ void DatabaseViewer::resetConstraint() if(iter != linksRefined_.end()) { linksRefined_.erase(iter); + this->updateGraphView(); } iter = util3d::findLink(links_, from, to); @@ -2255,6 +2325,8 @@ void DatabaseViewer::rejectConstraint() return; } + bool removed = false; + // find the original one std::multimap::iterator iter; iter = util3d::findLink(links_, from, to); @@ -2266,6 +2338,7 @@ void DatabaseViewer::rejectConstraint() return; } linksRemoved_.insert(*iter); + removed = true; } // remove from refined and added @@ -2273,11 +2346,17 @@ void DatabaseViewer::rejectConstraint() if(iter != linksRefined_.end()) { linksRefined_.erase(iter); + removed = true; } iter = util3d::findLink(linksAdded_, from, to); if(iter != linksAdded_.end()) { linksAdded_.erase(iter); + removed = true; + } + if(removed) + { + this->updateGraphView(); } updateLoopClosuresSlider(); } diff --git a/guilib/src/LoopClosureViewer.cpp b/guilib/src/LoopClosureViewer.cpp index a1dbc528da..a79e490bff 100644 --- a/guilib/src/LoopClosureViewer.cpp +++ b/guilib/src/LoopClosureViewer.cpp @@ -169,8 +169,8 @@ void LoopClosureViewer::updateView(const Transform & transform) //cloud 2d pcl::PointCloud::Ptr scanA, scanB; - scanA = util3d::depth2DToPointCloud(sA_.getDepth2DRaw()); - scanB = util3d::depth2DToPointCloud(sB_.getDepth2DRaw()); + scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw()); + scanB = util3d::laserScanToPointCloud(sB_.getLaserScanRaw()); scanB = util3d::transformPointCloud(scanB, t); ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size())); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 1ead7b4d7e..6541c0eb80 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -358,7 +358,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics))); qRegisterMetaType("rtabmap::SensorData"); - connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float, int, int)), this, SLOT(processOdometry(rtabmap::SensorData, int, float, int, int))); + qRegisterMetaType("rtabmap::OdometryInfo"); + connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo))); connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection())); @@ -570,7 +571,7 @@ void MainWindow::handleEvent(UEvent* anEvent) !_processingStatistics) { _lastOdometryProcessed = false; // if we receive too many odometry events! - emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time(), odomEvent->features(), odomEvent->localMapSize()); + emit odometryReceived(odomEvent->data(), odomEvent->info()); } } else if(anEvent->getClassName().compare("ULogEvent") == 0) @@ -580,7 +581,7 @@ void MainWindow::handleEvent(UEvent* anEvent) { QMetaObject::invokeMethod(_ui->dockWidget_console, "show"); // The timer prevents multiple calls to pauseDetection() before the state can be changed - if(_state != kPaused && _logEventTime->elapsed() > 1000) + if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000) { _logEventTime->start(); if(_preferencesDialog->beepOnPause()) @@ -593,7 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent) } } -void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize) +void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info) { Transform pose = data.pose(); bool lost = false; @@ -607,11 +608,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, pose = _lastOdomPose; lost = true; } - else if(quality>=0 && + else if(info.inliers>=0 && _preferencesDialog->getOdomQualityWarnThr() && - quality < _preferencesDialog->getOdomQualityWarnThr()) + info.inliers < _preferencesDialog->getOdomQualityWarnThr()) { - UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr()); + UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr()); _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow); _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow)); } @@ -621,22 +622,42 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, _ui->widget_cloudViewer->setBackgroundColor(Qt::black); _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black)); } - if(quality >= 0) + if(info.inliers >= 0) { - _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)quality); + _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers); } - if(time > 0) + if(info.matches >= 0) { - _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f); + _ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches); } - if(features >=0) + if(info.variance >= 0) { - _ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)features); + _ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance)); } - if(localMapSize >=0) + if(info.variance >= 0) { - _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize); + _ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance); } + if(info.time > 0) + { + _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f); + } + if(info.features >=0) + { + _ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features); + } + if(info.localMapSize >=0) + { + _ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize); + } + float x,y,z, roll,pitch,yaw; + pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + _ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x); + _ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y); + _ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z); + _ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI); if(_ui->dockWidget_cloudViewer->isVisible()) { @@ -677,11 +698,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, } // 2d cloud - if(!data.depth2d().empty() && + if(!data.laserScan().empty() && _preferencesDialog->isScansShown(1)) { pcl::PointCloud::Ptr cloud; - cloud = util3d::depth2DToPointCloud(data.depth2d()); + cloud = util3d::laserScanToPointCloud(data.laserScan()); cloud = util3d::transformPointCloud(cloud, pose); if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) { @@ -1039,7 +1060,7 @@ void MainWindow::updateMapCloud( if(!_ui->actionView_scans->isEnabled() && _cachedSignatures.size() && - !(--_cachedSignatures.end())->getDepth2DCompressed().empty()) + !(--_cachedSignatures.end())->getLaserScanCompressed().empty()) { _ui->actionExport_2D_scans_ply_pcd->setEnabled(true); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true); @@ -1158,7 +1179,7 @@ void MainWindow::updateMapCloud( else if(_cachedSignatures.contains(iter->first)) { QMap::iterator jter = _cachedSignatures.find(iter->first); - if(!jter->getDepth2DCompressed().empty()) + if(!jter->getLaserScanCompressed().empty()) { this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); } @@ -1441,13 +1462,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m return; } - if(!iter->getDepth2DCompressed().empty()) + if(!iter->getLaserScanCompressed().empty()) { cv::Mat depth2D; iter->uncompressData(0, 0, &depth2D); pcl::PointCloud::Ptr cloud; - cloud = util3d::depth2DToPointCloud(depth2D); + cloud = util3d::laserScanToPointCloud(depth2D); QColor color = Qt::red; if(mapId >= 0) { @@ -2342,6 +2363,7 @@ void MainWindow::startDetection() } return; } + if(_odomThread) { UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent"); @@ -2746,9 +2768,11 @@ void MainWindow::postProcessing() _initProgressDialog->show(); ParametersMap parameters = _preferencesDialog->getAllParameters(); - int toroIterations = 100; - bool toroOptimizeFromGraphEnd = false; + int toroIterations = Parameters::defaultRGBDToroIterations(); + bool ignoreVariance = Parameters::defaultRGBDToroIgnoreVariance(); + bool toroOptimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd(); Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations); + Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), ignoreVariance); Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd); int loopClosuresAdded = 0; @@ -2819,7 +2843,8 @@ void MainWindow::postProcessing() Transform transform; std::string rejectedMsg; - int inliers; + int inliers = -1; + double variance = -1.0; if(reextractFeatures) { memory.init("", true); // clear previously added signatures @@ -2846,7 +2871,7 @@ void MainWindow::postProcessing() memory.update(dataTo); } - transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers); + transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance); } else { @@ -2855,14 +2880,14 @@ void MainWindow::postProcessing() } else { - transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers); + transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance); } if(!transform.isNull()) { UINFO("Added new loop closure between %d and %d.", from, to); addedLinks.insert(from); addedLinks.insert(to); - _currentLinksMap.insert(std::make_pair(from, Link(from, to, transform, Link::kUserClosure))); + _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance))); ++loopClosuresAdded; _initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size())); } @@ -2882,7 +2907,7 @@ void MainWindow::postProcessing() .arg(odomPoses.size()).arg(_currentLinksMap.size())); std::map optimizedPoses; std::map depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first); - util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations); + util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance); _currentPosesMap = optimizedPoses; _initProgressDialog->appendText(tr("Optimizing graph with new links... done!")); } @@ -2903,14 +2928,14 @@ void MainWindow::postProcessing() float maxDepth=2.0f; float voxelSize=0.01f; int samples = 0; - float minFitness = 1.0f; float maxCorrespondences = 0.05f; + float correspondenceRatio = 0.7f; float icpIterations = 30; Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation); Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth); Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize); Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples); - Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), minFitness); + Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio); Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences); Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations); bool pointToPlane = false; @@ -2974,7 +2999,8 @@ void MainWindow::postProcessing() iter->second.transform() * signatureTo.getLocalTransform()); bool hasConverged = false; - double fitness = -1; + double variance = -1; + int correspondences = 0; Transform transform; if(pointToPlane) { @@ -2997,8 +3023,9 @@ void MainWindow::postProcessing() cloudANormals, maxCorrespondences, icpIterations, - hasConverged, - fitness); + &hasConverged, + &variance, + &correspondences); } else { @@ -3006,18 +3033,22 @@ void MainWindow::postProcessing() cloudA, maxCorrespondences, icpIterations, - hasConverged, - fitness); + &hasConverged, + &variance, + &correspondences); } - if(hasConverged && !transform.isNull() && fitness>=0.0f && fitness <= minFitness) + float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size()); + + if(!transform.isNull() && hasConverged && + correspondencesRatio >= correspondenceRatio) { - Link newLink(from, to, transform*iter->second.transform(), iter->second.type()); + Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance); iter->second = newLink; } else { - UWARN("Cannot refine link %d->%d (converged=%s fitness=%f)", from, to, hasConverged?"true":"false", fitness); + UWARN("Cannot refine link %d->%d (converged=%s variance=%f)", from, to, hasConverged?"true":"false", variance); } } } @@ -3029,7 +3060,7 @@ void MainWindow::postProcessing() .arg(odomPoses.size()).arg(_currentLinksMap.size())); std::map optimizedPoses; std::map depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first); - util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations); + util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance); _initProgressDialog->appendText(tr("Optimizing graph with updated links... done!")); _initProgressDialog->incrementStep(); @@ -4698,7 +4729,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->statusbar->showMessage(tr("Paused...")); _ui->actionDump_the_memory->setEnabled(true); _ui->actionDump_the_prediction_matrix->setEnabled(true); - _ui->actionDelete_memory->setEnabled(true); + _ui->actionDelete_memory->setEnabled(false); _ui->actionGenerate_map->setEnabled(true); _ui->actionGenerate_local_map->setEnabled(true); _ui->actionGenerate_TORO_graph_graph->setEnabled(true); diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 624cdb7138..afffebf4c0 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -187,7 +187,7 @@ void OdometryViewer::handleEvent(UEvent * event) { data_.back() = odomEvent->data(); } - dataQuality_ = odomEvent->quality(); + dataQuality_ = odomEvent->info().inliers; dataMutex_.unlock(); if(empty) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 97318dfc46..2413c57378 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -440,6 +440,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str()); _ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str()); _ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str()); + _ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str()); _ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); _ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str()); @@ -469,13 +470,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpSamples->setObjectName(Parameters::kLccIcp3Samples().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str()); _ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str()); - _ui->loopClosure_icpMaxFitness->setObjectName(Parameters::kLccIcp3MaxFitness().c_str()); + _ui->loopClosure_icpRatio->setObjectName(Parameters::kLccIcp3CorrespondenceRatio().c_str()); _ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str()); _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str()); _ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str()); _ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str()); - _ui->loopClosure_icp2MaxFitness->setObjectName(Parameters::kLccIcp2MaxFitness().c_str()); _ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str()); _ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str()); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index dfbc9bc0f8..8de442f880 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -7,7 +7,7 @@ 0 0 1187 - 851 + 862 @@ -354,31 +354,31 @@ - - + + - + Transform - - + + - Transform + - + - Fitness + Optimized - + - - + @@ -511,14 +511,14 @@ - + Optimize from: - + @@ -538,20 +538,37 @@ - + Total path length (m): - + + + + + Ignore covariance: + + + + + + + + + + false + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 86d8eabc94..9c73075845 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 + -305 744 - 1074 + 1252 @@ -86,7 +86,7 @@ QFrame::Raised - 1 + 19 @@ -5174,7 +5174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Optimize graph from the newest node. @@ -5187,13 +5187,30 @@ Warning when set to false: when some nodes are transferred, the first referentia - + + + + + + + + + + + + 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. + + + true + + + @@ -5209,7 +5226,7 @@ Warning when set to false: when some nodes are transferred, the first referentia - Activate local detection over all locations in STM. The Bayes filter is not used here, so it may results in more false detections. The same 2-steps technique as for global loop closure constraints is used here. + Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. The same 2-steps technique as for global loop closure constraints is used here. This generates more constraints in the map's graph, so more time is required to optimize the graph. true @@ -5837,19 +5854,25 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + 0.000000000000000 + + + 1.000000000000000 + + 0.010000000000000 - 30.000000000000000 + 0.700000000000000 - Maximum fitness to accept the computed transform. + Ratio of matching correspondences to accept the transform. true @@ -5955,26 +5978,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - 0.010000000000000 - - - 30.000000000000000 - - - - - - - Maximum fitness to accept the computed transform. - - - true - - - - 0.000000000000000 @@ -5986,11 +5989,11 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0.010000000000000 - 0.900000000000000 + 0.700000000000000 - + Ratio of matching correspondences to accept the transform. @@ -6000,17 +6003,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - Voxel size. Set to 0 to disable voxel filtering. - - - true - - - - + m @@ -6032,6 +6025,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + Voxel size. Set to 0 to disable voxel filtering. + + + true + + + diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 47245b5029..5691810d71 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -19,6 +19,7 @@ #include "UPlot.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMath.h" #include #include @@ -1341,6 +1342,8 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) : _aResetText = new QAction(tr("Reset text..."), this); _aChangeColor = new QAction(tr("Change color..."), this); _aCopyToClipboard = new QAction(tr("Copy curve data to the clipboard"), this); + _aShowStdDev = new QAction(tr("Show std deviation"), this); + _aShowStdDev->setCheckable(true); _aMoveUp = new QAction(tr("Move up"), this); _aMoveDown = new QAction(tr("Move down"), this); _aRemoveCurve = new QAction(tr("Remove this curve"), this); @@ -1349,6 +1352,7 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) : _menu->addAction(_aResetText); _menu->addAction(_aChangeColor); _menu->addAction(_aCopyToClipboard); + _menu->addAction(_aShowStdDev); _menu->addSeparator(); _menu->addAction(_aMoveUp); _menu->addAction(_aMoveDown); @@ -1416,6 +1420,20 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event) clipboard->setText((textX+"\n")+textY); } } + else if(action == _aShowStdDev) + { + if(_aShowStdDev->isChecked()) + { + connect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev())); + } + else + { + disconnect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev())); + QString nameSpaced = _curve->name(); + nameSpaced.replace('_', ' '); + this->setText(nameSpaced); + } + } else if(action == _aRemoveCurve) { emit legendItemRemoved(_curve); @@ -1442,6 +1460,17 @@ QPixmap UPlotLegendItem::createSymbol(const QPen & pen, const QBrush & brush) return pixmap; } +void UPlotLegendItem::updateStdDev() +{ + QVector x, y; + _curve->getData(x, y); + float stdDev = std::sqrt(uVariance(y.data(), y.size())); + QString nameSpaced = _curve->name(); + nameSpaced.replace('_', ' '); + nameSpaced += QString(" (%1=%2)").arg(QChar(0xc3, 0x03)).arg(stdDev); + this->setText(nameSpaced); +} + diff --git a/guilib/src/utilite/UPlot.h b/guilib/src/utilite/UPlot.h index f6b43b4d5f..e354aa98e6 100644 --- a/guilib/src/utilite/UPlot.h +++ b/guilib/src/utilite/UPlot.h @@ -356,6 +356,9 @@ class UPlotLegendItem : public QPushButton void moveUpRequest(UPlotLegendItem *); void moveDownRequest(UPlotLegendItem *); +private slots: + void updateStdDev(); + protected: virtual void contextMenuEvent(QContextMenuEvent * event); @@ -366,6 +369,7 @@ class UPlotLegendItem : public QPushButton QAction * _aResetText; QAction * _aChangeColor; QAction * _aCopyToClipboard; + QAction * _aShowStdDev; QAction * _aRemoveCurve; QAction * _aMoveUp; QAction * _aMoveDown; diff --git a/package.xml b/package.xml index dfa80eaf58..7f6c31da07 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.7.3 + 0.8.0 RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index d05cb173ac..a9d0f1adfd 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -46,7 +46,8 @@ void showUsage() " -debug Set debug level for the logger.\n" " -rate #.# Input rate Hz (default 0=inf)\n" " -openni Use openni camera instead of the usb camera.\n" - " -openni2 Use openni2 camera instead of the usb camera.\n"); + " -openni2 Use openni2 camera instead of the usb camera.\n" + " -freenect Use freenect camera instead of the usb camera.\n"); exit(1); } @@ -76,6 +77,7 @@ int main (int argc, char * argv[]) bool show = true; bool openni = false; bool openni2 = false; + bool freenect = false; float rate = 0.0f; if(argc < 2) @@ -121,6 +123,11 @@ int main (int argc, char * argv[]) openni2 = true; continue; } + if(strcmp(argv[i], "-freenect") == 0) + { + freenect = true; + continue; + } printf("Unrecognized option : %s\n", argv[i]); showUsage(); @@ -135,7 +142,33 @@ int main (int argc, char * argv[]) UINFO("Output = %s", fileName.toStdString().c_str()); UINFO("Show = %s", show?"true":"false"); - UINFO("Openni = %s", openni?"true":"false"); + if(openni) + { + UINFO("Openni = true"); + if(!CameraOpenni::available()) + { + UERROR("Openni is not available. Please select another driver."); + return -1; + } + } + else if(openni2) + { + UINFO("Openni2 = true"); + if(!CameraOpenNI2::available()) + { + UERROR("Openni2 is not available. Please select another driver."); + return -1; + } + } + else if(freenect) + { + UINFO("Freenect = true"); + if(!CameraFreenect::available()) + { + UERROR("Freenect is not available. Please select another driver."); + return -1; + } + } UINFO("Rate =%f Hz", rate); app = new QApplication(argc, argv); @@ -154,6 +187,10 @@ int main (int argc, char * argv[]) { cam = new rtabmap::CameraThread(new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0))); } + else if(freenect) + { + cam = new rtabmap::CameraThread(new rtabmap::CameraFreenect(0, rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0))); + } else { cam = new rtabmap::CameraThread(new rtabmap::CameraVideo(0, rate)); diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 7d3bf18603..984ca6cb60 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -73,7 +73,7 @@ void showUsage() " -d # ICP decimation (default 4)\n" " -v # ICP voxel size (default 0.005)\n" " -s # ICP samples (default 0, not used if voxel is set.)\n" - " -f #.# ICP fitness (default 0.01)\n" + " -cr #.# ICP correspondence ratio (default 0.7)\n" " -p2p ICP point to point (default point to plane)" "\n" " -debug Log debug messages\n" @@ -114,7 +114,7 @@ int main (int argc, char * argv[]) int decimation = 4; float voxel = 0.005; int samples = 10000; - float fitness = 0.01f; + float ratio = 0.7f; int maxClouds = 10; int briefBytes = 32; int fastThr = 30; @@ -466,13 +466,13 @@ int main (int argc, char * argv[]) } continue; } - if(strcmp(argv[i], "-f") == 0) + if(strcmp(argv[i], "-cr") == 0) { ++i; if(i < argc) { - fitness = std::atof(argv[i]); - if(fitness < 0.0f) + ratio = std::atof(argv[i]); + if(ratio < 0.0f) { showUsage(); } @@ -494,7 +494,7 @@ int main (int argc, char * argv[]) if(i < argc) { localHistory = std::atoi(argv[i]); - if(fitness <= 0) + if(localHistory < 0) { showUsage(); } @@ -735,10 +735,10 @@ int main (int argc, char * argv[]) UINFO("Cloud decimation = %d", decimation); UINFO("Cloud voxel size = %f", voxel); UINFO("Cloud samples = %d", samples); - UINFO("Cloud fitness = %f", fitness); + UINFO("Cloud correspondence ratio = %f", ratio); UINFO("Cloud point to plane = %s", p2p?"false":"true"); - odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness, !p2p); + odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p); } rtabmap::OdometryThread odomThread(odom); rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50); diff --git a/utilite/include/rtabmap/utilite/UMath.h b/utilite/include/rtabmap/utilite/UMath.h index 54bc476233..3647717b12 100644 --- a/utilite/include/rtabmap/utilite/UMath.h +++ b/utilite/include/rtabmap/utilite/UMath.h @@ -460,15 +460,15 @@ inline T uMeanSquaredError(const std::vector & x, const std::vector & y) } /** - * Compute the standard deviation of an array. + * Compute the variance of an array. * @param v the array * @param size the size of the array * @param meanV the mean of the array - * @return the std dev + * @return the variance * @see mean() */ template -inline T uStdDev(const T * v, unsigned int size, T meanV) +inline T uVariance(const T * v, unsigned int size, T meanV) { T buf = 0; if(v && size>1) @@ -478,20 +478,20 @@ inline T uStdDev(const T * v, unsigned int size, T meanV) { sum += (v[i]-meanV)*(v[i]-meanV); } - buf = sqrt(sum/(size-1)); + buf = sum/(size-1); } return buf; } /** - * Get the standard deviation of a list. Provided for convenience. + * Get the variance of a list. Provided for convenience. * @param list the list * @param m the mean of the list - * @return the std dev + * @return the variance * @see mean() */ template -inline T uStdDev(const std::list & list, const T & m) +inline T uVariance(const std::list & list, const T & m) { T buf = 0; if(list.size()>1) @@ -501,35 +501,35 @@ inline T uStdDev(const std::list & list, const T & m) { sum += (*i-m)*(*i-m); } - buf = sqrt(sum/(list.size()-1)); + buf = sum/(list.size()-1); } return buf; } /** - * Compute the standard deviation of an array. + * Compute the variance of an array. * @param v the array * @param size the size of the array - * @return the std dev + * @return the variance */ template -inline T uStdDev(const T * v, unsigned int size) +inline T uVariance(const T * v, unsigned int size) { T m = uMean(v, size); - return uStdDev(v, size, m); + return uVariance(v, size, m); } /** - * Get the standard deviation of a vector. Provided for convenience. + * Get the variance of a vector. Provided for convenience. * @param v the vector * @param m the mean of the vector - * @return the std dev + * @return the variance * @see mean() */ template -inline T uStdDev(const std::vector & v, const T & m) +inline T uVariance(const std::vector & v, const T & m) { - return uStdDev(v.data(), v.size(), m); + return uVariance(v.data(), v.size(), m); } /**