From c6d0d47b1c8a82ae97dc3c384e45c99d3ab72527 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Fri, 29 May 2015 14:46:48 -0400 Subject: [PATCH 01/45] Added multi-camera feature --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/CameraModel.h | 56 +- corelib/include/rtabmap/core/DBDriver.h | 7 +- corelib/include/rtabmap/core/DBReader.h | 4 +- corelib/include/rtabmap/core/Link.h | 71 +- corelib/include/rtabmap/core/Memory.h | 17 +- corelib/include/rtabmap/core/OdometryEvent.h | 52 +- corelib/include/rtabmap/core/Rtabmap.h | 16 +- corelib/include/rtabmap/core/RtabmapEvent.h | 20 +- corelib/include/rtabmap/core/RtabmapThread.h | 11 +- corelib/include/rtabmap/core/SensorData.h | 203 ++++-- corelib/include/rtabmap/core/Signature.h | 61 +- corelib/include/rtabmap/core/util3d.h | 14 + .../include/rtabmap/core/util3d_features.h | 30 +- corelib/src/CameraModel.cpp | 66 +- corelib/src/CameraThread.cpp | 17 +- corelib/src/DBDriver.cpp | 48 +- corelib/src/DBDriverSqlite3.cpp | 632 ++++++++++++----- corelib/src/DBDriverSqlite3.h | 29 +- corelib/src/DBReader.cpp | 81 +-- corelib/src/Graph.cpp | 102 ++- corelib/src/Memory.cpp | 657 +++++++++--------- corelib/src/Odometry.cpp | 15 +- corelib/src/OdometryBOW.cpp | 18 +- corelib/src/OdometryICP.cpp | 23 +- corelib/src/OdometryMono.cpp | 71 +- corelib/src/OdometryOpticalFlow.cpp | 130 ++-- corelib/src/OdometryThread.cpp | 9 +- corelib/src/Rtabmap.cpp | 163 ++--- corelib/src/RtabmapThread.cpp | 68 +- corelib/src/SensorData.cpp | 503 +++++++++++--- corelib/src/Signature.cpp | 152 +--- corelib/src/resources/DatabaseSchema.sql.in | 23 +- corelib/src/util3d.cpp | 241 +++++++ corelib/src/util3d_features.cpp | 96 +-- examples/RGBDMapping/MapBuilder.h | 74 +- guilib/include/rtabmap/gui/DataRecorder.h | 2 +- guilib/include/rtabmap/gui/DatabaseViewer.h | 4 +- guilib/include/rtabmap/gui/MainWindow.h | 19 +- guilib/include/rtabmap/gui/OdometryViewer.h | 5 +- guilib/src/CalibrationDialog.cpp | 4 +- guilib/src/CameraViewer.cpp | 16 +- guilib/src/CloudViewer.cpp | 1 + guilib/src/DataRecorder.cpp | 12 +- guilib/src/DatabaseViewer.cpp | 562 ++++++--------- guilib/src/LoopClosureViewer.cpp | 71 +- guilib/src/MainWindow.cpp | 492 ++++++------- guilib/src/OdometryViewer.cpp | 130 ++-- guilib/src/PdfPlot.cpp | 4 +- tools/Camera/main.cpp | 4 +- utilite/include/rtabmap/utilite/UMath.h | 22 + 51 files changed, 2833 insertions(+), 2297 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 11158b8d..7c8126a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") # VERSION ####################### SET(RTABMAP_MAJOR_VERSION 0) -SET(RTABMAP_MINOR_VERSION 9) +SET(RTABMAP_MINOR_VERSION 10) SET(RTABMAP_PATCH_VERSION 0) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 5687d8d1..702d6865 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -43,16 +43,29 @@ public: // D is the distortion coefficients 1x5 CV_64FC1 // R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity) // P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]']) - CameraModel(const std::string & name, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P); + CameraModel( + const std::string & name, + const cv::Size & imageSize, + const cv::Mat & K, + const cv::Mat & D, + const cv::Mat & R, + const cv::Mat & P, + const Transform & localTransform = Transform::getIdentity()); + + // minimal + CameraModel( + double fx, + double fy, + double cx, + double cy, + const Transform & localTransform = Transform::getIdentity(), + double Tx = 0.0f); virtual ~CameraModel() {} bool isValid() const {return !K_.empty() && !D_.empty() && !R_.empty() && - !P_.empty() && - imageSize_.height && - imageSize_.width && - !name_.empty();} + !P_.empty();} const std::string & name() const {return name_;} @@ -67,6 +80,8 @@ public: const cv::Mat & R() const {return R_;} //rectification matrix const cv::Mat & P() const {return P_;} //projection matrix + const Transform & localTransform() const {return localTransform_;} + const cv::Size & imageSize() const {return imageSize_;} int imageWidth() const {return imageSize_.width;} int imageWeight() const {return imageSize_.height;} @@ -74,6 +89,8 @@ public: bool load(const std::string & filePath); bool save(const std::string & filePath); + void scale(double scale); + // For depth images, your should use cv::INTER_NEAREST cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const; cv::Mat rectifyDepth(const cv::Mat & raw) const; @@ -87,20 +104,23 @@ private: cv::Mat P_; cv::Mat mapX_; cv::Mat mapY_; + Transform localTransform_; }; class RTABMAP_EXP StereoCameraModel { public: StereoCameraModel() {} - StereoCameraModel(const std::string & name, + StereoCameraModel( + const std::string & name, const cv::Size & imageSize1, const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1, const cv::Size & imageSize2, const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2, - const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) : - left_(name+"_left", imageSize1, K1, D1, R1, P1), - right_(name+"_right", imageSize2, K2, D2, R2, P2), + const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F, + const Transform & localTransform = Transform::getIdentity()) : + left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform), + right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform), name_(name), R_(R), T_(T), @@ -108,9 +128,21 @@ public: F_(F) { } + //minimal + StereoCameraModel( + double fx, + double fy, + double cx, + double cy, + double baseline, + const Transform & localTransform = Transform::getIdentity()) : + left_(fx, fy, cx, cy, localTransform), + right_(fx, fy, cx, cy, localTransform, baseline*-right_.fx()) + { + } virtual ~StereoCameraModel() {} - bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();} + bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;} const std::string & name() const {return name_;} bool load(const std::string & directory, const std::string & cameraName); @@ -123,7 +155,9 @@ public: const cv::Mat & E() const {return E_;} //extrinsic essential matrix const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix - Transform transform() const; + void scale(double scale); + + Transform stereoTransform() const; const CameraModel & left() const {return left_;} const CameraModel & right() const {return right_;} diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index ead51805..8d18dbb6 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UMutex.h" #include "rtabmap/utilite/UThreadNode.h" #include "rtabmap/core/Parameters.h" +#include "rtabmap/core/SensorData.h" #include #include @@ -95,8 +96,7 @@ public: // Specific queries... void loadNodeData(std::list & signatures, bool loadMetricData) const; - void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const; - void getNodeData(int signatureId, cv::Mat & imageCompressed) const; + void getNodeData(int signatureId, SensorData & data) const; bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const; void loadLinks(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; void getWeight(int signatureId, int & weight) const; @@ -134,8 +134,7 @@ private: 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 & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0; - virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0; + virtual void getNodeDataQuery(int signatureId, SensorData & data) const = 0; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const = 0; virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0; diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index 4273e407..1be224a0 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include @@ -59,7 +59,7 @@ public: bool init(int startIndex=0); void setFrameRate(float frameRate); - SensorData getNextData(); + OdometryEvent getNextData(); protected: virtual void mainLoopBegin(); diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 7810cb55..358702ed 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -42,19 +43,33 @@ public: from_(0), to_(0), type_(kUndef), - rotVariance_(1.0f), - transVariance_(1.0f) + infMatrix_(cv::Mat::eye(6,6,CV_64FC1)) { } - Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) : + Link(int from, + int to, + Type type, + const Transform & transform, + const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) : from_(from), to_(to), transform_(transform), - type_(type), - rotVariance_(rotVariance), - transVariance_(transVariance) + type_(type) { - UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); + setInfMatrix(infMatrix); + } + Link(int from, + int to, + Type type, + const Transform & transform, + double rotVariance, + double transVariance) : + from_(from), + to_(to), + transform_(transform), + type_(type) + { + setVariance(rotVariance, transVariance); } bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;} @@ -63,17 +78,44 @@ public: int to() const {return to_;} const Transform & transform() const {return transform_;} Type type() const {return type_;} - float rotVariance() const {return rotVariance_;} - float transVariance() const {return transVariance_;} + const cv::Mat & infMatrix() const {return infMatrix_;} + double rotVariance() const + { + double min = uMin3(infMatrix_.at(3,3), infMatrix_.at(4,4), infMatrix_.at(5,5)); + UASSERT(min > 0.0); + return 1.0/min; + } + double transVariance() const + { + double min = uMin3(infMatrix_.at(0,0), infMatrix_.at(1,1), infMatrix_.at(2,2)); + UASSERT(min > 0.0); + return 1.0/min; + } 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 rotVariance, float transVariance) { - UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); - rotVariance_ = rotVariance; - transVariance_ = transVariance; + void setInfMatrix(const cv::Mat & infMatrix) { + UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1); + UASSERT_MSG(uIsFinite(infMatrix.at(0,0)) && infMatrix.at(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(1,1)) && infMatrix.at(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(2,2)) && infMatrix.at(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(3,3)) && infMatrix.at(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(4,4)) && infMatrix.at(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(5,5)) && infMatrix.at(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)"); + infMatrix_ = infMatrix; + } + void setVariance(double rotVariance, double transVariance) { + UASSERT(uIsFinite(rotVariance) && rotVariance>0); + UASSERT(uIsFinite(transVariance) && transVariance>0); + infMatrix_ = cv::Mat::eye(6,6,CV_64FC1); + infMatrix_.at(0,0) = 1.0/transVariance; + infMatrix_.at(1,1) = 1.0/transVariance; + infMatrix_.at(2,2) = 1.0/transVariance; + infMatrix_.at(3,3) = 1.0/rotVariance; + infMatrix_.at(4,4) = 1.0/rotVariance; + infMatrix_.at(5,5) = 1.0/rotVariance; } private: @@ -81,8 +123,7 @@ private: int to_; Transform transform_; Type type_; - float rotVariance_; - float transVariance_; + cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1 }; } diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index fedb271f..de50fde1 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -65,7 +65,12 @@ public: virtual ~Memory(); virtual void parseParameters(const ParametersMap & parameters); - bool update(const SensorData & data, Statistics * stats = 0); + bool update(const SensorData & data, + Statistics * stats = 0); + bool update(const SensorData & data, + const Transform & pose, + const cv::Mat & covariance, + Statistics * stats = 0); bool init(const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap(), @@ -81,8 +86,9 @@ public: std::list cleanup(const std::list & ignoredIds = std::list()); void emptyTrash(); void joinTrashThread(); - bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance); + bool addLink(const Link & link); void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance); + void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance); void removeAllVirtualLinks(); void removeVirtualLinks(int signatureId); std::map getNeighborsId( @@ -130,8 +136,8 @@ public: std::vector & userData, bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; - Signature getSignatureData(int locationId, bool uncompressedData = false); - Signature getSignatureDataConst(int locationId) const; + SensorData getNodeData(int nodeId, bool uncompressedData = false); + SensorData getSignatureDataConst(int locationId) const; std::set getAllSignatureIds() const; bool memoryChanged() const {return _memoryChanged;} bool isIncremental() const {return _incrementalMemory;} @@ -184,7 +190,7 @@ public: private: void preUpdate(); - void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance); + void addSignatureToStm(Signature * signature, const cv::Mat & covariance); void clear(); void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list * deletedWords = 0); @@ -202,6 +208,7 @@ private: void copyData(const Signature * from, Signature * to); Signature * createSignature( const SensorData & data, + const Transform & pose, Statistics * stats = 0); //keypoint stuff diff --git a/corelib/include/rtabmap/core/OdometryEvent.h b/corelib/include/rtabmap/core/OdometryEvent.h index 68ae5cda..0fa8b2d9 100644 --- a/corelib/include/rtabmap/core/OdometryEvent.h +++ b/corelib/include/rtabmap/core/OdometryEvent.h @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define ODOMETRYEVENT_H_ #include "rtabmap/utilite/UEvent.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMath.h" #include "rtabmap/core/SensorData.h" #include "rtabmap/core/OdometryInfo.h" @@ -37,20 +39,64 @@ namespace rtabmap { class OdometryEvent : public UEvent { public: + OdometryEvent() : + _covariance(cv::Mat::eye(6,6,CV_64FC1)) + { + } OdometryEvent( - const SensorData & data, const OdometryInfo & info = OdometryInfo()) : + const SensorData & data, + const Transform & pose, + const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1), + const OdometryInfo & info = OdometryInfo()) : _data(data), + _pose(pose), _info(info) - {} + { + UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1); + UASSERT_MSG(uIsFinite(covariance.at(0,0)) && covariance.at(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(covariance.at(1,1)) && covariance.at(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(covariance.at(2,2)) && covariance.at(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(covariance.at(3,3)) && covariance.at(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(covariance.at(4,4)) && covariance.at(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(covariance.at(5,5)) && covariance.at(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + _covariance = covariance; + } + OdometryEvent( + const SensorData & data, + const Transform & pose, + double rotVariance = 1.0, + double transVariance = 1.0, + const OdometryInfo & info = OdometryInfo()) : + _data(data), + _pose(pose), + _covariance(cv::Mat::eye(6,6,CV_64FC1)), + _info(info) + { + UASSERT(uIsFinite(rotVariance) && rotVariance>0); + UASSERT(uIsFinite(transVariance) && transVariance>0); + _covariance.at(0,0) = transVariance; + _covariance.at(1,1) = transVariance; + _covariance.at(2,2) = transVariance; + _covariance.at(3,3) = rotVariance; + _covariance.at(4,4) = rotVariance; + _covariance.at(5,5) = rotVariance; + } virtual ~OdometryEvent() {} virtual std::string getClassName() const {return "OdometryEvent";} - bool isValid() const {return !_data.pose().isNull();} + bool isValid() const {return !_pose.isNull();} + SensorData & data() {return _data;} const SensorData & data() const {return _data;} + const Transform & pose() const {return _pose;} + const cv::Mat & covariance() const {return _covariance;} const OdometryInfo & info() const {return _info;} + double rotVariance() const {return uMax3(_covariance.at(3,3), _covariance.at(4,4), _covariance.at(5,5));} + double transVariance() const {return uMax3(_covariance.at(0,0), _covariance.at(1,1), _covariance.at(2,2));} private: SensorData _data; + Transform _pose; + cv::Mat _covariance; OdometryInfo _info; }; diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 23c6838c..bdc6641d 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -66,7 +66,10 @@ public: virtual ~Rtabmap(); bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0 - bool process(const SensorData & data); // for convenience + bool process( + const SensorData & data, + const Transform & odomPose, + const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience void init(const ParametersMap & parameters, const std::string & databasePath = ""); void init(const std::string & configFile = "", const std::string & databasePath = ""); @@ -115,20 +118,13 @@ public: void get3DMap(std::map & signatures, std::map & poses, std::multimap & constraints, - std::map & mapIds, - std::map & stamps, - std::map & labels, - std::map > & userDatas, bool optimized, bool global) const; void getGraph(std::map & poses, std::multimap & constraints, - std::map & mapIds, - std::map & stamps, - std::map & labels, - std::map > & userDatas, bool optimized, - bool global); + bool global, + std::map * signatures = 0); void clearPath(); bool computePath(int targetNode, bool global); bool computePath(const Transform & targetPose, bool global); diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index 980874d4..d7ff5d5b 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -148,19 +148,11 @@ public: RtabmapEvent3DMap( const std::map & signatures, const std::map & poses, - const std::multimap & constraints, - const std::map & mapIds, - const std::map & stamps, - const std::map & labels, - const std::map > & userDatas) : + const std::multimap & constraints) : UEvent(0), _signatures(signatures), _poses(poses), - _constraints(constraints), - _mapIds(mapIds), - _stamps(stamps), - _labels(labels), - _userDatas(userDatas) + _constraints(constraints) {} virtual ~RtabmapEvent3DMap() {} @@ -168,10 +160,6 @@ public: const std::map & getSignatures() const {return _signatures;} const std::map & getPoses() const {return _poses;} const std::multimap & getConstraints() const {return _constraints;} - const std::map & getMapIds() const {return _mapIds;} - const std::map & getStamps() const {return _stamps;} - const std::map & getLabels() const {return _labels;} - const std::map > & getUserDatas() const {return _userDatas;} virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");} @@ -179,10 +167,6 @@ private: std::map _signatures; std::map _poses; std::multimap _constraints; - std::map _mapIds; - std::map _stamps; - std::map _labels; - std::map > _userDatas; }; class RtabmapGlobalPathEvent : public UEvent diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index 961464fc..478caaf4 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/SensorData.h" #include "rtabmap/core/Parameters.h" +#include "rtabmap/core/OdometryEvent.h" #include @@ -90,8 +91,8 @@ private: virtual void mainLoop(); virtual void mainLoopKill(); void process(); - void addData(const SensorData & data); - void getData(SensorData & data); + void addData(const OdometryEvent & odomEvent); + void getData(OdometryEvent & data); void pushNewState(State newState, const ParametersMap & parameters = ParametersMap()); void setDataBufferSize(int size); void publishMap(bool optimized, bool full) const; @@ -102,7 +103,7 @@ private: std::stack _state; std::stack _stateParam; - std::list _dataBuffer; + std::list _dataBuffer; UMutex _dataMutex; USemaphore _dataAdded; int _dataBufferMaxSize; @@ -112,8 +113,8 @@ private: Rtabmap * _rtabmap; bool _paused; Transform lastPose_; - float _rotVariance; - float _transVariance; + double _rotVariance; + double _transVariance; std::vector _userData; UMutex _userDataMutex; diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index eea96b47..078c577e 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include #include #include @@ -42,71 +44,133 @@ namespace rtabmap class RTABMAP_EXP SensorData { public: - SensorData(); // empty constructor - SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector & userData = std::vector()); + // empty constructor + SensorData(); - // Metric constructor - SensorData(const cv::Mat & image, - const cv::Mat & depthOrRightImage, - float fx, - float fyOrBaseline, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float poseRotVariance, - float poseTransVariance, - int id, - double stamp, - const std::vector & userData = std::vector()); + // Appearance-only constructor + SensorData( + const cv::Mat & image, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); - // Metric constructor + 2d laser scan - SensorData(const cv::Mat & laserScan, + // Mono constructor + SensorData( + const cv::Mat & image, + const CameraModel & cameraModel, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // RGB-D constructor + SensorData( + const cv::Mat & rgb, + const cv::Mat & depth, + const CameraModel & cameraModel, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // RGB-D constructor + 2d laser scan + SensorData( + const cv::Mat & laserScan, int laserScanMaxPts, - const cv::Mat & image, - const cv::Mat & depthOrRightImage, - float fx, - float fyOrBaseline, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float poseRotVariance, - float poseTransVariance, - int id, - double stamp, - const std::vector & userData = std::vector()); + const cv::Mat & rgb, + const cv::Mat & depth, + const CameraModel & cameraModel, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // Multi-cameras RGB-D constructor + SensorData( + const cv::Mat & rgb, + const cv::Mat & depth, + const std::vector & cameraModels, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // Multi-cameras RGB-D constructor + 2d laser scan + SensorData( + const cv::Mat & laserScan, + int laserScanMaxPts, + const cv::Mat & rgb, + const cv::Mat & depth, + const std::vector & cameraModels, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // Stereo constructor + SensorData( + const cv::Mat & left, + const cv::Mat & right, + const StereoCameraModel & cameraModel, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); + + // Stereo constructor + 2d laser scan + SensorData( + const cv::Mat & laserScan, + int laserScanMaxPts, + const cv::Mat & left, + const cv::Mat & right, + const StereoCameraModel & cameraModel, + int id = 0, + double stamp = 0.0, + const std::vector & userData = std::vector()); virtual ~SensorData() {} - bool isValid() const {return !_image.empty();} + bool isValid() const { + return !(_id == 0 && + _stamp == 0.0 && + _laserScanMaxPts == 0 && + _imageRaw.empty() && + _imageCompressed.empty() && + _depthOrRightRaw.empty() && + _depthOrRightCompressed.empty() && + _laserScanRaw.empty() && + _laserScanCompressed.empty() && + _cameraModels.size() == 0 && + !_stereoCameraModel.isValid() && + _userData.size() == 0 && + _keypoints.size() == 0 && + _descriptors.empty()); + } - // use isValid() instead - RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead."); - - const cv::Mat & image() const {return _image;} int id() const {return _id;} void setId(int id) {_id = id;} double stamp() const {return _stamp;} void setStamp(double stamp) {_stamp = stamp;} - - bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();} - void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;} - 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 & laserScan() const {return _laserScan;} int laserScanMaxPts() const {return _laserScanMaxPts;} - float fx() const {return _fx;} - float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;} - float cx() const {return _cx;} - float cy() const {return _cy;} - float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;} - float fyOrBaseline() const {return _fyOrBaseline;} - const Transform & pose() const {return _pose;} - const Transform & localTransform() const {return _localTransform;} - float poseRotVariance() const {return _poseRotVariance;} - float poseTransVariance() const {return _poseTransVariance;} + + const cv::Mat & imageCompressed() const {return _imageCompressed;} + const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;} + const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;} + + const cv::Mat & imageRaw() const {return _imageRaw;} + const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;} + const cv::Mat & laserScanRaw() const {return _laserScanRaw;} + void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;} + void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;} + void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;} + + //for convenience + cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();} + cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();} + + void uncompressData(); + void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw); + void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const; + + const std::vector & cameraModels() const {return _cameraModels;} + StereoCameraModel stereoCameraModel() const {return _stereoCameraModel;} + + void setUserData(const std::vector & data) {_userData = data;} + const std::vector & userData() const {return _userData;} void setFeatures(const std::vector & keypoints, const cv::Mat & descriptors) { @@ -116,33 +180,28 @@ public: const std::vector & keypoints() const {return _keypoints;} const cv::Mat & descriptors() const {return _descriptors;} - void setUserData(const std::vector & data) {_userData = data;} - const std::vector & userData() const {return _userData;} - private: - cv::Mat _image; int _id; double _stamp; - - // Metric stuff - cv::Mat _depthOrRightImage; - cv::Mat _laserScan; - float _fx; - float _fyOrBaseline; - float _cx; - float _cy; - Transform _pose; - Transform _localTransform; - float _poseRotVariance; - float _poseTransVariance; int _laserScanMaxPts; + cv::Mat _imageCompressed; // compressed image + cv::Mat _depthOrRightCompressed; // compressed image + cv::Mat _laserScanCompressed; // compressed data + + cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3 + cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 + cv::Mat _laserScanRaw; // CV_32FC2 + + std::vector _cameraModels; + StereoCameraModel _stereoCameraModel; + + // user data + std::vector _userData; + // features std::vector _keypoints; cv::Mat _descriptors; - - // user data - std::vector _userData; }; } diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index a155d62a..37da953a 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -61,15 +61,7 @@ public: const std::multimap & words3, const Transform & pose = Transform(), const std::vector & userData = std::vector(), - const cv::Mat & laserScan = cv::Mat(), - const cv::Mat & image = cv::Mat(), - const cv::Mat & depth = cv::Mat(), - float fx = 0.0f, - float fy = 0.0f, - float cx = 0.0f, - float cy = 0.0f, - const Transform & localTransform =Transform::getIdentity(), - int laserScanMaxPts = 0); + const SensorData & sensorData = SensorData()); virtual ~Signature(); /** @@ -121,41 +113,17 @@ public: void setEnabled(bool enabled) {_enabled = enabled;} const std::multimap & getWords() const {return _words;} const std::map & getWordsChanged() const {return _wordsChanged;} - void setImageCompressed(const cv::Mat & bytes) {_imageCompressed = bytes;} - const cv::Mat & getImageCompressed() const {return _imageCompressed;} - void setImageRaw(const cv::Mat & image) {_imageRaw = image;} - const cv::Mat & getImageRaw() const {return _imageRaw;} //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 setLaserScanCompressed(const cv::Mat & bytes, int maxPts) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;} - 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 & getLaserScanCompressed() const {return _laserScanCompressed;} - RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead."); - RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead."); - RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead."); - RTABMAP_DEPRECATED(float getDepthCy() const, "Use getCy() instead."); - float getFx() const {return _fx;} - float getFy() const {return _fy;} - float getCx() const {return _cx;} - float getCy() const {return _cy;} - const Transform & getPose() const {return _pose;} - void getPoseVariance(float & rotVariance, float & transVariance) const; - const Transform & getLocalTransform() const {return _localTransform;} - void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;} - const cv::Mat & getDepthRaw() const {return _depthRaw;} - void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;} - const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;} - int getLaserScanMaxPts() const {return _laserScanMaxPts;} - SensorData toSensorData(); - void uncompressData(); - void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw); - void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const; + const std::multimap & getWords3() const {return _words3;} + const Transform & getPose() const {return _pose;} + cv::Mat getPoseCovariance() const; + + SensorData & sensorData() {return _sensorData;} + const SensorData & sensorData() const {return _sensorData;} private: int _id; @@ -173,24 +141,13 @@ private: // times in the signature, it will be 2 times in this list) // Words match with the CvSeq keypoints and descriptors std::multimap _words; // word + std::multimap _words3; // word std::map _wordsChanged; // bool _enabled; - cv::Mat _imageCompressed; // compressed image - cv::Mat _depthCompressed; // compressed image - cv::Mat _laserScanCompressed; // compressed data - float _fx; - float _fy; - float _cx; - float _cy; Transform _pose; - Transform _localTransform; // camera_link -> base_link - std::multimap _words3; // word - int _laserScanMaxPts; - cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3 - cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 - cv::Mat _laserScanRaw; // CV_32FC2 + SensorData _sensorData; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5164b158..951532cc 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -103,6 +104,19 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromStereoImages( float fx, float baseline, int decimation = 1); +pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( + const SensorData & sensorData, + int decimation = 1, + float maxDepth = 0.0f, + float voxelSize = 0.0f, + int samples = 0); +pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( + const SensorData & sensorData, + int decimation = 1, + float maxDepth = 0.0f, + float voxelSize = 0.0f, + int samples = 0); + pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D( const cv::Point2f & pt, float disparity, diff --git a/corelib/include/rtabmap/core/util3d_features.h b/corelib/include/rtabmap/core/util3d_features.h index 6670f321..89d9f787 100644 --- a/corelib/include/rtabmap/core/util3d_features.h +++ b/corelib/include/rtabmap/core/util3d_features.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap @@ -46,30 +47,23 @@ namespace util3d pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DDepth( const std::vector & keypoints, const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - const Transform & transform); + const CameraModel & cameraModel); + +pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DDepth( + const std::vector & keypoints, + const cv::Mat & depth, + const std::vector & cameraModels); pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DDisparity( const std::vector & keypoints, const cv::Mat & disparity, - float fx, - float baseline, - float cx, - float cy, - const Transform & transform); + const StereoCameraModel & stereoCameraMode); pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( const std::vector & keypoints, const cv::Mat & leftImage, const cv::Mat & rightImage, - float fx, - float baseline, - float cx, - float cy, - const Transform & transform = Transform::getIdentity(), + const StereoCameraModel & stereoCameraMode, int flowWinSize = 9, int flowMaxLevel = 4, int flowIterations = 20, @@ -78,11 +72,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( std::multimap RTABMAP_EXP generateWords3DMono( const std::multimap & kpts, const std::multimap & previousKpts, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, + const CameraModel & cameraModel, Transform & cameraTransform, int pnpIterations = 100, float pnpReprojError = 8.0f, diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 401f5b40..71eb939a 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap { @@ -39,13 +40,21 @@ CameraModel::CameraModel() : } -CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P) : +CameraModel::CameraModel( + const std::string & cameraName, + const cv::Size & imageSize, + const cv::Mat & K, + const cv::Mat & D, + const cv::Mat & R, + const cv::Mat & P, + const Transform & localTransform) : name_(cameraName), imageSize_(imageSize), K_(K), D_(D), R_(R), - P_(P) + P_(P), + localTransform_(localTransform) { UASSERT(!name_.empty()); UASSERT(imageSize_.width > 0 && imageSize_.height > 0); @@ -59,6 +68,35 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_); } +CameraModel::CameraModel( + double fx, + double fy, + double cx, + double cy, + const Transform & localTransform, + double Tx) : + K_(cv::Mat::eye(3, 3, CV_64FC1)), + D_(cv::Mat::zeros(1, 5, CV_64FC1)), + R_(cv::Mat::eye(3, 3, CV_64FC1)), + P_(cv::Mat::eye(3, 4, CV_64FC1)), + localTransform_(localTransform) +{ + UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str()); + UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str()); + UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str()); + UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str()); + P_.at(0,0) = fx; + P_.at(1,1) = fy; + P_.at(0,2) = cx; + P_.at(1,2) = cy; + P_.at(0,3) = Tx; + + K_.at(0,0) = fx; + K_.at(1,1) = fy; + K_.at(0,2) = cx; + K_.at(1,2) = cy; +} + bool CameraModel::load(const std::string & filePath) { K_ = cv::Mat(); @@ -172,6 +210,22 @@ bool CameraModel::save(const std::string & filePath) return false; } +void CameraModel::scale(double scale) +{ + UASSERT(scale > 0.0); + // has only effect on K and P + imageSize_.width *= scale; + imageSize_.height *= scale; + K_.at(0,0) *= scale; + K_.at(1,1) *= scale; + K_.at(0,2) *= scale; + K_.at(1,2) *= scale; + P_.at(0,0) *= scale; + P_.at(1,1) *= scale; + P_.at(0,2) *= scale; + P_.at(1,2) *= scale; +} + cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const { if(!mapX_.empty() && !mapY_.empty()) @@ -348,7 +402,13 @@ bool StereoCameraModel::save(const std::string & directory, const std::string & return false; } -Transform StereoCameraModel::transform() const +void StereoCameraModel::scale(double scale) +{ + left_.scale(scale); + right_.scale(scale); +} + +Transform StereoCameraModel::stereoTransform() const { if(!R_.empty() && !T_.empty()) { diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 6d0dbb4a..0ef90777 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -109,12 +109,12 @@ void CameraThread::mainLoop() UDEBUG(""); cv::Mat rgb, depth; float fx = 0.0f; - float fy = 0.0f; + float fyOrBaseline = 0.0f; float cx = 0.0f; float cy = 0.0f; if(_cameraRGBD) { - _cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy); + _cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy); } else { @@ -125,7 +125,18 @@ void CameraThread::mainLoop() { if(_cameraRGBD) { - SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now()); + SensorData data; + if(dynamic_cast(_cameraRGBD) || dynamic_cast(_cameraRGBD)) + { + //stereo + data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, UTimer::now()); + UASSERT(data.stereoCameraModel().isValid()); + } + else + { + data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, UTimer::now()); + UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()); + } this->post(new CameraEvent(data, _cameraRGBD->getSerial())); } else diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 707aadba..6e9ecf5c 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -412,15 +412,7 @@ void DBDriver::loadNodeData(std::list & signatures, bool loadMetric void DBDriver::getNodeData( int signatureId, - cv::Mat & imageCompressed, - cv::Mat & depthCompressed, - cv::Mat & laserScanCompressed, - float & fx, - float & fy, - float & cx, - float & cy, - Transform & localTransform, - int & laserScanMaxPts) const + SensorData & data) const { bool found = false; // look in the trash @@ -428,17 +420,9 @@ void DBDriver::getNodeData( if(uContains(_trashSignatures, signatureId)) { const Signature * s = _trashSignatures.at(signatureId); - if(!s->getImageCompressed().empty() || !s->isSaved()) + if(!s->sensorData().imageCompressed().empty() || !s->isSaved()) { - imageCompressed = s->getImageCompressed(); - depthCompressed = s->getDepthCompressed(); - laserScanCompressed = s->getLaserScanCompressed(); - fx = s->getFx(); - fy = s->getFy(); - cx = s->getCx(); - cy = s->getCy(); - localTransform = s->getLocalTransform(); - laserScanMaxPts = s->getLaserScanMaxPts(); + data = (SensorData)s->sensorData(); found = true; } } @@ -447,31 +431,7 @@ void DBDriver::getNodeData( if(!found) { _dbSafeAccessMutex.lock(); - this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts); - _dbSafeAccessMutex.unlock(); - } -} - -void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const -{ - bool found = false; - // look in the trash - _trashesMutex.lock(); - if(uContains(_trashSignatures, signatureId)) - { - const Signature * s = _trashSignatures.at(signatureId); - if(!s->getImageCompressed().empty() || !s->isSaved()) - { - imageCompressed = s->getImageCompressed(); - found = true; - } - } - _trashesMutex.unlock(); - - if(!found) - { - _dbSafeAccessMutex.lock(); - this->getNodeDataQuery(signatureId, imageCompressed); + this->getNodeDataQuery(signatureId, data); _dbSafeAccessMutex.unlock(); } } diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 1c42a37b..364fc8d3 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -458,10 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo if(loadMetricData) { - if(uStrNumCmp(_version, "0.8.11") >= 0) + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + query << "SELECT image, depth, calibration, scan_max_pts, scan " + << "FROM Data " + << "WHERE id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.8.11") >= 0) { query << "SELECT Image.data, " - "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -471,7 +478,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo else if(uStrNumCmp(_version, "0.7.0") >= 0) { query << "SELECT Image.data, " - "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -481,7 +488,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo else { query << "SELECT Image.data, " - "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -491,10 +498,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo } else { - query << "SELECT data " - << "FROM Image " - << "WHERE id = ?" - <<";"; + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + query << "SELECT image " + << "FROM Data " + << "WHERE id = ?" + <<";"; + } + else + { + query << "SELECT data " + << "FROM Image " + << "WHERE id = ?" + <<";"; + } } rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); @@ -519,13 +536,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo { index = 0; + cv::Mat imageCompressed; + cv::Mat depthOrRightCompressed; + std::vector models; + StereoCameraModel stereoModel; + Transform localTransform = Transform::getIdentity(); + cv::Mat scanCompressed; + data = sqlite3_column_blob(ppStmt, index); dataSize = sqlite3_column_bytes(ppStmt, index++); //Create the image if(dataSize>4 && data) { - (*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); + imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); } if(loadMetricData) @@ -534,35 +558,92 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo dataSize = sqlite3_column_bytes(ppStmt, index++); //Create the depth image - cv::Mat depthCompressed; if(dataSize>4 && data) { - depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); + depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); } - if(uStrNumCmp(_version, "0.7.0") < 0) + if(uStrNumCmp(_version, "0.10.0") < 0) { - float depthConstant = sqlite3_column_double(ppStmt, index++); - (*iter)->setDepthCompressed(depthCompressed, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0); + data = sqlite3_column_blob(ppStmt, index); // local transform + dataSize = sqlite3_column_bytes(ppStmt, index++); + if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) + { + memcpy(localTransform.data(), data, dataSize); + } + } + + // calibration + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(dataSize > 0 && data) + { + float * dataFloat = (float*)data; + if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) + { + int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); + UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); + int max = cameraCount*(4+localTransform.size()); + for(int i=0; i= 0) + { + double fx = sqlite3_column_double(ppStmt, index++); + double fyOrBaseline = sqlite3_column_double(ppStmt, index++); + double cx = sqlite3_column_double(ppStmt, index++); + double cy = sqlite3_column_double(ppStmt, index++); + if(fyOrBaseline < 1.0) + { + //it is a baseline + stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); + } + else + { + models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); + } } else { - float fx = sqlite3_column_double(ppStmt, index++); - float fy = sqlite3_column_double(ppStmt, index++); - float cx = sqlite3_column_double(ppStmt, index++); - float cy = sqlite3_column_double(ppStmt, index++); - (*iter)->setDepthCompressed(depthCompressed, fx, fy, cx, cy); + float depthConstant = sqlite3_column_double(ppStmt, index++); + float fx = 1.0f/depthConstant; + float fy = 1.0f/depthConstant; + float cx = 0.0f; + float cy = 0.0f; + models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); } - data = sqlite3_column_blob(ppStmt, index); // local transform - dataSize = sqlite3_column_bytes(ppStmt, index++); - Transform localTransform; - if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) - { - memcpy(localTransform.data(), data, dataSize); - } - (*iter)->setLocalTransform(localTransform); - int laserScanMaxPts = 0; if(uStrNumCmp(_version, "0.8.11") >= 0) { @@ -574,8 +655,30 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo //Create the laserScan if(dataSize>4 && data) { - (*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d + scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d } + + if(models.size()) + { + (*iter)->sensorData() = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + models, + (*iter)->id()); + } + else + { + (*iter)->sensorData() = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + stereoModel, + (*iter)->id()); + } + } rc = sqlite3_step(ppStmt); // next result... @@ -596,15 +699,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo void DBDriverSqlite3::getNodeDataQuery( int signatureId, - cv::Mat & imageCompressed, - cv::Mat & depthCompressed, - cv::Mat & laserScanCompressed, - float & fx, - float & fy, - float & cx, - float & cy, - Transform & localTransform, - int & laserScanMaxPts) const + SensorData & sensorData) const { if(_ppDb) { @@ -614,10 +709,17 @@ void DBDriverSqlite3::getNodeDataQuery( sqlite3_stmt * ppStmt = 0; std::stringstream query; - if(uStrNumCmp(_version, "0.8.11") >= 0) + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + query << "SELECT image, depth, calibration, scan_max_pts, scan " + << "FROM Data " + << "WHERE id = " << signatureId + <<";"; + } + else if(uStrNumCmp(_version, "0.8.11") >= 0) { query << "SELECT Image.data, " - "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -627,7 +729,7 @@ void DBDriverSqlite3::getNodeDataQuery( else if(uStrNumCmp(_version, "0.7.0") >= 0) { query << "SELECT Image.data, " - "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -637,7 +739,7 @@ void DBDriverSqlite3::getNodeDataQuery( else { query << "SELECT Image.data, " - "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d " + "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d " << "FROM Image " << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data << "ON Image.id = Depth.id " @@ -650,7 +752,15 @@ void DBDriverSqlite3::getNodeDataQuery( const void * data = 0; int dataSize = 0; - int index = 0;; + int index = 0; + + cv::Mat imageCompressed; + cv::Mat depthOrRightCompressed; + std::vector models; + StereoCameraModel stereoModel; + Transform localTransform = Transform::getIdentity(); + int laserScanMaxPts; + cv::Mat scanCompressed; ULOGGER_DEBUG("Loading data for %d...", signatureId); @@ -675,30 +785,88 @@ void DBDriverSqlite3::getNodeDataQuery( //Create the depth image if(dataSize>4 && data) { - depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); + depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); } - if(uStrNumCmp(_version, "0.7.0") < 0) + if(uStrNumCmp(_version, "0.10.0") < 0) { - float depthConstant = sqlite3_column_double(ppStmt, index++); - fx = 1.0f/depthConstant; - fy = 1.0f/depthConstant; - cx = 0.0f; - cy = 0.0f; + data = sqlite3_column_blob(ppStmt, index); // local transform + dataSize = sqlite3_column_bytes(ppStmt, index++); + if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) + { + memcpy(localTransform.data(), data, dataSize); + } + } + + // calibration + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(dataSize > 0 && data) + { + float * dataFloat = (float*)data; + if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) + { + int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); + UDEBUG("Loading calibration for %d cameras", cameraCount); + int max = cameraCount*(4+localTransform.size()); + for(int i=0; i= 0) + { + double fx = sqlite3_column_double(ppStmt, index++); + double fyOrBaseline = sqlite3_column_double(ppStmt, index++); + double cx = sqlite3_column_double(ppStmt, index++); + double cy = sqlite3_column_double(ppStmt, index++); + if(fyOrBaseline < 1.0) + { + //it is a baseline + stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); + } + else + { + models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); + } } else { - fx = sqlite3_column_double(ppStmt, index++); - fy = sqlite3_column_double(ppStmt, index++); - cx = sqlite3_column_double(ppStmt, index++); - cy = sqlite3_column_double(ppStmt, index++); - } - - data = sqlite3_column_blob(ppStmt, index); // local transform - dataSize = sqlite3_column_bytes(ppStmt, index++); - if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) - { - memcpy(localTransform.data(), data, dataSize); + float depthConstant = sqlite3_column_double(ppStmt, index++); + float fx = 1.0f/depthConstant; + float fy = 1.0f/depthConstant; + float cx = 0.0f; + float cy = 0.0f; + models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); } laserScanMaxPts = 0; @@ -712,63 +880,28 @@ void DBDriverSqlite3::getNodeDataQuery( //Create the depth2d if(dataSize>4 && data) { - laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); + scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); } - if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0) + if(models.size()) { - UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only."); + sensorData = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + models, + signatureId); } - - rc = sqlite3_step(ppStmt); // next result... - } - 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()); - ULOGGER_DEBUG("Time=%fs", timer.ticks()); - } -} - -void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const -{ - if(_ppDb) - { - UTimer timer; - timer.start(); - int rc = SQLITE_OK; - sqlite3_stmt * ppStmt = 0; - std::stringstream query; - - query << "SELECT data " - << "FROM Image " - << "WHERE id = " << signatureId - <<";"; - - 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()); - - const void * data = 0; - int dataSize = 0; - int index = 0;; - - ULOGGER_DEBUG("Loading data for %d...", signatureId); - - // Process the result if one - rc = sqlite3_step(ppStmt); - if(rc == SQLITE_ROW) - { - index = 0; - - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); - - //Create the image - if(dataSize>4 && data) + else { - imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); + sensorData = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + stereoModel, + signatureId); } rc = sqlite3_step(ppStmt); // next result... @@ -1900,7 +2033,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd const std::map & links = (*j)->getLinks(); for(std::map::const_iterator i=links.begin(); i!=links.end(); ++i) { - stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform()); + stepLink(ppStmt, i->second); } } } @@ -2008,7 +2141,7 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) const const std::map & links = (*jter)->getLinks(); for(std::map::const_iterator i=links.begin(); i!=links.end(); ++i) { - stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform()); + stepLink(ppStmt, i->second); } } // Finalize (delete) the statement @@ -2048,40 +2181,66 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) const UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); UDEBUG("Time=%fs", timer.ticks()); - // Add images - query = queryStepImage(); - rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - UDEBUG("Saving %d images", signatures.size()); - - for(std::list::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + if(uStrNumCmp(_version, "0.10.0") >= 0) { - if(!(*i)->getImageCompressed().empty()) + // Add SensorData + query = queryStepSensorData(); + rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + UDEBUG("Saving %d images", signatures.size()); + + for(std::list::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) { - stepImage(ppStmt, (*i)->id(), (*i)->getImageCompressed()); + if(!(*i)->sensorData().imageCompressed().empty()) + { + UASSERT((*i)->id() == (*i)->sensorData().id()); + stepSensorData(ppStmt, (*i)->sensorData()); + } } + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + UDEBUG("Time=%fs", timer.ticks()); } - - // Finalize (delete) the statement - rc = sqlite3_finalize(ppStmt); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - UDEBUG("Time=%fs", timer.ticks()); - - // Add depths - query = queryStepDepth(); - rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - for(std::list::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + else { - //metric - if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty()) + // Add images + query = queryStepImage(); + rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + UDEBUG("Saving %d images", signatures.size()); + + for(std::list::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) { - stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts()); + if(!(*i)->sensorData().imageCompressed().empty()) + { + stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed()); + } } + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + UDEBUG("Time=%fs", timer.ticks()); + + // Add depths + query = queryStepDepth(); + rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + for(std::list::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + { + //metric + if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty()) + { + UASSERT((*i)->id() == (*i)->sensorData().id()); + stepDepth(ppStmt, (*i)->sensorData()); + } + } + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, 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()); UDEBUG("Time=%fs", timer.ticks()); } @@ -2216,12 +2375,14 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const std::string DBDriverSqlite3::queryStepImage() const { + UASSERT(uStrNumCmp(_version, "0.10.0") < 0); return "INSERT INTO Image(id, data) VALUES(?,?);"; } void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const { + UASSERT(uStrNumCmp(_version, "0.10.0") < 0); UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols); if(!ppStmt) { @@ -2254,6 +2415,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt, std::string DBDriverSqlite3::queryStepDepth() const { + UASSERT(uStrNumCmp(_version, "0.10.0") < 0); if(uStrNumCmp(_version, "0.8.11") >= 0) { return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);"; @@ -2267,18 +2429,13 @@ std::string DBDriverSqlite3::queryStepDepth() const return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);"; } } -void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, - int id, - const cv::Mat & depthBytes, - const cv::Mat & depth2dBytes, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, - int depth2dMaxPts) const +void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const { - UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols); + UASSERT(uStrNumCmp(_version, "0.10.0") < 0); + UDEBUG("Save depth %d (size=%d) depth2d = %d", + sensorData.id(), + (int)sensorData.depthOrRightCompressed().cols, + (int)sensorData.laserScanCompressed().cols); if(!ppStmt) { UFATAL(""); @@ -2287,12 +2444,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, int rc = SQLITE_OK; int index = 1; - rc = sqlite3_bind_int(ppStmt, index++, id); + rc = sqlite3_bind_int(ppStmt, index++, sensorData.id()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - if(!depthBytes.empty()) + if(!sensorData.depthOrRightCompressed().empty()) { - rc = sqlite3_bind_blob(ppStmt, index++, depthBytes.data, (int)depthBytes.cols, SQLITE_STATIC); + rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC); } else { @@ -2300,11 +2457,33 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, } UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + float fx=0, fyOrBaseline=0, cx=0, cy=0; + Transform localTransform = Transform::getIdentity(); + if(sensorData.cameraModels().size()) + { + UASSERT_MSG(sensorData.cameraModels().size() == 1, + uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str()); + + fx = sensorData.cameraModels()[0].fx(); + fyOrBaseline = sensorData.cameraModels()[0].fy(); + cx = sensorData.cameraModels()[0].cx(); + cy = sensorData.cameraModels()[0].cy(); + localTransform = sensorData.cameraModels()[0].localTransform(); + } + else if(sensorData.stereoCameraModel().isValid()) + { + fx = sensorData.stereoCameraModel().left().fx(); + fyOrBaseline = sensorData.stereoCameraModel().baseline(); + cx = sensorData.stereoCameraModel().left().cx(); + cy = sensorData.stereoCameraModel().left().cy(); + localTransform = sensorData.stereoCameraModel().left().localTransform(); + } + if(uStrNumCmp(_version, "0.7.0") >= 0) { rc = sqlite3_bind_double(ppStmt, index++, fx); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - rc = sqlite3_bind_double(ppStmt, index++, fy); + rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); rc = sqlite3_bind_double(ppStmt, index++, cx); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); @@ -2320,9 +2499,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - if(!depth2dBytes.empty()) + if(!sensorData.laserScanCompressed().empty()) { - rc = sqlite3_bind_blob(ppStmt, index++, depth2dBytes.data, (int)depth2dBytes.cols, SQLITE_STATIC); + rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC); } else { @@ -2332,7 +2511,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, if(uStrNumCmp(_version, "0.8.11") >= 0) { - rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts); + rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } @@ -2344,6 +2523,116 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } +std::string DBDriverSqlite3::queryStepSensorData() const +{ + UASSERT(uStrNumCmp(_version, "0.10.0") >= 0); + return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);"; +} +void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, + const SensorData & sensorData) const +{ + UASSERT(uStrNumCmp(_version, "0.10.0") >= 0); + UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d", + sensorData.id(), + (int)sensorData.imageCompressed().cols, + (int)sensorData.depthOrRightCompressed().cols, + (int)sensorData.laserScanCompressed().cols); + if(!ppStmt) + { + UFATAL(""); + } + + int rc = SQLITE_OK; + int index = 1; + + // id + rc = sqlite3_bind_int(ppStmt, index++, sensorData.id()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + + // image + if(!sensorData.imageCompressed().empty()) + { + rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC); + } + else + { + rc = sqlite3_bind_zeroblob(ppStmt, index++, 4); + } + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + + // depth or right image + if(!sensorData.depthOrRightCompressed().empty()) + { + rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC); + } + else + { + rc = sqlite3_bind_zeroblob(ppStmt, index++, 4); + } + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + + // calibration + std::vector calibration; + // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(sensorData.cameraModels().size()) + { + calibration.resize(sensorData.cameraModels().size() * (4+Transform().size())); + for(unsigned int i=0; i= 0) @@ -2361,21 +2650,16 @@ std::string DBDriverSqlite3::queryStepLink() const } void DBDriverSqlite3::stepLink( sqlite3_stmt * ppStmt, - int fromId, - int toId, - Link::Type type, - float rotVariance, - float transVariance, - const Transform & transform) const + const Link & link) const { if(!ppStmt) { UFATAL(""); } - UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type); + UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type()); // Don't save virtual links - if(type==Link::kVirtualClosure) + if(link.type()==Link::kVirtualClosure) { UDEBUG("Virtual link ignored...."); return; @@ -2383,27 +2667,27 @@ void DBDriverSqlite3::stepLink( int rc = SQLITE_OK; int index = 1; - rc = sqlite3_bind_int(ppStmt, index++, fromId); + rc = sqlite3_bind_int(ppStmt, index++, link.from()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - rc = sqlite3_bind_int(ppStmt, index++, toId); + rc = sqlite3_bind_int(ppStmt, index++, link.to()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - rc = sqlite3_bind_int(ppStmt, index++, type); + rc = sqlite3_bind_int(ppStmt, index++, link.type()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); if(uStrNumCmp(_version, "0.8.4") >= 0) { - rc = sqlite3_bind_double(ppStmt, index++, rotVariance); + rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - rc = sqlite3_bind_double(ppStmt, index++, transVariance); + rc = sqlite3_bind_double(ppStmt, index++, link.transVariance()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } else if(uStrNumCmp(_version, "0.7.4") >= 0) { - rc = sqlite3_bind_double(ppStmt, index++, rotVariance & 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 & laserScanCompressed, - float & fx, - float & fy, - float & cx, - float & cy, - Transform & localTransform, - int & laserScanMaxPts) const; - virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const; + virtual void getNodeDataQuery(int signatureId, SensorData & data) const; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const; virtual void getLastIdQuery(const std::string & tableName, int & id) const; @@ -94,6 +83,7 @@ private: std::string queryStepNode() const; std::string queryStepImage() const; std::string queryStepDepth() const; + std::string queryStepSensorData() const; std::string queryStepLink() const; std::string queryStepWordsChanged() const; std::string queryStepKeypoint() const; @@ -102,18 +92,9 @@ private: sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const; - void stepDepth( - sqlite3_stmt * ppStmt, - int id, - const cv::Mat & depthBytes, - const cv::Mat & depth2dBytes, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, - int depth2dMaxPts) const; - void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const; + void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; + void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; + void stepLink(sqlite3_stmt * ppStmt, const Link & link) 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 9c3094a9..8d3f6d3b 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -148,39 +148,39 @@ void DBReader::mainLoopBegin() void DBReader::mainLoop() { - SensorData data = this->getNextData(); - if(data.isValid()) + OdometryEvent odom = this->getNextData(); + if(odom.isValid()) { int goalId = 0; - double previousStamp = data.stamp(); - data.setStamp(UTimer::now()); - if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0) + double previousStamp = odom.data().stamp(); + odom.data().setStamp(UTimer::now()); + if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event - std::string goalStr = uBytes2Str(data.userData()); + std::string goalStr = uBytes2Str(odom.data().userData()); if(!goalStr.empty()) { std::list strs = uSplit(goalStr, ':'); if(strs.size() == 2) { goalId = atoi(strs.rbegin()->c_str()); - data.setUserData(std::vector()); + odom.data().setUserData(std::vector()); } } } if(!_odometryIgnored) { - if(data.pose().isNull()) + if(odom.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)); + this->post(new OdometryEvent(odom)); } else { - this->post(new CameraEvent(data)); + this->post(new CameraEvent(odom.data())); } if(goalId > 0) @@ -237,31 +237,26 @@ void DBReader::mainLoop() } -SensorData DBReader::getNextData() +OdometryEvent DBReader::getNextData() { - SensorData data; + OdometryEvent odom; if(_dbDriver) { if(!this->isKilled() && _currentId != _ids.end()) { - cv::Mat imageBytes; - cv::Mat depthBytes; - cv::Mat laserScanBytes; int mapId; - float fx,fy,cx,cy; - Transform localTransform, pose; - float rotVariance = 1.0f; - float transVariance = 1.0f; std::vector userData; - int laserScanMaxPts = 0; - _dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts); + SensorData data; + _dbDriver->getNodeData(*_currentId, data); // info + Transform pose; int weight; std::string label; double stamp; _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData); + cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); if(!_odometryIgnored) { std::map links; @@ -269,8 +264,7 @@ SensorData DBReader::getNextData() if(links.size()) { // assume the first is the backward neighbor, take its variance - rotVariance = links.begin()->second.rotVariance(); - transVariance = links.begin()->second.transVariance(); + infMatrix = links.begin()->second.infMatrix(); } } else @@ -280,7 +274,7 @@ SensorData DBReader::getNextData() int seq = *_currentId; ++_currentId; - if(imageBytes.empty()) + if(data.imageCompressed().empty()) { UWARN("No image loaded from the database for id=%d!", *_currentId); } @@ -334,33 +328,16 @@ SensorData DBReader::getNextData() if(!this->isKilled()) { - rtabmap::CompressionThread ctImage(imageBytes, true); - rtabmap::CompressionThread ctDepth(depthBytes, true); - rtabmap::CompressionThread ctLaserScan(laserScanBytes, false); - ctImage.start(); - ctDepth.start(); - ctLaserScan.start(); - ctImage.join(); - ctDepth.join(); - ctLaserScan.join(); - data = SensorData( - ctLaserScan.getUncompressedData(), - laserScanMaxPts, - ctImage.getUncompressedData(), - ctDepth.getUncompressedData(), - fx,fy,cx,cy, - localTransform, - pose, - rotVariance, - transVariance, - seq, - stamp, - userData); - UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d", - data.laserScan().empty()?0:1, - data.image().empty()?0:1, - data.depth().empty()?0:1, - data.rightImage().empty()?0:1); + data.uncompressData(); + data.setId(seq); + data.setStamp(stamp); + data.setUserData(userData); + UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d", + data.laserScanRaw().empty()?0:1, + data.imageRaw().empty()?0:1, + data.depthOrRightRaw().empty()?0:1); + + odom = OdometryEvent(data, pose, infMatrix.inv()); } } } @@ -368,7 +345,7 @@ SensorData DBReader::getNextData() { UERROR("Not initialized..."); } - return data; + return odom; } } /* namespace rtabmap */ diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index f228c6d3..bc591307 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -260,20 +260,23 @@ std::map TOROOptimizer::optimize( AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()); AISNavigation::TreePoseGraph2::InformationMatrix inf; //Identity: - inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x - inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y - inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta - if(!isCovarianceIgnored()) + if(isCovarianceIgnored()) { - if(iter->second.transVariance()>0) - { - inf.values[0][0] = 1.0f/iter->second.transVariance(); // x - inf.values[1][1] = 1.0f/iter->second.transVariance(); // y - } - if(iter->second.rotVariance()>0) - { - inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta - } + inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x + inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y + inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw + } + else + { + inf.values[0][0] = iter->second.infMatrix().at(0,0); // x-x + inf.values[0][1] = iter->second.infMatrix().at(0,1); // x-y + inf.values[0][2] = iter->second.infMatrix().at(0,5); // x-theta + inf.values[1][0] = iter->second.infMatrix().at(1,0); // y-x + inf.values[1][1] = iter->second.infMatrix().at(1,1); // y-y + inf.values[1][2] = iter->second.infMatrix().at(1,5); // y-theta + inf.values[2][0] = iter->second.infMatrix().at(5,0); // theta-x + inf.values[2][1] = iter->second.infMatrix().at(5,1); // theta-y + inf.values[2][2] = iter->second.infMatrix().at(5,5); // theta-theta } int id1 = iter->first; @@ -301,18 +304,7 @@ std::map TOROOptimizer::optimize( AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix::I(6); if(!isCovarianceIgnored()) { - if(iter->second.rotVariance()>0) - { - inf[0][0] = 1.0f/iter->second.rotVariance(); // roll - inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch - inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw - } - if(iter->second.transVariance()>0) - { - inf[3][3] = 1.0f/iter->second.transVariance(); // x - inf[4][4] = 1.0f/iter->second.transVariance(); // y - inf[5][5] = 1.0f/iter->second.transVariance(); // z - } + memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); } int id1 = iter->first; @@ -476,7 +468,7 @@ bool TOROOptimizer::saveGraph( { float x,y,z, yaw,pitch,roll; pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw); - 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", + fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", iter->first, iter->second.to(), x, @@ -485,12 +477,27 @@ bool TOROOptimizer::saveGraph( roll, pitch, yaw, - iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f, - iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f, - iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f, - iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f, - iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f, - iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f); + iter->second.infMatrix().at(0,0), + iter->second.infMatrix().at(0,1), + iter->second.infMatrix().at(0,2), + iter->second.infMatrix().at(0,3), + iter->second.infMatrix().at(0,4), + iter->second.infMatrix().at(0,5), + iter->second.infMatrix().at(1,1), + iter->second.infMatrix().at(1,2), + iter->second.infMatrix().at(1,3), + iter->second.infMatrix().at(1,4), + iter->second.infMatrix().at(1,5), + iter->second.infMatrix().at(2,2), + iter->second.infMatrix().at(2,3), + iter->second.infMatrix().at(2,4), + iter->second.infMatrix().at(2,5), + iter->second.infMatrix().at(3,3), + iter->second.infMatrix().at(3,4), + iter->second.infMatrix().at(3,5), + iter->second.infMatrix().at(4,4), + iter->second.infMatrix().at(4,5), + iter->second.infMatrix().at(5,5)); } UINFO("Graph saved to %s", fileName.c_str()); fclose(file); @@ -674,15 +681,15 @@ std::map G2OOptimizer::optimize( Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) { - if(iter->second.transVariance()>0) - { - information(0,0) = 1.0f/iter->second.transVariance(); // x - information(1,1) = 1.0f/iter->second.transVariance(); // y - } - if(iter->second.rotVariance()>0) - { - information(2,2) = 1.0f/iter->second.rotVariance(); // theta - } + information(0,0) = iter->second.infMatrix().at(0,0); // x-x + information(0,1) = iter->second.infMatrix().at(0,1); // x-y + information(0,2) = iter->second.infMatrix().at(0,5); // x-theta + information(1,0) = iter->second.infMatrix().at(1,0); // y-x + information(1,1) = iter->second.infMatrix().at(1,1); // y-y + information(1,2) = iter->second.infMatrix().at(1,5); // y-theta + information(2,0) = iter->second.infMatrix().at(5,0); // theta-x + information(2,1) = iter->second.infMatrix().at(5,1); // theta-y + information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } g2o::EdgeSE2 * e = new g2o::EdgeSE2(); @@ -701,18 +708,7 @@ std::map G2OOptimizer::optimize( Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) { - if(iter->second.transVariance()>0) - { - information(0,0) = 1.0f/iter->second.transVariance(); // x - information(1,1) = 1.0f/iter->second.transVariance(); // y - information(2,2) = 1.0f/iter->second.transVariance(); // z - } - if(iter->second.rotVariance()>0) - { - information(3,3) = 1.0f/iter->second.rotVariance(); // roll - information(4,4) = 1.0f/iter->second.rotVariance(); // pitch - information(5,5) = 1.0f/iter->second.rotVariance(); // yaw - } + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); } Eigen::Affine3d a = iter->second.transform().toEigen3d(); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 609d8659..c13b4b58 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -47,6 +47,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_correspondences.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_surface.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Compression.h" @@ -537,7 +539,18 @@ void Memory::preUpdate() } } -bool Memory::update(const SensorData & data, Statistics * stats) +bool Memory::update( + const SensorData & data, + Statistics * stats) +{ + return update(data, Transform(), cv::Mat(), stats); +} + +bool Memory::update( + const SensorData & data, + const Transform & pose, + const cv::Mat & covariance, + Statistics * stats) { UDEBUG(""); UTimer timer; @@ -557,7 +570,7 @@ bool Memory::update(const SensorData & data, Statistics * stats) //============================================================ // Create a signature with the image received. //============================================================ - Signature * signature = this->createSignature(data, stats); + Signature * signature = this->createSignature(data, pose, stats); if (signature == 0) { UERROR("Failed to create a signature..."); @@ -569,7 +582,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, data.poseRotVariance(), data.poseTransVariance()); + this->addSignatureToStm(signature, covariance); _lastSignature = signature; @@ -678,7 +691,7 @@ void Memory::setRoi(const std::string & roi) } } -void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance) +void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance) { UTimer timer; // add signature on top of the short-term memory @@ -694,14 +707,15 @@ void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, flo if(!signature->getPose().isNull() && !_signatures.at(*_stMem.rbegin())->getPose().isNull()) { + cv::Mat infMatrix = covariance.inv(); motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose(); - _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseRotVariance, poseTransVariance)); - signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseRotVariance, poseTransVariance)); + _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix)); + signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix)); } else { - _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f, 1.0f)); - signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f, 1.0f)); + _signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform())); + signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform())); } UDEBUG("Min STM id = %d", *_stMem.begin()); } @@ -1918,18 +1932,14 @@ Transform Memory::computeVisualTransform( if(_bowEpipolarGeometry) { // we only need the camera transform, send guess words3 for scale estimation - if(oldS.getWords3().size()) + if(oldS.getWords3().size() && oldS.sensorData().cameraModels().size()) { Transform cameraTransform; double variance = 1; std::multimap inliers3D = util3d::generateWords3DMono( oldS.getWords(), newS.getWords(), - oldS.getFx(), - oldS.getFy(), - oldS.getCx(), - oldS.getCy(), - oldS.getLocalTransform(), + oldS.sensorData().cameraModels()[0], cameraTransform, 100, 4.0f, @@ -1980,11 +1990,16 @@ Transform Memory::computeVisualTransform( UINFO(msg.c_str()); } } - else + else if(oldS.getWords3().size() == 0) { msg = uFormat("No 3D guess words found"); UWARN(msg.c_str()); } + else + { + msg = uFormat("No camera model"); + UWARN(msg.c_str()); + } } else { @@ -2106,12 +2121,12 @@ Transform Memory::computeIcpTransform( if(icp3D) { //Depth required, if not in RAM, load it from LTM - if(oldS->getDepthCompressed().empty()) + if(oldS->sensorData().depthOrRightCompressed().empty()) { depthToLoad.push_back(oldS); added.insert(oldS->id()); } - if(newS->getDepthCompressed().empty()) + if(newS->sensorData().depthOrRightCompressed().empty()) { depthToLoad.push_back(newS); added.insert(newS->id()); @@ -2120,11 +2135,11 @@ Transform Memory::computeIcpTransform( else { //Depth required, if not in RAM, load it from LTM - if(oldS->getLaserScanCompressed().empty() && added.find(oldS->id()) == added.end()) + if(oldS->sensorData().laserScanCompressed().empty() && added.find(oldS->id()) == added.end()) { depthToLoad.push_back(oldS); } - if(newS->getLaserScanCompressed().empty() && added.find(newS->id()) == added.end()) + if(newS->sensorData().laserScanCompressed().empty() && added.find(newS->id()) == added.end()) { depthToLoad.push_back(newS); } @@ -2142,14 +2157,14 @@ Transform Memory::computeIcpTransform( if(icp3D) { cv::Mat tmp1, tmp2; - oldS->uncompressData(0, &tmp1, 0); - newS->uncompressData(0, &tmp2, 0); + oldS->sensorData().uncompressData(0, &tmp1, 0); + newS->sensorData().uncompressData(0, &tmp2, 0); } else { cv::Mat tmp1, tmp2; - oldS->uncompressData(0, 0, &tmp1); - newS->uncompressData(0, 0, &tmp2); + oldS->sensorData().uncompressData(0, 0, &tmp1); + newS->sensorData().uncompressData(0, 0, &tmp2); } t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance, inliersRatio); @@ -2196,135 +2211,123 @@ Transform Memory::computeIcpTransform( if(icp3D) { UDEBUG("3D ICP"); - if(!oldS.getDepthRaw().empty() && !newS.getDepthRaw().empty()) + if(!oldS.sensorData().depthOrRightRaw().empty() && + !newS.sensorData().depthOrRightRaw().empty() && + (oldS.sensorData().cameraModels().size() || oldS.sensorData().stereoCameraModel().isValid()) && + (newS.sensorData().cameraModels().size() || newS.sensorData().stereoCameraModel().isValid())) { - if(oldS.getDepthRaw().type() == CV_8UC1 || newS.getDepthRaw().type() == CV_8UC1) - { - UERROR("ICP 3D cannot be done on stereo images!"); - } - else - { - pcl::PointCloud::Ptr oldCloudXYZ = util3d::getICPReadyCloud( - oldS.getDepthRaw(), - oldS.getFx(), - oldS.getFy(), - oldS.getCx(), - oldS.getCy(), - _icpDecimation, - _icpMaxDepth, - _icpVoxelSize, - _icpSamples, - oldS.getLocalTransform()); - pcl::PointCloud::Ptr newCloudXYZ = util3d::getICPReadyCloud( - newS.getDepthRaw(), - newS.getFx(), - newS.getFy(), - newS.getCx(), - newS.getCy(), - _icpDecimation, - _icpMaxDepth, - _icpVoxelSize, - _icpSamples, - guess * newS.getLocalTransform()); + pcl::PointCloud::Ptr oldCloudXYZ = util3d::cloudFromSensorData( + oldS.sensorData(), + _icpDecimation, + _icpMaxDepth, + _icpVoxelSize, + _icpSamples); + pcl::PointCloud::Ptr newCloudXYZ = util3d::cloudFromSensorData( + newS.sensorData(), + _icpDecimation, + _icpMaxDepth, + _icpVoxelSize, + _icpSamples); - // 3D - if(newCloudXYZ->size() && oldCloudXYZ->size()) + // 3D + if(newCloudXYZ->size() && oldCloudXYZ->size()) + { + newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, guess); + + bool hasConverged = false; + Transform icpT; + int correspondences = 0; + float correspondencesRatio = -1.0f; + double variance = 1; + if(_icpPointToPlane) { - bool hasConverged = false; - Transform icpT; - int correspondences = 0; - float correspondencesRatio = -1.0f; - double variance = 1; - if(_icpPointToPlane) - { - pcl::PointCloud::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors); - pcl::PointCloud::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors); + pcl::PointCloud::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors); + pcl::PointCloud::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors); - std::vector indices; - newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud); - oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud); + std::vector indices; + newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud); + oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud); - if(newCloud->size() && oldCloud->size()) - { - icpT = util3d::icpPointToPlane(newCloud, - oldCloud, - _icpMaxCorrespondenceDistance, - _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); - } - } - else + if(newCloud->size() && oldCloud->size()) { - icpT = util3d::icp(newCloudXYZ, - oldCloudXYZ, - _icpMaxCorrespondenceDistance, - _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); - } - - // verify if there are enough correspondences - correspondencesRatio = float(correspondences)/float(newS.getDepthRaw().total()); - - UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", - hasConverged?"true":"false", - variance, - correspondences, - (int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()), - correspondencesRatio*100.0f); - - if(varianceOut) - { - *varianceOut = variance; - } - if(correspondencesOut) - { - *correspondencesOut = correspondences; - } - if(correspondencesRatioOut) - { - *correspondencesRatioOut = correspondencesRatio; - } - - if(!icpT.isNull() && hasConverged && - correspondencesRatio >= _icpCorrespondenceRatio) - { - float x,y,z, roll,pitch,yaw; - icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - if((_icpMaxTranslation>0.0f && - (fabs(x) > _icpMaxTranslation || - fabs(y) > _icpMaxTranslation || - fabs(z) > _icpMaxTranslation)) - || - (_icpMaxRotation>0.0f && - (fabs(roll) > _icpMaxRotation || - fabs(pitch) > _icpMaxRotation || - fabs(yaw) > _icpMaxRotation))) - { - msg = uFormat("Cannot compute transform (ICP correction too large)"); - UINFO(msg.c_str()); - } - else - { - transform = icpT * guess; - transform = transform.inverse(); - } - } - else - { - msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)", - hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio); - UINFO(msg.c_str()); + icpT = util3d::icpPointToPlane(newCloud, + oldCloud, + _icpMaxCorrespondenceDistance, + _icpMaxIterations, + &hasConverged, + &variance, + &correspondences); } } else { - msg = "Clouds empty ?!?"; - UWARN(msg.c_str()); + icpT = util3d::icp(newCloudXYZ, + oldCloudXYZ, + _icpMaxCorrespondenceDistance, + _icpMaxIterations, + &hasConverged, + &variance, + &correspondences); } + + // verify if there are enough correspondences + correspondencesRatio = float(correspondences)/float(newS.sensorData().depthOrRightRaw().total()); + + UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", + hasConverged?"true":"false", + variance, + correspondences, + (int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()), + correspondencesRatio*100.0f); + + if(varianceOut) + { + *varianceOut = variance; + } + if(correspondencesOut) + { + *correspondencesOut = correspondences; + } + if(correspondencesRatioOut) + { + *correspondencesRatioOut = correspondencesRatio; + } + + if(!icpT.isNull() && hasConverged && + correspondencesRatio >= _icpCorrespondenceRatio) + { + float x,y,z, roll,pitch,yaw; + icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + if((_icpMaxTranslation>0.0f && + (fabs(x) > _icpMaxTranslation || + fabs(y) > _icpMaxTranslation || + fabs(z) > _icpMaxTranslation)) + || + (_icpMaxRotation>0.0f && + (fabs(roll) > _icpMaxRotation || + fabs(pitch) > _icpMaxRotation || + fabs(yaw) > _icpMaxRotation))) + { + msg = uFormat("Cannot compute transform (ICP correction too large)"); + UINFO(msg.c_str()); + } + else + { + transform = icpT * guess; + transform = transform.inverse(); + } + } + else + { + msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)", + hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio); + UINFO(msg.c_str()); + } + } + else + { + msg = "Clouds empty ?!?"; + UWARN(msg.c_str()); } } else @@ -2346,11 +2349,11 @@ Transform Memory::computeIcpTransform( UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p); } - if(!oldS.getLaserScanRaw().empty() && !newS.getLaserScanRaw().empty()) + if(!oldS.sensorData().laserScanRaw().empty() && !newS.sensorData().laserScanRaw().empty()) { // 2D - pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldS.getLaserScanRaw()); - pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.getLaserScanRaw(), guess); + pcl::PointCloud::Ptr oldCloud = util3d::cvMat2Cloud(oldS.sensorData().laserScanRaw()); + pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.sensorData().laserScanRaw(), guess); //voxelize if(_icp2VoxelSize > _laserScanVoxelSize) @@ -2376,9 +2379,9 @@ Transform Memory::computeIcpTransform( // verify if there are enough correspondences - if(newS.getLaserScanMaxPts()) + if(newS.sensorData().laserScanMaxPts()) { - correspondencesRatio = float(correspondences)/float(newS.getLaserScanMaxPts()); + correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); } else { @@ -2489,7 +2492,7 @@ Transform Memory::computeScanMatchingTransform( { Signature * s = _getSignature(iter->first); UASSERT(s != 0); - if(s->getLaserScanCompressed().empty()) + if(s->sensorData().laserScanCompressed().empty()) { depthToLoad.push_back(s); } @@ -2506,10 +2509,10 @@ Transform Memory::computeScanMatchingTransform( if(iter->first != newId) { Signature * s = this->_getSignature(iter->first); - if(!s->getLaserScanCompressed().empty()) + if(!s->sensorData().laserScanCompressed().empty()) { cv::Mat scan; - s->uncompressData(0, 0, &scan); + s->sensorData().uncompressData(0, 0, &scan); *assembledOldClouds += *util3d::cvMat2Cloud(scan, iter->second); } else @@ -2529,7 +2532,7 @@ Transform Memory::computeScanMatchingTransform( Signature * newS = _getSignature(newId); pcl::PointCloud::Ptr newCloud; cv::Mat newScan; - newS->uncompressData(0, 0, &newScan); + newS->sensorData().uncompressData(0, 0, &newScan); newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId)); //voxelize @@ -2555,9 +2558,9 @@ Transform Memory::computeScanMatchingTransform( // verify if there enough correspondences float correspondencesRatio = 0.0f; - if(newS->getLaserScanMaxPts()) + if(newS->sensorData().laserScanMaxPts()) { - correspondencesRatio = float(correspondences)/float(newS->getLaserScanMaxPts()); + correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); } else { @@ -2616,71 +2619,59 @@ Transform Memory::computeScanMatchingTransform( return transform; } -// Transform from new to old -bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float rotVariance, float transVariance) +bool Memory::addLink(const Link & link) { - UASSERT(type > Link::kNeighbor && type != Link::kUndef); + UASSERT(link.type() > Link::kNeighbor && link.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) + ULOGGER_INFO("to=%d, from=%d transform: %s", link.to(), link.from(), link.transform().prettyPrint().c_str()); + Signature * toS = _getSignature(link.to()); + Signature * fromS = _getSignature(link.from()); + if(toS && fromS) { - if(oldS->hasLink(newId)) + if(toS->hasLink(link.from())) { // do nothing, already merged - UINFO("already linked! old=%d, new=%d", oldId, newId); + UINFO("already linked! to=%d, from=%d", link.to(), link.from()); return true; } - UDEBUG("Add link between %d and %d", oldS->id(), newS->id()); + UDEBUG("Add link between %d and %d", toS->id(), fromS->id()); - if(rotVariance == 0) - { - rotVariance = 0.000001; // set small variance (0.001 m x 0.001 m) - UWARN("Null rotation variance detected, set to something very small (0.001m^2)!"); - } - if(transVariance == 0) - { - transVariance = 0.000001; // set small variance (0.001 m x 0.001 m) - UWARN("Null transitional variance detected, set to something very small (0.001m^2)!"); - } + toS->addLink(Link(link.to(), link.from(), link.type(), link.transform().inverse(), link.infMatrix())); + fromS->addLink(link); - oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), rotVariance, transVariance)); - newS->addLink(Link(newS->id(), oldS->id(), type, transform, rotVariance, transVariance)); - - if(type!=Link::kVirtualClosure) + if(link.type()!=Link::kVirtualClosure) { _linksChanged = true; } - if(_incrementalMemory && type == Link::kGlobalClosure) + if(_incrementalMemory && link.type() == Link::kGlobalClosure) { - _lastGlobalLoopClosureId = newS->id()>oldS->id()?newS->id():oldS->id(); + _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id(); // udpate weights only if the memory is incremental - if(newS->id() > oldS->id()) + if(fromS->id() > toS->id()) { - newS->setWeight(newS->getWeight() + oldS->getWeight()); - oldS->setWeight(0); + fromS->setWeight(fromS->getWeight() + toS->getWeight()); + toS->setWeight(0); } else { - oldS->setWeight(oldS->getWeight() + newS->getWeight()); - newS->setWeight(0); + toS->setWeight(toS->getWeight() + fromS->getWeight()); + fromS->setWeight(0); } } return true; } else { - if(!newS) + if(!fromS) { - UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId); + UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.from()); } - if(!oldS) + if(!toS) { - UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId); + UERROR("from=%d, to=%d, Signature %d not found in working/st memories", link.from(), link.to(), link.to()); } } return false; @@ -2711,6 +2702,32 @@ void Memory::updateLink(int fromId, int toId, const Transform & transform, float } } +void Memory::updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance) +{ + Signature * fromS = this->_getSignature(fromId); + Signature * toS = this->_getSignature(toId); + + if(fromS->hasLink(toId) && toS->hasLink(fromId)) + { + Link::Type type = fromS->getLinks().at(toId).type(); + fromS->removeLink(toId); + toS->removeLink(fromId); + + cv::Mat infMatrix = covariance.inv(); + fromS->addLink(Link(fromId, toId, type, transform, infMatrix)); + toS->addLink(Link(toId, fromId, type, transform.inverse(), infMatrix)); + + if(type!=Link::kVirtualClosure) + { + _linksChanged = true; + } + } + else + { + UERROR("fromId=%d and toId=%d are not linked!", fromId, toId); + } +} + void Memory::removeAllVirtualLinks() { UDEBUG(""); @@ -3002,7 +3019,7 @@ bool Memory::rehearsalMerge(int oldId, int newId) newS->setLabel(oldS->getLabel()); oldS->setLabel(""); oldS->removeLinks(); // remove all links - oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location + oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location // Set old image to new signature this->copyData(oldS, newS); @@ -3017,7 +3034,7 @@ bool Memory::rehearsalMerge(int oldId, int newId) } else { - newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location + newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location // update weight oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); @@ -3091,23 +3108,29 @@ cv::Mat Memory::getImageCompressed(int signatureId) const const Signature * s = this->getSignature(signatureId); if(s) { - image = s->getImageCompressed(); + image = s->sensorData().imageCompressed(); } if(image.empty() && this->isBinDataKept() && _dbDriver) { - _dbDriver->getNodeData(signatureId, image); + SensorData data; + _dbDriver->getNodeData(signatureId, data); + image = data.imageCompressed(); } return image; } -Signature Memory::getSignatureData(int locationId, bool uncompressedData) +SensorData Memory::getNodeData(int nodeId, bool uncompressedData) { - UDEBUG("locationId=%d", locationId); - Signature r; - Signature * s = this->_getSignature(locationId); - if(s && !s->getImageCompressed().empty()) + UDEBUG("nodeId=%d", nodeId); + SensorData r; + Signature * s = this->_getSignature(nodeId); + if(s && !s->sensorData().imageCompressed().empty()) { - r = *s; + if(uncompressedData) + { + s->sensorData().uncompressData(); + } + r = s->sensorData(); } else if(_dbDriver) { @@ -3117,65 +3140,29 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData) std::list signatures; signatures.push_back(s); _dbDriver->loadNodeData(signatures, true); - r = *s; - } - else - { - std::list ids; - ids.push_back(locationId); - std::list signatures; - std::set loadedFromTrash; - _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash); - if(signatures.size()) + if(uncompressedData) { - Signature * sTmp = signatures.front(); - if(sTmp->getImageCompressed().empty()) - { - _dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull()); - } - r = *sTmp; - if(loadedFromTrash.size()) - { - //put it back to trash - _dbDriver->asyncSave(sTmp); - } - else - { - delete sTmp; - } + s->sensorData().uncompressData(); } - } - } - UDEBUG(""); - - if(uncompressedData && r.getImageRaw().empty() && !r.getImageCompressed().empty()) - { - //uncompress data - if(s) - { - s->uncompressData(); - r.setImageRaw(s->getImageRaw()); - r.setDepthRaw(s->getDepthRaw()); - r.setLaserScanRaw(s->getLaserScanRaw(), s->getLaserScanMaxPts()); + r = s->sensorData(); } else { - r.uncompressData(); + _dbDriver->getNodeData(nodeId, r); } } - UDEBUG(""); return r; } -Signature Memory::getSignatureDataConst(int locationId) const +SensorData Memory::getSignatureDataConst(int locationId) const { UDEBUG(""); - Signature r; + SensorData r; const Signature * s = this->getSignature(locationId); - if(s && !s->getImageCompressed().empty()) + if(s && !s->sensorData().imageCompressed().empty()) { - r = *s; + r = s->sensorData(); } else if(_dbDriver) { @@ -3183,9 +3170,10 @@ Signature Memory::getSignatureDataConst(int locationId) const if(s) { std::list signatures; - r = *s; - signatures.push_back(&r); + Signature tmp = *s; + signatures.push_back(&tmp); _dbDriver->loadNodeData(signatures, true); + r = tmp.sensorData(); } else { @@ -3197,11 +3185,11 @@ Signature Memory::getSignatureDataConst(int locationId) const if(signatures.size()) { Signature * sTmp = signatures.front(); - if(sTmp->getImageCompressed().empty()) + if(sTmp->sensorData().imageCompressed().empty()) { _dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull()); } - r = *sTmp; + r = sTmp->sensorData(); if(loadedFromTrash.size()) { //put it back to trash @@ -3488,28 +3476,14 @@ void Memory::copyData(const Signature * from, Signature * to) if(from->isSaved() && _dbDriver) { - cv::Mat image; - cv::Mat depth; - cv::Mat laserScan; - float fx, fy, cx, cy; - Transform localTransform; - int laserScanMaxPts = 0; - _dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform, laserScanMaxPts); - - to->setImageCompressed(image); - to->setDepthCompressed(depth, fx, fy, cx, cy); - to->setLaserScanCompressed(laserScan, laserScanMaxPts); - to->setLocalTransform(localTransform); - + _dbDriver->getNodeData(from->id(), to->sensorData()); UDEBUG("Loaded image data from database"); } else { - to->setImageCompressed(from->getImageCompressed()); - to->setDepthCompressed(from->getDepthCompressed(), from->getFx(), from->getFy(), from->getCx(), from->getCy()); - to->setLaserScanCompressed(from->getLaserScanCompressed(), from->getLaserScanMaxPts()); - to->setLocalTransform(from->getLocalTransform()); + to->sensorData() = (SensorData)from->sensorData(); } + to->sensorData().setId(to->id()); to->setPose(from->getPose()); to->setWords3(from->getWords3()); @@ -3537,22 +3511,32 @@ private: VWDictionary * _vwp; }; -Signature * Memory::createSignature(const SensorData & data, Statistics * stats) +Signature * Memory::createSignature(const SensorData & data, const Transform & pose, Statistics * stats) { UDEBUG(""); - 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) && data.depth().rows == data.image().rows && data.depth().cols == data.image().cols)); - UASSERT(data.rightImage().empty() || (data.rightImage().type() == CV_8UC1 && data.rightImage().rows == data.image().rows && data.rightImage().cols == data.image().cols)); - UASSERT(data.laserScan().empty() || data.laserScan().type() == CV_32FC2); + UASSERT(data.imageRaw().empty() || + data.imageRaw().type() == CV_8UC1 || + data.imageRaw().type() == CV_8UC3); + UASSERT_MSG(data.depthOrRightRaw().empty() || + ( (data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_8UC1) && + data.depthOrRightRaw().rows == data.imageRaw().rows && + data.depthOrRightRaw().cols == data.imageRaw().cols), + uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])", + data.imageRaw().cols, + data.imageRaw().rows, + data.depthOrRightRaw().cols, + data.depthOrRightRaw().rows, + data.depthOrRightRaw().type(), + CV_16UC1, CV_32FC1, CV_8UC1).c_str()); + UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2); - if(!data.depthOrRightImage().empty() && (data.fx() <= 0 || data.fyOrBaseline() <= 0)) + if(!data.depthOrRightRaw().empty() && + data.cameraModels().size() == 0 && + !data.stereoCameraModel().isValid()) { - UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)", - data.fx(), data.fyOrBaseline(), data.cx(), data.cy()); + UERROR("Rectified images required! Calibrate your camera."); return 0; } - UASSERT(data.depthOrRightImage().empty() || data.fx() > 0); - UASSERT(data.depthOrRightImage().empty() || data.fyOrBaseline() > 0); UASSERT(_feature2D != 0); PreUpdateThread preUpdateThread(_vwd); @@ -3613,17 +3597,17 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) // Extract features cv::Mat imageMono; // convert to grayscale - if(data.image().channels() > 1) + if(data.imageRaw().channels() > 1) { - cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY); + cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY); } else { - imageMono = data.image(); + imageMono = data.imageRaw(); } cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios); - if(!data.rightImage().empty()) + if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid()) { //stereo cv::Mat disparity; @@ -3671,7 +3655,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) //generate a disparity map disparity = util2d::disparityFromStereoImages( imageMono, - data.rightImage(), + data.depthOrRightRaw(), leftCorners, _stereoFlowWinSize, _stereoFlowMaxLevel, @@ -3685,7 +3669,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) if(_wordsMaxDepth > 0.0f) { // disparity = baseline * fx / depth; - float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth; + float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth; Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity); UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size()); } @@ -3700,14 +3684,17 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); } - keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform()); + keypoints3D = util3d::generateKeypoints3DDisparity( + keypoints, + disparity, + data.stereoCameraModel()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } } } - else if(!data.depth().empty()) + else if(!data.depthOrRightRaw().empty() && data.cameraModels().size()) { //depth bool subPixelOn = false; @@ -3749,7 +3736,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) if(_wordsMaxDepth > 0.0f) { - Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depth(), _wordsMaxDepth); + Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depthOrRightRaw(), _wordsMaxDepth); UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size()); } @@ -3763,7 +3750,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); } - keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform()); + keypoints3D = util3d::generateKeypoints3DDepth( + keypoints, + data.depthOrRightRaw(), + data.cameraModels()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); @@ -3823,25 +3813,25 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) descriptors = data.descriptors().clone(); // filter by depth - if(!data.rightImage().empty()) + if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid()) { //stereo cv::Mat imageMono; // convert to grayscale - if(data.image().channels() > 1) + if(data.imageRaw().channels() > 1) { - cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY); + cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY); } else { - imageMono = data.image(); + imageMono = data.imageRaw(); } //generate a disparity map std::vector leftCorners; cv::KeyPoint::convert(keypoints, leftCorners); cv::Mat disparity = util2d::disparityFromStereoImages( imageMono, - data.rightImage(), + data.depthOrRightRaw(), leftCorners, _stereoFlowWinSize, _stereoFlowMaxLevel, @@ -3855,16 +3845,19 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) if(_wordsMaxDepth) { // disparity = baseline * fx / depth; - float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth; + float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth; Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity); } - keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform()); + keypoints3D = util3d::generateKeypoints3DDisparity( + keypoints, + disparity, + data.stereoCameraModel()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); } - else if(!data.depth().empty()) + else if(!data.depthOrRightRaw().empty() && data.cameraModels().size()) { //depth if(_wordsMaxDepth) @@ -3873,7 +3866,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size()); } - keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform()); + keypoints3D = util3d::generateKeypoints3DDepth( + keypoints, + data.depthOrRightRaw(), + data.cameraModels()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t); @@ -3939,21 +3935,20 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) if(words.size() > 8 && words3D.size() == 0 && - !data.pose().isNull() && + !pose.isNull() && + data.cameraModels().size() == 1 && _signatures.size()) { UDEBUG("Generate 3D words using odometry"); Signature * previousS = _signatures.rbegin()->second; if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull()) { - Transform cameraTransform = data.pose().inverse() * previousS->getPose(); + Transform cameraTransform = pose.inverse() * previousS->getPose(); // compute 3D words by epipolar geometry with the previous signature std::multimap inliers = util3d::generateWords3DMono( words, previousS->getWords(), - data.fx(), data.fy()?data.fy():data.fx(), - data.cx(), data.cy(), - data.localTransform(), + data.cameraModels()[0], cameraTransform); // words3D should have the same size than words @@ -3978,29 +3973,28 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) } } - cv::Mat image = data.image(); - cv::Mat depthOrRightImage = data.depthOrRightImage(); - float fx = data.fx(); - float fyOrBaseline = data.fyOrBaseline(); - float cx = data.cx(); - float cy = data.cy(); + cv::Mat image = data.imageRaw(); + cv::Mat depthOrRightImage = data.depthOrRightRaw(); + std::vector cameraModels = data.cameraModels(); + StereoCameraModel stereoCameraModel = data.stereoCameraModel(); // apply decimation? if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1) { image = util2d::decimate(image, _imageDecimation); depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation); - cx/=float(_imageDecimation); - cy/=float(_imageDecimation); - fx/=float(_imageDecimation); - if(data.fy() != 0.0f) + for(unsigned int i=0; i 0.0f) { laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize)); @@ -4035,17 +4029,23 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) "", words, words3D, - data.pose(), + pose, data.userData(), - ctDepth2d.getCompressedData(), - ctImage.getCompressedData(), - ctDepth.getCompressedData(), - fx, - fyOrBaseline, - cx, - cy, - data.localTransform(), - data.laserScanMaxPts()); + stereoCameraModel.isValid()? + SensorData( + ctDepth2d.getCompressedData(), + data.laserScanMaxPts(), + ctImage.getCompressedData(), + ctDepth.getCompressedData(), + stereoCameraModel, + id): + SensorData( + ctDepth2d.getCompressedData(), + data.laserScanMaxPts(), + ctImage.getCompressedData(), + ctDepth.getCompressedData(), + cameraModels, + id)); } else { @@ -4056,23 +4056,18 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) "", words, words3D, - data.pose(), + pose, data.userData(), - rtabmap::compressData2(laserScan), - cv::Mat(), - cv::Mat(), - 0, - 0, - 0, - 0, - Transform(), - data.laserScanMaxPts()); + SensorData( + rtabmap::compressData2(laserScan), + data.laserScanMaxPts(), + cv::Mat(), cv::Mat(), CameraModel(), id)); } if(this->isRawDataKept()) { - s->setImageRaw(image); - s->setDepthRaw(depthOrRightImage); - s->setLaserScanRaw(laserScan, data.laserScanMaxPts()); + s->sensorData().setImageRaw(image); + s->sensorData().setDepthOrRightRaw(depthOrRightImage); + s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts()); } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ac7777e4..a6eb5dc8 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -89,16 +89,21 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) _pose.setIdentity(); // initialized } - UASSERT(!data.image().empty()); + UASSERT(!data.imageRaw().empty()); if(dynamic_cast(this) == 0) { - UASSERT(!data.depthOrRightImage().empty()); + UASSERT(!data.depthOrRightRaw().empty()); } - if(data.fx() <= 0 || data.fyOrBaseline() <= 0) + if(data.cameraModels().size() > 1) { - UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)", - data.fx(), data.fyOrBaseline(), data.cx(), data.cy()); + UERROR("Odometry doesn't support multi-camera yet."); + return Transform(); + } + else if(!data.stereoCameraModel().isValid() && + (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) + { + UERROR("Rectified images required! Calibrate your camera."); return Transform(); } diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 003b249f..905cc338 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -160,8 +160,15 @@ Transform OdometryBOW::computeTransform( { if(this->isPnPEstimationUsed()) { - if((int)newSignature->getWords().size() >= this->getMinInliers()) + if(data.cameraModels().size() > 1) { + UERROR("PnP cannot be used on multi-cameras setup."); + } + else if((int)newSignature->getWords().size() >= this->getMinInliers()) + { + UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid())); + const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0]; + // find correspondences std::vector ids = uListToVector(uUniqueKeys(newSignature->getWords())); std::vector objectPoints(ids.size()); @@ -194,11 +201,8 @@ Transform OdometryBOW::computeTransform( if((int)matches.size() >= this->getMinInliers()) { //PnPRansac - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fy()>0?data.fy():data.fx(), data.cy(), - 0, 0, 1); - Transform guess = (this->getPose() * data.localTransform()).inverse(); + cv::Mat K = cameraModel.K(); + Transform guess = (this->getPose() * cameraModel.localTransform()).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), @@ -229,7 +233,7 @@ Transform OdometryBOW::computeTransform( R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); // make it incremental - transform = (data.localTransform() * pnp * this->getPose()).inverse(); + transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse(); UDEBUG("Odom transform = %s", transform.prettyPrint().c_str()); diff --git a/corelib/src/OdometryICP.cpp b/corelib/src/OdometryICP.cpp index 813e7ddd..2ca731a7 100644 --- a/corelib/src/OdometryICP.cpp +++ b/corelib/src/OdometryICP.cpp @@ -72,25 +72,32 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * bool hasConverged = false; double variance = 0; unsigned int minPoints = 100; - if(!data.depth().empty()) + if(!data.depthOrRightRaw().empty()) { - if(data.depth().type() == CV_8UC1) + if(data.depthOrRightRaw().type() == CV_8UC1) { UERROR("ICP 3D cannot be done on stereo images!"); return output; } + if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid())) + { + UERROR("ICP 3D cannot be done without calibration or on multi-camera!"); + return output; + } + const CameraModel & cameraModel = data.cameraModels()[0]; + pcl::PointCloud::Ptr newCloudXYZ = util3d::getICPReadyCloud( - data.depth(), - data.fx(), - data.fy(), - data.cx(), - data.cy(), + data.depthOrRightRaw(), + cameraModel.fx(), + cameraModel.fy(), + cameraModel.cx(), + cameraModel.cy(), _decimation, this->getMaxDepth(), _voxelSize, _samples, - data.localTransform()); + cameraModel.localTransform()); if(_pointToPlane) { diff --git a/corelib/src/OdometryMono.cpp b/corelib/src/OdometryMono.cpp index eb0f9abc..42951ab0 100644 --- a/corelib/src/OdometryMono.cpp +++ b/corelib/src/OdometryMono.cpp @@ -147,11 +147,24 @@ void OdometryMono::reset(const Transform & initialPose) Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info) { - UASSERT(!data.image().empty()); - UASSERT(data.fx()); + Transform output; + + if(data.imageRaw().empty()) + { + UERROR("Image empty! Cannot compute odometry..."); + return output; + } + + if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()))) + { + UERROR("Odometry cannot be done without calibration or on multi-camera!"); + return output; + } + + + const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0]; UTimer timer; - Transform output; int inliers = 0; int correspondences = 0; @@ -159,13 +172,13 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * cv::Mat newFrame; // convert to grayscale - if(data.image().channels() > 1) + if(data.imageRaw().channels() > 1) { - cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY); + cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY); } else { - newFrame = data.image().clone(); + newFrame = data.imageRaw().clone(); } if(memory_->getStMem().size() >= 1) @@ -190,11 +203,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * nFeatures = (int)newS->getWords().size(); if((int)newS->getWords().size() > this->getMinInliers()) { - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fy()==0?data.fx():data.fy(), data.cy(), - 0, 0, 1); - Transform guess = (this->getPose() * data.localTransform()).inverse(); + cv::Mat K = cameraModel.K(); + Transform guess = (this->getPose() * cameraModel.localTransform()).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), @@ -216,7 +226,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * UDEBUG("project points to previous image"); std::vector prevImagePoints; const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin())); - Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse(); + Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse(); cv::Mat prevR = (cv::Mat_(3,3) << (double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(), (double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(), @@ -240,8 +250,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * { if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) && uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) && - uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) && - uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows)) + uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) && + uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows)) { refCorners[oi] = prevImagePoints[i]; newCorners[oi] = imagePoints[i]; @@ -273,7 +283,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * std::vector err; UDEBUG("cv::calcOpticalFlowPyrLK() begin"); cv::calcOpticalFlowPyrLK( - prevS->getImageRaw(), + prevS->sensorData().imageRaw(), newFrame, refCorners, newCorners, @@ -357,7 +367,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * Transform pnp = Transform(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse(); + output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse(); if(this->isInfoDataFilled() && info && inliersV.size()) { @@ -402,9 +412,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * std::multimap inliers3D = util3d::generateWords3DMono( previousS->getWords(), newS->getWords(), - data.fx(), data.fy()?data.fy():data.fx(), - data.cx(), data.cy(), - data.localTransform(), + cameraModel, cameraTransform, this->getIterations(), this->getPnPReprojError(), @@ -515,7 +523,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * std::vector err; UDEBUG("cv::calcOpticalFlowPyrLK() begin"); cv::calcOpticalFlowPyrLK( - refS->getImageRaw(), + refS->sensorData().imageRaw(), newFrame, refCorners, refCornersGuess, @@ -652,10 +660,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * //UDEBUG("Correcting matches...done!"); UDEBUG("Computing P..."); - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fy()==0?data.fx():data.fy(), data.cy(), - 0, 0, 1); + cv::Mat K = cameraModel.K(); cv::Mat Kinv = K.inv(); cv::Mat E = K.t()*F*K; @@ -716,7 +721,15 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * (*inliersRef)[oi] = cloud->at(i); if(!refDepth_.empty()) { - (*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true); + (*inliersRefGuess)[oi] = util3d::projectDepthTo3D( + refDepth_, + refCorners[i].x, + refCorners[i].y, + cameraModel.cx(), + cameraModel.cy(), + cameraModel.fx(), + cameraModel.fy(), + true); } ++oi; } @@ -824,7 +837,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - output = data.localTransform() * pnp.inverse() * data.localTransform().inverse(); + output = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse(); if(output.getNorm() < minTranslation_*5) { reject = true; @@ -844,7 +857,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * int index =inliersPnP.at(i); int id = cornerIds[index]; UASSERT(id > 0 && id <= *wordsId.rbegin()); - pcl::PointXYZ pt = util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform()); + pcl::PointXYZ pt = util3d::transformPoint( + pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), + this->getPose()*cameraModel.localTransform()); localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z))); keyFrameWords3D.insert(std::make_pair(id, pt)); } @@ -890,7 +905,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * { cornersMap_.insert(std::make_pair(iter->first, iter->second.pt)); } - refDepth_ = data.depth().clone(); + refDepth_ = data.depthOrRightRaw().clone(); keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity())); } else diff --git a/corelib/src/OdometryOpticalFlow.cpp b/corelib/src/OdometryOpticalFlow.cpp index 36ea6d0c..212aaeda 100644 --- a/corelib/src/OdometryOpticalFlow.cpp +++ b/corelib/src/OdometryOpticalFlow.cpp @@ -128,7 +128,7 @@ Transform OdometryOpticalFlow::computeTransform( info->type = 1; } - if(!data.rightImage().empty()) + if(data.stereoCameraModel().isValid()) { //stereo return computeTransformStereo(data, info); @@ -144,8 +144,13 @@ Transform OdometryOpticalFlow::computeTransformStereo( const SensorData & data, OdometryInfo * info) { - UTimer timer; Transform output; + if(!data.stereoCameraModel().isValid()) + { + UERROR("Calibrated camera required."); + return output; + } + UTimer timer; double variance = 0; int inliers = 0; @@ -153,15 +158,15 @@ Transform OdometryOpticalFlow::computeTransformStereo( cv::Mat newLeftFrame; // convert to grayscale - if(data.image().channels() > 1) + if(data.imageRaw().channels() > 1) { - cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY); + cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY); } else { - newLeftFrame = data.image().clone(); + newLeftFrame = data.imageRaw().clone(); } - cv::Mat newRightFrame = data.rightImage().clone(); + cv::Mat newRightFrame = data.depthOrRightRaw().clone(); std::vector newCorners; UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1); @@ -259,13 +264,16 @@ Transform OdometryOpticalFlow::computeTransformStereo( pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D( lastCornersKept[i], lastDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth()))) { //Add 3D correspondences! - lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); + lastPt3D = util3d::transformPoint(lastPt3D, data.stereoCameraModel().left().localTransform()); objectPoints[oi].x = lastPt3D.x; objectPoints[oi].y = lastPt3D.y; objectPoints[oi].z = lastPt3D.z; @@ -278,11 +286,14 @@ Transform OdometryOpticalFlow::computeTransformStereo( pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D( newCornersKept[i], newDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); if(pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))) { - image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform()); + image3DPoints[oi] = util3d::transformPoint(newPt3D, data.stereoCameraModel().left().localTransform()); } } @@ -313,11 +324,8 @@ Transform OdometryOpticalFlow::computeTransformStereo( if(correspondences >= this->getMinInliers()) { //PnPRansac - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fx(), data.cy(), - 0, 0, 1); - Transform guess = (data.localTransform()).inverse(); + cv::Mat K = data.stereoCameraModel().left().K(); + Transform guess = (data.stereoCameraModel().left().localTransform()).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), @@ -348,7 +356,7 @@ Transform OdometryOpticalFlow::computeTransformStereo( R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); // make it incremental - output = (data.localTransform() * pnp).inverse(); + output = (data.stereoCameraModel().left().localTransform() * pnp).inverse(); UDEBUG("Odom transform = %s", output.prettyPrint().c_str()); @@ -416,18 +424,24 @@ Transform OdometryOpticalFlow::computeTransformStereo( pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D( lastCornersKept[i], lastDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D( newCornersKept[i], newDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) && pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))) { //Add 3D correspondences! - lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); - newPt3D = util3d::transformPoint(newPt3D, data.localTransform()); + lastPt3D = util3d::transformPoint(lastPt3D, data.stereoCameraModel().left().localTransform()); + newPt3D = util3d::transformPoint(newPt3D, data.stereoCameraModel().left().localTransform()); correspondencesLast->at(oi) = lastPt3D; correspondencesNew->at(oi) = newPt3D; if(this->isInfoDataFilled() && info) @@ -566,8 +580,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD( const SensorData & data, OdometryInfo * info) { - UTimer timer; Transform output; + if(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()) + { + UERROR("Calibrated camera required (multi-cameras not supported)."); + return output; + } + const CameraModel & cameraModel = data.cameraModels()[0]; + UTimer timer; double variance = 0; int inliers = 0; @@ -575,13 +595,13 @@ Transform OdometryOpticalFlow::computeTransformRGBD( cv::Mat newFrame; // convert to grayscale - if(data.image().channels() > 1) + if(data.imageRaw().channels() > 1) { - cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY); + cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY); } else { - newFrame = data.image().clone(); + newFrame = data.imageRaw().clone(); } std::vector newCorners; @@ -634,18 +654,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD( // new 3D points, used to compute variance image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point); - if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) && - uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows))) + if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) && + uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows))) { - pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, - data.cx(), data.cy(), data.fx(), data.fy(), true); + pcl::PointXYZ pt = util3d::projectDepthTo3D( + data.depthOrRightRaw(), + newCorners[i].x, + newCorners[i].y, + cameraModel.cx(), + cameraModel.cy(), + cameraModel.fx(), + cameraModel.fy(), + true); if(pcl::isFinite(pt) && (this->getMaxDepth() == 0.0f || ( uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) { - image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform()); + image3DPoints[oi] = util3d::transformPoint(pt, cameraModel.localTransform()); } } @@ -676,11 +703,8 @@ Transform OdometryOpticalFlow::computeTransformRGBD( if(correspondences >= this->getMinInliers()) { //PnPRansac - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fy(), data.cy(), - 0, 0, 1); - Transform guess = (data.localTransform()).inverse(); + cv::Mat K = cameraModel.K(); + Transform guess = (cameraModel.localTransform()).inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), @@ -711,7 +735,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD( R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); // make it incremental - output = (data.localTransform() * pnp).inverse(); + output = (cameraModel.localTransform() * pnp).inverse(); UDEBUG("Odom transform = %s", output.prettyPrint().c_str()); @@ -771,18 +795,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD( for(unsigned int i=0; iat(i)) && - uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) && - uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows))) + uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) && + uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows))) { - pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, - data.cx(), data.cy(), data.fx(), data.fy(), true); + pcl::PointXYZ pt = util3d::projectDepthTo3D( + data.depthOrRightRaw(), + newCorners[i].x, + newCorners[i].y, + cameraModel.cx(), + cameraModel.cy(), + cameraModel.fx(), + cameraModel.fy(), + true); if(pcl::isFinite(pt) && (this->getMaxDepth() == 0.0f || ( uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) { - pt = util3d::transformPoint(pt, data.localTransform()); + pt = util3d::transformPoint(pt, cameraModel.localTransform()); correspondencesLast->at(oi) = refCorners3D_->at(i); correspondencesNew->at(oi) = pt; @@ -867,7 +898,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD( std::vector newKtps; cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios()); newKtps = feature2D_->generateKeypoints(newFrame, roi); - Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth()); + Feature2D::filterKeypointsByDepth(newKtps, data.depthOrRightRaw(), this->getMaxDepth()); if(newKtps.size()) { @@ -892,18 +923,25 @@ Transform OdometryOpticalFlow::computeTransformRGBD( int oi=0; for(unsigned int i=0; igetMaxDepth() == 0.0f || ( uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) { - pt = util3d::transformPoint(pt, data.localTransform()); + pt = util3d::transformPoint(pt, cameraModel.localTransform()); newCorners3D->at(oi) = pt; newCornersFiltered[oi] = newCorners[i]; ++oi; diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 23d7e2fe..860e2816 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -97,8 +97,9 @@ void OdometryThread::mainLoop() { OdometryInfo info; Transform pose = _odometry->process(data, &info); - data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed - this->post(new OdometryEvent(data, info)); + // a null pose notify that odometry could not be computed + double variance = info.variance>0?info.variance:1; + this->post(new OdometryEvent(data, pose, variance, variance, info)); } } @@ -106,7 +107,7 @@ void OdometryThread::addData(const SensorData & data) { if(dynamic_cast(_odometry) == 0) { - if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f) + if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) { ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?"); return; @@ -114,7 +115,7 @@ void OdometryThread::addData(const SensorData & data) } else { - if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f) + if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) { ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?"); return; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index a3a9f032..4f2788c8 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -763,7 +763,10 @@ void Rtabmap::resetMemory() //============================================================ // MAIN LOOP //============================================================ -bool Rtabmap::process(const SensorData & data) +bool Rtabmap::process( + const SensorData & data, + const Transform & odomPose, + const cv::Mat & covariance) { UDEBUG(""); @@ -821,11 +824,6 @@ bool Rtabmap::process(const SensorData & data) // Wait for an image... //============================================================ ULOGGER_INFO("getting data..."); - if(!data.isValid()) - { - ULOGGER_INFO("image is not valid..."); - return false; - } timer.start(); timerTotal.start(); @@ -839,7 +837,7 @@ bool Rtabmap::process(const SensorData & data) //============================================================ if(_rgbdSlamMode) { - if(data.pose().isNull()) + if(odomPose.isNull()) { UERROR("RGB-D SLAM mode is enabled and no odometry is provided. " "Image %d is ignored!", data.id()); @@ -853,7 +851,7 @@ bool Rtabmap::process(const SensorData & data) const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry // look for identity - if(!lastPose.isIdentity() && data.pose().isIdentity()) + if(!lastPose.isIdentity() && odomPose.isIdentity()) { int mapId = triggerNewMap(); UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId); @@ -861,7 +859,7 @@ bool Rtabmap::process(const SensorData & data) else if(_newMapOdomChangeDistance > 0.0) { // look for large change - Transform lastPoseToNewPose = lastPose.inverse() * data.pose(); + Transform lastPoseToNewPose = lastPose.inverse() * odomPose; float x,y,z, roll,pitch,yaw; lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance) @@ -871,7 +869,7 @@ bool Rtabmap::process(const SensorData & data) _newMapOdomChangeDistance, mapId, lastPose.prettyPrint().c_str(), - data.pose().prettyPrint().c_str()); + odomPose.prettyPrint().c_str()); } } } @@ -884,16 +882,14 @@ bool Rtabmap::process(const SensorData & data) ULOGGER_INFO("Updating memory..."); if(_rgbdSlamMode) { - if(!_memory->update(data, &statistics_)) + if(!_memory->update(data, odomPose, covariance, &statistics_)) { return false; } } else { - SensorData dataWithoutOdom = data; - dataWithoutOdom.setPose(Transform(), 1, 1); - if(!_memory->update(dataWithoutOdom, &statistics_)) + if(!_memory->update(data, Transform(), cv::Mat(), &statistics_)) { return false; } @@ -905,6 +901,7 @@ bool Rtabmap::process(const SensorData & data) { UFATAL("Not supposed to be here...last signature is null?!?"); } + ULOGGER_INFO("Processing signature %d", signature->id()); timeMemoryUpdate = timer.ticks(); ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate); @@ -956,7 +953,7 @@ bool Rtabmap::process(const SensorData & data) //============================================================ if(_poseScanMatching && signature->getLinks().size() == 1 && - !signature->getLaserScanCompressed().empty() && + !signature->sensorData().laserScanCompressed().empty() && rehearsedId == 0) // don't do it if rehearsal happened { UINFO("Odometry correction by scan matching"); @@ -1039,7 +1036,7 @@ bool Rtabmap::process(const SensorData & data) *iter, transform.prettyPrint().c_str()); // Add a loop constraint - if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance)) + if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance))) { ++localLoopClosuresInTimeFound; UINFO("Local loop closure found between %d and %d with t=%s", @@ -1584,16 +1581,13 @@ bool Rtabmap::process(const SensorData & data) // Add signatures SensorData dataFrom = data; dataFrom.setId(signature->id()); - Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true); - SensorData dataTo = tmpTo.toSensorData(); + SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true); UDEBUG("timeTo = %fs", timeT.ticks()); - if(dataFrom.isValid() && - dataFrom.isMetric() && - dataTo.isValid() && - dataTo.isMetric() && + if(!dataFrom.depthOrRightRaw().empty() && + !dataTo.depthOrRightRaw().empty() && dataFrom.id() != Memory::kIdInvalid && - tmpTo.id() != Memory::kIdInvalid) + dataTo.id() != Memory::kIdInvalid) { memory.update(dataTo); UDEBUG("timeUpTo = %fs", timeT.ticks()); @@ -1629,7 +1623,7 @@ bool Rtabmap::process(const SensorData & data) if(!rejectedHypothesis) { // Make the new one the parent of the old one - rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance); + rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance)); } if(rejectedHypothesis) @@ -1743,16 +1737,13 @@ bool Rtabmap::process(const SensorData & data) // Add signatures SensorData dataFrom = data; dataFrom.setId(signature->id()); - Signature tmpTo = _memory->getSignatureData(nearestId, true); - SensorData dataTo = tmpTo.toSensorData(); + SensorData dataTo = _memory->getNodeData(nearestId, true); UDEBUG("timeTo = %fs", timeT.ticks()); - if(dataFrom.isValid() && - dataFrom.isMetric() && - dataTo.isValid() && - dataTo.isMetric() && + if(!dataFrom.depthOrRightRaw().empty() && + !dataTo.depthOrRightRaw().empty() && dataFrom.id() != Memory::kIdInvalid && - tmpTo.id() != Memory::kIdInvalid) + dataTo.id() != Memory::kIdInvalid) { memory.update(dataTo); UDEBUG("timeUpTo = %fs", timeT.ticks()); @@ -1784,7 +1775,7 @@ bool Rtabmap::process(const SensorData & data) signature->id(), nearestId, transform.prettyPrint().c_str()); - _memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance); + _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); if(_loopClosureHypothesis.first == 0) { @@ -1802,7 +1793,7 @@ bool Rtabmap::process(const SensorData & data) // // 2) compare locally with nearest locations by scan matching // - if( !signature->getLaserScanCompressed().empty() && + if( !signature->sensorData().laserScanCompressed().empty() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0)) { // In localization mode, no need to check local loop @@ -1873,7 +1864,7 @@ bool Rtabmap::process(const SensorData & data) nearestId, transform.prettyPrint().c_str()); // set Identify covariance for laser scan matching only - _memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1); + _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1)); ++localSpaceClosuresAddedByICPOnly; @@ -1913,6 +1904,7 @@ bool Rtabmap::process(const SensorData & data) UINFO("Update map correction: SLAM mode"); // SLAM mode! optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints); + UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()); // Update map correction, it should be identify when optimizing from the last node _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); @@ -1964,7 +1956,7 @@ bool Rtabmap::process(const SensorData & data) Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first); if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius) { - _memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance + _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance } } } @@ -2344,7 +2336,7 @@ bool Rtabmap::process(const SensorData & data) bool Rtabmap::process(const cv::Mat & image, int id) { - return this->process(SensorData(image, id)); + return this->process(SensorData(image, id), Transform()); } // SETTERS @@ -2730,13 +2722,10 @@ void Rtabmap::dumpPrediction() const } } -void Rtabmap::get3DMap(std::map & signatures, +void Rtabmap::get3DMap( + std::map & signatures, std::map & poses, std::multimap & constraints, - std::map & mapIds, - std::map & stamps, - std::map & labels, - std::map > & userDatas, bool optimized, bool global) const { @@ -2762,22 +2751,6 @@ void Rtabmap::get3DMap(std::map & signatures, _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - Transform odomPose; - int weight = -1; - int mapId = -1; - std::string label; - double stamp = 0; - std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true); - mapIds.insert(std::make_pair(iter->first, mapId)); - stamps.insert(std::make_pair(iter->first, stamp)); - labels.insert(std::make_pair(iter->first, label)); - userDatas.insert(std::make_pair(iter->first, userData)); - } - - // Get data std::set ids = uKeysSet(_memory->getWorkingMem()); // WM @@ -2792,11 +2765,26 @@ void Rtabmap::get3DMap(std::map & signatures, for(std::set::iterator iter = ids.begin(); iter!=ids.end(); ++iter) { - Signature data = _memory->getSignatureData(*iter); - if(data.id() != Memory::kIdInvalid) - { - signatures.insert(std::make_pair(*iter, Signature())).first->second = data; - } + Transform odomPose; + int weight = -1; + int mapId = -1; + std::string label; + double stamp = 0; + std::vector userData; + _memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true); + SensorData data = _memory->getNodeData(*iter); + data.setId(*iter); + signatures.insert(std::make_pair(*iter, + Signature(*iter, + mapId, + weight, + stamp, + label, + std::multimap(), + std::multimap(), + odomPose, + userData, + data))); } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1)) @@ -2812,12 +2800,9 @@ void Rtabmap::get3DMap(std::map & signatures, void Rtabmap::getGraph( std::map & poses, std::multimap & constraints, - std::map & mapIds, - std::map & stamps, - std::map & labels, - std::map > & userDatas, bool optimized, - bool global) + bool global, + std::map * signatures) { if(_memory && _memory->getLastWorkingSignature()) { @@ -2840,19 +2825,29 @@ void Rtabmap::getGraph( _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + if(signatures) { - Transform odomPose; - int weight = -1; - int mapId = -1; - std::string label; - double stamp = 0; - std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true); - mapIds.insert(std::make_pair(iter->first, mapId)); - stamps.insert(std::make_pair(iter->first, stamp)); - labels.insert(std::make_pair(iter->first, label)); - userDatas.insert(std::make_pair(iter->first, userData)); + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + Transform odomPose; + int weight = -1; + int mapId = -1; + std::string label; + double stamp = 0; + std::vector userData; + _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true); + signatures->insert(std::make_pair(iter->first, + Signature(iter->first, + mapId, + weight, + stamp, + label, + std::multimap(), + std::multimap(), + odomPose, + userData, + SensorData()))); + } } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size())) @@ -3002,11 +2997,7 @@ bool Rtabmap::computePath(int targetNode, bool global) UTimer timer; std::map nodes; std::multimap constraints; - std::map mapIds; - std::map stamps; - std::map labels; - std::map > userDatas; - this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global); + this->getGraph(nodes, constraints, true, global); UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks()); if(computePath(targetNode, nodes, constraints)) @@ -3037,7 +3028,7 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global) std::map stamps; std::map labels; std::map > userDatas; - this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global); + this->getGraph(nodes, constraints, true, global); UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks()); int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); @@ -3189,7 +3180,7 @@ void Rtabmap::updateGoalIndex() if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) { Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; - _memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance + _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); } } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index fd262aba..3f24e0ac 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -125,20 +125,13 @@ void RtabmapThread::publishMap(bool optimized, bool full) const _rtabmap->get3DMap(signatures, poses, constraints, - mapIds, - stamps, - labels, - userDatas, optimized, full); - this->post(new RtabmapEvent3DMap(signatures, + this->post(new RtabmapEvent3DMap( + signatures, poses, - constraints, - mapIds, - stamps, - labels, - userDatas)); + constraints)); } void RtabmapThread::publishTOROGraph(bool optimized, bool full) const @@ -153,20 +146,14 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const _rtabmap->getGraph(poses, constraints, - mapIds, - stamps, - labels, - userDatas, optimized, - full); + full, + &signatures); - this->post(new RtabmapEvent3DMap(signatures, + this->post(new RtabmapEvent3DMap( + signatures, poses, - constraints, - mapIds, - stamps, - labels, - userDatas)); + constraints)); } @@ -301,7 +288,7 @@ void RtabmapThread::handleEvent(UEvent* event) CameraEvent * e = (CameraEvent*)event; if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth) { - this->addData(e->data()); + this->addData(OdometryEvent(e->data(), Transform(), 1, 1)); } } else if(event->getClassName().compare("OdometryEvent") == 0) @@ -310,7 +297,7 @@ void RtabmapThread::handleEvent(UEvent* event) OdometryEvent * e = (OdometryEvent*)event; if(e->isValid()) { - this->addData(e->data()); + this->addData(*e); } else { @@ -487,13 +474,13 @@ void RtabmapThread::handleEvent(UEvent* event) //============================================================ void RtabmapThread::process() { - SensorData data; + OdometryEvent data; getData(data); if(data.isValid() && _state.empty()) { if(_rtabmap->getMemory()) { - if(_rtabmap->process(data)) + if(_rtabmap->process(data.data(), data.pose(), data.covariance())) { Statistics stats = _rtabmap->getStatistics(); stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size()); @@ -508,11 +495,11 @@ void RtabmapThread::process() } } -void RtabmapThread::addData(const SensorData & sensorData) +void RtabmapThread::addData(const OdometryEvent & odomEvent) { if(!_paused) { - if(!sensorData.isValid()) + if(!odomEvent.isValid()) { ULOGGER_ERROR("data not valid !?"); return; @@ -522,7 +509,7 @@ void RtabmapThread::addData(const SensorData & sensorData) { if(_frameRateTimer->getElapsedTime() < 1.0f/_rate) { - if(!lastPose_.isIdentity() && sensorData.pose().isIdentity()) + if(!lastPose_.isIdentity() && odomEvent.pose().isIdentity()) { UWARN("Odometry is reset (identity pose detected). Increment map id!"); pushNewState(kStateTriggeringMap); @@ -533,7 +520,7 @@ void RtabmapThread::addData(const SensorData & sensorData) return; } } - if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity()) + if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity()) { UWARN("Odometry is reset (identity pose detected). Increment map id!"); pushNewState(kStateTriggeringMap); @@ -542,29 +529,30 @@ void RtabmapThread::addData(const SensorData & sensorData) } _frameRateTimer->start(); - lastPose_ = sensorData.pose(); - if(sensorData.poseRotVariance() > _rotVariance) + lastPose_ = odomEvent.pose(); + double maxRotVar = odomEvent.rotVariance(); + double maxTransVar = odomEvent.transVariance(); + if(maxRotVar > _rotVariance) { - _rotVariance = sensorData.poseRotVariance(); + _rotVariance = maxRotVar; } - if(sensorData.poseTransVariance() > _transVariance) + if(maxTransVar > _transVariance) { - _transVariance = sensorData.poseTransVariance(); + _transVariance = maxTransVar; } bool notify = true; _dataMutex.lock(); { - _dataBuffer.push_back(sensorData); if(_rotVariance <= 0) { - _rotVariance = 1.0f; + _rotVariance = 1.0; } if(_transVariance <= 0) { - _transVariance = 1.0f; + _transVariance = 1.0; } - _dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance); + _dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance)); _rotVariance = 0; _transVariance = 0; while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize) @@ -583,7 +571,7 @@ void RtabmapThread::addData(const SensorData & sensorData) } } -void RtabmapThread::getData(SensorData & image) +void RtabmapThread::getData(OdometryEvent & data) { ULOGGER_DEBUG(""); @@ -595,7 +583,7 @@ void RtabmapThread::getData(SensorData & image) { if(!_dataBuffer.empty()) { - image = _dataBuffer.front(); + data = _dataBuffer.front(); _dataBuffer.pop_front(); } } diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 947b173a..96e4e303 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -27,138 +27,417 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/SensorData.h" +#include "rtabmap/core/Compression.h" #include "rtabmap/utilite/ULogger.h" #include namespace rtabmap { -/** - * An id is automatically generated if id=0. - */ +// empty constructor SensorData::SensorData() : - _id(0), - _stamp(0.0), - _fx(0.0f), - _fyOrBaseline(0.0f), - _cx(0.0f), - _cy(0.0f), - _localTransform(Transform::getIdentity()), - _poseRotVariance(1.0f), - _poseTransVariance(1.0f), - _laserScanMaxPts(0) + _id(0), + _stamp(0.0), + _laserScanMaxPts(0) { } -SensorData::SensorData(const cv::Mat & image, - int id, - double stamp, - const std::vector & userData) : - _image(image), - _id(id), - _stamp(stamp), - _fx(0.0f), - _fyOrBaseline(0.0f), - _cx(0.0f), - _cy(0.0f), - _localTransform(Transform::getIdentity()), - _poseRotVariance(1.0f), - _poseTransVariance(1.0f), - _laserScanMaxPts(0), - _userData(userData) +// Appearance-only constructor +SensorData::SensorData( + const cv::Mat & image, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(0), + _userData(userData) { - UASSERT(image.empty() || - image.type() == CV_8UC1 || // Mono - image.type() == CV_8UC3); // RGB + if(image.rows == 1) + { + UASSERT(image.type() == CV_8UC1); // Bytes + _imageCompressed = image; + } + else if(!image.empty()) + { + UASSERT(image.type() == CV_8UC1 || // Mono + image.type() == CV_8UC3); // RGB + _imageRaw = image; + } } - // Metric constructor -SensorData::SensorData(const cv::Mat & image, - const cv::Mat & depthOrRightImage, - float fx, - float fyOrBaseline, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float poseRotVariance, - float poseTransVariance, - int id, - double stamp, - const std::vector & userData) : - _image(image), - _id(id), - _stamp(stamp), - _depthOrRightImage(depthOrRightImage), - _fx(fx), - _fyOrBaseline(fyOrBaseline), - _cx(cx), - _cy(cy), - _pose(pose), - _localTransform(localTransform), - _poseRotVariance(poseRotVariance), - _poseTransVariance(poseTransVariance), - _laserScanMaxPts(0), - _userData(userData) +// Mono constructor +SensorData::SensorData( + const cv::Mat & image, + const CameraModel & cameraModel, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(0), + _cameraModels(std::vector(1, cameraModel)), + _userData(userData) { - UASSERT(image.empty() || - image.type() == CV_8UC1 || // Mono - image.type() == CV_8UC3); // RGB - UASSERT(depthOrRightImage.empty() || - depthOrRightImage.type() == CV_32FC1 || // Depth in meter - depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre - depthOrRightImage.type() == CV_8U); // Right stereo image - UASSERT(!_localTransform.isNull()); - UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); + if(image.rows == 1) + { + UASSERT(image.type() == CV_8UC1); // Bytes + _imageCompressed = image; + } + else if(!image.empty()) + { + UASSERT(image.type() == CV_8UC1 || // Mono + image.type() == CV_8UC3); // RGB + _imageRaw = image; + } } - // Metric constructor + 2d depth -SensorData::SensorData(const cv::Mat & laserScan, - int laserScanMaxPts, - const cv::Mat & image, - const cv::Mat & depthOrRightImage, - float fx, - float fyOrBaseline, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float poseRotVariance, - float poseTransVariance, - int id, - double stamp, - const std::vector & userData) : - _image(image), - _id(id), - _stamp(stamp), - _depthOrRightImage(depthOrRightImage), - _laserScan(laserScan), - _fx(fx), - _fyOrBaseline(fyOrBaseline), - _cx(cx), - _cy(cy), - _pose(pose), - _localTransform(localTransform), - _poseRotVariance(poseRotVariance), - _poseTransVariance(poseTransVariance), - _laserScanMaxPts(laserScanMaxPts), - _userData(userData) +// RGB-D constructor +SensorData::SensorData( + const cv::Mat & rgb, + const cv::Mat & depth, + const CameraModel & cameraModel, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(0), + _cameraModels(std::vector(1, cameraModel)), + _userData(userData) { - UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2); - UASSERT(image.empty() || - image.type() == CV_8UC1 || // Mono - image.type() == CV_8UC3); // RGB - UASSERT(depthOrRightImage.empty() || - depthOrRightImage.type() == CV_32FC1 || // Depth in meter - depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre - depthOrRightImage.type() == CV_8U); // Right stereo image - UASSERT(!_localTransform.isNull()); - UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); + if(rgb.rows == 1) + { + UASSERT(rgb.type() == CV_8UC1); // Bytes + _imageCompressed = rgb; + } + else if(!rgb.empty()) + { + UASSERT(rgb.type() == CV_8UC1 || // Mono + rgb.type() == CV_8UC3); // RGB + _imageRaw = rgb; + } + + if(depth.rows == 1) + { + UASSERT(depth.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = depth; + } + else if(!depth.empty()) + { + UASSERT(depth.type() == CV_32FC1 || // Depth in meter + depth.type() == CV_16UC1); // Depth in millimetre + _depthOrRightRaw = depth; + } } -bool SensorData::empty() const +// RGB-D constructor + 2d laser scan +SensorData::SensorData( + const cv::Mat & laserScan, + int laserScanMaxPts, + const cv::Mat & rgb, + const cv::Mat & depth, + const CameraModel & cameraModel, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(laserScanMaxPts), + _cameraModels(std::vector(1, cameraModel)), + _userData(userData) { - return _image.empty(); + if(rgb.rows == 1) + { + UASSERT(rgb.type() == CV_8UC1); // Bytes + _imageCompressed = rgb; + } + else if(!rgb.empty()) + { + UASSERT(rgb.type() == CV_8UC1 || // Mono + rgb.type() == CV_8UC3); // RGB + _imageRaw = rgb; + } + if(depth.rows == 1) + { + UASSERT(depth.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = depth; + } + else if(!depth.empty()) + { + UASSERT(depth.type() == CV_32FC1 || // Depth in meter + depth.type() == CV_16UC1); // Depth in millimetre + _depthOrRightRaw = depth; + } + if(laserScan.rows == 1) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_32FC2); + _laserScanRaw = laserScan; + } +} + +// Multi-cameras RGB-D constructor +SensorData::SensorData( + const cv::Mat & rgb, + const cv::Mat & depth, + const std::vector & cameraModels, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(0), + _cameraModels(cameraModels), + _userData(userData) +{ + if(rgb.rows == 1) + { + UASSERT(rgb.type() == CV_8UC1); // Bytes + _imageCompressed = rgb; + } + else if(!rgb.empty()) + { + UASSERT(rgb.type() == CV_8UC1 || // Mono + rgb.type() == CV_8UC3); // RGB + _imageRaw = rgb; + } + if(depth.rows == 1) + { + UASSERT(depth.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = depth; + } + else if(!depth.empty()) + { + UASSERT(depth.type() == CV_32FC1 || // Depth in meter + depth.type() == CV_16UC1); // Depth in millimetre + _depthOrRightRaw = depth; + } + for(unsigned int i=0; i & cameraModels, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(laserScanMaxPts), + _cameraModels(cameraModels), + _userData(userData) +{ + if(rgb.rows == 1) + { + UASSERT(rgb.type() == CV_8UC1); // Bytes + _imageCompressed = rgb; + } + else if(!rgb.empty()) + { + UASSERT(rgb.type() == CV_8UC1 || // Mono + rgb.type() == CV_8UC3); // RGB + _imageRaw = rgb; + } + if(depth.rows == 1) + { + UASSERT(depth.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = depth; + } + else if(!depth.empty()) + { + UASSERT(depth.type() == CV_32FC1 || // Depth in meter + depth.type() == CV_16UC1); // Depth in millimetre + _depthOrRightRaw = depth; + } + + if(laserScan.rows == 1) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_32FC2); + _laserScanRaw = laserScan; + } + + for(unsigned int i=0; i & userData): + _id(id), + _stamp(stamp), + _laserScanMaxPts(0), + _stereoCameraModel(cameraModel), + _userData(userData) +{ + if(left.rows == 1) + { + UASSERT(left.type() == CV_8UC1); // Bytes + _imageCompressed = left; + } + else if(!left.empty()) + { + UASSERT(left.type() == CV_8UC1 || // Mono + left.type() == CV_8UC3); // RGB + _imageRaw = left; + } + if(right.rows == 1) + { + UASSERT(right.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = right; + } + else if(!right.empty()) + { + UASSERT(right.type() == CV_8UC1); // Mono + _depthOrRightRaw = right; + } + +} + +// Stereo constructor + 2d laser scan +SensorData::SensorData( + const cv::Mat & laserScan, + int laserScanMaxPts, + const cv::Mat & left, + const cv::Mat & right, + const StereoCameraModel & cameraModel, + int id, + double stamp, + const std::vector & userData) : + _id(id), + _stamp(stamp), + _laserScanMaxPts(laserScanMaxPts), + _stereoCameraModel(cameraModel), + _userData(userData) +{ + if(left.rows == 1) + { + UASSERT(left.type() == CV_8UC1); // Bytes + _imageCompressed = left; + } + else if(!left.empty()) + { + UASSERT(left.type() == CV_8UC1 || // Mono + left.type() == CV_8UC3); // RGB + _imageRaw = left; + } + if(right.rows == 1) + { + UASSERT(right.type() == CV_8UC1); // Bytes + _depthOrRightCompressed = right; + } + else if(!right.empty()) + { + UASSERT(right.type() == CV_8UC1); // Mono + _depthOrRightRaw = right; + } + + if(laserScan.rows == 1) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_32FC2); + _laserScanRaw = laserScan; + } +} + +void SensorData::uncompressData() +{ + uncompressData(&_imageRaw, &_depthOrRightRaw, &_laserScanRaw); +} + +void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) +{ + uncompressDataConst(imageRaw, depthRaw, laserScanRaw); + if(imageRaw && !imageRaw->empty() && _imageRaw.empty()) + { + _imageRaw = *imageRaw; + } + if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty()) + { + _depthOrRightRaw = *depthRaw; + } + if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty()) + { + _laserScanRaw = *laserScanRaw; + } +} + +void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const +{ + if(imageRaw) + { + *imageRaw = _imageRaw; + } + if(depthRaw) + { + *depthRaw = _depthOrRightRaw; + } + if(laserScanRaw) + { + *laserScanRaw = _laserScanRaw; + } + if( (imageRaw && imageRaw->empty()) || + (depthRaw && depthRaw->empty()) || + (laserScanRaw && laserScanRaw->empty())) + { + rtabmap::CompressionThread ctImage(_imageCompressed, true); + rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true); + rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false); + if(imageRaw && imageRaw->empty()) + { + ctImage.start(); + } + if(depthRaw && depthRaw->empty()) + { + ctDepth.start(); + } + if(laserScanRaw && laserScanRaw->empty()) + { + ctLaserScan.start(); + } + ctImage.join(); + ctDepth.join(); + ctLaserScan.join(); + if(imageRaw && imageRaw->empty()) + { + *imageRaw = ctImage.getUncompressedData(); + } + if(depthRaw && depthRaw->empty()) + { + *depthRaw = ctDepth.getUncompressedData(); + } + if(laserScanRaw && laserScanRaw->empty()) + { + *laserScanRaw = ctLaserScan.getUncompressedData(); + } + } } } // namespace rtabmap diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index f6424e69..371d1941 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -44,12 +44,7 @@ Signature::Signature() : _saved(false), _modified(true), _linksModified(true), - _enabled(false), - _fx(0.0f), - _fy(0.0f), - _cx(0.0f), - _cy(0.0f), - _laserScanMaxPts(0) + _enabled(false) { } @@ -63,15 +58,7 @@ Signature::Signature( const std::multimap & words3, // in base_link frame (localTransform applied) const Transform & pose, const std::vector & userData, - 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, - float fy, - float cx, - float cy, - const Transform & localTransform, - int laserScanMaxPts) : + const SensorData & sensorData) : _id(id), _mapId(mapId), _stamp(stamp), @@ -82,18 +69,10 @@ Signature::Signature( _modified(true), _linksModified(true), _words(words), - _enabled(false), - _imageCompressed(imageCompressed), - _depthCompressed(depthCompressed), - _laserScanCompressed(laserScanCompressed), - _fx(fx), - _fy(fy), - _cx(cx), - _cy(cy), - _pose(pose), - _localTransform(localTransform), _words3(words3), - _laserScanMaxPts(laserScanMaxPts) + _enabled(false), + _pose(pose), + _sensorData(sensorData) { } @@ -239,25 +218,9 @@ void Signature::removeWord(int wordId) _words3.erase(wordId); } -void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy) +cv::Mat Signature::getPoseCovariance() const { - UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str()); - _depthCompressed = bytes; - _fx=fx; - _fy=fy; - _cx=cx; - _cy=cy; -} - -float Signature::getDepthFx() const {return getFx();} -float Signature::getDepthFy() const {return getFy();} -float Signature::getDepthCx() const {return getCx();} -float Signature::getDepthCy() const {return getCy();} - -void Signature::getPoseVariance(float & rotVariance, float & transVariance) const -{ - rotVariance = 1.0f; - transVariance = 1.0f; + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(_links.size()) { for(std::map::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter) @@ -267,110 +230,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons //Assume the first neighbor to be the backward neighbor link if(iter->second.to() < iter->second.from()) { - rotVariance = iter->second.rotVariance(); - transVariance = iter->second.transVariance(); + covariance = iter->second.infMatrix().inv(); break; } } } } -} - -SensorData Signature::toSensorData() -{ - this->uncompressData(); - float rotVariance = 1.0f; - float transVariance = 1.0f; - this->getPoseVariance(rotVariance, transVariance); - - return SensorData(_laserScanRaw, - _laserScanMaxPts, - _imageRaw, - _depthRaw, - _fx, - _fy, - _cx, - _cy, - _localTransform, - _pose, - rotVariance, - transVariance, - _id, - _stamp, - _userData); -} - -void Signature::uncompressData() -{ - uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw); -} - -void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) -{ - uncompressDataConst(imageRaw, depthRaw, laserScanRaw); - if(imageRaw && !imageRaw->empty() && _imageRaw.empty()) - { - _imageRaw = *imageRaw; - } - if(depthRaw && !depthRaw->empty() && _depthRaw.empty()) - { - _depthRaw = *depthRaw; - } - if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty()) - { - _laserScanRaw = *laserScanRaw; - } -} - -void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const -{ - if(imageRaw) - { - *imageRaw = _imageRaw; - } - if(depthRaw) - { - *depthRaw = _depthRaw; - } - if(laserScanRaw) - { - *laserScanRaw = _laserScanRaw; - } - if( (imageRaw && imageRaw->empty()) || - (depthRaw && depthRaw->empty()) || - (laserScanRaw && laserScanRaw->empty())) - { - rtabmap::CompressionThread ctImage(_imageCompressed, true); - rtabmap::CompressionThread ctDepth(_depthCompressed, true); - rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false); - if(imageRaw && imageRaw->empty()) - { - ctImage.start(); - } - if(depthRaw && depthRaw->empty()) - { - ctDepth.start(); - } - if(laserScanRaw && laserScanRaw->empty()) - { - ctLaserScan.start(); - } - ctImage.join(); - ctDepth.join(); - ctLaserScan.join(); - if(imageRaw && imageRaw->empty()) - { - *imageRaw = ctImage.getUncompressedData(); - } - if(depthRaw && depthRaw->empty()) - { - *depthRaw = ctDepth.getUncompressedData(); - } - if(laserScanRaw && laserScanRaw->empty()) - { - *laserScanRaw = ctLaserScan.getUncompressedData(); - } - } + return covariance; } } //namespace rtabmap diff --git a/corelib/src/resources/DatabaseSchema.sql.in b/corelib/src/resources/DatabaseSchema.sql.in index d324362d..e9a5f27e 100644 --- a/corelib/src/resources/DatabaseSchema.sql.in +++ b/corelib/src/resources/DatabaseSchema.sql.in @@ -25,24 +25,13 @@ CREATE TABLE Node ( PRIMARY KEY (id) ); -CREATE TABLE Image ( +CREATE TABLE Data ( id INTEGER NOT NULL, - data BLOB, -- compressed image (RGB) - time_enter DATE, - PRIMARY KEY (id) -); - --- TODO: Merge "Image" and "Depth" tables to "Data" table. -CREATE TABLE Depth ( - id INTEGER NOT NULL, - data BLOB, -- compressed image (Depth or Right image) - fx FLOAT, - fy FLOAT, -- baseline if stereo - cx FLOAT, - cy FLOAT, - local_transform BLOB, - data2d BLOB, -- compressed data (Laser scan) - data2d_max_pts INTEGER, -- Laser scan max points + image BLOB, -- compressed image (Grayscale or RGB) + depth BLOB, -- compressed image (Depth or Right image) + calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform + scan BLOB, -- compressed data (Laser scan) + scan_max_pts INTEGER, -- Laser scan max points time_enter DATE, PRIMARY KEY (id) ); diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 906d09fe..30905df9 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -482,6 +483,246 @@ pcl::PointCloud::Ptr cloudFromStereoImages( decimation); } +pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( + const SensorData & sensorData, + int decimation, + float maxDepth, + float voxelSize, + int samples) +{ + pcl::PointCloud::Ptr cloud; + + if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) + { + //depth + UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols); + int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); + cloud.reset(new pcl::PointCloud); + for(unsigned int i=0; i::Ptr tmp = util3d::cloudFromDepth( + cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), + sensorData.cameraModels()[i].cx(), + sensorData.cameraModels()[i].cy(), + sensorData.cameraModels()[i].fx(), + sensorData.cameraModels()[i].fy(), + decimation); + + if(tmp->size()) + { + bool filtered = false; + if(tmp->size() && maxDepth) + { + tmp = util3d::passThrough(tmp, "z", 0, maxDepth); + filtered = true; + } + + if(tmp->size() && voxelSize) + { + tmp = util3d::voxelize(tmp, voxelSize); + filtered = true; + } + + if(tmp->size() && samples) + { + tmp = util3d::sampling(tmp, samples); + filtered = true; + } + + if(tmp->size() && !filtered) + { + tmp = util3d::removeNaNFromPointCloud(tmp); + } + + if(tmp->size()) + { + tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform()); + } + + *cloud += *tmp; + } + } + else + { + UERROR("Camera model %d is invalid", i); + } + } + + if(cloud->size() && voxelSize) + { + cloud = util3d::voxelize(cloud, voxelSize); + } + } + else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) + { + //stereo + UASSERT(sensorData.rightRaw().type() == CV_8UC1); + + cv::Mat leftMono; + if(sensorData.imageRaw().channels() == 3) + { + cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY); + } + else + { + leftMono = sensorData.imageRaw(); + } + return cloudFromDisparity( + util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()), + sensorData.stereoCameraModel().left().cx(), + sensorData.stereoCameraModel().left().cy(), + sensorData.stereoCameraModel().left().fx(), + sensorData.stereoCameraModel().baseline(), + decimation); + + if(cloud->size()) + { + bool filtered = false; + if(cloud->size() && maxDepth) + { + cloud = util3d::passThrough(cloud, "z", 0, maxDepth); + filtered = true; + } + + if(cloud->size() && voxelSize) + { + cloud = util3d::voxelize(cloud, voxelSize); + filtered = true; + } + + if(cloud->size() && !filtered) + { + cloud = util3d::removeNaNFromPointCloud(cloud); + } + + if(cloud->size()) + { + cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); + } + } + } + return cloud; +} + +pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( + const SensorData & sensorData, + int decimation, + float maxDepth, + float voxelSize, + int samples) +{ + pcl::PointCloud::Ptr cloud; + + if(!sensorData.imageRaw().empty()) + { + if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) + { + //depth + UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols); + UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size()); + int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size(); + cloud.reset(new pcl::PointCloud); + for(unsigned int i=0; i::Ptr tmp = util3d::cloudFromDepthRGB( + cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)), + cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)), + sensorData.cameraModels()[i].cx(), + sensorData.cameraModels()[i].cy(), + sensorData.cameraModels()[i].fx(), + sensorData.cameraModels()[i].fy(), + decimation); + + if(tmp->size()) + { + bool filtered = false; + if(tmp->size() && maxDepth) + { + tmp = util3d::passThrough(tmp, "z", 0, maxDepth); + filtered = true; + } + + if(tmp->size() && voxelSize) + { + tmp = util3d::voxelize(tmp, voxelSize); + filtered = true; + } + + if(tmp->size() && samples) + { + tmp = util3d::sampling(tmp, samples); + filtered = true; + } + + if(tmp->size() && !filtered) + { + tmp = util3d::removeNaNFromPointCloud(tmp); + } + + if(tmp->size()) + { + tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform()); + } + + *cloud += *tmp; + } + } + else + { + UERROR("Camera model %d is invalid", i); + } + } + + if(cloud->size() && voxelSize) + { + cloud = util3d::voxelize(cloud, voxelSize); + } + } + else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()) + { + //stereo + cloud = cloudFromStereoImages(sensorData.imageRaw(), + sensorData.rightRaw(), + sensorData.stereoCameraModel().left().cx(), + sensorData.stereoCameraModel().left().cy(), + sensorData.stereoCameraModel().left().fx(), + sensorData.stereoCameraModel().baseline(), + decimation); + + if(cloud->size()) + { + bool filtered = false; + if(cloud->size() && maxDepth) + { + cloud = util3d::passThrough(cloud, "z", 0, maxDepth); + filtered = true; + } + + if(cloud->size() && voxelSize) + { + cloud = util3d::voxelize(cloud, voxelSize); + filtered = true; + } + + if(cloud->size() && !filtered) + { + cloud = util3d::removeNaNFromPointCloud(cloud); + } + + if(cloud->size()) + { + cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform()); + } + } + } + } + return cloud; +} + // inspired from ROS image_geometry/src/stereo_camera_model.cpp pcl::PointXYZ projectDisparityTo3D( const cv::Point2f & pt, diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index 5352a7cf..999d5f22 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -44,36 +44,48 @@ namespace rtabmap namespace util3d { +pcl::PointCloud::Ptr generateKeypoints3DDepth( + const std::vector & keypoints, + const cv::Mat & depth, + const CameraModel & cameraModel) +{ + UASSERT(cameraModel.isValid()); + std::vector models; + models.push_back(cameraModel); + return generateKeypoints3DDepth(keypoints, depth, models); +} pcl::PointCloud::Ptr generateKeypoints3DDepth( const std::vector & keypoints, const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - const Transform & transform) + const std::vector & cameraModels) { UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1)); + UASSERT(cameraModels.size()); pcl::PointCloud::Ptr keypoints3d(new pcl::PointCloud); if(!depth.empty()) { + UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols); + float subImageWidth = depth.cols/cameraModels.size(); keypoints3d->resize(keypoints.size()); for(unsigned int i=0; i!=keypoints.size(); ++i) { + int cameraIndex = int(keypoints[i].pt.x / subImageWidth); + UASSERT(cameraIndex < (int)cameraModels.size()); pcl::PointXYZ pt = util3d::projectDepthTo3D( depth, - keypoints[i].pt.x, + keypoints[i].pt.x-subImageWidth*cameraIndex, keypoints[i].pt.y, - cx, - cy, - fx, - fy, + cameraModels.at(cameraIndex).cx(), + cameraModels.at(cameraIndex).cy(), + cameraModels.at(cameraIndex).fx(), + cameraModels.at(cameraIndex).fy(), true); - if(!transform.isNull() && !transform.isIdentity()) + if(!cameraModels.at(cameraIndex).localTransform().isNull() && + !cameraModels.at(cameraIndex).localTransform().isIdentity()) { - pt = util3d::transformPoint(pt, transform); + pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform()); } keypoints3d->at(i) = pt; } @@ -84,13 +96,10 @@ pcl::PointCloud::Ptr generateKeypoints3DDepth( pcl::PointCloud::Ptr generateKeypoints3DDisparity( const std::vector & keypoints, const cv::Mat & disparity, - float fx, - float baseline, - float cx, - float cy, - const Transform & transform) + const StereoCameraModel & stereoCameraModel) { UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F)); + UASSERT(stereoCameraModel.isValid()); pcl::PointCloud::Ptr keypoints3d(new pcl::PointCloud); keypoints3d->resize(keypoints.size()); for(unsigned int i=0; i!=keypoints.size(); ++i) @@ -98,14 +107,16 @@ pcl::PointCloud::Ptr generateKeypoints3DDisparity( pcl::PointXYZ pt = util3d::projectDisparityTo3D( keypoints[i].pt, disparity, - cx, - cy, - fx, - baseline); + stereoCameraModel.left().cx(), + stereoCameraModel.left().cy(), + stereoCameraModel.left().fx(), + stereoCameraModel.baseline()); - if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity()) + if(pcl::isFinite(pt) && + !stereoCameraModel.left().localTransform().isNull() && + !stereoCameraModel.left().localTransform().isIdentity()) { - pt = util3d::transformPoint(pt, transform); + pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform()); } keypoints3d->at(i) = pt; } @@ -116,11 +127,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( const std::vector & keypoints, const cv::Mat & leftImage, const cv::Mat & rightImage, - float fx, - float baseline, - float cx, - float cy, - const Transform & transform, + const StereoCameraModel & stereoCameraModel, int flowWinSize, int flowMaxLevel, int flowIterations, @@ -129,6 +136,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( UASSERT(!leftImage.empty() && !rightImage.empty() && leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 && leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols); + UASSERT(stereoCameraModel.isValid()); std::vector leftCorners; cv::KeyPoint::convert(keypoints, leftCorners); @@ -165,14 +173,18 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D( leftCorners[i], disparity, - cx, cy, fx, baseline); + stereoCameraModel.left().cx(), + stereoCameraModel.left().cy(), + stereoCameraModel.left().fx(), + stereoCameraModel.baseline()); if(pcl::isFinite(tmpPt)) { pt = tmpPt; - if(!transform.isNull() && !transform.isIdentity()) + if(!stereoCameraModel.left().localTransform().isNull() && + !stereoCameraModel.left().localTransform().isIdentity()) { - pt = util3d::transformPoint(pt, transform); + pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform()); } } } @@ -190,11 +202,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( std::multimap generateWords3DMono( const std::multimap & refWords, const std::multimap & nextWords, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, + const CameraModel & cameraModel, Transform & cameraTransform, int pnpIterations, float pnpReprojError, @@ -204,6 +212,7 @@ std::multimap generateWords3DMono( const std::multimap & refGuess3D, double * varianceOut) { + UASSERT(cameraModel.isValid()); std::multimap words3D; std::list > > pairs; if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8) @@ -257,10 +266,7 @@ std::multimap generateWords3DMono( xp.at(2, i) = 1; } - cv::Mat K = (cv::Mat_(3,3) << - fx, 0, cx, - 0, fy, cy, - 0, 0, 1); + cv::Mat K = cameraModel.K(); cv::Mat Kinv = K.inv(); cv::Mat E = K.t()*F*K; cv::Mat x_norm = Kinv * x; @@ -280,7 +286,7 @@ std::multimap generateWords3DMono( //if camera transform is set, use it instead of the computed one from epipolar geometry if(useCameraTransformGuess) { - Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse(); + Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse(); P = (cv::Mat_(3,4) << (double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(), (double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(), @@ -303,7 +309,7 @@ std::multimap generateWords3DMono( pts4D.col(i) /= pts4D.at(3,i); if(pts4D.at(2,i) > 0) { - words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at(0,i), pts4D.at(1,i), pts4D.at(2,i)), localTransform))); + words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at(0,i), pts4D.at(1,i), pts4D.at(2,i)), cameraModel.localTransform()))); } } @@ -316,7 +322,7 @@ std::multimap generateWords3DMono( R.at(1,0), R.at(1,1), R.at(1,2), T.at(1), R.at(2,0), R.at(2,1), R.at(2,2), T.at(2)); - cameraTransform = (localTransform * t).inverse() * localTransform; + cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform(); } if(refGuess3D.size()) @@ -408,7 +414,7 @@ std::multimap generateWords3DMono( imagePoints.resize(oi); //PnPRansac - Transform guess = localTransform.inverse(); + Transform guess = cameraModel.localTransform().inverse(); cv::Mat R = (cv::Mat_(3,3) << (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), @@ -440,7 +446,7 @@ std::multimap generateWords3DMono( R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - cameraTransform = (localTransform * pnp).inverse(); + cameraTransform = (cameraModel.localTransform() * pnp).inverse(); } else { diff --git a/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index 00b4d08d..d675b064 100644 --- a/examples/RGBDMapping/MapBuilder.h +++ b/examples/RGBDMapping/MapBuilder.h @@ -71,8 +71,8 @@ public: layout->addWidget(cloudViewer_); this->setLayout(layout); + qRegisterMetaType("rtabmap::OdometryEvent"); qRegisterMetaType("rtabmap::Statistics"); - qRegisterMetaType("rtabmap::SensorData"); QAction * pause = new QAction(this); this->addAction(pause); @@ -102,14 +102,14 @@ protected slots: } } - virtual void processOdometry(const rtabmap::SensorData & data) + virtual void processOdometry(const rtabmap::OdometryEvent & odom) { if(!this->isVisible()) { return; } - Transform pose = data.pose(); + Transform pose = odom.pose(); if(pose.isNull()) { //Odometry lost @@ -126,38 +126,33 @@ protected slots: lastOdomPose_ = pose; // 3d cloud - if(data.depth().cols == data.image().cols && - data.depth().rows == data.image().rows && - !data.depth().empty() && - data.fx() > 0.0f && - data.fy() > 0.0f) + if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols && + odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows && + !odom.data().depthOrRightRaw().empty() && + (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size())) { - pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB( - data.image(), - data.depth(), - data.cx(), - data.cy(), - data.fx(), - data.fy(), - 2); // decimation // high definition + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + odom.data(), + 2, // decimation + 4.0f); // max depth if(cloud->size()) { - cloud = util3d::passThrough(cloud, "z", 0, 4.0f); - if(cloud->size()) + if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose)) { - cloud = util3d::transformPointCloud(cloud, data.localTransform()); + UERROR("Adding cloudOdom to viewer failed!"); } } - if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose)) + else { - UERROR("Adding cloudOdom to viewer failed!"); + cloudViewer_->setCloudVisibility("cloudOdom", false); + UWARN("Empty cloudOdom!"); } } - if(!data.pose().isNull()) + if(!odom.pose().isNull()) { // update camera position - cloudViewer_->updateCameraTargetPosition(odometryCorrection_*data.pose()); + cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose()); } } cloudViewer_->update(); @@ -196,35 +191,32 @@ protected slots: } cloudViewer_->setCloudVisibility(cloudName, true); } - else if(iter->first == stats.refImageId() && - stats.getSignature().id() == iter->first) + else if(stats.getSignature().id() == iter->first) { Signature s = stats.getSignature(); - s.uncompressData(); // make sure data is uncompressed + s.sensorData().uncompressData(); // make sure data is uncompressed // Add the new cloud - pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB( - s.getImageRaw(), - s.getDepthRaw(), - s.getCx(), - s.getCy(), - s.getFx(), - s.getFy(), - 4); // decimation - + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + s.sensorData(), + 4, // decimation + 4.0f); // max depth if(cloud->size()) { - cloud = util3d::passThrough(cloud, "z", 0, 4.0f); - if(cloud->size()) + if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second)) { - cloud = util3d::transformPointCloud(cloud, stats.getSignature().getLocalTransform()); + UERROR("Adding cloud %d to viewer failed!", iter->first); } } - if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second)) + else { - UERROR("Adding cloud %d to viewer failed!", iter->first); + UWARN("Empty cloud %d!", iter->first); } } } + else + { + UWARN("Null pose for %d ?!?", iter->first); + } } //============================ @@ -278,7 +270,7 @@ protected slots: !processingStatistics_) { lastOdometryProcessed_ = false; // if we receive too many odometry events! - QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data())); + QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent)); } } } diff --git a/guilib/include/rtabmap/gui/DataRecorder.h b/guilib/include/rtabmap/gui/DataRecorder.h index 6b0a3d45..8359e3ae 100644 --- a/guilib/include/rtabmap/gui/DataRecorder.h +++ b/guilib/include/rtabmap/gui/DataRecorder.h @@ -57,7 +57,7 @@ public: const QString & path() const {return path_;} public slots: - void addData(const rtabmap::SensorData & data); + void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)); void showImage(const cv::Mat & image, const cv::Mat & depth); protected: virtual void closeEvent(QCloseEvent* event); diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 2ffe609b..62d2c3fa 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -52,7 +52,7 @@ namespace rtabmap { class Memory; class ImageView; -class Signature; +class SensorData; class CloudViewer; class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow @@ -124,7 +124,7 @@ private: QLabel * labelMapId, QLabel * labelPose, bool updateConstraintView); - void updateStereo(const Signature * data); + void updateStereo(const SensorData * data); void updateWordsMatching(); void updateConstraintView( const rtabmap::Link & link, diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index e55e3c6e..cc6f8e1f 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -35,7 +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/core/OdometryEvent.h" #include "rtabmap/gui/PreferencesDialog.h" #include @@ -161,7 +161,7 @@ private slots: void selectScreenCaptureFormat(bool checked); void takeScreenshot(); void updateElapsedTime(); - void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info); + void processOdometry(const rtabmap::OdometryEvent & odom); void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags); void applyPrefSettings(const rtabmap::ParametersMap & parameters); void processRtabmapEventInit(int status, const QString & info); @@ -194,7 +194,7 @@ private slots: signals: void statsReceived(const rtabmap::Statistics &); - void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &); + void odometryReceived(const rtabmap::OdometryEvent &); void thresholdsChanged(int, int); void stateChanged(MainWindow::State); void rtabmapEventInitReceived(int status, const QString & info); @@ -227,19 +227,6 @@ private: int regenerateDecimation, float regenerateVoxelSize, float regenerateMaxDepth) const; - pcl::PointCloud::Ptr createCloud( - int id, - const cv::Mat & rgb, - const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float voxelSize, - int decimation, - float maxDepth) const; std::map::Ptr > getClouds( const std::map & poses, bool regenerateClouds, diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h index ff295613..948b326d 100644 --- a/guilib/include/rtabmap/gui/OdometryViewer.h +++ b/guilib/include/rtabmap/gui/OdometryViewer.h @@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/OdometryEvent.h" #include #include "rtabmap/utilite/UEventsHandler.h" @@ -59,7 +58,7 @@ protected: virtual void handleEvent(UEvent * event); private slots: - void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info); + void processData(const rtabmap::OdometryEvent & odom); private: ImageView* imageView_; diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 5a722b09..24cd1f84 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -239,8 +239,8 @@ void CalibrationDialog::handleEvent(UEvent * event) { processingData_ = true; QMetaObject::invokeMethod(this, "processImages", - Q_ARG(cv::Mat, e->data().image()), - Q_ARG(cv::Mat, e->data().depthOrRightImage()), + Q_ARG(cv::Mat, e->data().imageRaw()), + Q_ARG(cv::Mat, e->data().depthOrRightRaw()), Q_ARG(QString, QString(e->cameraName().c_str()))); } } diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 4ca160a2..a8da041d 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -76,19 +76,11 @@ CameraViewer::~CameraViewer() void CameraViewer::showImage(const rtabmap::SensorData & data) { processingImages_ = true; - imageView_->setImage(uCvMat2QImage(data.image())); - imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage())); - if(!data.depth().empty() && data.fx() && data.fy()) + imageView_->setImage(uCvMat2QImage(data.imageRaw())); + imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw())); + if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size())) { - cloudView_->addOrUpdateCloud("cloud", - util3d::cloudFromDepthRGB(data.image(), data.depth(), data.cx(), data.cy(), data.fx(), data.fy()), - data.localTransform()); - } - else if(!data.rightImage().empty() && data.fx() && data.baseline()) - { - cloudView_->addOrUpdateCloud("cloud", - util3d::cloudFromStereoImages(data.image(), data.rightImage(), data.cx(), data.cy(), data.fx(), data.baseline()), - data.localTransform()); + cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data)); } else { diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index be610ba4..09734748 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -93,6 +93,7 @@ CloudViewer::CloudViewer(QWidget *parent) : -1, 0, 0, 0, 0, 0, 0, 0, 1); + _visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0); //setup menu/actions createMenu(); diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 4752127c..7d9734d5 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -120,7 +120,7 @@ DataRecorder::~DataRecorder() this->closeRecorder(); } -void DataRecorder::addData(const rtabmap::SensorData & data) +void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance) { memoryMutex_.lock(); if(memory_) @@ -134,10 +134,10 @@ void DataRecorder::addData(const rtabmap::SensorData & data) //save to database UTimer time; - memory_->update(data); + memory_->update(data, pose, covariance); const Signature * s = memory_->getLastWorkingSignature(); - totalSizeKB_ += (int)s->getImageCompressed().total()/1000; - totalSizeKB_ += (int)s->getDepthCompressed().total()/1000; + totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000; + totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000; memory_->cleanup(); if(++count_ % 30) @@ -183,8 +183,8 @@ void DataRecorder::handleEvent(UEvent * event) { processingImages_ = true; QMetaObject::invokeMethod(this, "showImage", - Q_ARG(cv::Mat, camEvent->data().image()), - Q_ARG(cv::Mat, camEvent->data().depthOrRightImage())); + Q_ARG(cv::Mat, camEvent->data().imageRaw()), + Q_ARG(cv::Mat, camEvent->data().depthOrRightRaw())); } } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 4e409e4e..addab20f 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -579,23 +579,21 @@ void DatabaseViewer::closeEvent(QCloseEvent* event) std::multimap::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to()); if(refinedIter != linksRefined_.end()) { - memory_->addLink( - refinedIter->second.to(), + memory_->addLink(Link( refinedIter->second.from(), - refinedIter->second.transform(), + refinedIter->second.to(), refinedIter->second.type(), - refinedIter->second.rotVariance(), - refinedIter->second.transVariance()); + refinedIter->second.transform(), + refinedIter->second.infMatrix())); } else { - memory_->addLink( - iter->second.to(), + memory_->addLink(Link( iter->second.from(), - iter->second.transform(), + iter->second.to(), iter->second.type(), - iter->second.rotVariance(), - iter->second.transVariance()); + iter->second.transform(), + iter->second.infMatrix())); } } @@ -608,8 +606,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event) iter->second.from(), iter->second.to(), iter->second.transform(), - iter->second.rotVariance(), - iter->second.transVariance()); + iter->second.infMatrix()); } } @@ -708,6 +705,7 @@ void DatabaseViewer::exportDatabase() double previousStamp = 0; std::vector delays(ids_.size()); int oi=0; + std::map poses; for(int i=0; i= 0 && mapId > sessionExported) @@ -753,31 +753,47 @@ void DatabaseViewer::exportDatabase() { int id = ids.at(i); - Signature data = memory_->getSignatureData(id, true); - float rotVariance = 1.0f; - float transVariance = 1.0f; + SensorData data = memory_->getNodeData(id, true); + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); if(dialog.isOdomExported()) { - data.getPoseVariance(rotVariance, transVariance); + if(memory_->getSignature(id) == 0) + { + UERROR("could not find node %d in memory.", id); + } + else + { + covariance = memory_->getSignature(id)->getPoseCovariance(); + } } - rtabmap::SensorData sensorData( - dialog.isDepth2dExported()?data.getLaserScanRaw():cv::Mat(), - dialog.isDepth2dExported()?data.getLaserScanMaxPts():0, - dialog.isRgbExported()?data.getImageRaw():cv::Mat(), - dialog.isDepthExported()?data.getDepthRaw():cv::Mat(), - dialog.isRgbExported() || dialog.isDepthExported()?data.getFx():0, - dialog.isRgbExported() || dialog.isDepthExported()?data.getFy():0, - dialog.isRgbExported() || dialog.isDepthExported()?data.getCx():0, - dialog.isRgbExported() || dialog.isDepthExported()?data.getCy():0, - dialog.isRgbExported() || dialog.isDepthExported()?data.getLocalTransform():Transform::getIdentity(), - dialog.isOdomExported()?data.getPose():Transform(), - rotVariance, - transVariance, - data.id(), - data.getStamp(), - dialog.isUserDataExported()?data.getUserData():std::vector()); - recorder.addData(sensorData); + rtabmap::SensorData sensorData; + if(data.cameraModels().size()) + { + sensorData = rtabmap::SensorData( + dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(), + dialog.isDepth2dExported()?data.laserScanMaxPts():0, + dialog.isRgbExported()?data.imageRaw():cv::Mat(), + dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(), + data.cameraModels(), + data.id(), + data.stamp(), + dialog.isUserDataExported()?data.userData():std::vector()); + } + else + { + sensorData = rtabmap::SensorData( + dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(), + dialog.isDepth2dExported()?data.laserScanMaxPts():0, + dialog.isRgbExported()?data.imageRaw():cv::Mat(), + dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(), + data.stereoCameraModel(), + data.id(), + data.stamp(), + dialog.isUserDataExported()?data.userData():std::vector()); + } + + recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance); progressDialog->appendText(tr("Exported node %1").arg(id)); progressDialog->incrementStep(); @@ -1064,7 +1080,7 @@ void DatabaseViewer::view3DMap() 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); + float maxDepth = (float)QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok); if(ok) { std::map optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); @@ -1102,60 +1118,35 @@ void DatabaseViewer::view3DMap() rtabmap::Transform pose = iter->second; if(!pose.isNull()) { - Signature data = memory_->getSignatureData(iter->first, true); + SensorData data = memory_->getNodeData(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) + UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1); + UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1); + cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth); + + if(cloud->size()) { - cv::Mat leftImg; - if(data.getImageRaw().channels() == 3) + QColor color = Qt::red; + int mapId, weight; + Transform odomPose; + std::string label; + double stamp; + std::vector userData; + if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true)) { - cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY); + color = (Qt::GlobalColor)(mapId % 12 + 7 ); } - else - { - leftImg = data.getImageRaw(); - } - cloud = rtabmap::util3d::cloudFromDisparityRGB( - data.getImageRaw(), - util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - decimation); + + 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())); } else { - cloud = rtabmap::util3d::cloudFromDepthRGB( - data.getImageRaw(), - data.getDepthRaw(), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - decimation); + UINFO("Empty cloud %d", iter->first); + progressDialog.appendText(QString("Empty cloud %1").arg(iter->first)); } - - if(maxDepth) - { - cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth); - } - - cloud = rtabmap::util3d::transformPointCloud(cloud, data.getLocalTransform()); - - QColor color = Qt::red; - int mapId, weight; - Transform odomPose; - std::string label; - double stamp; - std::vector userData; - if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true)) - { - 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())); progressDialog.incrementStep(); QApplication::processEvents(); } @@ -1188,7 +1179,7 @@ void DatabaseViewer::generate3DMap() 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); + float maxDepth = (float)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_); @@ -1212,48 +1203,24 @@ void DatabaseViewer::generate3DMap() const rtabmap::Transform & pose = iter->second; if(!pose.isNull()) { - Signature data = memory_->getSignatureData(iter->first, true); + SensorData data = memory_->getNodeData(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) + UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1); + UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1); + cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth); + std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first); + if(cloud->size()) { - 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(), - util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - decimation); + cloud = rtabmap::util3d::transformPointCloud(cloud, pose); + 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())); } else { - cloud = rtabmap::util3d::cloudFromDepthRGB( - data.getImageRaw(), - data.getDepthRaw(), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - decimation); + UINFO("Ignored empty cloud %s", name.c_str()); + progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str())); } - - 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(); } @@ -1500,19 +1467,21 @@ void DatabaseViewer::update(int value, QImage imgDepth; if(memory_) { - Signature data = memory_->getSignatureData(id, true); - if(!data.getImageRaw().empty()) + SensorData data = memory_->getNodeData(id, true); + if(!data.imageRaw().empty()) { - img = uCvMat2QImage(data.getImageRaw()); + img = uCvMat2QImage(data.imageRaw()); } - if(!data.getDepthRaw().empty()) + if(!data.depthOrRightRaw().empty()) { - imgDepth = uCvMat2QImage(data.getDepthRaw()); + imgDepth = uCvMat2QImage(data.depthOrRightRaw()); } - if(data.getWords().size()) + const Signature * signature = memory_->getSignature(id); + + if(signature && signature->getWords().size()) { - view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow); + view->setFeatures(signature->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow); } Transform odomPose; @@ -1522,47 +1491,36 @@ void DatabaseViewer::update(int value, std::vector d; memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true); - weight->setNum(data.getWeight()); - label->setText(data.getLabel().c_str()); + weight->setNum(w); + label->setText(l.c_str()); labelPose->setText(QString("%1%2, %3, %4").arg(odomPose.isIdentity()?"* ":"").arg(odomPose.x()).arg(odomPose.y()).arg(odomPose.z())); - if(data.getStamp()!=0.0) + if(s!=0.0) { - stamp->setText(QDateTime::fromMSecsSinceEpoch(data.getStamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); + stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); } //stereo - if(!data.getDepthRaw().empty() && data.getDepthRaw().type() == CV_8UC1) + if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1) { this->updateStereo(&data); } // 3d view - if(view3D->isVisible() && !data.getDepthRaw().empty()) + if(view3D->isVisible() && !data.depthOrRightRaw().empty()) { pcl::PointCloud::Ptr cloud; - if(data.getDepthRaw().type() == CV_8UC1) + cloud = util3d::cloudRGBFromSensorData(data); + if(cloud->size()) { - cloud = util3d::cloudFromStereoImages( - data.getImageRaw(), - data.getDepthRaw(), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - 1); + view3D->addOrUpdateCloud("0", cloud); } - else - { - cloud = util3d::cloudFromDepthRGB( - data.getImageRaw(), - data.getDepthRaw(), - data.getCx(), data.getCy(), - data.getFx(), data.getFy(), - 1); - } - view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform()); //add scan - pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw()); - view3D->addOrUpdateCloud("1", scan); + pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw()); + if(scan->size()) + { + view3D->addOrUpdateCloud("1", scan); + } view3D->update(); } @@ -1688,18 +1646,23 @@ void DatabaseViewer::update(int value, } } -void DatabaseViewer::updateStereo(const Signature * data) +void DatabaseViewer::updateStereo(const SensorData * data) { - if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1) + if(data && + ui_->dockWidget_stereoView->isVisible() && + !data->imageRaw().empty() && + !data->depthOrRightRaw().empty() && + data->depthOrRightRaw().type() == CV_8UC1 && + data->stereoCameraModel().isValid()) { cv::Mat leftMono; - if(data->getImageRaw().channels() == 3) + if(data->imageRaw().channels() == 3) { - cv::cvtColor(data->getImageRaw(), leftMono, CV_BGR2GRAY); + cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY); } else { - leftMono = data->getImageRaw(); + leftMono = data->imageRaw(); } UTimer timer; @@ -1726,7 +1689,7 @@ void DatabaseViewer::updateStereo(const Signature * data) std::vector rightCorners; cv::calcOpticalFlowPyrLK( leftMono, - data->getDepthRaw(), + data->depthOrRightRaw(), leftCorners, rightCorners, status, @@ -1754,11 +1717,14 @@ void DatabaseViewer::updateStereo(const Signature * data) pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D( leftCorners[i], disparity, - data->getCx(), data->getCy(), data->getFx(), data->getFy()); + data->stereoCameraModel().left().cx(), + data->stereoCameraModel().left().cy(), + data->stereoCameraModel().left().fx(), + data->stereoCameraModel().baseline()); if(pcl::isFinite(tmpPt)) { - pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f()); + pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform()); if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2) { status[i] = 100; //blue @@ -1809,8 +1775,8 @@ void DatabaseViewer::updateStereo(const Signature * data) ui_->graphicsView_stereo->setFeaturesShown(false); ui_->graphicsView_stereo->setImageDepthShown(true); - ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->getImageRaw())); - ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->getDepthRaw())); + ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw())); + ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->depthOrRightRaw())); // Draw lines between corresponding features... for(unsigned int i=0; ilabel_type->setNum(link.type()); - ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance()))); + ui_->label_variance->setText(QString("%1, %2") + .arg(sqrt(link.rotVariance())) + .arg(sqrt(link.transVariance()))); ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n")); if(link.type() == Link::kNeighbor && graphes_.size() && @@ -2044,15 +2012,15 @@ void DatabaseViewer::updateConstraintView( if(ui_->constraintsViewer->isVisible()) { - Signature dataFrom, dataTo; + SensorData dataFrom, dataTo; - dataFrom = memory_->getSignatureData(link.from(), true); - UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1); - UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1); + dataFrom = memory_->getNodeData(link.from(), true); + UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1); + UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1); - dataTo = memory_->getSignatureData(link.to(), true); - UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1); - UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1); + dataTo = memory_->getNodeData(link.to(), true); + UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1); + UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1); if(cloudFrom->size() == 0 && cloudTo->size() == 0) @@ -2060,51 +2028,9 @@ void DatabaseViewer::updateConstraintView( //cloud 3d if(!ui_->checkBox_show3DWords->isChecked()) { - pcl::PointCloud::Ptr cloudFrom; - if(dataFrom.getDepthRaw().type() == CV_8UC1) - { - cloudFrom = rtabmap::util3d::cloudFromStereoImages( - dataFrom.getImageRaw(), - dataFrom.getDepthRaw(), - dataFrom.getCx(), dataFrom.getCy(), - dataFrom.getFx(), dataFrom.getFy(), - 1); - } - else - { - cloudFrom = rtabmap::util3d::cloudFromDepthRGB( - dataFrom.getImageRaw(), - dataFrom.getDepthRaw(), - dataFrom.getCx(), dataFrom.getCy(), - dataFrom.getFx(), dataFrom.getFy(), - 1); - } - - cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom); - cloudFrom = rtabmap::util3d::transformPointCloud(cloudFrom, dataFrom.getLocalTransform()); - - pcl::PointCloud::Ptr cloudTo; - if(dataTo.getDepthRaw().type() == CV_8UC1) - { - cloudTo = rtabmap::util3d::cloudFromStereoImages( - dataTo.getImageRaw(), - dataTo.getDepthRaw(), - dataTo.getCx(), dataTo.getCy(), - dataTo.getFx(), dataTo.getFy(), - 1); - } - else - { - cloudTo = rtabmap::util3d::cloudFromDepthRGB( - dataTo.getImageRaw(), - dataTo.getDepthRaw(), - dataTo.getCx(), dataTo.getCy(), - dataTo.getFx(), dataTo.getFy(), - 1); - } - - cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo); - cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t*dataTo.getLocalTransform()); + pcl::PointCloud::Ptr cloudFrom, cloudTo; + cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1); + cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1); if(cloudFrom->size()) { @@ -2112,6 +2038,7 @@ void DatabaseViewer::updateConstraintView( } if(cloudTo->size()) { + cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan); } } @@ -2196,8 +2123,8 @@ void DatabaseViewer::updateConstraintView( { //cloud 2d pcl::PointCloud::Ptr scanA, scanB; - scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw()); - scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw()); + scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw()); + scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw()); scanB = rtabmap::util3d::transformPointCloud(scanB, t); if(scanA->size()) { @@ -2309,40 +2236,17 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) bool added = false; if(ui_->groupBox_gridFromProjection->isChecked()) { - Signature data = memory_->getSignatureData(ids_.at(i), true); - if(!data.getDepthRaw().empty()) + SensorData data = memory_->getNodeData(ids_.at(i), true); + if(!data.depthOrRightRaw().empty()) { pcl::PointCloud::Ptr cloud; - if(data.getDepthRaw().type() == CV_8UC1) - { - cloud = rtabmap::util3d::cloudFromDisparity( - util2d::disparityFromStereoImages(data.getImageRaw(), data.getDepthRaw()), - data.getCx(), - data.getCy(), - data.getFx(), - data.getFy(), - ui_->spinBox_projDecimation->value()); - } - else - { - cloud = util3d::cloudFromDepth( - data.getDepthRaw(), - data.getCx(), - data.getCy(), - data.getFx(), - data.getFy(), - ui_->spinBox_projDecimation->value()); - } - if(cloud->size()) - { - cloud = util3d::passThrough(cloud, "z", 0, ui_->doubleSpinBox_projMaxDepth->value()); - } + cloud = util3d::cloudFromSensorData(data, + ui_->spinBox_projDecimation->value(), + ui_->doubleSpinBox_projMaxDepth->value(), + ui_->doubleSpinBox_gridCellSize->value()); if(cloud->size()) { - cloud = util3d::voxelize(cloud, ui_->doubleSpinBox_gridCellSize->value()); - cloud = util3d::transformPointCloud(cloud, data.getLocalTransform()); - UTimer timer; float cellSize = ui_->doubleSpinBox_gridCellSize->value(); float groundNormalMaxAngle = M_PI_4; @@ -2364,8 +2268,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } else { - Signature data = memory_->getSignatureData(ids_.at(i), false); - if(!data.getLaserScanCompressed().empty()) + SensorData data = memory_->getNodeData(ids_.at(i), false); + if(!data.laserScanCompressed().empty()) { pcl::PointCloud::Ptr cloud; cv::Mat laserScan; @@ -2700,9 +2604,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update int correspondences = 0; Transform transform; - Signature dataFrom, dataTo; - dataFrom = memory_->getSignatureData(currentLink.from(), false); - dataTo = memory_->getSignatureData(currentLink.to(), false); + SensorData dataFrom, dataTo; + dataFrom = memory_->getNodeData(currentLink.from(), false); + dataTo = memory_->getNodeData(currentLink.to(), false); pcl::PointCloud::Ptr cloudA(new pcl::PointCloud); pcl::PointCloud::Ptr cloudB(new pcl::PointCloud); @@ -2712,8 +2616,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(ui_->checkBox_icp_2d->isChecked()) { //2D - cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed()); - cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed()); + cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.laserScanCompressed()); + cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.laserScanCompressed()); if(!oldLaserScan.empty() && !newLaserScan.empty()) { @@ -2740,9 +2644,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(!transform.isNull()) { - if(dataTo.getLaserScanMaxPts()) + if(dataTo.laserScanMaxPts()) { - correspondenceRatio = float(correspondences)/float(dataTo.getLaserScanMaxPts()); + correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts()); } else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()) { @@ -2755,112 +2659,60 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update else { //3D - cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed()); - cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed()); - - if(depthA.type() == CV_8UC1) + cv::Mat im,de; + dataFrom.uncompressData(&im, &de, 0); + dataTo.uncompressData(&im, &de, 0); + cloudA = util3d::cloudFromSensorData(dataFrom, + ui_->spinBox_icp_decimation->value(), + ui_->doubleSpinBox_icp_maxDepth->value(), + ui_->doubleSpinBox_icp_voxel->value()); + cloudB = util3d::cloudFromSensorData(dataTo, + ui_->spinBox_icp_decimation->value(), + ui_->doubleSpinBox_icp_maxDepth->value(), + ui_->doubleSpinBox_icp_voxel->value()); + if(cloudA->size() && cloudB->size()) { - cv::Mat leftMono; - cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed()); - if(left.channels() > 1) + cloudB = util3d::transformPointCloud(cloudB, t); + if(ui_->checkBox_icp_p2plane->isChecked()) { - cv::cvtColor(left, leftMono, CV_BGR2GRAY); + pcl::PointCloud::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value()); + pcl::PointCloud::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value()); + + cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals); + if(cloudA->size() != cloudANormals->size()) + { + UWARN("removed nan normals..."); + } + + cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals); + if(cloudB->size() != cloudBNormals->size()) + { + UWARN("removed nan normals..."); + } + + transform = util3d::icpPointToPlane(cloudBNormals, + cloudANormals, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + ui_->spinBox_icp_iteration->value(), + &hasConverged, + &variance, + &correspondences); } else { - leftMono = left; + transform = util3d::icp(cloudB, + cloudA, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + ui_->spinBox_icp_iteration->value(), + &hasConverged, + &variance, + &correspondences); } - cloudA = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthA), dataFrom.getCx(), dataFrom.getCy(), dataFrom.getFx(), dataFrom.getFy(), ui_->spinBox_icp_decimation->value()); - if(ui_->doubleSpinBox_icp_maxDepth->value() > 0) - { - cloudA = util3d::passThrough(cloudA, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value()); - } - if(ui_->doubleSpinBox_icp_voxel->value() > 0) - { - cloudA = util3d::voxelize(cloudA, ui_->doubleSpinBox_icp_voxel->value()); - } - cloudA = util3d::transformPointCloud(cloudA, dataFrom.getLocalTransform()); + correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total()); } else { - cloudA = util3d::getICPReadyCloud(depthA, - dataFrom.getFx(), dataFrom.getFy(), dataFrom.getCx(), dataFrom.getCy(), - ui_->spinBox_icp_decimation->value(), - ui_->doubleSpinBox_icp_maxDepth->value(), - ui_->doubleSpinBox_icp_voxel->value(), - 0, // no sampling - dataFrom.getLocalTransform()); - } - if(depthB.type() == CV_8UC1) - { - cv::Mat leftMono; - cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed()); - if(left.channels() > 1) - { - cv::cvtColor(left, leftMono, CV_BGR2GRAY); - } - else - { - leftMono = left; - } - cloudB = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthB), dataTo.getCx(), dataTo.getCy(), dataTo.getFx(), dataTo.getFy(), ui_->spinBox_icp_decimation->value()); - if(ui_->doubleSpinBox_icp_maxDepth->value() > 0) - { - cloudB = util3d::passThrough(cloudB, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value()); - } - if(ui_->doubleSpinBox_icp_voxel->value() > 0) - { - cloudB = util3d::voxelize(cloudB, ui_->doubleSpinBox_icp_voxel->value()); - } - cloudB = util3d::transformPointCloud(cloudB, t * dataTo.getLocalTransform()); - } - else - { - cloudB = util3d::getICPReadyCloud(depthB, - dataTo.getFx(), dataTo.getFy(), dataTo.getCx(), dataTo.getCy(), - ui_->spinBox_icp_decimation->value(), - ui_->doubleSpinBox_icp_maxDepth->value(), - ui_->doubleSpinBox_icp_voxel->value(), - 0, // no sampling - t * dataTo.getLocalTransform()); - } - - if(ui_->checkBox_icp_p2plane->isChecked()) - { - pcl::PointCloud::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value()); - pcl::PointCloud::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value()); - - cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals); - if(cloudA->size() != cloudANormals->size()) - { - UWARN("removed nan normals..."); - } - - cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals); - if(cloudB->size() != cloudBNormals->size()) - { - UWARN("removed nan normals..."); - } - - transform = util3d::icpPointToPlane(cloudBNormals, - cloudANormals, - ui_->doubleSpinBox_icp_maxCorrespDistance->value(), - ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); - } - else - { - transform = util3d::icp(cloudB, - cloudA, - ui_->doubleSpinBox_icp_maxCorrespDistance->value(), - ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); - - correspondenceRatio = float(correspondences)/float(depthB.total()); + UWARN("No cloud generated!"); } } @@ -2963,8 +2815,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo Memory tmpMemory(parameters); // Add signatures - SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData(); - SensorData dataTo = memory_->getSignatureData(to, true).toSensorData(); + SensorData dataFrom = memory_->getNodeData(from, true); + SensorData dataTo = memory_->getNodeData(to, true); if(from > to) { @@ -3081,8 +2933,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra Memory tmpMemory(parameters); // Add signatures - SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData(); - SensorData dataTo = memory_->getSignatureData(to, true).toSensorData(); + SensorData dataFrom = memory_->getNodeData(from, true); + SensorData dataTo = memory_->getNodeData(to, true); if(from > to) { @@ -3100,8 +2952,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra if(!silent) { - ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth()); - ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth()); + ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw()); + ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw()); updateWordsMatching(); } } diff --git a/guilib/src/LoopClosureViewer.cpp b/guilib/src/LoopClosureViewer.cpp index 1c671bd8..cb9bd8ea 100644 --- a/guilib/src/LoopClosureViewer.cpp +++ b/guilib/src/LoopClosureViewer.cpp @@ -106,74 +106,14 @@ void LoopClosureViewer::updateView(const Transform & transform) if(!t.isNull()) { //cloud 3d - pcl::PointCloud::Ptr cloudA; - if(sA_.getDepthRaw().type() == CV_8UC1) - { - cloudA = util3d::cloudFromStereoImages( - sA_.getImageRaw(), - sA_.getDepthRaw(), - sA_.getCx(), sA_.getCy(), - sA_.getFx(), sA_.getFy(), - decimation); - } - else - { - cloudA = util3d::cloudFromDepthRGB( - sA_.getImageRaw(), - sA_.getDepthRaw(), - sA_.getCx(), sA_.getCy(), - sA_.getFx(), sA_.getFy(), - decimation); - } - - cloudA = util3d::removeNaNFromPointCloud(cloudA); - - if(maxDepth>0.0) - { - cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth); - } - if(samples>0 && (int)cloudA->size() > samples) - { - cloudA = util3d::sampling(cloudA, samples); - } - cloudA = util3d::transformPointCloud(cloudA, sA_.getLocalTransform()); - - pcl::PointCloud::Ptr cloudB; - if(sB_.getDepthRaw().type() == CV_8UC1) - { - cloudB = util3d::cloudFromStereoImages( - sB_.getImageRaw(), - sB_.getDepthRaw(), - sB_.getCx(), sB_.getCy(), - sB_.getFx(), sB_.getFy(), - decimation); - } - else - { - cloudB = util3d::cloudFromDepthRGB( - sB_.getImageRaw(), - sB_.getDepthRaw(), - sB_.getCx(), sB_.getCy(), - sB_.getFx(), sB_.getFy(), - decimation); - } - - cloudB = util3d::removeNaNFromPointCloud(cloudB); - - if(maxDepth>0.0) - { - cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth); - } - if(samples>0 && (int)cloudB->size() > samples) - { - cloudB = util3d::sampling(cloudB, samples); - } - cloudB = util3d::transformPointCloud(cloudB, t*sB_.getLocalTransform()); + pcl::PointCloud::Ptr cloudA, cloudB; + cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, 0.0f, samples); + cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, 0.0f, samples); //cloud 2d pcl::PointCloud::Ptr scanA, scanB; - scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw()); - scanB = util3d::laserScanToPointCloud(sB_.getLaserScanRaw()); + scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw()); + scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw()); 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())); @@ -184,6 +124,7 @@ void LoopClosureViewer::updateView(const Transform & transform) } if(cloudB->size()) { + cloudB = util3d::transformPointCloud(cloudB, t); ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB); } if(scanA->size()) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 7a4c08d6..cd3c5f9d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -437,9 +437,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : qRegisterMetaType("rtabmap::Statistics"); connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics))); - qRegisterMetaType("rtabmap::SensorData"); - qRegisterMetaType("rtabmap::OdometryInfo"); - connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo))); + qRegisterMetaType("rtabmap::OdometryEvent"); + connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent)), this, SLOT(processOdometry(rtabmap::OdometryEvent))); connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection())); @@ -671,7 +670,7 @@ void MainWindow::handleEvent(UEvent* anEvent) if(!_processingOdometry && !_processingStatistics) { _processingOdometry = true; // if we receive too many odometry events! - emit odometryReceived(odomEvent->data(), odomEvent->info()); + emit odometryReceived(*odomEvent); } } } @@ -695,11 +694,11 @@ void MainWindow::handleEvent(UEvent* anEvent) } } -void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info) +void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) { _processingOdometry = true; UTimer time; - Transform pose = data.pose(); + Transform pose = odom.pose(); bool lost = false; bool lostStateChanged = false; @@ -713,11 +712,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap pose = _lastOdomPose; lost = true; } - else if(info.inliers>0 && + else if(odom.info().inliers>0 && _preferencesDialog->getOdomQualityWarnThr() && - info.inliers < _preferencesDialog->getOdomQualityWarnThr()) + odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr()) { - UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr()); + UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr()); lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed; _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow); _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow); @@ -730,44 +729,44 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _ui->imageView_odometry->setBackgroundColor(Qt::black); } - if(info.inliers >= 0) + if(odom.info().inliers >= 0) { - _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers); + _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers); } - if(info.matches >= 0) + if(odom.info().matches >= 0) { - _ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches); + _ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches); } - if(info.variance >= 0) + if(odom.info().variance >= 0) { - _ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance)); + _ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance)); } - if(info.variance >= 0) + if(odom.info().variance >= 0) { - _ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance); + _ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance); } - if(info.time > 0) + if(odom.info().time > 0) { - _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f); + _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)odom.data().id(), (float)odom.info().time*1000.0f); } - if(info.features >=0) + if(odom.info().features >=0) { - _ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features); + _ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features); } - if(info.localMapSize >=0) + if(odom.info().localMapSize >=0) { - _ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize); + _ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)odom.data().id(), (float)odom.info().localMapSize); } - _ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id()); + _ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id()); 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); + _ui->statsToolBox->updateStat("Odometry/T_x/m", (float)odom.data().id(), x); + _ui->statsToolBox->updateStat("Odometry/T_y/m", (float)odom.data().id(), y); + _ui->statsToolBox->updateStat("Odometry/T_z/m", (float)odom.data().id(), z); + _ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)odom.data().id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI); if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible())) { @@ -780,42 +779,42 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap if(!pose.isNull()) { // 3d cloud - if(data.depthOrRightImage().cols == data.image().cols && - data.depthOrRightImage().rows == data.image().rows && - !data.depthOrRightImage().empty() && - data.fx() > 0.0f && - data.fyOrBaseline() > 0.0f && + if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols && + odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows && + !odom.data().depthOrRightRaw().empty() && + (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) && _preferencesDialog->isCloudsShown(1)) { pcl::PointCloud::Ptr cloud; - cloud = createCloud(0, - data.image(), - data.depthOrRightImage(), - data.fx(), - data.fyOrBaseline(), - data.cx(), - data.cy(), - data.localTransform(), - pose, - _preferencesDialog->getCloudVoxelSize(1), + cloud = util3d::cloudRGBFromSensorData(odom.data(), _preferencesDialog->getCloudDecimation(1), - _preferencesDialog->getCloudMaxDepth(1)); - - if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) + _preferencesDialog->getCloudMaxDepth(1), + _preferencesDialog->getCloudVoxelSize(1)); + if(cloud->size()) { - UERROR("Adding cloudOdom to viewer failed!"); + cloud = util3d::transformPointCloud(cloud, pose); + + if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); + _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); + _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); + } + else + { + UWARN("Empty cloudOdom!"); + _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false); } - _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); - _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); - _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); } // 2d cloud - if(!data.laserScan().empty() && + if(!odom.data().laserScanRaw().empty() && _preferencesDialog->isScansShown(1)) { pcl::PointCloud::Ptr cloud; - cloud = util3d::laserScanToPointCloud(data.laserScan()); + cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw()); cloud = util3d::transformPointCloud(cloud, pose); if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) { @@ -826,10 +825,10 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); } - if(!data.pose().isNull()) + if(!odom.pose().isNull()) { // update camera position - _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose()); + _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); } } _ui->widget_cloudViewer->update(); @@ -837,27 +836,33 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap if(_ui->graphicsView_graphView->isVisible()) { - if(!pose.isNull() && !data.pose().isNull()) + if(!pose.isNull() && !odom.pose().isNull()) { - _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose()); + _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose()); _ui->graphicsView_graphView->update(); } } if(_ui->dockWidget_odometry->isVisible() && - !data.image().empty()) + !odom.data().imageRaw().empty()) { if(_ui->imageView_odometry->isFeaturesShown()) { - if(info.type == 0) + if(odom.info().type == 0) { - _ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow); + _ui->imageView_odometry->setFeatures( + odom.info().words, + odom.data().depthRaw(), + Qt::yellow); } - else if(info.type == 1) + else if(odom.info().type == 1) { std::vector kpts; - cv::KeyPoint::convert(info.refCorners, kpts); - _ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red); + cv::KeyPoint::convert(odom.info().refCorners, kpts); + _ui->imageView_odometry->setFeatures( + kpts, + odom.data().depthRaw(), + Qt::red); } } @@ -870,7 +875,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _odomImageShow = _ui->imageView_odometry->isImageShown(); _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown(); } - _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image())); + _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw())); _ui->imageView_odometry->setImageShown(true); _ui->imageView_odometry->setImageDepthShown(true); } @@ -883,54 +888,54 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow); } - _ui->imageView_odometry->setImage(uCvMat2QImage(data.image())); + _ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw())); if(_ui->imageView_odometry->isImageDepthShown()) { - _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage())); + _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); } - if(info.type == 0) + if(odom.info().type == 0) { if(_ui->imageView_odometry->isFeaturesShown()) { - for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers + _ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers } - for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers + _ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers } } } } - if(info.type == 1 && info.cornerInliers.size()) + if(odom.info().type == 1 && odom.info().cornerInliers.size()) { if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) { //draw lines - UASSERT(info.refCorners.size() == info.newCorners.size()); - for(unsigned int i=0; iimageView_odometry->isFeaturesShown()) { - _ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers + _ui->imageView_odometry->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers } if(_ui->imageView_odometry->isLinesShown()) { _ui->imageView_odometry->addLine( - info.refCorners[info.cornerInliers[i]].x, - info.refCorners[info.cornerInliers[i]].y, - info.newCorners[info.cornerInliers[i]].x, - info.newCorners[info.cornerInliers[i]].y, + odom.info().refCorners[odom.info().cornerInliers[i]].x, + odom.info().refCorners[odom.info().cornerInliers[i]].y, + odom.info().newCorners[odom.info().cornerInliers[i]].x, + odom.info().newCorners[odom.info().cornerInliers[i]].y, Qt::blue); } } } } - if(!data.image().empty()) + if(!odom.data().imageRaw().empty()) { - _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows)); + _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows)); } _ui->imageView_odometry->update(); @@ -941,7 +946,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap this->captureScreen(); } - _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0); + _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0); _processingOdometry = false; } @@ -981,7 +986,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) // update cache Signature signature = stat.getSignature(); - signature.uncompressData(); // make sure data are uncompressed + signature.sensorData().uncompressData(); // make sure data are uncompressed _cachedSignatures.insert(stat.getSignature().id(), signature); int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); @@ -1055,7 +1060,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) QMap::iterator iter = _cachedSignatures.find(shownLoopId); if(iter != _cachedSignatures.end()) { - iter.value().uncompressData(); + iter.value().sensorData().uncompressData(); loopSignature = iter.value(); } } @@ -1065,10 +1070,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) //update image views { - UCvMat2QImageThread qimageThread(signature.getImageRaw()); - UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw()); - UCvMat2QImageThread qdepthThread(signature.getDepthRaw()); - UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw()); + UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw()); + UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw()); + UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw()); + UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw()); qimageThread.start(); qdepthThread.start(); qimageLoopThread.start(); @@ -1170,7 +1175,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) // loop closure view if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) && !stat.loopClosureTransform().isNull() && - !loopSignature.getImageRaw().empty()) + !loopSignature.sensorData().imageRaw().empty()) { // the last loop closure data Transform loopClosureTransform = stat.loopClosureTransform(); @@ -1247,7 +1252,7 @@ void MainWindow::updateMapCloud( { if(!_ui->actionSave_point_cloud->isEnabled() && _cachedSignatures.size() && - (!(--_cachedSignatures.end())->getDepthCompressed().empty() || + (!(--_cachedSignatures.end())->sensorData().depthOrRightCompressed().empty() || !(--_cachedSignatures.end())->getWords3().empty())) { //enable save cloud action @@ -1257,7 +1262,7 @@ void MainWindow::updateMapCloud( if(!_ui->actionView_scans->isEnabled() && _cachedSignatures.size() && - !(--_cachedSignatures.end())->getLaserScanCompressed().empty()) + !(--_cachedSignatures.end())->sensorData().laserScanCompressed().empty()) { _ui->actionExport_2D_scans_ply_pcd->setEnabled(true); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true); @@ -1344,7 +1349,7 @@ void MainWindow::updateMapCloud( else if(_cachedSignatures.contains(iter->first)) { QMap::iterator jter = _cachedSignatures.find(iter->first); - if((!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty()) || jter->getWords3().size()) + if((!jter->sensorData().imageCompressed().empty() && !jter->sensorData().depthOrRightCompressed().empty()) || jter->getWords3().size()) { this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); } @@ -1380,7 +1385,7 @@ void MainWindow::updateMapCloud( else if(_cachedSignatures.contains(iter->first)) { QMap::iterator jter = _cachedSignatures.find(iter->first); - if(!jter->getLaserScanCompressed().empty()) + if(!jter->sensorData().laserScanCompressed().empty()) { this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); } @@ -1558,25 +1563,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int return; } - if(!iter->getImageCompressed().empty() && !iter->getDepthCompressed().empty()) + if(!iter->sensorData().imageCompressed().empty() && !iter->sensorData().depthOrRightCompressed().empty()) { cv::Mat image, depth; - iter->uncompressData(&image, &depth, 0); + SensorData data = iter->sensorData(); + data.uncompressData(&image, &depth, 0); pcl::PointCloud::Ptr cloud; - cloud = createCloud(nodeId, - image, - depth, - iter->getFx(), - iter->getFy(), - iter->getCx(), - iter->getCy(), - iter->getLocalTransform(), - Transform::getIdentity(), - _preferencesDialog->getCloudVoxelSize(0), + UASSERT(nodeId == data.id()); + cloud = util3d::cloudRGBFromSensorData(data, _preferencesDialog->getCloudDecimation(0), - _preferencesDialog->getCloudMaxDepth(0)); + _preferencesDialog->getCloudMaxDepth(0), + _preferencesDialog->getCloudVoxelSize(0)); if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud()) { @@ -1711,10 +1710,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m return; } - if(!iter->getLaserScanCompressed().empty()) + if(!iter->sensorData().laserScanCompressed().empty()) { cv::Mat depth2D; - iter->uncompressData(0, 0, &depth2D); + iter->sensorData().uncompressData(0, 0, &depth2D); pcl::PointCloud::Ptr cloud; cloud = util3d::laserScanToPointCloud(depth2D); @@ -1930,10 +1929,12 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve QApplication::processEvents(); int addedSignatures = 0; + std::map mapIds; for(std::map::const_iterator iter = event.getSignatures().begin(); iter!=event.getSignatures().end(); ++iter) { + mapIds.insert(std::make_pair(iter->first, iter->second.mapId())); if(!_cachedSignatures.contains(iter->first)) { _cachedSignatures.insert(iter->first, iter->second); @@ -1953,7 +1954,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve _initProgressDialog->appendText("Updating the 3D map cloud..."); _initProgressDialog->incrementStep(); QApplication::processEvents(); - this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), event.getMapIds(), true); + this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), mapIds, true); _initProgressDialog->appendText("Updating the 3D map cloud... done."); } else @@ -3167,15 +3168,15 @@ void MainWindow::postProcessing() { odomPoses.insert(*iter); // fill raw poses } - if(jter->getLocalTransform().isNull()) + if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid()) { - UWARN("Local transform of %d is null.", iter->first); + UWARN("Calibration of %d is null.", iter->first); allDataAvailable = false; } if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures) { // depth data required - if(jter->getDepthCompressed().empty() || jter->getFx() <= 0.0f || jter->getFy() <= 0.0f) + if(jter->sensorData().depthOrRightCompressed().empty()) { UWARN("Depth data of %d missing.", iter->first); allDataAvailable = false; @@ -3184,7 +3185,7 @@ void MainWindow::postProcessing() if(reextractFeatures) { // rgb required - if(jter->getImageCompressed().empty()) + if(jter->sensorData().imageCompressed().empty()) { UWARN("Rgb of %d missing.", iter->first); allDataAvailable = false; @@ -3233,6 +3234,7 @@ void MainWindow::postProcessing() int loopClosuresAdded = 0; if(detectMoreLoopClosures) { + UDEBUG(""); Memory memory(parameters); if(reextractFeatures) { @@ -3305,13 +3307,15 @@ void MainWindow::postProcessing() memory.init("", true); // clear previously added signatures // Add signatures - SensorData dataFrom = signatureFrom.toSensorData(); - SensorData dataTo = signatureTo.toSensorData(); + SensorData dataFrom = signatureFrom.sensorData(); + SensorData dataTo = signatureTo.sensorData(); + + cv::Mat image, depth; + dataFrom.uncompressData(&image, &depth, 0); + dataTo.uncompressData(&image, &depth, 0); if(dataFrom.isValid() && - dataFrom.isMetric() && dataTo.isValid() && - dataTo.isMetric() && dataFrom.id() != Memory::kIdInvalid && signatureFrom.id() != Memory::kIdInvalid) { @@ -3381,6 +3385,7 @@ void MainWindow::postProcessing() if(refineNeighborLinks || refineLoopClosureLinks) { + UDEBUG(""); if(refineLoopClosureLinks) { _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded); @@ -3435,83 +3440,96 @@ void MainWindow::postProcessing() Signature & signatureTo = _cachedSignatures[to]; //3D + UDEBUG(""); cv::Mat depthA, depthB; - signatureFrom.uncompressData(0, &depthA, 0); - signatureTo.uncompressData(0, &depthB, 0); - - if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1) + if(signatureFrom.sensorData().stereoCameraModel().isValid()) { - QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!")); - UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP..."); - break; - } - - pcl::PointCloud::Ptr cloudA = util3d::getICPReadyCloud(depthA, - signatureFrom.getFx(), signatureFrom.getFy(), signatureFrom.getCx(), signatureFrom.getCy(), - decimation, - maxDepth, - voxelSize, - samples, - signatureFrom.getLocalTransform()); - pcl::PointCloud::Ptr cloudB = util3d::getICPReadyCloud(depthB, - signatureTo.getFx(), signatureTo.getFy(), signatureTo.getCx(), signatureTo.getCy(), - decimation, - maxDepth, - voxelSize, - samples, - iter->second.transform() * signatureTo.getLocalTransform()); - - bool hasConverged = false; - double variance = -1; - int correspondences = 0; - Transform transform; - if(pointToPlane) - { - pcl::PointCloud::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors); - pcl::PointCloud::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors); - - cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals); - if(cloudA->size() != cloudANormals->size()) - { - UWARN("removed nan normals..."); - } - - cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals); - if(cloudB->size() != cloudBNormals->size()) - { - UWARN("removed nan normals..."); - } - - transform = util3d::icpPointToPlane(cloudBNormals, - cloudANormals, - maxCorrespondences, - icpIterations, - &hasConverged, - &variance, - &correspondences); + cv::Mat leftA, leftB; + signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0); + signatureTo.sensorData().uncompressData(&leftB, &depthB, 0); } else { - transform = util3d::icp(cloudB, - cloudA, - maxCorrespondences, - icpIterations, - &hasConverged, - &variance, - &correspondences); + signatureFrom.sensorData().uncompressData(0, &depthA, 0); + signatureTo.sensorData().uncompressData(0, &depthB, 0); } - float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size()); - - if(!transform.isNull() && hasConverged && - correspondencesRatio >= correspondenceRatio) + pcl::PointCloud::Ptr cloudA = util3d::cloudFromSensorData( + signatureFrom.sensorData(), + decimation, + maxDepth, + voxelSize, + samples); + pcl::PointCloud::Ptr cloudB = util3d::cloudFromSensorData( + signatureTo.sensorData(), + decimation, + maxDepth, + voxelSize, + samples); + if(cloudA->size() && cloudB->size()) { - Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance); - iter->second = newLink; + cloudB = util3d::transformPointCloud(cloudB, iter->second.transform()); + + bool hasConverged = false; + double variance = -1; + int correspondences = 0; + Transform transform; + if(pointToPlane) + { + UDEBUG(""); + pcl::PointCloud::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors); + pcl::PointCloud::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors); + + cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals); + if(cloudA->size() != cloudANormals->size()) + { + UWARN("removed nan normals..."); + } + + cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals); + if(cloudB->size() != cloudBNormals->size()) + { + UWARN("removed nan normals..."); + } + + transform = util3d::icpPointToPlane(cloudBNormals, + cloudANormals, + maxCorrespondences, + icpIterations, + &hasConverged, + &variance, + &correspondences); + } + else + { + UDEBUG(""); + transform = util3d::icp(cloudB, + cloudA, + maxCorrespondences, + icpIterations, + &hasConverged, + &variance, + &correspondences); + } + + float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size()); + + if(!transform.isNull() && hasConverged && + correspondencesRatio >= correspondenceRatio) + { + Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance); + iter->second = newLink; + } + else + { + QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio); + _initProgressDialog->appendText(str, Qt::darkYellow); + UWARN("%s", str.toStdString().c_str()); + } } else { - QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio); + QString str = tr("Cannot refine link %1->%2 (clouds empty!)").arg(from).arg(to); _initProgressDialog->appendText(str, Qt::darkYellow); UWARN("%s", str.toStdString().c_str()); } @@ -4824,70 +4842,6 @@ void MainWindow::saveScans(const std::map::P } } -pcl::PointCloud::Ptr MainWindow::createCloud( - int id, - const cv::Mat & rgb, - const cv::Mat & depth, - float fx, - float fy, - float cx, - float cy, - const Transform & localTransform, - const Transform & pose, - float voxelSize, - int decimation, - float maxDepth) const -{ - UTimer timer; - pcl::PointCloud::Ptr cloud; - if(depth.type() == CV_8UC1) - { - cloud = util3d::cloudFromStereoImages( - rgb, - depth, - cx, cy, - fx, fy, - decimation); - } - else - { - cloud = util3d::cloudFromDepthRGB( - rgb, - depth, - cx, cy, - fx, fy, - decimation); - } - - if(cloud->size()) - { - bool filtered = false; - if(cloud->size() && maxDepth) - { - cloud = util3d::passThrough(cloud, "z", 0, maxDepth); - filtered = true; - } - - if(cloud->size() && voxelSize) - { - cloud = util3d::voxelize(cloud, voxelSize); - filtered = true; - } - - if(cloud->size() && !filtered) - { - cloud = util3d::removeNaNFromPointCloud(cloud); - } - - if(cloud->size()) - { - cloud = util3d::transformPointCloud(cloud, pose * localTransform); - } - } - UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks()); - return cloud; -} - pcl::PointCloud::Ptr MainWindow::getAssembledCloud( const std::map & poses, float assembledVoxelSize, @@ -4910,23 +4864,22 @@ pcl::PointCloud::Ptr MainWindow::getAssembledCloud( if(_cachedSignatures.contains(iter->first)) { const Signature & s = _cachedSignatures.find(iter->first).value(); + SensorData d = s.sensorData(); cv::Mat image, depth; - s.uncompressDataConst(&image, &depth, 0); + d.uncompressData(&image, &depth, 0); if(!image.empty() && !depth.empty()) { - cloud = createCloud(iter->first, - image, - depth, - s.getFx(), - s.getFy(), - s.getCx(), - s.getCy(), - s.getLocalTransform(), - iter->second, - regenerateVoxelSize, + UASSERT(iter->first == d.id()); + cloud = util3d::cloudRGBFromSensorData( + d, regenerateDecimation, - regenerateMaxDepth); + regenerateMaxDepth, + regenerateVoxelSize); + if(cloud->size()) + { + cloud = util3d::transformPointCloud(cloud, iter->second); + } } else if(s.getWords3().size()) { @@ -5014,22 +4967,17 @@ std::map::Ptr > MainWindow::getClouds( if(_cachedSignatures.contains(iter->first)) { const Signature & s = _cachedSignatures.find(iter->first).value(); + SensorData d = s.sensorData(); cv::Mat image, depth; - s.uncompressDataConst(&image, &depth, 0); + d.uncompressData(&image, &depth, 0); if(!image.empty() && !depth.empty()) { - cloud = createCloud(iter->first, - image, - depth, - s.getFx(), - s.getFy(), - s.getCx(), - s.getCy(), - s.getLocalTransform(), - Transform::getIdentity(), - regenerateVoxelSize, + UASSERT(iter->first == d.id()); + cloud = util3d::cloudRGBFromSensorData( + d, regenerateDecimation, - regenerateMaxDepth); + regenerateMaxDepth, + regenerateVoxelSize); } else if(s.getWords3().size()) { diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 5125720f..69dc1cb7 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -61,8 +61,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i validDecimationValue_(1) { - qRegisterMetaType("rtabmap::SensorData"); - qRegisterMetaType("rtabmap::OdometryInfo"); + qRegisterMetaType("rtabmap::OdometryEvent"); imageView_->setImageDepthShown(false); imageView_->setMinimumSize(320, 240); @@ -136,15 +135,15 @@ void OdometryViewer::clear() cloudView_->clear(); } -void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info) +void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) { processingData_ = true; - int quality = info.inliers; + int quality = odom.info().inliers; bool lost = false; bool lostStateChanged = false; - if(data.pose().isNull()) + if(odom.pose().isNull()) { UDEBUG("odom lost"); // use last pose lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed; @@ -153,11 +152,11 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap lost = true; } - else if(info.inliers>0 && + else if(odom.info().inliers>0 && qualityWarningThr_ && - info.inliers < qualityWarningThr_) + odom.info().inliers < qualityWarningThr_) { - UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, qualityWarningThr_); + UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_); lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed; imageView_->setBackgroundColor(Qt::darkYellow); cloudView_->setBackgroundColor(Qt::darkYellow); @@ -170,14 +169,16 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap cloudView_->setBackgroundColor(Qt::black); } - timeLabel_->setText(QString("%1 s").arg(info.time)); + timeLabel_->setText(QString("%1 s").arg(odom.info().time)); - if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f) + if(!odom.data().imageRaw().empty() && + !odom.data().depthOrRightRaw().empty() && + (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size())) { - UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality); + UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality); - if(data.image().cols % decimationSpin_->value() == 0 && - data.image().rows % decimationSpin_->value() == 0) + if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 && + odom.data().imageRaw().rows % decimationSpin_->value() == 0) { validDecimationValue_ = decimationSpin_->value(); } @@ -186,8 +187,8 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap UWARN("Decimation (%d) must be a denominator of the width and height of " "the image (%d/%d). Using last valid decimation value (%d).", decimationSpin_->value(), - data.image().cols, - data.image().rows, + odom.data().imageRaw().cols, + odom.data().imageRaw().rows, validDecimationValue_); } @@ -195,35 +196,15 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap // visualization: buffering the clouds // Create the new cloud pcl::PointCloud::Ptr cloud; - if(!data.depth().empty()) - { - cloud = util3d::cloudFromDepthRGB( - data.image(), - data.depth(), - data.cx(), data.cy(), - data.fx(), data.fy(), - validDecimationValue_); - } - else if(!data.rightImage().empty()) - { - cloud = util3d::cloudFromStereoImages( - data.image(), - data.rightImage(), - data.cx(), data.cy(), - data.fx(), data.baseline(), - validDecimationValue_); - } - - if(voxelSpin_->value() > 0.0f && cloud->size()) - { - cloud = util3d::voxelize(cloud, voxelSpin_->value()); - } + cloud = util3d::cloudRGBFromSensorData( + odom.data(), + validDecimationValue_, + 0.0f, + voxelSpin_->value()); if(cloud->size()) { - cloud = util3d::transformPointCloud(cloud, data.localTransform()); - - if(!data.pose().isNull()) + if(!odom.pose().isNull()) { if(cloudView_->getAddedClouds().contains("cloudtmp")) { @@ -236,10 +217,10 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap addedClouds_.pop_front(); } - data.id()?id_=data.id():++id_; + odom.data().id()?id_=odom.data().id():++id_; std::string cloudName = uFormat("cloud%d", id_); addedClouds_.push_back(cloudName); - UASSERT(cloudView_->addCloud(cloudName, cloud, data.pose())); + UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose())); } else { @@ -248,18 +229,18 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap } } - if(!data.pose().isNull()) + if(!odom.pose().isNull()) { - lastOdomPose_ = data.pose(); - cloudView_->updateCameraTargetPosition(data.pose()); + lastOdomPose_ = odom.pose(); + cloudView_->updateCameraTargetPosition(odom.pose()); } - if(info.localMap.size()) + if(odom.info().localMap.size()) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - cloud->resize(info.localMap.size()); + cloud->resize(odom.info().localMap.size()); int i=0; - for(std::multimap::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter) + for(std::multimap::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter) { (*cloud)[i].x = iter->second.x; (*cloud)[i].y = iter->second.y; @@ -268,17 +249,17 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap cloudView_->addOrUpdateCloud("localmap", cloud); } - if(!data.image().empty()) + if(!odom.data().imageRaw().empty()) { - if(info.type == 0) + if(odom.info().type == 0) { - imageView_->setFeatures(info.words, data.depth(), Qt::yellow); + imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow); } - else if(info.type == 1) + else if(odom.info().type == 1) { std::vector kpts; - cv::KeyPoint::convert(info.refCorners, kpts); - imageView_->setFeatures(kpts, data.depth(), Qt::red); + cv::KeyPoint::convert(odom.info().refCorners, kpts); + imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red); } imageView_->clearLines(); @@ -290,7 +271,7 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap odomImageShow_ = imageView_->isImageShown(); odomImageDepthShow_ = imageView_->isImageDepthShown(); } - imageView_->setImageDepth(uCvMat2QImage(data.image())); + imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw())); imageView_->setImageShown(true); imageView_->setImageDepthShown(true); } @@ -303,55 +284,55 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap imageView_->setImageDepthShown(odomImageDepthShow_); } - imageView_->setImage(uCvMat2QImage(data.image())); + imageView_->setImage(uCvMat2QImage(odom.data().imageRaw())); if(imageView_->isImageDepthShown()) { - imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage())); + imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); } - if(info.type == 0) + if(odom.info().type == 0) { if(imageView_->isFeaturesShown()) { - for(unsigned int i=0; isetFeatureColor(info.wordMatches[i], Qt::red); // outliers + imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers } - for(unsigned int i=0; isetFeatureColor(info.wordInliers[i], Qt::green); // inliers + imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers } } } } - if(info.type == 1 && info.cornerInliers.size()) + if(odom.info().type == 1 && odom.info().cornerInliers.size()) { if(imageView_->isFeaturesShown() || imageView_->isLinesShown()) { //draw lines - UASSERT(info.refCorners.size() == info.newCorners.size()); - for(unsigned int i=0; iisFeaturesShown()) { - imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers + imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers } if(imageView_->isLinesShown()) { imageView_->addLine( - info.refCorners[info.cornerInliers[i]].x, - info.refCorners[info.cornerInliers[i]].y, - info.newCorners[info.cornerInliers[i]].x, - info.newCorners[info.cornerInliers[i]].y, + odom.info().refCorners[odom.info().cornerInliers[i]].x, + odom.info().refCorners[odom.info().cornerInliers[i]].y, + odom.info().newCorners[odom.info().cornerInliers[i]].x, + odom.info().newCorners[odom.info().cornerInliers[i]].y, Qt::blue); } } } } - if(!data.image().empty()) + if(!odom.data().imageRaw().empty()) { - imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows)); + imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows)); } } @@ -372,8 +353,7 @@ void OdometryViewer::handleEvent(UEvent * event) { processingData_ = true; QMetaObject::invokeMethod(this, "processData", - Q_ARG(rtabmap::SensorData, odomEvent->data()), - Q_ARG(rtabmap::OdometryInfo, odomEvent->info())); + Q_ARG(rtabmap::OdometryEvent, *odomEvent)); } } } diff --git a/guilib/src/PdfPlot.cpp b/guilib/src/PdfPlot.cpp index 59630be8..27f075ef 100644 --- a/guilib/src/PdfPlot.cpp +++ b/guilib/src/PdfPlot.cpp @@ -70,10 +70,10 @@ void PdfPlotItem::showDescription(bool shown) { QImage img; QMap::const_iterator iter = _signaturesRef->find(int(this->data().x())); - if(iter != _signaturesRef->constEnd() && !iter.value().getImageCompressed().empty()) + if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty()) { cv::Mat image; - iter.value().uncompressDataConst(&image, 0, 0); + iter.value().sensorData().uncompressDataConst(&image, 0, 0); if(!image.empty()) { img = uCvMat2QImage(image); diff --git a/tools/Camera/main.cpp b/tools/Camera/main.cpp index 92923a03..d60c0910 100644 --- a/tools/Camera/main.cpp +++ b/tools/Camera/main.cpp @@ -189,7 +189,7 @@ int main(int argc, char * argv[]) } cv::Mat rgb; - rgb = camera?camera->takeImage():dbReader->getNextData().image(); + rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw(); cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window while(!rgb.empty()) { @@ -199,7 +199,7 @@ int main(int argc, char * argv[]) if(c == 27) break; // if ESC, break and quit - rgb = camera?camera->takeImage():dbReader->getNextData().image(); + rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw(); } cv::destroyWindow("Video"); if(camera) diff --git a/utilite/include/rtabmap/utilite/UMath.h b/utilite/include/rtabmap/utilite/UMath.h index b8a35cf9..db096131 100644 --- a/utilite/include/rtabmap/utilite/UMath.h +++ b/utilite/include/rtabmap/utilite/UMath.h @@ -63,6 +63,28 @@ inline bool uIsFinite(const T & value) #endif } +/** + * Get the minimum of the 3 values. + * @return the minimum value + */ +template +inline T uMin3( const T& a, const T& b, const T& c) +{ + float m=a +inline T uMax3( const T& a, const T& b, const T& c) +{ + float m=a>b?a:b; + return m>c?m:c; +} + /** * Get the maximum of a vector. * @param v the array From 9e13642a47108b4789ff38ef11a8ebcb6c0dc164 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Sat, 30 May 2015 20:05:35 -0400 Subject: [PATCH 02/45] fixed runtime errors for single depth camera and stereo --- corelib/include/rtabmap/core/CameraModel.h | 2 +- corelib/include/rtabmap/core/OdometryEvent.h | 25 ++-- corelib/include/rtabmap/core/Rtabmap.h | 2 +- corelib/include/rtabmap/core/SensorData.h | 2 +- corelib/include/rtabmap/core/Signature.h | 12 +- corelib/include/rtabmap/core/Statistics.h | 21 +-- corelib/include/rtabmap/core/Transform.h | 51 ++++--- corelib/src/DBDriverSqlite3.cpp | 2 - corelib/src/DBReader.cpp | 2 +- corelib/src/Memory.cpp | 6 +- corelib/src/Rtabmap.cpp | 126 ++++++---------- corelib/src/RtabmapThread.cpp | 11 +- corelib/src/SensorData.cpp | 28 ++-- corelib/src/Signature.cpp | 14 +- corelib/src/Transform.cpp | 146 +++++++++---------- corelib/src/util3d.cpp | 2 +- examples/RGBDMapping/MapBuilder.h | 4 +- examples/WifiMapping/MapBuilderWifi.h | 21 ++- guilib/src/DatabaseViewer.cpp | 2 + guilib/src/MainWindow.cpp | 28 +++- 20 files changed, 232 insertions(+), 275 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 702d6865..ab4f4cea 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -137,7 +137,7 @@ public: double baseline, const Transform & localTransform = Transform::getIdentity()) : left_(fx, fy, cx, cy, localTransform), - right_(fx, fy, cx, cy, localTransform, baseline*-right_.fx()) + right_(fx, fy, cx, cy, localTransform, baseline*-fx) { } virtual ~StereoCameraModel() {} diff --git a/corelib/include/rtabmap/core/OdometryEvent.h b/corelib/include/rtabmap/core/OdometryEvent.h index 0fa8b2d9..365cb350 100644 --- a/corelib/include/rtabmap/core/OdometryEvent.h +++ b/corelib/include/rtabmap/core/OdometryEvent.h @@ -38,6 +38,20 @@ namespace rtabmap { class OdometryEvent : public UEvent { +public: + static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance) + { + UASSERT(uIsFinite(rotVariance) && rotVariance>0); + UASSERT(uIsFinite(transVariance) && transVariance>0); + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); + covariance.at(0,0) = transVariance; + covariance.at(1,1) = transVariance; + covariance.at(2,2) = transVariance; + covariance.at(3,3) = rotVariance; + covariance.at(4,4) = rotVariance; + covariance.at(5,5) = rotVariance; + return covariance; + } public: OdometryEvent() : _covariance(cv::Mat::eye(6,6,CV_64FC1)) @@ -69,22 +83,13 @@ public: const OdometryInfo & info = OdometryInfo()) : _data(data), _pose(pose), - _covariance(cv::Mat::eye(6,6,CV_64FC1)), + _covariance(generateCovarianceMatrix(rotVariance, transVariance)), _info(info) { - UASSERT(uIsFinite(rotVariance) && rotVariance>0); - UASSERT(uIsFinite(transVariance) && transVariance>0); - _covariance.at(0,0) = transVariance; - _covariance.at(1,1) = transVariance; - _covariance.at(2,2) = transVariance; - _covariance.at(3,3) = rotVariance; - _covariance.at(4,4) = rotVariance; - _covariance.at(5,5) = rotVariance; } virtual ~OdometryEvent() {} virtual std::string getClassName() const {return "OdometryEvent";} - bool isValid() const {return !_pose.isNull();} SensorData & data() {return _data;} const SensorData & data() const {return _data;} const Transform & pose() const {return _pose;} diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 096a0c98..1744db8b 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -159,7 +159,7 @@ private: private: // Modifiable parameters bool _publishStats; - bool _publishLastSignature; + bool _publishLastSignatureData; bool _publishPdf; bool _publishLikelihood; float _maxTimeAllowed; // in ms diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 078c577e..48a1085d 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -167,7 +167,7 @@ public: void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const; const std::vector & cameraModels() const {return _cameraModels;} - StereoCameraModel stereoCameraModel() const {return _stereoCameraModel;} + const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;} void setUserData(const std::vector & data) {_userData = data;} const std::vector & userData() const {return _userData;} diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index 37da953a..e7fa84ad 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -53,12 +53,10 @@ class RTABMAP_EXP Signature public: Signature(); Signature(int id, - int mapId, - int weight, - double stamp, - const std::string & label, - const std::multimap & words, - const std::multimap & words3, + int mapId = -1, + int weight = 0, + double stamp = 0.0, + const std::string & label = std::string(), const Transform & pose = Transform(), const std::vector & userData = std::vector(), const SensorData & sensorData = SensorData()); @@ -141,7 +139,7 @@ private: // times in the signature, it will be 2 times in this list) // Words match with the CvSeq keypoints and descriptors std::multimap _words; // word - std::multimap _words3; // word + std::multimap _words3; // word // in base_link frame (localTransform applied)) std::map _wordsChanged; // bool _enabled; diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 55cb0a9c..cc2dcf56 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -136,11 +136,7 @@ public: void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;} void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;} - void setMapIds(const std::map & mapIds) {_mapIds = mapIds;} - void setLabels(const std::map & labels) {_labels = labels;} - void setStamps(const std::map & stamps) {_stamps = stamps;} - void setUserDatas(const std::map > & userDatas) {_userDatas = userDatas;} - void setSignature(const Signature & s) {_signature = s;} + void setSignatures(const std::map & signatures) {_signatures = signatures;} void setPoses(const std::map & poses) {_poses = poses;} void setConstraints(const std::multimap & constraints) {_constraints = constraints;} @@ -159,11 +155,7 @@ public: int loopClosureId() const {return _loopClosureId;} int localLoopClosureId() const {return _localLoopClosureId;} - const std::map & getMapIds() const {return _mapIds;} - const std::map & getLabels() const {return _labels;} - const std::map & getStamps() const {return _stamps;} - const std::map > & getUserDatas() const {return _userDatas;} - const Signature & getSignature() const {return _signature;} + const std::map & getSignatures() const {return _signatures;} const std::map & poses() const {return _poses;} const std::multimap & constraints() const {return _constraints;} @@ -185,14 +177,7 @@ private: int _loopClosureId; int _localLoopClosureId; - // extended data start here... - std::map _mapIds; - std::map _labels; - std::map _stamps; - std::map > _userDatas; - - // Signature data - Signature _signature; + std::map _signatures; std::map _poses; std::multimap _constraints; diff --git a/corelib/include/rtabmap/core/Transform.h b/corelib/include/rtabmap/core/Transform.h index 83221795..2ed7ead8 100644 --- a/corelib/include/rtabmap/core/Transform.h +++ b/corelib/include/rtabmap/core/Transform.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -46,25 +47,27 @@ public: Transform(float r11, float r12, float r13, float o14, float r21, float r22, float r23, float o24, float r31, float r32, float r33, float o34); + // should have 3 rows, 4 cols and type CV_32FC1 + Transform(const cv::Mat & transformationMatrix); // x,y,z, roll,pitch,yaw Transform(float x, float y, float z, float roll, float pitch, float yaw); - float r11() const {return data_[0];} - float r12() const {return data_[1];} - float r13() const {return data_[2];} - float r21() const {return data_[4];} - float r22() const {return data_[5];} - float r23() const {return data_[6];} - float r31() const {return data_[8];} - float r32() const {return data_[9];} - float r33() const {return data_[10];} + float r11() const {return data()[0];} + float r12() const {return data()[1];} + float r13() const {return data()[2];} + float r21() const {return data()[4];} + float r22() const {return data()[5];} + float r23() const {return data()[6];} + float r31() const {return data()[8];} + float r32() const {return data()[9];} + float r33() const {return data()[10];} - float o14() const {return data_[3];} - float o24() const {return data_[7];} - float o34() const {return data_[11];} + float o14() const {return data()[3];} + float o24() const {return data()[7];} + float o34() const {return data()[11];} - float & operator[](int index) {return data_[index];} - const float & operator[](int index) const {return data_[index];} + float & operator[](int index) {return data()[index];} + const float & operator[](int index) const {return data()[index];} bool isNull() const; bool isIdentity() const; @@ -72,16 +75,16 @@ public: void setNull(); void setIdentity(); - const float * data() const {return data_.data();} - float * data() {return data_.data();} - int size() const {return (int)data_.size();} + const float * data() const {return (const float *)data_.data;} + float * data() {return (float *)data_.data;} + int size() const {return 12;} - float & x() {return data_[3];} - float & y() {return data_[7];} - float & z() {return data_[11];} - const float & x() const {return data_[3];} - const float & y() const {return data_[7];} - const float & z() const {return data_[11];} + float & x() {return data()[3];} + float & y() {return data()[7];} + float & z() {return data()[11];} + const float & x() const {return data()[3];} + const float & y() const {return data()[7];} + const float & z() const {return data()[11];} float theta() const; @@ -121,7 +124,7 @@ public: static Transform fromEigen3d(const Eigen::Isometry3d & matrix); private: - std::vector data_; + cv::Mat data_; }; RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s); diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 364fc8d3..68f67d63 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -1349,8 +1349,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< weight, stamp, label, - std::multimap(), - std::multimap(), pose, userData); s->setSaved(true); diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index ba248bd7..e1478778 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -149,7 +149,7 @@ void DBReader::mainLoopBegin() void DBReader::mainLoop() { OdometryEvent odom = this->getNextData(); - if(odom.isValid()) + if(odom.data().id()) { int goalId = 0; double previousStamp = odom.data().stamp(); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index c13b4b58..509c6e9c 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -4027,8 +4027,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p 0, data.stamp(), "", - words, - words3D, pose, data.userData(), stereoCameraModel.isValid()? @@ -4054,8 +4052,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p 0, data.stamp(), "", - words, - words3D, pose, data.userData(), SensorData( @@ -4063,6 +4059,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p data.laserScanMaxPts(), cv::Mat(), cv::Mat(), CameraModel(), id)); } + s->setWords(words); + s->setWords3(words3D); if(this->isRawDataKept()) { s->sensorData().setImageRaw(image); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3e624c21..ef9ac059 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -73,7 +73,7 @@ namespace rtabmap Rtabmap::Rtabmap() : _publishStats(Parameters::defaultRtabmapPublishStats()), - _publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()), + _publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()), _publishPdf(Parameters::defaultRtabmapPublishPdf()), _publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()), _maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms @@ -364,7 +364,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) } Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats); - Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature); + Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData); Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf); Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood); Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed); @@ -2023,44 +2023,6 @@ bool Rtabmap::process( statistics_.setMapCorrection(_mapCorrection); UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); - // Set local graph - if(!_rgbdSlamMode) - { - // no optimization on appearance-only mode, create a local graph - std::map ids = _memory->getNeighborsId(signature->id(), 0, 0, true); - std::map poses; - std::map mapIds; - std::map labels; - std::map stamps; - std::map > userDatas; - std::multimap constraints; - _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false); - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - Transform odomPose; - int weight = -1; - int mapId = -1; - std::string label; - double stamp = 0; - std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false); - mapIds.insert(std::make_pair(iter->first, mapId)); - labels.insert(std::make_pair(iter->first, label)); - stamps.insert(std::make_pair(iter->first, stamp)); - userDatas.insert(std::make_pair(iter->first, userData)); - } - statistics_.setPoses(poses); - statistics_.setConstraints(constraints); - statistics_.setMapIds(mapIds); - statistics_.setLabels(labels); - statistics_.setStamps(stamps); - statistics_.setUserDatas(userDatas); - } - else // RGBD-SLAM mode - { - //see after transfer below - } - // timings... statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000); @@ -2084,11 +2046,6 @@ bool Rtabmap::process( //Epipolar geometry constraint statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0); - if(_publishLastSignature) - { - statistics_.setSignature(*signature); - } - if(_publishLikelihood || _publishPdf) { // Child count by parent signature on the root of the memory ... for statistics @@ -2146,6 +2103,12 @@ bool Rtabmap::process( _memory->deleteLocation(signature->id()); } + Signature lastSignatureData(signature->id()); + if(_publishLastSignatureData) + { + lastSignatureData = *signature; + } + // Pass this point signature should not be used, since it could have been transferred... signature = 0; @@ -2231,36 +2194,48 @@ bool Rtabmap::process( // place after transfer because the memory/local graph may have changed statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size()); statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size()); - statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size()); - if(_rgbdSlamMode) + std::map signatures; + if(_publishLastSignatureData) { - std::map mapIds; - std::map labels; - std::map stamps; - std::map > userDatas; - for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) - { - Transform odomPose; - int weight = -1; - int mapId = -1; - std::string label; - double stamp = 0; - std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true); - mapIds.insert(std::make_pair(iter->first, mapId)); - labels.insert(std::make_pair(iter->first, label)); - stamps.insert(std::make_pair(iter->first, stamp)); - userDatas.insert(std::make_pair(iter->first, userData)); - } - statistics_.setPoses(_optimizedPoses); - statistics_.setConstraints(_constraints); - statistics_.setMapIds(mapIds); - statistics_.setLabels(labels); - statistics_.setStamps(stamps); - statistics_.setUserDatas(userDatas); + signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData)); } - + // Set local graph + std::map poses; + std::multimap constraints; + if(!_rgbdSlamMode) + { + // no optimization on appearance-only mode, create a local graph + std::map ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true); + _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false); + } + else // RGBD-SLAM mode + { + poses = _optimizedPoses; + constraints = _constraints; + } + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + Transform odomPose; + int weight = -1; + int mapId = -1; + std::string label; + double stamp = 0; + std::vector userData; + _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false); + signatures.insert(std::make_pair(iter->first, + Signature(iter->first, + mapId, + weight, + stamp, + label, + odomPose, + userData))); + } + statistics_.setPoses(poses); + statistics_.setConstraints(constraints); + statistics_.setSignatures(signatures); + statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size()); } //Start trashing @@ -2780,8 +2755,6 @@ void Rtabmap::get3DMap( weight, stamp, label, - std::multimap(), - std::multimap(), odomPose, userData, data))); @@ -2842,11 +2815,8 @@ void Rtabmap::getGraph( weight, stamp, label, - std::multimap(), - std::multimap(), odomPose, - userData, - SensorData()))); + userData))); } } } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index ac106802..51f3d4d8 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -295,7 +295,7 @@ void RtabmapThread::handleEvent(UEvent* event) { UDEBUG("OdometryEvent"); OdometryEvent * e = (OdometryEvent*)event; - if(e->isValid()) + if(!e->pose().isNull()) { this->addData(*e); } @@ -476,7 +476,7 @@ void RtabmapThread::process() { OdometryEvent data; getData(data); - if(data.isValid() && _state.empty()) + if(data.data().isValid() && _state.empty()) { if(_rtabmap->getMemory()) { @@ -499,12 +499,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) { if(!_paused) { - if(!odomEvent.isValid()) - { - ULOGGER_ERROR("data not valid !?"); - return; - } - if(_rate>0.0f) { if(_frameRateTimer->getElapsedTime() < 1.0f/_rate) @@ -552,6 +546,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) { _transVariance = 1.0; } + UDEBUG("Added data %d", odomEvent.data().id()); _dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance)); _rotVariance = 0; _transVariance = 0; diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 96e4e303..4384d362 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -169,15 +169,15 @@ SensorData::SensorData( depth.type() == CV_16UC1); // Depth in millimetre _depthOrRightRaw = depth; } - if(laserScan.rows == 1) + + if(laserScan.type() == CV_32FC2) { - UASSERT(laserScan.type() == CV_8UC1); // Bytes - _laserScanCompressed = laserScan; + _laserScanRaw = laserScan; } else if(!laserScan.empty()) { - UASSERT(laserScan.type() == CV_32FC2); - _laserScanRaw = laserScan; + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; } } @@ -262,15 +262,14 @@ SensorData::SensorData( _depthOrRightRaw = depth; } - if(laserScan.rows == 1) + if(laserScan.type() == CV_32FC2) { - UASSERT(laserScan.type() == CV_8UC1); // Bytes - _laserScanCompressed = laserScan; + _laserScanRaw = laserScan; } else if(!laserScan.empty()) { - UASSERT(laserScan.type() == CV_32FC2); - _laserScanRaw = laserScan; + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; } for(unsigned int i=0; i & words, - const std::multimap & words3, // in base_link frame (localTransform applied) const Transform & pose, const std::vector & userData, - const SensorData & sensorData) : + const SensorData & sensorData): _id(id), _mapId(mapId), _stamp(stamp), @@ -68,12 +65,15 @@ Signature::Signature( _saved(false), _modified(true), _linksModified(true), - _words(words), - _words3(words3), _enabled(false), _pose(pose), _sensorData(sensorData) { + if(_sensorData.id() == 0) + { + _sensorData.setId(id); + } + UASSERT(_sensorData.id() == _id); } Signature::~Signature() diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index 863a8340..cb025873 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -31,44 +31,33 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap { -Transform::Transform() : data_(12) +Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1)) { - data_[0] = 0.0f; - data_[1] = 0.0f; - data_[2] = 0.0f; - data_[3] = 0.0f; - data_[4] = 0.0f; - data_[5] = 0.0f; - data_[6] = 0.0f; - data_[7] = 0.0f; - data_[8] = 0.0f; - data_[9] = 0.0f; - data_[10] = 0.0f; - data_[11] = 0.0f; } // rotation matrix r## and origin o## -Transform::Transform(float r11, float r12, float r13, float o14, - float r21, float r22, float r23, float o24, - float r31, float r32, float r33, float o34) : - data_(12) +Transform::Transform( + float r11, float r12, float r13, float o14, + float r21, float r22, float r23, float o24, + float r31, float r32, float r33, float o34) { - data_[0] = r11; - data_[1] = r12; - data_[2] = r13; - data_[3] = o14; - data_[4] = r21; - data_[5] = r22; - data_[6] = r23; - data_[7] = o24; - data_[8] = r31; - data_[9] = r32; - data_[10] = r33; - data_[11] = o34; + data_ = (cv::Mat_(3,4) << + r11, r12, r13, o14, + r21, r22, r23, o24, + r31, r32, r33, o34); +} + +Transform::Transform(const cv::Mat & transformationMatrix) +{ + UASSERT(transformationMatrix.cols == 4 && + transformationMatrix.rows == 3 && + transformationMatrix.type() == CV_32FC1); + data_ = transformationMatrix; } Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw) @@ -79,46 +68,46 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y bool Transform::isNull() const { - return (data_[0] == 0.0f && - data_[1] == 0.0f && - data_[2] == 0.0f && - data_[3] == 0.0f && - data_[4] == 0.0f && - data_[5] == 0.0f && - data_[6] == 0.0f && - data_[7] == 0.0f && - data_[8] == 0.0f && - data_[9] == 0.0f && - data_[10] == 0.0f && - data_[11] == 0.0f) || - uIsNan(data_[0]) || - uIsNan(data_[1]) || - uIsNan(data_[2]) || - uIsNan(data_[3]) || - uIsNan(data_[4]) || - uIsNan(data_[5]) || - uIsNan(data_[6]) || - uIsNan(data_[7]) || - uIsNan(data_[8]) || - uIsNan(data_[9]) || - uIsNan(data_[10]) || - uIsNan(data_[11]); + return (data()[0] == 0.0f && + data()[1] == 0.0f && + data()[2] == 0.0f && + data()[3] == 0.0f && + data()[4] == 0.0f && + data()[5] == 0.0f && + data()[6] == 0.0f && + data()[7] == 0.0f && + data()[8] == 0.0f && + data()[9] == 0.0f && + data()[10] == 0.0f && + data()[11] == 0.0f) || + uIsNan(data()[0]) || + uIsNan(data()[1]) || + uIsNan(data()[2]) || + uIsNan(data()[3]) || + uIsNan(data()[4]) || + uIsNan(data()[5]) || + uIsNan(data()[6]) || + uIsNan(data()[7]) || + uIsNan(data()[8]) || + uIsNan(data()[9]) || + uIsNan(data()[10]) || + uIsNan(data()[11]); } bool Transform::isIdentity() const { - return data_[0] == 1.0f && - data_[1] == 0.0f && - data_[2] == 0.0f && - data_[3] == 0.0f && - data_[4] == 0.0f && - data_[5] == 1.0f && - data_[6] == 0.0f && - data_[7] == 0.0f && - data_[8] == 0.0f && - data_[9] == 0.0f && - data_[10] == 1.0f && - data_[11] == 0.0f; + return data()[0] == 1.0f && + data()[1] == 0.0f && + data()[2] == 0.0f && + data()[3] == 0.0f && + data()[4] == 0.0f && + data()[5] == 1.0f && + data()[6] == 0.0f && + data()[7] == 0.0f && + data()[8] == 0.0f && + data()[9] == 0.0f && + data()[10] == 1.0f && + data()[11] == 0.0f; } void Transform::setNull() @@ -145,16 +134,17 @@ Transform Transform::inverse() const Transform Transform::rotation() const { - return Transform(data_[0], data_[1], data_[2], 0, - data_[4], data_[5], data_[6], 0, - data_[8], data_[9], data_[10], 0); + return Transform( + data()[0], data()[1], data()[2], 0, + data()[4], data()[5], data()[6], 0, + data()[8], data()[9], data()[10], 0); } Transform Transform::translation() const { - return Transform(1,0,0, data_[3], - 0,1,0, data_[7], - 0,0,1, data_[11]); + return Transform(1,0,0, data()[3], + 0,1,0, data()[7], + 0,0,1, data()[11]); } void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const @@ -215,7 +205,7 @@ Transform & Transform::operator*=(const Transform & t) bool Transform::operator==(const Transform & t) const { - return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0; + return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0; } bool Transform::operator!=(const Transform & t) const @@ -239,18 +229,18 @@ std::ostream& operator<<(std::ostream& os, const Transform& s) Eigen::Matrix4f Transform::toEigen4f() const { Eigen::Matrix4f m; - m << data_[0], data_[1], data_[2], data_[3], - data_[4], data_[5], data_[6], data_[7], - data_[8], data_[9], data_[10], data_[11], + m << data()[0], data()[1], data()[2], data()[3], + data()[4], data()[5], data()[6], data()[7], + data()[8], data()[9], data()[10], data()[11], 0,0,0,1; return m; } Eigen::Matrix4d Transform::toEigen4d() const { Eigen::Matrix4d m; - m << data_[0], data_[1], data_[2], data_[3], - data_[4], data_[5], data_[6], data_[7], - data_[8], data_[9], data_[10], data_[11], + m << data()[0], data()[1], data()[2], data()[3], + data()[4], data()[5], data()[6], data()[7], + data()[8], data()[9], data()[10], data()[11], 0,0,0,1; return m; } diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 30905df9..e1d9e1aa 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -569,7 +569,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( { leftMono = sensorData.imageRaw(); } - return cloudFromDisparity( + cloud = cloudFromDisparity( util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()), sensorData.stereoCameraModel().left().cx(), sensorData.stereoCameraModel().left().cy(), diff --git a/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index d675b064..6166fe18 100644 --- a/examples/RGBDMapping/MapBuilder.h +++ b/examples/RGBDMapping/MapBuilder.h @@ -191,9 +191,9 @@ protected slots: } cloudViewer_->setCloudVisibility(cloudName, true); } - else if(stats.getSignature().id() == iter->first) + else if(uContains(stats.getSignatures(), iter->first)) { - Signature s = stats.getSignature(); + Signature s = stats.getSignatures().at(iter->first); s.sensorData().uncompressData(); // make sure data is uncompressed // Add the new cloud pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( diff --git a/examples/WifiMapping/MapBuilderWifi.h b/examples/WifiMapping/MapBuilderWifi.h index 24e7b86f..1328fa10 100644 --- a/examples/WifiMapping/MapBuilderWifi.h +++ b/examples/WifiMapping/MapBuilderWifi.h @@ -78,26 +78,25 @@ protected slots: std::map nodeStamps; // std::map > wifiLevels; - UASSERT(stats.getStamps().size() == stats.getUserDatas().size()); - std::map::const_iterator iterStamps = stats.getStamps().begin(); - std::map >::const_iterator iterUserDatas = stats.getUserDatas().begin(); - for(; iterStamps!=stats.getStamps().end() && iterUserDatas!=stats.getUserDatas().end(); ++iterStamps, ++iterUserDatas) + for(std::map::const_iterator iter=stats.getSignatures().begin(); + iter!=stats.getSignatures().end(); + ++iter) { - // Sort stamps by stamps - nodeStamps.insert(std::make_pair(iterStamps->second, iterStamps->first)); + // Sort stamps by stamps->id + nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first)); // convert userData to wifi levels - if(iterUserDatas->second.size()) + if(iter->second.getUserData().size()) { - UASSERT(iterUserDatas->second.size() == sizeof(int)+sizeof(double)); + UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double)); // format [int level, double stamp] int level; double stamp; - memcpy(&level, iterUserDatas->second.data(), sizeof(int)); - memcpy(&stamp, iterUserDatas->second.data()+sizeof(int), sizeof(double)); + memcpy(&level, iter->second.getUserData().data(), sizeof(int)); + memcpy(&stamp, iter->second.getUserData().data()+sizeof(int), sizeof(double)); - wifiLevels.insert(std::make_pair(iterUserDatas->first, std::make_pair(level, stamp))); + wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp))); } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index addab20f..ff0381eb 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2252,12 +2252,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) float groundNormalMaxAngle = M_PI_4; int minClusterSize = 20; cv::Mat ground, obstacles; + util3d::occupancy2DFromCloud3D( cloud, ground, obstacles, cellSize, groundNormalMaxAngle, minClusterSize); + if(!ground.empty() || !obstacles.empty()) { localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles))); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 39a10edf..259e8cf6 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -960,8 +960,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) totalTime.start(); //Affichage des stats et images - int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1); - int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1)); + int refMapId = -1, loopMapId = -1; + if(uContains(stat.getSignatures(), stat.refImageId())) + { + refMapId = stat.getSignatures().at(stat.refImageId()).mapId(); + } + if(uContains(stat.getSignatures(), stat.loopClosureId())) + { + loopMapId = stat.getSignatures().at(stat.loopClosureId()).mapId(); + } _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId)); _ui->label_matchId->clear(); @@ -985,9 +992,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->imageView_loopClosure->setBackgroundColor(Qt::black); // update cache - Signature signature = stat.getSignature(); - signature.sensorData().uncompressData(); // make sure data are uncompressed - _cachedSignatures.insert(stat.getSignature().id(), signature); + Signature signature; + if(uContains(stat.getSignatures(), stat.refImageId())) + { + signature = stat.getSignatures().at(stat.refImageId()); + signature.sensorData().uncompressData(); // make sure data are uncompressed + _cachedSignatures.insert(signature.id(), signature); + } int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f); @@ -1164,10 +1175,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) if(stat.poses().size()) { // update pose only if odometry is not received + std::map mapIds; + for(std::map::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter) + { + mapIds.insert(std::make_pair(iter->first, iter->second.mapId())); + } updateMapCloud(stat.poses(), _odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second, stat.constraints(), - stat.getMapIds()); + mapIds); _odometryReceived = false; From bd9fb1027b16e53ebe668fd217261aa5b958ee6d Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Sun, 31 May 2015 01:26:57 -0400 Subject: [PATCH 03/45] Added util3d::laserScanFomrDepthImage() and some refactoring --- corelib/include/rtabmap/core/util3d.h | 19 +++ .../include/rtabmap/core/util3d_conversions.h | 57 ------- corelib/src/CMakeLists.txt | 1 - corelib/src/Memory.cpp | 1 - corelib/src/Odometry.cpp | 9 +- corelib/src/Rtabmap.cpp | 26 ++-- corelib/src/util3d.cpp | 138 +++++++++++++++++ corelib/src/util3d_conversions.cpp | 142 ------------------ corelib/src/util3d_features.cpp | 5 +- corelib/src/util3d_mapping.cpp | 1 - guilib/src/DatabaseViewer.cpp | 1 - guilib/src/LoopClosureViewer.cpp | 1 - guilib/src/MainWindow.cpp | 1 - tools/CameraRGBD/main.cpp | 1 - 14 files changed, 177 insertions(+), 226 deletions(-) delete mode 100644 corelib/include/rtabmap/core/util3d_conversions.h delete mode 100644 corelib/src/util3d_conversions.cpp diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 951532cc..4de46ae3 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -117,6 +117,25 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( float voxelSize = 0.0f, int samples = 0); +pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( + const cv::Mat & depthImage, + float fx, + float fy, + float cx, + float cy, + float maxDepth = 0, + const Transform & localTransform = Transform::getIdentity()); + +cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); +cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); + +cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud); +pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); + +pcl::PointCloud::Ptr RTABMAP_EXP cvMat2Cloud( + const cv::Mat & matrix, + const Transform & tranform = Transform::getIdentity()); + pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D( const cv::Point2f & pt, float disparity, diff --git a/corelib/include/rtabmap/core/util3d_conversions.h b/corelib/include/rtabmap/core/util3d_conversions.h deleted file mode 100644 index 9d70388e..00000000 --- a/corelib/include/rtabmap/core/util3d_conversions.h +++ /dev/null @@ -1,57 +0,0 @@ -/* -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 UTIL3D_CONVERSIONS_H_ -#define UTIL3D_CONVERSIONS_H_ - -#include - -#include -#include -#include -#include - -namespace rtabmap -{ - -namespace util3d -{ - -cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); -cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); - -cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud); -pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); - -pcl::PointCloud::Ptr RTABMAP_EXP cvMat2Cloud( - const cv::Mat & matrix, - const Transform & tranform = Transform::getIdentity()); - -} // namespace util3d -} // namespace rtabmap - -#endif /* UTIL3D_CONVERSIONS_H_ */ diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 847b7b61..757a160c 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -35,7 +35,6 @@ SET(SRC_FILES util3d_surface.cpp util3d_features.cpp util3d_correspondences.cpp - util3d_conversions.cpp SensorData.cpp Graph.cpp diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 509c6e9c..091369ef 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -41,7 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "VisualWord.h" #include "rtabmap/core/Features2d.h" #include "DBDriverSqlite3.h" -#include "rtabmap/core/util3d_conversions.h" #include "rtabmap/core/util3d_features.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_correspondences.h" diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index a6eb5dc8..38bbcacd 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -95,13 +95,8 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) UASSERT(!data.depthOrRightRaw().empty()); } - if(data.cameraModels().size() > 1) - { - UERROR("Odometry doesn't support multi-camera yet."); - return Transform(); - } - else if(!data.stereoCameraModel().isValid() && - (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) + if(!data.stereoCameraModel().isValid() && + (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) { UERROR("Rectified images required! Calibrate your camera."); return Transform(); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index ef9ac059..a57d0b65 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1413,17 +1413,21 @@ bool Rtabmap::process( { if(immunizedLocally >= maxLocalLocationsImmunized) { - UWARN("Could not immunize the whole local path (%d) between " - "%d and %d (max location immunized=%d). You may want " - "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) " - "to be able to immunize longer paths.", - (int)path.size(), - nearestId, - signature->id(), - maxLocalLocationsImmunized, - _localImmunizationRatio, - maxLocalLocationsImmunized, - (int)_memory->getWorkingMem().size()); + // set 20 to avoid this warning when starting mapping + if(maxLocalLocationsImmunized > 20) + { + UWARN("Could not immunize the whole local path (%d) between " + "%d and %d (max location immunized=%d). You may want " + "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) " + "to be able to immunize longer paths.", + (int)path.size(), + nearestId, + signature->id(), + maxLocalLocationsImmunized, + _localImmunizationRatio, + maxLocalLocationsImmunized, + (int)_memory->getWorkingMem().size()); + } break; } else if(!_memory->isInSTM(iter->first)) diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index e1d9e1aa..69ab45fa 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap @@ -723,6 +724,143 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( return cloud; } +pcl::PointCloud laserScanFromDepthImage( + const cv::Mat & depthImage, + float fx, + float fy, + float cx, + float cy, + float maxDepth, + const Transform & localTransform) +{ + UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1); + UASSERT(!localTransform.isNull()); + + pcl::PointCloud scan; + int middle = depthImage.rows/2; + if(middle) + { + scan.resize(depthImage.cols); + int oi = 0; + for(int i=0; i(i,j)*1000.0f); + unsigned short depthMM = 0; + if(depth <= (float)USHRT_MAX) + { + depthMM = (unsigned short)depth; + } + depth16U.at(i, j) = depthMM; + } + } + } + return depth16U; +} + +cv::Mat cvtDepthToFloat(const cv::Mat & depth16U) +{ + UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1); + cv::Mat depth32F; + if(!depth16U.empty()) + { + depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1); + for(int i=0; i(i,j))/1000.0f; + depth32F.at(i, j) = depth; + } + } + } + return depth32F; +} + +cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud) +{ + cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); + for(unsigned int i=0; i(i)[0] = cloud.at(i).x; + laserScan.at(i)[1] = cloud.at(i).y; + } + return laserScan; +} + +pcl::PointCloud::Ptr laserScanToPointCloud(const cv::Mat & laserScan) +{ + UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2); + + pcl::PointCloud::Ptr output(new pcl::PointCloud); + output->resize(laserScan.cols); + for(int i=0; iat(i).x = laserScan.at(i)[0]; + output->at(i).y = laserScan.at(i)[1]; + } + return output; +} + +pcl::PointCloud::Ptr cvMat2Cloud( + const cv::Mat & matrix, + const Transform & tranform) +{ + UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3); + UASSERT(matrix.rows == 1); + + Eigen::Affine3f t = tranform.toEigen3f(); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->resize(matrix.cols); + if(matrix.channels() == 2) + { + for(int i=0; iat(i).x = matrix.at(0,i)[0]; + cloud->at(i).y = matrix.at(0,i)[1]; + cloud->at(i).z = 0.0f; + cloud->at(i) = pcl::transformPoint(cloud->at(i), t); + } + } + else // channels=3 + { + for(int i=0; iat(i).x = matrix.at(0,i)[0]; + cloud->at(i).y = matrix.at(0,i)[1]; + cloud->at(i).z = matrix.at(0,i)[2]; + cloud->at(i) = pcl::transformPoint(cloud->at(i), t); + } + } + return cloud; +} + // inspired from ROS image_geometry/src/stereo_camera_model.cpp pcl::PointXYZ projectDisparityTo3D( const cv::Point2f & pt, diff --git a/corelib/src/util3d_conversions.cpp b/corelib/src/util3d_conversions.cpp deleted file mode 100644 index b48fd441..00000000 --- a/corelib/src/util3d_conversions.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* -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. -*/ - -#include "rtabmap/core/util3d_conversions.h" - -#include "rtabmap/utilite/ULogger.h" -#include - -namespace rtabmap -{ - -namespace util3d -{ - -cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F) -{ - UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1); - cv::Mat depth16U; - if(!depth32F.empty()) - { - depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1); - for(int i=0; i(i,j)*1000.0f); - unsigned short depthMM = 0; - if(depth <= (float)USHRT_MAX) - { - depthMM = (unsigned short)depth; - } - depth16U.at(i, j) = depthMM; - } - } - } - return depth16U; -} - -cv::Mat cvtDepthToFloat(const cv::Mat & depth16U) -{ - UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1); - cv::Mat depth32F; - if(!depth16U.empty()) - { - depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1); - for(int i=0; i(i,j))/1000.0f; - depth32F.at(i, j) = depth; - } - } - } - return depth32F; -} - -cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud) -{ - cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2); - for(unsigned int i=0; i(i)[0] = cloud.at(i).x; - laserScan.at(i)[1] = cloud.at(i).y; - } - return laserScan; -} - -pcl::PointCloud::Ptr laserScanToPointCloud(const cv::Mat & laserScan) -{ - UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2); - - pcl::PointCloud::Ptr output(new pcl::PointCloud); - output->resize(laserScan.cols); - for(int i=0; iat(i).x = laserScan.at(i)[0]; - output->at(i).y = laserScan.at(i)[1]; - } - return output; -} - -pcl::PointCloud::Ptr cvMat2Cloud( - const cv::Mat & matrix, - const Transform & tranform) -{ - UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3); - UASSERT(matrix.rows == 1); - - Eigen::Affine3f t = tranform.toEigen3f(); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - cloud->resize(matrix.cols); - if(matrix.channels() == 2) - { - for(int i=0; iat(i).x = matrix.at(0,i)[0]; - cloud->at(i).y = matrix.at(0,i)[1]; - cloud->at(i).z = 0.0f; - cloud->at(i) = pcl::transformPoint(cloud->at(i), t); - } - } - else // channels=3 - { - for(int i=0; iat(i).x = matrix.at(0,i)[0]; - cloud->at(i).y = matrix.at(0,i)[1]; - cloud->at(i).z = matrix.at(0,i)[2]; - cloud->at(i) = pcl::transformPoint(cloud->at(i), t); - } - } - return cloud; -} - -} - -} diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index 999d5f22..9b5f8693 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -82,8 +82,9 @@ pcl::PointCloud::Ptr generateKeypoints3DDepth( cameraModels.at(cameraIndex).fy(), true); - if(!cameraModels.at(cameraIndex).localTransform().isNull() && - !cameraModels.at(cameraIndex).localTransform().isIdentity()) + if(pcl::isFinite(pt) && + !cameraModels.at(cameraIndex).localTransform().isNull() && + !cameraModels.at(cameraIndex).localTransform().isIdentity()) { pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform()); } diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index f724ea92..9cb312e8 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_mapping.h" -#include #include #include #include diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index ff0381eb..ab45a203 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -49,7 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/KeypointItem.h" #include "rtabmap/gui/UCv2Qt.h" #include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_conversions.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_surface.h" diff --git a/guilib/src/LoopClosureViewer.cpp b/guilib/src/LoopClosureViewer.cpp index cb9bd8ea..841a663f 100644 --- a/guilib/src/LoopClosureViewer.cpp +++ b/guilib/src/LoopClosureViewer.cpp @@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Memory.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_transforms.h" -#include "rtabmap/core/util3d_conversions.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/Signature.h" #include "rtabmap/utilite/ULogger.h" diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 259e8cf6..acb1fbf9 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -86,7 +86,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_conversions.h" #include "rtabmap/core/util3d_mapping.h" #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_registration.h" diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 7c20e38f..7577c7e6 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_conversions.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UMath.h" From 7d72aa83bc66d3d2ee857a6ba5b0ed7e36fa12a4 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Sat, 6 Jun 2015 19:20:38 -0400 Subject: [PATCH 04/45] fixed sensor data not loaded on 0.10.0 database version --- corelib/src/DBDriverSqlite3.cpp | 8 ++++---- corelib/src/Memory.cpp | 12 ++++++++++-- corelib/src/Rtabmap.cpp | 10 +++++++--- corelib/src/SensorData.cpp | 17 ++++++++++++++++- 4 files changed, 37 insertions(+), 10 deletions(-) diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 68f67d63..2dbd4d07 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -2608,6 +2608,10 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, } UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + // scan_max_pts + rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + // scan if(!sensorData.laserScanCompressed().empty()) { @@ -2619,10 +2623,6 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, } UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - // scan_max_pts - rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts()); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - //step rc=sqlite3_step(ppStmt); UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 091369ef..f70c1d33 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2331,7 +2331,9 @@ Transform Memory::computeIcpTransform( } else { - msg = "Depths 3D empty?!?"; + msg = uFormat("Depths 3D empty?!? (new[%d]=%d old[%d]=%d)", + newS.id(), newS.sensorData().depthOrRightRaw().total(), + oldS.id(), oldS.sensorData().depthOrRightRaw().total()); UERROR(msg.c_str()); } } @@ -2459,7 +2461,9 @@ Transform Memory::computeIcpTransform( } else { - msg = "Depths 2D empty?!?"; + msg = uFormat("Depths 2D empty?!? (new[%d]=%d old[%d]=%d)", + newS.id(), newS.sensorData().laserScanRaw().total(), + oldS.id(), oldS.sensorData().laserScanRaw().total()); UERROR(msg.c_str()); } } @@ -3148,6 +3152,10 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData) else { _dbDriver->getNodeData(nodeId, r); + if(uncompressedData) + { + r.uncompressData(); + } } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index a57d0b65..e7c5b29e 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2180,6 +2180,7 @@ bool Rtabmap::process( //============================================================== // Finalize statistics and log files //============================================================== + int localGraphSize = 0; if(_publishStats) { statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000); @@ -2240,6 +2241,7 @@ bool Rtabmap::process( statistics_.setConstraints(constraints); statistics_.setSignatures(signatures); statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size()); + localGraphSize = poses.size(); } //Start trashing @@ -2271,7 +2273,7 @@ bool Rtabmap::process( timeEmptyingTrash, timeRetrievalDbAccess, timeAddLoopClosureLink); - std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", + std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", _loopClosureHypothesis.first, _highestHypothesis.first, (int)signaturesRemoved.size(), @@ -2286,9 +2288,11 @@ bool Rtabmap::process( lcHypothesisReactivated, refUniqueWordsCount, retrievalId, - 0.0f, + 0, rehearsalMaxId, - rehearsalMaxId>0?1:0); + rehearsalMaxId>0?1:0, + localGraphSize, + data.id()); if(_statisticLogsBufferedInRAM) { _bufferedLogsF.push_back(logF); diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 4384d362..b378de28 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -367,7 +367,9 @@ SensorData::SensorData( void SensorData::uncompressData() { - uncompressData(&_imageRaw, &_depthOrRightRaw, &_laserScanRaw); + uncompressData(_imageCompressed.empty()?0:&_imageRaw, + _depthOrRightCompressed.empty()?0:&_depthOrRightRaw, + _laserScanCompressed.empty()?0:&_laserScanRaw); } void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) @@ -426,14 +428,27 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv: if(imageRaw && imageRaw->empty()) { *imageRaw = ctImage.getUncompressedData(); + if(imageRaw->empty()) + { + UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id()); + } } if(depthRaw && depthRaw->empty()) { *depthRaw = ctDepth.getUncompressedData(); + if(depthRaw->empty()) + { + UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id()); + } } if(laserScanRaw && laserScanRaw->empty()) { *laserScanRaw = ctLaserScan.getUncompressedData(); + + if(laserScanRaw->empty()) + { + UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id()); + } } } } From b8dccc2228762157a382e90c658a3409c9ef14a3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 11 Jun 2015 16:57:16 -0400 Subject: [PATCH 05/45] Added CameraStereoImages class to read stereo images from a directory. Added a particle filter to smooth odometry trajectory. Added parameter RGBD/OptimizeEpsilon to limit TORO iterations when error improvement is small. Added Rtabmap/CreateIntermediateNodes parameter: this can be used to keep all odometry poses 'between' nodes used for loop closure detection. Added PnP approach to loop closure constraint estimation. Fixed decimation of stereo images when image size is odd. --- Matlab/ParticleFilter/pf_filter.m | 17 + Matlab/ParticleFilter/pf_filter.m~ | 18 + Matlab/ParticleFilter/pf_resample.m | 23 + Matlab/ParticleFilter/test_kinect.m | 67 ++ Matlab/ParticleFilter/test_kinect.m~ | 50 + Matlab/ParticleFilter/test_kitti_datasets.m | 28 + Matlab/ParticleFilter/test_odometry.m | 28 + corelib/include/rtabmap/core/Camera.h | 1 + corelib/include/rtabmap/core/CameraModel.h | 6 +- corelib/include/rtabmap/core/CameraRGBD.h | 53 +- corelib/include/rtabmap/core/Graph.h | 8 +- corelib/include/rtabmap/core/Link.h | 21 + corelib/include/rtabmap/core/Memory.h | 4 + corelib/include/rtabmap/core/Odometry.h | 12 +- corelib/include/rtabmap/core/OdometryInfo.h | 11 +- corelib/include/rtabmap/core/OdometryThread.h | 8 +- corelib/include/rtabmap/core/Parameters.h | 24 +- corelib/include/rtabmap/core/Rtabmap.h | 2 + corelib/include/rtabmap/core/RtabmapEvent.h | 2 + corelib/include/rtabmap/core/RtabmapThread.h | 11 +- corelib/src/BayesFilter.cpp | 10 +- corelib/src/BayesFilter.h | 2 + corelib/src/Camera.cpp | 13 + corelib/src/CameraModel.cpp | 20 +- corelib/src/CameraRGBD.cpp | 231 ++++- corelib/src/CameraThread.cpp | 9 +- corelib/src/Graph.cpp | 29 +- corelib/src/Memory.cpp | 367 ++++++-- corelib/src/Odometry.cpp | 112 ++- corelib/src/OdometryBOW.cpp | 21 +- corelib/src/OdometryThread.cpp | 26 +- corelib/src/ParticleFilter.h | 173 ++++ corelib/src/Rtabmap.cpp | 134 ++- corelib/src/RtabmapThread.cpp | 107 ++- corelib/src/util2d.cpp | 2 +- corelib/src/util3d.cpp | 52 +- guilib/include/rtabmap/gui/MainWindow.h | 3 + guilib/include/rtabmap/gui/OdometryViewer.h | 3 +- .../include/rtabmap/gui/PreferencesDialog.h | 6 +- guilib/src/DatabaseViewer.cpp | 8 +- guilib/src/MainWindow.cpp | 536 ++++++----- guilib/src/OdometryViewer.cpp | 47 +- guilib/src/PdfPlot.cpp | 5 +- guilib/src/PreferencesDialog.cpp | 77 +- guilib/src/ui/DatabaseViewer.ui | 112 ++- guilib/src/ui/mainWindow.ui | 10 +- guilib/src/ui/preferencesDialog.ui | 855 +++++++++++++----- guilib/src/utilite/UPlot.cpp | 11 + tools/CameraRGBD/main.cpp | 5 +- 49 files changed, 2651 insertions(+), 729 deletions(-) create mode 100644 Matlab/ParticleFilter/pf_filter.m create mode 100644 Matlab/ParticleFilter/pf_filter.m~ create mode 100644 Matlab/ParticleFilter/pf_resample.m create mode 100644 Matlab/ParticleFilter/test_kinect.m create mode 100644 Matlab/ParticleFilter/test_kinect.m~ create mode 100644 Matlab/ParticleFilter/test_kitti_datasets.m create mode 100644 Matlab/ParticleFilter/test_odometry.m create mode 100644 corelib/src/ParticleFilter.h diff --git a/Matlab/ParticleFilter/pf_filter.m b/Matlab/ParticleFilter/pf_filter.m new file mode 100644 index 00000000..37a7339a --- /dev/null +++ b/Matlab/ParticleFilter/pf_filter.m @@ -0,0 +1,17 @@ + +function filtered = pf_filter(x, nParticles, noise, lambda) + +particles = zeros(nParticles,1) ; +weights = zeros(nParticles,1); +filtered=zeros(1,length(x)); +for i = 1:length(x); + for j = 1:nParticles + rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++ + particles(j) = particles(j) + noise*rn ; + dist = abs(particles(j) - x(i)); + weights(j) = exp(-lambda*dist); + end + weights = weights ./(sum(weights(:))); + filtered(i) = weights'*particles; + particles = pf_resample(particles, weights); +end \ No newline at end of file diff --git a/Matlab/ParticleFilter/pf_filter.m~ b/Matlab/ParticleFilter/pf_filter.m~ new file mode 100644 index 00000000..339fdf02 --- /dev/null +++ b/Matlab/ParticleFilter/pf_filter.m~ @@ -0,0 +1,18 @@ + +function filtered = pf_filter(x, nParticles, noise, lambda) + +particles = zeros(nParticles,1) ; +weights = zeros(nParticles,1); +filtered=zeros(1,length(x)); +for i = 1:length(x); + for j = 1:nParticles + rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++ + bruit= noise*rn; + particles(j) = particles(j) + bruit ; + dist = abs(particles(j) - x(i)); + weights(j) = exp(-lambda*dist); + end + weights = weights ./(sum(weights(:))); + filtered(i) = weights'*particles; + particles = Rresample2(particles,weights); +end \ No newline at end of file diff --git a/Matlab/ParticleFilter/pf_resample.m b/Matlab/ParticleFilter/pf_resample.m new file mode 100644 index 00000000..55f1018f --- /dev/null +++ b/Matlab/ParticleFilter/pf_resample.m @@ -0,0 +1,23 @@ + +function newParticles=pf_resample(particles,weights) +pcum = zeros(length(weights),1); +sum = 0; +for i=1:length(weights) + pcum(i) = weights(i) + sum; + sum = sum + weights(i); +end +pcum = pcum./pcum(end); +newParticles = 0.*particles; + +% +for i = 1:length(newParticles) + indexx = 1; + randnum = rand; + for j = 1:length(pcum) + if(randnum < pcum(j)) + indexx = j; + break; + end + end + newParticles(i) = particles(indexx); +end diff --git a/Matlab/ParticleFilter/test_kinect.m b/Matlab/ParticleFilter/test_kinect.m new file mode 100644 index 00000000..cc7760f5 --- /dev/null +++ b/Matlab/ParticleFilter/test_kinect.m @@ -0,0 +1,67 @@ + +%close all + +% signals +index = [1 2 4 5 6 8 10 12 13 14 15 17 18 20 21 23 24 25 26 28 29 31 32 33 35 36 38 39 41 42 43 45 46 48 49 50 51 52 53 55 56 58 59 60 62 63 65 66 68 70 72 73 74 75 77 78 80 81 83 84 86 88 90 91 92 94 95 96 98 100 101 103 105 106 108 109 111 113 114 116 117 118 120 122 123 125 126 128 129 131 132 134 135 137 138 139 141 142 144 145 146 148 149 150 152 153 154 156 157 159 161 162 164 165 167 168 169 171 172 174 176 177 178 180 181 182 184 186 187 188 189 191 193 195 196 198 199 201 202 203 205 206 207 208 210 212 213 216 217 218 220 221 223 224 225 227 228 229 230 232 233 234 236 237 239 240 241 243 244 246 247 249 250 251 253 254 256 257 259 260 262 263 264 265 266 268 269 270 273 274 276 277 280 281 283 284 286 288 289 291 293 294 296 297 299 301 302 303 304 305 307 308 310 311 313 314 316 317 318 320 322 323 325 326 328 329 330 331 333 335 338 339 340 342 343 345 347 348 350 352 354 355 357 359 361 363 365 368 369 370 372 375 378 380 383 386 389 390 392 394 396 398 401 404 407 410 413 415 418 421 423 425 428 431 434 437 440 443 446 449 452 455 459 462 464 467 469 472 475 478 481 484 487 490 493 496 499 501 503 506 509 512 514 517 520 521 523 526 529 531 534 536 538 540]; + +stddev = [0 0.00183007 0.00192045 0.00161173 0.00109756 0.0016129 0.00187094 0.00164845 0.00172004 0.00178055 0.00146903 0.00153716 0.00153812 0.00185564 0.00165944 0.00178402 0.00177258 0.0110605 0.0186308 0.00726018 0.0096298 0.00578044 0.0173129 0.010495 0.00641252 0.0140946 0.00828691 0.00646527 0.0134895 0.00693482 0.00649181 0.0181309 0.0131438 0.00996371 0.00707931 0.0103485 0.0061651 0.00802035 0.0132984 0.00562768 0.00741177 0.0116417 0.00769641 0.00804565 0.05021 0.00682934 0.0129143 0.0225555 0.0127159 0.01415 0.0380939 0.0259584 0.0158027 0.0211238 0.0110875 0.0276258 0.0280592 0.0166966 0.0130543 0.0215521 0.0142676 0.0153512 0.0316784 0.0118649 0.0123691 0.0205413 0.0135362 0.0216125 0.0212237 0.00991641 0.0184909 0.0261524 0.0119713 0.0201614 0.0124039 0.0147738 0.0264302 0.0159957 0.0253929 0.0102058 0.0243234 0.0377172 0.0165959 0.0337177 0.0311854 0.0129289 0.0306891 0.0156638 0.0129385 0.0346115 0.0108297 0.0267145 0.0143579 0.0151814 0.0120711 0.0234515 0.010673 0.0141592 0.0133022 0.0140912 0.0109111 0.00720432 0.00984503 0.00544388 0.0150391 0.0120823 0.00699634 0.00620808 0.00564909 0.00469504 0.00484 0.0103237 0.00416761 0.00430465 0.00704729 0.004031 0.00422873 0.00754686 0.00478419 0.00442305 0.00741142 0.00629216 0.00676839 0.0068492 0.00492891 0.00640475 0.00507572 0.010178 0.0131225 0.00749722 0.00502731 0.00653215 0.00653063 0.00557653 0.00488872 0.00889771 0.0062636 0.00854236 0.00660393 0.00829196 0.00756908 0.00466529 0.00435093 0.00407168 0.00518314 0.00739503 0.0108294 0.00535951 0.00556133 0.00516625 0.0107237 0.00540061 0.0066455 0.00579536 0.00673659 0.00604048 0.00707398 0.0170932 0.00686887 0.0070607 0.00701904 0.0063693 0.00857325 0.00773534 0.0148109 0.0138909 0.013436 0.00893684 0.0087548 0.0108629 0.023048 0.011821 0.0163904 0.00790121 0.0069128 0.0110736 0.0111562 0.00968563 0.00775927 0.00795869 0.0080748 0.00909579 0.011114 0.00957061 0.0114517 0.011365 0.0113641 0.012989 0.0115229 0.012728 0.0104824 0.012118 0.0156755 0.0312968 0.0221914 0.0130828 0.0245588 0.00755494 0.00518046 0.00578518 0.0165867 0.0193008 0.0113112 0.0081156 0.00917008 0.00480015 0.0041285 0.0042448 0.00499233 0.00531906 0.00434526 0.00711454 0.00767021 0.00522772 0.00435821 0.00478461 0.00454364 0.00498411 0.00459049 0.00635743 0.00710944 0.00546137 0.00624892 0.0101038 0.00895114 0.00736796 0.00727758 0.00951425 0.0115899 0.00858932 0.0374993 0.0078321 0.00838064 0.0191884 0.0116717 0.0297484 0.0114346 0.0109484 0.02485 0.0117224 0.0167142 0.0107344 0.0188225 0.0123141 0.0273968 0.014611 0.0341862 0.0134783 0.0271164 0.0268258 0.0128071 0.0106398 0.0125586 0.0319245 0.0107098 0.0147609 0.0120215 0.0106572 0.0162843 0.0153122 0.010042 0.011171 0.0121647 0.0102679 0.00730296 0.0124738 0.0115997 0.0179616 0.0140927 0.0130449 0.0104011 0.0146438 0.0114065 0.0157396 0.0135855 0.0128285 0.00754485 0.0305995 0.0181798 0.0192597 0.048465 0.0189101 0.0121396 0.00705945 0.0104833 0.00804011 0.0114006 0.00754285 0.00809562 0.00543453 0.00707061 0.0126759 0.0128725 0.0104646 0.021338 0.00769287 0.00642344 0.00572439 0.00467889 0.00776383 0.00501854 0.0044633 0.00535785 0.00198798 0.00168449 0.00151264 0.00165532 0.0014229 0.0012654 0.00145315 0.00129016 0.00136739 0.00136014 0.00162558]; + +x = [0 -4.24346e-05 8.71535e-05 7.73439e-05 8.19646e-05 0.000129101 -0.000123236 -3.81299e-05 -9.68599e-05 -5.53157e-05 0.000115778 -2.39115e-05 0.000118613 0.00010519 3.61086e-05 -5.87477e-05 0.000160884 0.00621398 -0.0120009 0.00291489 0.003279 0.00576099 0.0150894 -0.00203845 0.000224768 0.00938943 -0.00811659 0.00119158 0.00746353 0.000121729 6.94127e-05 -0.00419329 -0.00568876 0.0139627 0.00477986 0.00962919 -0.0013915 -0.00812751 -0.00531474 -0.00482226 -0.00294706 -0.00801682 -0.0120678 -0.00846131 0.00427019 -0.0102197 -0.0124834 -0.0172146 -0.0103388 0.00629085 -0.0235327 0.00480739 -0.012399 0.00482727 -0.0133079 -0.0235706 -0.013214 -0.0103451 -0.0124942 -0.0113611 -0.0147532 -0.0156376 -0.0140083 -0.0117917 -0.00889636 -0.00979012 -0.0130616 -0.0115069 -0.00713748 -0.00853417 -0.0125016 -0.0152137 -0.0137736 -0.017015 -0.00826265 -0.011039 -0.010147 -0.0102702 -0.0115688 -0.00834822 -0.00480605 -0.00944234 -0.00613833 -0.00378726 -0.00531372 -0.00313342 0.0016912 -0.00846053 -0.000238551 0.00999922 0.00545356 0.00835875 0.00236814 0.00551584 0.0107257 0.0192157 0.00484362 0.0160764 0.0151525 0.00104963 0.0101495 0.0084537 0.00141839 0.00637473 0.0137394 -0.000386742 0.00881242 0.00421751 -9.91609e-05 0.00131641 -0.00112953 -0.00849616 0.00188975 0.00111134 0.000226679 0.00372954 0.0044746 0.000745515 0.00419199 0.00522998 0.00256299 0.00610309 0.00232603 0.00656134 0.00905603 0.0075174 0.00816977 0.00727421 0.0137445 0.00779005 0.00792792 0.00653672 0.00822117 0.00938434 0.00731794 0.0107173 0.00811708 0.0119406 0.00695679 0.00982725 0.0147284 0.0119065 0.0124385 0.0122512 0.0137476 0.0108564 0.0148077 0.00818148 0.00425363 0.00718716 0.00917207 0.00465425 0.00723085 0.00538666 0.00288388 0.00340082 0.00374876 -0.00419625 0.00672228 0.00246597 0.00614105 0.0041607 -0.00100455 0.000259257 0.00801794 0.00982123 0.00809857 0.00416525 -0.00185046 -0.00260187 0.0142119 0.0101674 0.0122865 0.00114529 0.000846205 0.00832562 -0.00102725 0.00684259 0.00459711 0.00405431 0.00127849 0.00401698 0.00291901 0.00223591 0.000508513 -0.000883496 -0.00164503 -0.00268374 -0.00238574 -0.00118393 0.000937702 -0.0055201 -0.0073194 0.0152312 0.011294 -0.0086854 -0.0147189 -0.00558232 -0.00394173 0.00198213 0.00608358 -0.0108543 0.00461987 -0.00406849 -0.00326745 -0.000672017 0.00256795 0.00272686 -0.00039404 0.000865531 0.0041187 0.00910968 0.00672891 0.00234949 0.00273352 0.00181897 0.00125234 0.00475895 0.00389612 0.00212937 0.00385255 0.00355574 0.00119965 -0.00219954 0.00283924 0.0045105 0.00317797 0.00898716 0.0114602 0.00378875 0.00679902 -0.00121378 0.00429642 0.00696563 0.0112035 0.000287787 -0.00228596 0.00212873 0.00858424 0.0097049 0.00384796 0.00699805 0.0061758 0.0122655 0.00828882 0.0117052 0.00224441 -0.000388189 0.00874184 0.01205 0.0113738 0.00235464 0.00850786 -0.00388995 0.0111763 0.000643106 0.00639816 0.00978384 0.0052487 0.0127941 0.00993185 0.00441308 0.00313138 -0.00533244 -0.0096972 -0.00973506 -0.00905878 -0.0100051 -0.00301525 0.00448708 0.00885024 0.0112419 0.0165715 0.00982144 0.0109893 0.011797 0.0129021 0.00862637 0.0115569 0.0131937 0.0185587 0.0220149 0.0183705 0.00855004 0.0065538 0.00464233 0.00377284 0.00377567 0.0098812 0.0093124 0.00208854 0.0107114 0.00588716 -0.00395249 -0.0138018 -0.00139743 0.00231474 -0.00210971 -0.00098593 -0.00416663 -0.000202776 -0.000503991 0.00291823 -0.000509565 -0.00013748 6.83721e-05 -6.62129e-05 -5.95589e-06 0.000163029 -2.94313e-05 -1.41226e-06 -0.000127159 0.00013134 -0.00025853]; +y = [0 0.000204328 -0.000359824 7.60799e-05 -9.37671e-05 -5.80152e-05 -5.49278e-05 0.000120296 0.000109765 0.000342621 -6.31708e-07 -0.000325124 2.26864e-05 0.000213688 0.000144319 0.000240986 0.000209789 -0.0086677 0.0129371 -0.00393705 -0.00240819 -0.00103602 -0.00633166 0.00905698 0.00757496 -0.00216864 0.0126588 0.00563069 0.000585525 0.0093867 0.0127259 0.0111814 0.0333927 0.00932608 0.0160619 0.0021804 0.0121537 0.00945414 0.0325899 0.0149477 0.0227223 0.0196483 0.0188425 0.029973 0.0479662 0.0196213 0.0120299 0.00944461 0.0119208 0.0183874 -0.00192976 0.0143755 -0.000162104 0.00960468 0.00514583 0.00164096 0.00554287 0.00691961 0.00311745 0.00258077 0.00452352 0.00261956 0.00523992 0.00732337 0.00983647 0.00289151 -0.00233132 0.00542192 0.00533552 0.00271515 0.00571588 -0.00239608 -0.000750886 0.00362278 -0.00334313 -0.00153819 -0.000611185 -0.00239244 -0.00338887 -0.000845023 -0.00546571 -0.000248397 -0.000156743 4.33485e-05 0.00194943 0.00384201 -0.00337436 0.00377249 0.00241832 0.00141299 0.00767455 0.00826595 0.0109429 0.0111026 0.00696724 -0.000415096 0.0110438 0.00361291 0.00107296 0.0129179 0.00114137 0.00249299 0.00862582 0.00377351 -0.00451957 0.00962203 -0.0015557 5.56536e-05 0.00136782 -0.00384426 -0.0067822 -0.00139039 -0.00507732 -0.0041727 -0.00435909 -0.00464438 -0.00542727 -0.00391032 -0.00389525 -0.00482716 -0.000177655 -0.00450206 -0.00320455 -0.00202606 -0.00690437 -0.00214849 -0.0044473 -0.00944357 -0.000894671 -0.00796149 -0.0046371 -0.00569116 -0.00547117 -0.00617576 -0.00381936 -0.00761608 -0.00664516 -0.00607395 -0.00741986 -0.00435547 0.000791578 -0.00294566 -0.00471792 -0.00947074 -0.00851501 -0.00565964 -0.00622709 -0.00840516 -0.00855547 -0.00568607 -0.00380322 -0.00484016 -0.00861393 -0.00668283 -0.00647052 -0.00811103 -0.00901533 -0.00616074 -0.00704351 -0.00571293 -0.0072006 -0.00452923 -0.00799317 -0.0106958 -0.010318 -0.00990502 -0.00847562 -0.00616359 -0.00389208 -0.0031538 -0.00541939 -0.00591785 -0.00397182 -0.00323923 -0.0043797 -0.00777217 -0.00716407 -0.00353097 -0.00364774 -0.0043592 -0.00272548 -0.00140546 -0.00137506 -0.00372805 -0.00316043 -0.0042664 -0.00533948 -0.00370825 -0.00949356 -0.00887874 -0.0106823 -0.0057666 0.00114163 -0.0238717 -0.0185981 0.000724678 0.00785414 -0.00517492 -0.00994796 -0.00950159 -0.0174094 0.00158522 -0.0131021 -0.00228096 -0.00421623 -0.00705121 -0.0104503 -0.0109667 -0.0102918 -0.00969282 -0.0110911 -0.00812833 -0.00159531 -0.0049659 -0.00643019 -0.0087362 -0.0100645 -0.00492639 -0.010123 -0.0101391 0.00277095 -0.00151114 -0.00212821 -0.0120646 -0.00629898 -0.00637808 -0.00718914 0.000731233 0.0133484 0.00803888 -0.0114201 -0.0030069 0.00310268 -0.00187037 0.000696273 -0.00652594 -0.00758808 -0.00539047 -0.00455091 -0.00298469 -0.00462718 -0.00434272 -0.0059528 -0.00445012 -0.00729047 -0.00572527 -0.010036 -0.0127272 -0.00523248 -0.00303387 -0.000822378 -0.00691088 -0.00660249 -0.0143434 -0.00203793 -0.0147616 -0.00603344 -0.00145005 -0.00892889 -0.000988243 -0.00505624 -0.00758437 -0.00707416 -0.00871257 -0.0121906 -0.0112068 -0.0108632 -0.0168984 -0.0152977 -0.00964994 -0.00796648 -0.00767111 -0.00484946 -0.00416775 -0.00416906 -0.000244006 -0.00435976 -0.00194637 0.00619571 0.000530164 -0.0129595 -0.00424634 -0.0075872 -0.00930663 -0.0115238 -0.00349045 -0.00358084 -0.00829199 0.00330293 -0.00697479 -0.000493523 -0.013539 -0.000709515 0.00552935 0.011406 0.00095419 -0.0025942 0.00235296 0.000760328 0.00441691 -0.000889965 0.000704515 -0.00292312 0.000802791 -0.00016234 -9.61823e-06 -1.9932e-05 0.000249597 3.93242e-05 -0.000204838 -5.2276e-05 0.000123329 -0.000438072 0.000151965]; +z = [0 -9.64658e-06 7.03945e-05 9.09483e-05 -0.000363336 -0.000193492 -0.000148889 0.000176356 -3.95041e-05 -7.41365e-05 -0.000638669 0.000226437 0.000136281 0.00015953 3.70733e-05 0.000581939 -0.000185053 0.000278609 -0.0039202 -0.00217225 -0.000538441 0.00511944 0.00411814 -6.7842e-06 0.00825446 0.0149711 0.0185738 0.0190497 0.0169868 0.0140352 0.0158413 0.00650451 0.0376733 0.014573 0.0217949 0.00371955 0.011917 0.0186783 0.0232513 0.0207271 0.00402345 0.0306006 0.00907937 0.00406955 0.0360196 0.0044247 0.0204511 0.0043904 0.00245758 0.0141277 0.00578564 -0.00518898 0.00735867 0.00863949 0.00066754 0.00645226 0.00660007 -0.00193774 0.0001358 0.00239511 0.00376228 -0.00560209 0.00468048 0.00514218 0.00427959 0.00962194 -0.00666892 0.00547005 0.0113137 0.00507462 0.0146809 -0.00753733 -0.00387708 0.000707309 0.00176431 -0.00205749 0.00436884 -0.00324613 0.00251866 0.000456466 0.00851096 -0.00379077 0.00780357 0.000613834 0.00198632 0.000343986 0.00882993 0.00645045 0.00365535 0.00520433 0.00646816 0.00627921 0.00254849 0.00404514 -0.000406334 -0.00153583 0.00284635 0.00095495 -0.000853335 0.00076378 -0.00310709 -0.000534566 0.000581196 -0.00187339 -0.00268137 0.000454393 -0.00326402 0.00179018 0.000687445 0.000546748 0.000607353 0.00388971 0.00168854 0.0028017 0.00227525 0.0043876 0.00392035 0.00228023 0.00298029 0.00317809 0.00610749 0.00420175 -0.00208311 0.00396719 0.0012112 -0.00268455 0.00281126 -0.0092204 0.0121282 -0.00560226 -0.000309605 0.00143829 -0.00334084 0.0039812 -8.42744e-05 0.00386651 -0.00361522 0.00143954 0.00190103 0.000522476 -0.0010711 -0.000751263 0.00468789 0.00365573 0.00291032 -0.000568013 0.000134285 0.00054685 -0.00054511 5.70824e-05 0.0035869 0.000179902 0.00419799 0.00451638 0.001585 0.00300293 0.00123728 0.000520918 0.00209513 0.00105725 0.00251548 0.000833801 0.00226301 0.000671691 0.0074015 0.00373656 0.00456477 0.00450154 0.00290425 -0.000299134 0.00107214 0.0023758 0.0054655 0.00332319 0.00425824 0.000836918 0.00776742 0.000956997 0.00232601 0.00316827 0.00522162 0.00109602 0.00303583 -0.000750361 -0.0023667 0.00120399 0.00126766 0.00083609 -0.00235571 8.67329e-06 0.00230422 0.00413383 0.008764 -0.0066263 0.000925412 0.00556172 0.00621337 0.00197899 -0.0026832 0.00249049 -0.0031991 0.0128274 -0.00813766 0.00700039 0.00605056 -0.000924999 0.00121924 0.00762761 0.000906501 0.00607177 0.00741256 0.00571461 0.0019757 -0.000709287 0.00524741 0.00284413 5.00776e-05 0.00946186 0.0075079 0.00183606 -0.00227517 0.00471794 0.00372035 0.0018374 0.00802605 0.00185333 -0.0047035 0.00742628 0.0142569 0.00707371 -0.0115996 -0.00380477 0.00530422 0.00250482 -0.00317097 0.00267832 0.00227734 -0.00941005 -0.000350822 0.00204698 0.00552164 -0.000160836 -0.0034101 -0.00234024 -0.00373448 -0.00287087 0.0133406 0.00085251 -0.00541296 0.00120797 0.00224371 0.0042547 -0.00159053 0.00826554 0.00149697 0.00388531 0.00121759 -0.00269208 -0.00163956 -0.0115823 -0.000228293 -0.00521276 -0.006834 -0.00751079 -0.00512234 -0.00131513 -0.0102579 -0.00768944 -0.00858772 -0.00162672 -0.00753255 -0.00359731 -0.00636032 -0.00375445 -0.00828524 -0.00457094 -0.000285845 0.0057742 0.0151166 -0.000743146 -0.0124465 0.00427572 -0.01353 -0.0103443 -0.0173464 -0.010804 0.00524001 -0.0103347 -0.00637103 -0.01197 -0.00308789 -0.0122685 0.00841686 -0.0105636 -0.00169108 -0.00256534 0.00272505 0.000674399 -0.000668501 0.00244924 -0.000958637 0.000649232 -0.000521342 -0.000155389 0.00024492 6.96772e-05 0.000150486 -0.000123244 1.85501e-06 -0.000152994 -0.000187548 9.34349e-05 -0.000212637 0.000369026]; + +roll = [0 -0.00697247 0.011439 -0.0037672 0.00391318 0.00124693 0.0030287 -0.00448122 -0.00347616 -0.0100247 0.00208189 0.0100625 -0.00181607 -0.011119 -0.00362276 -0.0116096 -0.00818997 -0.0343405 -0.706642 0.595677 0.203868 0.390061 1.04302 -0.154686 -0.177945 -0.212443 -0.9854 0.113863 0.279816 -0.546618 -0.436199 -2.37908 -2.08871 0.0553227 -0.510254 0.180382 -0.958035 -1.40451 -2.38267 -1.12667 -1.75796 -1.13053 -2.63525 -2.60907 -3.58601 -2.52121 -1.79654 -2.25613 -1.33478 -0.732903 -0.9298 -1.47892 -0.380567 -2.02699 -2.13208 -2.24147 -2.18517 -2.52275 -1.79502 -1.37363 -1.88748 -1.03175 -1.65507 -1.38477 -0.399072 -0.40913 -1.59339 -1.60899 -0.863818 -0.991065 -1.57005 -1.64782 -1.7006 -2.22423 -0.83406 -1.46165 -0.843498 -0.850411 -1.12993 -1.07788 -1.37277 -2.00294 -1.33077 -1.40567 -1.8182 -1.64814 -1.8247 -1.90814 -1.44697 -1.8769 -1.93877 -1.46014 -0.793834 -0.629669 -0.301444 -1.32545 -1.30499 -1.20415 -1.4845 -1.58845 -1.34412 -1.74224 -2.00197 -1.53316 -1.68767 -2.06981 -1.59841 -1.49462 -1.48013 -1.15663 -1.35156 -1.39071 -1.25367 -1.49356 -1.44043 -1.13617 -1.15862 -1.05363 -0.743625 -1.10127 -1.00328 -0.573638 -0.618629 -1.68267 -1.02222 -1.2801 -0.653413 -0.506172 -1.2871 -0.495107 -0.361678 -0.493461 -0.762048 -1.4813 -0.441775 -0.531445 -0.551943 -1.37474 -1.5587 -1.70981 0.208783 0.0606307 -0.265095 -0.676214 -1.16917 -1.69928 -1.38808 -0.686836 -0.478791 -0.543355 -0.951801 -1.10595 -1.32134 -1.09503 -1.2005 -1.41269 -2.37385 -1.00221 -1.36333 -0.722503 -0.800073 -0.199508 -1.32356 -1.11201 -0.803518 -0.372442 -0.699338 -0.716352 -0.178544 -0.627999 -1.17488 -1.45338 -1.70703 -1.46451 -1.83811 -1.37418 -1.36829 -1.56187 -1.8804 -2.02195 -1.23298 -1.32484 -1.36442 -1.19876 -1.05828 -0.79918 -1.14581 -1.26577 -1.6265 -1.91479 -1.76773 -1.67093 0.0926437 0.0319117 -0.333926 -1.41818 -1.17289 -1.52312 -1.84482 -1.38148 -1.46084 -3.27883 -0.549232 -1.75344 -1.06523 -0.537597 -0.630611 -0.969317 -1.32225 -1.92723 -1.27726 -1.5211 -1.12751 -0.428813 -0.807155 -0.574223 -0.952206 -1.10544 -1.34586 -1.65117 -1.62982 -1.53227 -1.05804 0.0488398 -0.953483 -1.00224 -0.657381 -0.868573 -1.65495 -1.79803 -1.18743 -0.0486594 -1.21401 -1.72776 -1.49407 -1.17884 -1.25465 -1.73811 -0.952386 -0.668799 -0.78184 -1.38569 -0.832198 -1.10262 -0.931131 -0.717825 -1.14293 -1.34193 -1.47225 -1.24035 -0.255386 -0.51049 0.249707 -0.578932 -0.112682 -0.649949 0.0897079 -0.161068 -0.540194 -0.162269 -0.422191 -0.454308 0.575092 0.000792985 0.591403 -0.220016 -0.0570524 -0.460092 -0.851059 -0.544888 0.265254 -0.165818 0.95184 0.00759833 -0.523103 -0.0706023 1.23796 0.403462 -0.0332513 1.4473 3.71438 1.40783 2.70682 1.59649 0.845672 0.921996 0.840801 1.86878 2.44023 2.72089 1.05189 1.47246 1.07162 0.470408 -0.573698 -0.780683 -0.517149 0.180919 -0.136984 -0.328051 -0.739093 0.0983447 0.021223 -0.0187313 0.00106841 -0.00111421 0.00227972 -0.00775409 -0.0021896 0.00944541 0.00373123 -0.0038741 0.0147257 -0.00457401]; +pitch = [0 -0.00229179 0.000499854 -0.00071162 0.003954 -0.00227537 0.000171042 -0.00431982 0.00310028 0.000464334 0.00325114 0.00218017 -0.000226423 -0.00666426 0.0040347 -0.00572827 -0.00252566 0.409739 -0.106965 0.422755 0.6149 1.00093 0.85863 -0.440056 -0.732248 -0.618373 0.675355 -0.377637 0.42139 0.455565 -0.306133 -0.0858105 0.380259 -0.817764 -0.464705 -0.382899 -0.608065 -0.081234 1.74275 -0.573262 0.260839 0.622831 0.783492 0.259257 -0.153289 0.124555 -0.161443 -0.327383 0.752554 0.665943 -0.633409 1.04821 2.11617 0.263793 0.429431 0.126911 0.784859 0.749246 -1.7759 -0.326125 -0.40169 0.583243 0.757008 0.654661 0.669717 0.969141 1.39932 2.11495 1.05803 0.175636 -0.126807 0.124209 1.24958 1.48403 0.71194 0.779027 0.970415 0.41065 0.897172 1.12004 1.26689 1.20792 0.821871 0.860311 0.837231 0.867408 0.530834 0.882044 1.03845 1.19206 0.918048 0.832736 0.611604 0.683121 0.636224 0.744393 1.03378 1.01271 1.49852 1.15168 0.943035 1.20495 0.865091 0.951612 1.21074 1.52488 1.21162 0.850206 0.572902 0.630126 0.684501 0.455256 0.623057 0.505167 0.449704 0.582241 0.374617 0.260643 -0.0204377 0.0256043 -0.26454 -0.319661 -0.0751296 0.32799 0.196522 0.0518103 0.382681 0.3405 0.347285 0.39426 0.388134 -0.339197 0.704119 0.272995 0.281863 -0.424403 0.346723 1.45742 0.276316 0.688671 0.321258 0.0583006 0.140622 0.94608 0.726127 0.757012 1.47441 1.67256 1.41073 0.889791 1.05242 1.17434 1.42673 1.1505 0.668725 0.474379 -0.85654 0.404712 0.177909 0.735431 0.507653 1.32758 0.992634 0.730767 1.74019 1.00772 0.948151 0.836323 0.941635 0.119619 0.158425 0.895213 0.644582 0.152967 0.799178 0.785468 1.37186 0.67333 0.512899 0.746732 -0.109947 0.563172 0.222358 -0.353246 -0.539362 -1.02317 -1.23095 -0.34159 1.01101 1.54783 1.71109 1.26118 1.7363 0.387466 0.254116 -0.090165 -0.0951025 -0.03808 0.947861 0.364725 0.237979 0.0200293 -0.0453891 0.31848 0.916517 0.691965 0.577344 0.63224 0.072274 0.687555 0.754695 0.727415 0.412926 0.368177 0.320089 0.298686 0.352057 0.0202537 -0.0360367 0.714147 0.151168 -0.131895 0.790423 0.397934 0.207399 0.210421 0.553233 0.136282 0.361315 0.694285 1.19553 0.346202 0.820755 0.913812 0.96262 0.531157 0.252745 0.317119 0.575038 0.752224 0.505492 0.900339 0.0876113 0.77845 0.673826 0.298813 -0.220698 0.140829 0.224464 0.169208 0.597158 -0.0845539 0.82377 -0.85227 0.197967 0.231534 -0.0349389 -0.136615 0.444754 -0.198224 -0.530315 -0.873437 -0.293603 0.0188371 0.415711 -0.536511 0.251122 -0.0210569 0.354703 -0.301084 0.359748 -0.204457 -0.340254 -1.98404 -0.14539 -0.278535 -1.04035 -0.646792 -0.353741 0.198721 -0.698947 -0.253543 -0.479373 -0.998304 0.329982 -0.0150488 -0.232996 0.11367 -0.263232 -0.656289 -0.316895 0.235207 0.276941 0.805215 -0.739925 -0.018484 -0.268712 0.115287 -0.0447731 -0.00582324 -0.0905591 0.0474295 0.0073314 -0.00392141 -0.000782993 0.00241239 -0.00109896 0.00113237 -0.00180142 0.0030126 0.00394887 0.0014572 0.000337865 7.99176e-05]; +yaw = [0 -0.000161686 0.0032161 0.00257986 -0.0148895 -0.00775527 -0.0066706 0.00822511 2.74797e-05 -0.00120073 -0.0232415 0.0100288 0.00670114 0.0100244 0.00112335 0.0232722 -0.00544454 -0.430297 0.197413 0.318077 1.19285 0.824785 0.522653 -0.166473 0.697412 -0.127265 1.36169 0.831942 0.756734 -0.247735 -0.508298 -0.983596 0.353067 0.215551 0.432429 0.216151 0.0397583 0.627661 1.3034 0.171752 -0.0724861 1.29244 0.495153 -1.94984 1.68422 -0.493211 0.499761 0.585196 1.1265 1.43676 0.991768 0.831315 0.977594 0.840594 -0.354331 0.778799 0.66594 0.564547 1.24686 1.49184 1.34736 -0.208165 0.506705 0.283346 0.599432 0.850818 0.869317 0.913312 0.614873 0.79196 -0.0188216 -0.247626 -0.239266 0.309909 -0.0584155 -0.0377374 -0.028739 -0.746392 0.0755308 0.181368 0.118504 -1.02735 -0.13515 -0.331222 -0.567641 0.329769 -0.296064 -0.974358 -0.140646 0.244365 0.25554 -0.163399 0.0851915 0.258937 0.0142945 -0.625995 0.113146 -0.307074 -0.440209 0.164854 -0.187281 -0.107339 -0.605807 0.194123 -0.0617479 -0.0317805 0.0771855 -0.0184895 -0.708873 -0.27359 -0.497444 -0.420419 -0.496697 -0.503741 -0.788029 -0.586933 -0.692232 -1.20842 0.278887 -0.346126 0.0570406 -0.124973 0.309732 -0.524967 -0.139757 0.115098 0.0836262 -0.443537 1.08471 0.17619 -0.271993 -0.219413 -0.777199 -0.302626 -0.368529 0.765426 0.194521 0.714603 -0.390747 -0.144703 0.85836 0.996679 0.524187 0.34606 0.381368 -0.418374 0.51134 0.0741821 0.453409 0.575322 -0.204904 -0.129898 -0.550175 -0.449117 -0.364079 -0.551881 -0.539426 -0.195489 -0.376189 0.0524624 0.0870607 0.373965 -0.592063 0.120945 0.805568 0.30351 -0.387133 -0.64074 -0.0218592 -0.0372922 -0.151495 0.136863 -0.766291 -0.90385 -1.13801 -0.865704 -0.0904608 -1.47664 -1.27541 -0.735828 -0.354242 -0.384258 -0.638418 -1.00881 -0.614472 -0.486547 -0.274801 0.132622 0.715475 1.61421 0.940596 0.709336 1.19849 -0.00257673 -0.180559 -1.08725 -0.636779 -0.649876 -0.409159 -0.192793 -1.02339 -0.510792 0.224068 0.597195 1.54105 0.535605 0.723087 0.730369 -0.371158 0.275971 0.144074 0.302977 0.285828 -0.883684 -0.360669 0.0180366 -0.292566 0.188446 -0.140343 -0.407566 -0.662773 -0.6005 -1.12309 -0.648977 -0.477609 0.0492447 -0.322557 -0.273662 0.253909 0.100575 -1.09786 0.154206 1.03418 0.572754 0.12635 -0.0903762 0.0308006 0.131756 0.0105281 -0.491536 -0.141761 -0.94862 0.414641 0.271906 0.604514 0.427947 -0.0365438 0.277222 0.0283782 0.657427 0.724527 -0.295281 0.22394 -0.858543 0.172253 -0.0814776 0.0564259 0.118736 -0.283271 0.592259 0.62106 0.258227 0.0823895 0.133218 -0.114389 0.159368 0.41058 0.408956 -0.23241 0.0399566 -0.378538 -0.470384 0.14432 -1.03465 -0.46647 -0.387396 -0.81182 -1.71838 -0.2637 -0.897728 -1.75969 1.10463 -1.08475 -1.08329 -0.132061 0.0132544 -0.238716 -0.390951 -1.36424 -2.35338 -1.49625 -0.684186 0.293722 -0.105389 -1.0492 -0.316408 -0.792347 0.0951415 0.176206 0.325846 -0.0698992 -0.100712 0.0210328 -0.00260751 0.0101087 0.00475391 0.00586795 -0.00439265 -0.000228314 -0.00558726 -0.00652761 0.00306191 -0.00700772 0.0135158]; + +roll = roll * pi / 180; % to radian +pitch = pitch * pi / 180; % to radian +yaw = yaw * pi / 180; % to radian + + +%parameters +n = 400; +noiseT = 0.002; +lambdaT = 100; +noiseR = 0.002; +lambdaR = 100; + +%filter +x_filtered = pf_filter(x, n, noiseT, lambdaT); +y_filtered = pf_filter(y, n, noiseT, lambdaT); +z_filtered = pf_filter(z, n, noiseT, lambdaT); +roll_filtered = pf_filter(roll, n, noiseR, lambdaR); +pitch_filtered = pf_filter(pitch, n, noiseR, lambdaR); +yaw_filtered = pf_filter(yaw, n, noiseR, lambdaR); + +%show +figure +subplot(4,1,1) +plot(index,x,'b', index,x_filtered,'r'); +legend('x', 'x filtered'); +subplot(4,1,2) +plot(index,y,'b', index,y_filtered,'r'); +legend('y', 'y filtered'); +subplot(4,1,3) +plot(index,z,'b', index,z_filtered,'r'); +legend('z', 'z filtered'); +subplot(4,1,4) +plot(index,stddev,'b'); +legend('stddev'); + +%show +figure +subplot(4,1,1) +plot(index,roll,'b', index,roll_filtered,'r'); +legend('roll', 'roll filtered'); +subplot(4,1,2) +plot(index,pitch,'b', index,pitch_filtered,'r'); +legend('pitch', 'pitch filtered'); +subplot(4,1,3) +plot(index,yaw,'b', index,yaw_filtered,'r'); +legend('yaw', 'yaw filtered') +subplot(4,1,4) +plot(index,stddev,'b'); +legend('stddev'); + + diff --git a/Matlab/ParticleFilter/test_kinect.m~ b/Matlab/ParticleFilter/test_kinect.m~ new file mode 100644 index 00000000..22800993 --- /dev/null +++ b/Matlab/ParticleFilter/test_kinect.m~ @@ -0,0 +1,50 @@ + + +% signals +x = [0 -7.17718e-06 0.000149943 -0.000276212 0.000118147 0.000132833 -7.68572e-05 -0.000388181 6.57036e-05 0.000244131 -0.000265382 0.000674275 -6.0332e-05 0.000352076 0.00041996 -0.000758339 0.00210934 0.000399089 -0.000409156 0.0047982 0.0039244 0.00435027 0.00485974 0.00346061 0.0018604 -0.000905861 0.00250076 0.00214402 0.000318011 -0.00352464 0.00774855 0.00641464 0.00011028 0.00181151 -0.00313881 -0.00159122 0.000872649 -0.00925038 -0.0109046 -0.0279911 -0.00284128 -0.00634648 -0.00987577 -0.00809708 0.00135329 0.00141078 -0.00508487 -0.00524154 -0.0157128 -0.0154952 -0.00648952 -0.011292 -0.00702953 -0.0134704 -0.0102974 -0.0237573 -0.0113637 -0.0136848 -0.0134357 -0.0167649 -0.00662601 -0.00718927 -0.0167545 -0.0117351 -0.00313139 -0.0128256 -0.00886583 -0.00601757 -0.00631785 -0.0136913 -0.0130796 -0.00640869 -0.000587583 -0.00776267 -8.30889e-05 -0.00764275 -0.0047673 -0.00250125 0.00450075 -0.00641263 -0.000849128 0.00847131 0.00656557 0.0119401 0.0175035 0.0104212 0.00938523 0.00605232 0.00872052 0.01063 0.00795197 0.00730991 0.00414711 0.00778383 0.0057314 0.00532299 0.00678048 0.00635234 0.00429028 0.00268266 0.00285921 -0.00125447 -0.00343326 -0.00295475 0.00206432 0.00212367 0.00511998 0.00407538 0.00399027 0.00342568 0.00493171 0.00332177 0.00336831 0.00544102 0.00988577 0.00802416 0.00964469 0.0067216 0.00695488 0.0103022 0.0071584 0.00841331 0.00945374 0.00898707 0.00970355 0.00735274 0.00824642 0.00641495 0.00757965 0.00610715 0.00713819 0.00928026 0.012055 0.0105106 0.0118662 0.0122392 0.0104792 0.00808734 0.00854826 0.00684047 0.0085988 0.00592375 0.0052588 0.00384319 0.00372607 0.00494432 0.00475228 0.00364202 0.00258315 0.00617284 0.00378108 0.00530612 0.00723338 0.00106525 -0.000163257 0.00137579 0.00218695 -0.0012542 0.00378215 0.002096 0.00185335 0.00194138 0.00380033 0.0037328 0.00214076 -0.000261605 0.00554895 0.00190693 0.00482333 0.00412196 0.00433248 0.0032922 0.00149733 -0.00198263 -0.00465655 -0.00101215 -0.00452882 -0.00389808 0.00365704 0.00196409 -0.00150266 0.00132278 7.86781e-06 -0.000436306 -0.000997692 -0.00151774 -0.00290582 -0.000986993 -0.00202984 -0.00306979 -0.000241861 -0.0023663 -0.000143617 -0.000616923 0.00071498 -0.00136444 0.000806952 0.00092167 0.00274599 0.000827327 0.00379314 0.00362612 0.0028308 0.00371683 0.00211945 0.000794172 0.00338793 0.00358349 0.00317407 0.00381386 0.00329965 0.0061408 0.00434172 0.000996351 0.00116277 0.00479227 0.00521219 0.00549781 0.00172538 -0.000565588 0.00500929 0.00481606 0.0127962 0.00188589 0.00616825 0.00509858 0.00305247 0.00618845 0.000248432 0.00634307 0.00892508 0.0057171 0.00271344 0.00343686 0.0140943 0.00703895 0.00574613 0.0124045 0.00739682 0.00651699 0.020498 -0.0110877 0.00433773 0.0106311 0.00961483 0.0140001 0.00312042 0.0108534 0.00135618 0.00830334 0.0153873 0.0108157 0.0169969 -0.00464851 0.00816596 0.0118423 0.00561047 0.00855923 0.00718778 0.0125443 0.00616348 0.00718147 0.00534147 0.00167203 -0.00419921 -0.00742251 -0.00552565 -0.00556844 -0.0102499 -0.0138872 -0.0103608 -0.00935405 -0.00743747 -0.00296772 -0.00247735 0.00845826 0.00505942 0.00908333 0.013812 0.00857067 0.0182686 0.00592947 0.0126474 0.00578821 0.0194814 0.00121719 0.0182926 0.0109192 0.0115457 0.014065 0.00213802 -0.0102426 0.00826228 0.00567901 0.0131235 0.0350397 0.0167757 0.0172057 0.0183465 0.0198563 0.0193069 0.01778 0.0103664 0.00986159 0.00473499 0.00137529 0.00420779 0.00812897 0.000113249 0.00592332 0.00339369 0.0012721 0.010083 0.00799991 0.00702102 0.00649881 0.0030404 0.00210004 -0.00165895 0.00292256 -0.00186083 0.00441258 0.00263329 -0.002474 4.10676e-05 0.000647455 -0.00121567 -0.000948012 0.000322014 0.000219762 -0.00038138 0.000393793 0.000276357 -0.000241026 -0.00152412 0.000302628 -0.000860468 -0.000610992 0.000937909 0.00117072 -0.000948384 -0.000560746 0.000261694 0.000298828 5.32866e-05 -0.000208184 -0.000209108 -0.000162363 -0.000302538 -0.000584394 0.000218138 -0.000334874 0.000398353 -0.000544533 0.0006098]; +y = 1; +z = 1; + +roll = 1; +pitch = 1; +yaw = [0 0.0138625 -0.0205169 0.0508271 -0.03702 0.0216298 -0.0071385 0.0486512 -0.0353205 0.0356788 0.0621803 -0.0675504 -0.0610086 0.0191248 -0.0686484 0.078647 -0.215955 0.222853 0.535986 -0.157076 0.239414 0.648483 -0.00482519 -0.0347738 -0.857926 -0.185939 -0.0811504 -0.333544 -0.922141 -1.57654 -0.104422 -0.212015 -1.09698 -1.808 -1.44291 -1.66115 -1.19908 -2.86579 -2.17578 -2.45784 -0.599695 -1.24934 -1.20622 0.219747 -0.348318 -1.18928 -0.53417 -1.82522 -2.2156 -2.53528 -2.64981 -1.21297 -1.67351 -1.94906 -1.13474 -1.1618 -0.51443 -0.27769 -1.8986 -1.57831 -1.25621 -0.86068 -1.6979 -1.55513 -1.92766 -2.04434 -0.852749 -0.95132 -1.20863 -0.748142 -1.04782 -1.19536 -1.37872 -1.89795 -1.37617 -1.31626 -1.93325 -1.511 -1.98064 -2.62878 -1.99733 -1.65566 -1.88769 -1.49137 -1.24202 -1.08672 -0.297613 -1.55247 -1.24222 -1.3313 -1.62514 -1.51867 -1.40812 -1.58506 -1.80194 -1.57594 -1.90908 -1.70978 -2.04307 -1.28751 -1.33265 -0.819239 -1.36529 -1.11071 -1.51813 -1.44329 -1.19389 -1.21335 -1.15439 -1.07836 -0.652882 -0.716911 -0.64296 -1.20633 -1.55259 -0.952261 -1.16282 -0.571938 -0.966765 -1.18008 -0.336663 -0.682695 -0.839598 -0.590307 -1.31757 -0.372847 -0.334298 -0.542365 -1.82461 -1.38264 -1.54329 -0.474113 0.131601 -0.165138 -0.712546 -1.3513 -1.48896 -1.70229 -1.11744 -1.26407 -0.898101 -0.475791 -0.505334 -0.911445 -1.05962 -1.34112 -1.11278 -1.09297 -2.30311 -1.36669 -1.50016 -0.731569 -0.974022 -1.34955 -0.962003 -0.724183 -0.492458 -1.18278 -0.0750827 -0.101046 -1.20587 -1.46872 -1.70565 -1.60328 -1.83232 -2.88432 -1.32855 -1.45884 -1.94864 -1.21744 -1.36144 -1.46579 -1.19324 -0.777264 -1.14617 -1.4781 -1.74714 -2.0015 -1.77974 -1.7534 -0.743309 -0.598297 -0.35454 -0.539937 -0.557158 -1.05504 -0.858589 -0.894771 -1.53595 -1.7775 -1.42647 -1.7212 -2.99655 -0.463739 -1.71624 -1.23245 -0.813187 -0.416194 -0.669693 -1.22522 -1.88152 -1.80743 -1.10323 -0.916615 -0.794212 -0.918734 -0.706259 -0.961836 -0.99244 -1.34899 -1.69983 -1.23063 -0.883386 -1.08027 -0.882478 -1.09176 -0.65819 -0.790655 -0.866973 -1.47657 -1.50859 -1.29712 -0.764263 0.0414162 -1.15544 -0.753635 -1.30834 -0.898253 -1.30238 -1.43098 -0.56996 0.115919 -1.03715 -1.0479 -1.21217 -1.05506 -1.07723 -1.2435 -0.484735 -0.48917 -1.23569 -1.49113 -1.37683 -1.7992 -0.595289 -0.729136 -0.59366 -2.04639 -0.0944586 0.335957 -0.773459 0.644048 -0.417065 -0.951768 -0.705456 0.0222041 -0.144679 -0.64648 -0.064453 0.627685 -0.529511 -0.529965 0.21694 0.53353 0.133591 0.0725028 0.349745 0.0435301 -0.00884339 -0.0296694 0.0036566 0.217436 -0.526511 -0.512979 -1.32967 -0.491154 0.224525 0.609691 -0.30598 1.04681 1.11591 -0.342271 0.181812 1.00252 -0.569827 -0.174485 0.621023 1.15114 0.85475 1.17868 0.418458 0.132773 0.09667 0.105433 3.05352 3.43917 1.56984 1.66762 1.91426 2.8391 2.61215 2.92371 1.4164 1.0191 0.490665 0.341318 1.48112 0.901748 0.991522 1.52663 1.06743 1.60625 2.42876 2.18304 1.5142 1.05712 1.21268 1.40639 -0.539377 1.00853 -0.254121 0.662136 0.29893 -0.0109695 0.387949 -0.573484 -0.838396 -0.198825 0.040522 -0.29886 -0.368194 0.130778 -0.474338 -0.762008 0.0665195 0.155289 0.0655191 -0.0332738 0.0774652 -0.000751732 0.0230952 0.0195789 -0.000320425 -0.00322251 -0.026999 0.000401567 0.0640858 -0.0439683 0.0527193 0.00422349 -0.0218244 0.0171353 -0.0126251 -0.0419359 0.03175]; +roll = roll * pi / 180; % to radian +pitch = pitch * pi / 180; % to radian +yaw = yaw * pi / 180; % to radian + + +%parameters +n = 400; +noiseT = 0.005; +lambdaT = 100; +noiseR = 0.005; +lambdaR = 150; + +%filter +x_filtered = pf_filter(x, n, noiseT, lambdaT); +y_filtered = pf_filter(x, n, noiseT, lambdaT); +z_filtered = pf_filter(x, n, noiseT, lambdaT); +roll_filtered = pf_filter(roll, n, noiseR, lambdaR); +pitch_filtered = pf_filter(pitch, n, noiseR, lambdaR); +yaw_filtered = pf_filter(yaw, n, noiseR, lambdaR); + +%show +index = 1:length(x); + +figure +plot(index,x,'b', index,x_filtered,'r'); +hold on; +plot(index,y,'c', index,y_filtered,'m'); +plot(index,z,'g', index,z_filtered,'y'); +legend('x', 'x filtered', 'y', 'y filtered', 'z', 'z filtered') + +%show +figure +plot(index,roll,'b', index,roll_filtered,'r'); +hold on; +plot(index,pitch,'c', index,pitch_filtered,'m'); +plot(index,yaw,'g', index,yaw_filtered,'y'); +legend('roll', 'roll filtered', 'pitch', 'pitch filtered', 'yaw', 'yaw filtered') +legend('yaw', 'yaw filtered') + + diff --git a/Matlab/ParticleFilter/test_kitti_datasets.m b/Matlab/ParticleFilter/test_kitti_datasets.m new file mode 100644 index 00000000..62b47cd8 --- /dev/null +++ b/Matlab/ParticleFilter/test_kitti_datasets.m @@ -0,0 +1,28 @@ + + +clc +clear all +close all + +% position (x) +x=[0 0.0958093 0.102248 0.121139 0.14751 0.168275 0.180045 0.189047 0.203946 0.213641 0.22573 0.243683 0.245992 0.254727 0.260212 0.246672 0.259118 0.273364 0.295793 0.317168 0.319033 0.330263 0.291336 0.342969 0.373641 0.406199 0.451661 0.49569 0.52575 0.558851 0.595505 0.617386 0.635253 0.663907 0.695187 0.721908 0.748228 0.775707 0.798103 0.817422 0.81696 0.834573 0.859951 0.866837 0.861447 0.859494 0.863317 0.86665 0.856142 0.861513 0.869291 0.86055 0.858222 0.850158 0.86469 0.853298 0.849211 0.85666 0.846806 0.834052 0.818975 0.816591 0.818847 0.812942 0.804087 0.802885 0.799035 0.793771 0.782671 0.783806 0.753611 0.735965 0.718761 0.70112 0.68173 0.637936 0.59822 0.570844 0.546408 0.503599 0.481603 0.481476 0.468762 0.468941 0.464934 0.457286 0.468546 0.449624 0.423916 0.398509 0.448139 0.463816 0.524683 0.543314 0.585886 0.629963 0.642661 0.689202 0.733252 0.720786 0.747091 0.769375 0.796756 0.804347 0.805587 0.792853 0.785854 0.791591 0.775623 0.772854 0.774293 0.775352 0.778641 0.773735 0.763698 0.770237 0.764968 0.782758 0.790375 0.794311 0.801609 0.807287 0.817432 0.834678 0.854601 0.860135 0.857215 0.876742 0.880283 0.885587 0.895356 0.901144 0.891045 0.904938 0.88169 0.875238 0.873543 0.879889 0.858548 0.848491 0.845521 0.832916 0.82651 0.817256 0.81536 0.807806 0.80487 0.789718 0.788688 0.790668 0.786018 0.783837 0.776029 0.766569 0.76899 0.772462 0.753362 0.75065 0.760805 0.771717 0.752581 0.777233 0.771257 0.784665 0.789536 0.783277 0.768278 0.775663 0.782527 0.801327 0.773798 0.783786 0.782585 0.777043 0.765654 0.757504 0.75159 0.745206 0.745253 0.694793 0.657977 0.627305 0.587667 0.558428 0.507883 0.429494 0.354626 0.276308 0.221929 0.216919 0.222186 0.2257 0.21829 0.21727 0.220388 0.234087 0.271358 0.365188 0.397868 0.464485 0.436238 0.473228 0.516054 0.581783 0.66704 0.702402 0.772848 0.836386 0.870549 0.883748 0.890929 0.9015 0.933377 0.993732 1.01464 1.01678 1.01107 1.00868 1.01584 1.0091 1.01149 1.00289 0.992969 1.00104 0.999807 1.00424 1.00244 1.00696 0.999669 0.989008 0.995648 0.977475 0.976959 0.986356 0.969375 0.973117 0.970714 0.96257 0.956754 0.954274 0.92185 0.935669 0.933797 0.918595 0.86588 0.831554 0.800549 0.77171 0.756608 0.738918 0.707971 0.681183 0.654234 0.644363 0.619473 0.607539 0.589974 0.569724 0.538563 0.524551 0.521722 0.497904 0.486697 0.453492 0.43853 0.449554 0.472133 0.481396 0.487145 0.49219 0.520493 0.56075 0.603805 0.616122 0.687576 0.726017 0.756402 0.862825 0.926746 0.974142 0.960781 0.925374 0.932887 0.938424 0.948403 0.924803 0.914451 0.920006 0.874211 0.873257 0.888927 0.905825 0.908478 0.932547 0.975009 1.03873 1.0638 1.06204 1.07596 1.07522 1.06945 1.05537 1.07162 1.03632 1.03053 1.02946 1.01674 1.0092 0.98566 0.979956 0.946627 0.933132 0.904111 0.826381 0.789019 0.738187 0.717317 0.656708 0.490356 0.434063 0.3134 0.213561 0.18305 0.174464 0.13647 0.127878 0.0663346 0.018491 -0.00133265 0.000999137 -0.00121855 0.000119434 0.000319056 3.22909e-05 -0.000269401 -0.000233193 0.00030071 -0.000469815 -5.96254e-05 0.000130806 9.13643e-05 2.98268e-05 4.07632e-05 7.35067e-05 0.0153078 0.0186766 0.0295282 0.0543363 0.0719927 0.086474 0.126864 0.161484 0.19345 0.300308 0.404477 0.422215 0.514335 0.513278 0.625498 0.93727 0.993653 1.03992 1.08643 1.1112 1.23404 1.22048 1.19735 1.20285 1.17902 1.17133 1.16618 1.13694 1.12139 1.10714 1.09385 1.08936 1.08 1.04605 1.03821 1.03905 1.02285 0.989178 0.935135 0.865405 0.723314 0.641447 0.602483 0.522911 0.491991 0.462587 0.500009 0.543585 0.668132 0.752387 0.782115 0.783924 0.772362 0.763335 0.683723 0.644116 0.627112 0.614968 0.57313 0.523073 0.435806 0.335873 0.269904 0.25352 0.260491 0.24324 0.325414 0.359984 0.411085 0.348953 0.272724 0.0950071 -0.00396737]; +%filter +x_filtered = pf_filter(x, 400, 0.07, 15); +%show +figure +index = 1:length(x); +plot(index,x,'b', index,x_filtered,'r'); +legend('x', 'x filtered') + +% rotation (yaw) +yaw=[0 0.367344 0.423404 0.640698 0.914954 1.12181 1.25882 1.36415 1.53533 1.68386 1.7685 1.984 1.98521 2.16754 2.31386 2.71056 2.92328 3.16512 3.30591 3.3121 3.39057 3.4986 3.38722 3.27962 2.94052 2.71029 2.14679 1.77782 1.1752 0.750571 0.374997 0.252442 0.0770365 -0.0714351 -0.0788259 -0.0948595 -0.0976391 -0.102182 -0.0721881 -0.0533192 -0.0312146 0.0153472 0.00734632 0.0203816 0.0274327 0.0244179 0.01258 -0.00401542 -0.0257598 -0.0261856 -0.030745 0.00809133 -0.0201015 -0.0173277 0.0202852 0.0392073 0.0418929 0.102132 0.114319 0.0860487 0.0639182 0.0709067 0.0550139 0.0589345 0.059498 0.0386182 0.0237123 0.0133304 0.0453462 -0.00725487 -0.0839458 -0.148932 -0.241809 -0.337385 -0.408145 -0.668709 -1.03016 -1.23355 -1.41645 -1.8998 -2.29763 -2.56527 -2.92972 -3.28132 -3.29212 -3.18606 -3.06397 -3.11467 -3.21917 -3.28397 -3.31198 -3.25927 -2.7551 -2.5298 -1.79784 -1.23766 -1.06653 -0.8034 -0.850659 -0.844026 -0.723554 -0.553498 -0.479462 -0.31188 -0.265967 -0.21797 -0.156605 -0.123484 -0.131099 -0.10232 -0.0613497 -0.109752 -0.125175 -0.129335 -0.0552938 -0.0475251 -0.0330476 -0.0433853 -0.000880761 0.115343 0.170772 0.131315 0.141837 0.0999629 0.121093 0.10641 0.0607733 -0.0309408 -0.109561 -0.066559 -0.0824674 -0.0320398 -0.0439917 -0.0623439 -0.0695493 -0.0444161 -0.0065631 0.057401 0.0976348 0.139955 0.19723 0.229909 0.191066 0.220196 0.264731 0.360177 0.346012 0.34643 0.351766 0.316502 0.367959 0.361346 0.403335 0.473158 0.519924 0.61397 0.64618 0.700117 0.700964 0.66132 0.5243 0.429438 0.456057 0.476463 0.407447 0.330845 0.331997 0.299632 0.234435 0.336388 0.294559 0.293602 0.27388 0.288828 0.275381 0.296273 0.263594 0.225263 0.186247 0.206763 0.172048 0.151194 0.154786 0.148777 0.132994 0.35366 0.471686 0.949712 1.25971 1.26477 1.3436 1.51096 1.752 1.74347 2.02086 2.12265 2.47172 3.06266 3.02326 3.0401 2.83478 2.6832 2.43665 1.51546 0.690584 0.442643 0.168241 0.0486004 0.0388517 0.063886 0.0594586 0.0671994 0.0716274 -0.00795655 0.00190975 0.0219664 0.0227349 0.0180559 0.026195 0.0434311 0.0426099 0.0737965 0.0520878 0.00251581 -0.057547 -0.0536053 -0.0872076 -0.0905081 -0.0275865 0.0106225 0.00939888 0.0564355 0.0535977 0.0664939 0.0494566 0.0104787 -0.0241714 -0.026226 -0.0377078 -0.0367821 -0.0307445 -0.00809363 -0.00967068 0.0169084 0.0220944 0.0305258 0.0240909 0.0441721 0.0568962 0.0938898 0.181372 0.425365 0.828993 0.970071 1.29186 1.49596 1.72977 1.9131 2.33526 2.67046 2.65711 2.77644 2.87891 2.92561 2.84321 2.86918 2.53234 2.38821 1.90655 1.70474 0.774424 0.242343 0.101759 0.0606305 -0.0168209 -0.0321093 0.0145107 0.0541553 0.0516316 0.0220795 -0.00039793 -0.0337128 -0.0595012 -0.0539847 -0.0388872 -0.635682 -1.3613 -1.84956 -1.77388 -1.23053 -1.08225 -1.05091 -1.00551 -0.779717 -0.0594095 0.0348849 0.0411509 -0.0188255 -0.0742438 -0.0677669 -0.0538894 -0.105211 -0.146425 -0.172514 -0.127626 -0.0157509 0.066493 0.055035 0.165349 0.141456 -0.0243789 -0.0492561 -0.108507 -0.436607 -0.421971 -0.397179 -0.342693 -0.298114 -0.136957 -0.0635116 -0.00672385 0.145058 0.134837 0.140056 0.299669 0.37401 0.256271 0.103959 0.00974883 -0.0162673 0.0043005 -0.00114822 0.011008 0.00846304 0.0198297 0.0207307 0.0143213 -0.000866516 0.00788459 0.0133711 -0.00467761 -0.000142124 -0.000778014 0.00123566 0.171455 0.267658 0.34448 0.719087 0.900269 0.958507 1.051 1.20626 1.3786 2.5974 3.06579 3.01829 3.05114 3.09404 2.95468 0.52717 0.138967 0.0976445 0.248206 0.247692 -0.0151054 -0.0351626 -0.0324181 -0.0302425 0.00564108 0.0495676 0.151858 0.0565908 -0.0921944 -0.0842249 0.0416879 0.0400479 0.0786246 -0.048536 -0.0493043 -0.0457081 -0.03454 -0.0353161 0.00897072 0.182576 0.686738 0.942024 1.22062 2.96135 2.9593 2.90432 1.59589 0.838234 0.57694 0.164348 0.107916 0.0222738 0.00653589 -0.0789186 -0.0262052 0.0161459 0.0682029 0.10532 0.00317246 -0.0800566 -0.0553356 -0.0542734 -0.0175716 0.344492 0.239568 0.117746 -0.269454 -0.184009 -0.436715 0.097191 -0.532786 -0.30076 -0.00929552]; +yaw = yaw * pi / 180; % to radian +%filter +yaw_filtered = pf_filter(yaw, 400, 0.005, 150); +%show +figure +index = 1:length(yaw); +plot(index,yaw,'b', index,yaw_filtered,'r'); +legend('yaw', 'yaw filtered') + + diff --git a/Matlab/ParticleFilter/test_odometry.m b/Matlab/ParticleFilter/test_odometry.m new file mode 100644 index 00000000..62b47cd8 --- /dev/null +++ b/Matlab/ParticleFilter/test_odometry.m @@ -0,0 +1,28 @@ + + +clc +clear all +close all + +% position (x) +x=[0 0.0958093 0.102248 0.121139 0.14751 0.168275 0.180045 0.189047 0.203946 0.213641 0.22573 0.243683 0.245992 0.254727 0.260212 0.246672 0.259118 0.273364 0.295793 0.317168 0.319033 0.330263 0.291336 0.342969 0.373641 0.406199 0.451661 0.49569 0.52575 0.558851 0.595505 0.617386 0.635253 0.663907 0.695187 0.721908 0.748228 0.775707 0.798103 0.817422 0.81696 0.834573 0.859951 0.866837 0.861447 0.859494 0.863317 0.86665 0.856142 0.861513 0.869291 0.86055 0.858222 0.850158 0.86469 0.853298 0.849211 0.85666 0.846806 0.834052 0.818975 0.816591 0.818847 0.812942 0.804087 0.802885 0.799035 0.793771 0.782671 0.783806 0.753611 0.735965 0.718761 0.70112 0.68173 0.637936 0.59822 0.570844 0.546408 0.503599 0.481603 0.481476 0.468762 0.468941 0.464934 0.457286 0.468546 0.449624 0.423916 0.398509 0.448139 0.463816 0.524683 0.543314 0.585886 0.629963 0.642661 0.689202 0.733252 0.720786 0.747091 0.769375 0.796756 0.804347 0.805587 0.792853 0.785854 0.791591 0.775623 0.772854 0.774293 0.775352 0.778641 0.773735 0.763698 0.770237 0.764968 0.782758 0.790375 0.794311 0.801609 0.807287 0.817432 0.834678 0.854601 0.860135 0.857215 0.876742 0.880283 0.885587 0.895356 0.901144 0.891045 0.904938 0.88169 0.875238 0.873543 0.879889 0.858548 0.848491 0.845521 0.832916 0.82651 0.817256 0.81536 0.807806 0.80487 0.789718 0.788688 0.790668 0.786018 0.783837 0.776029 0.766569 0.76899 0.772462 0.753362 0.75065 0.760805 0.771717 0.752581 0.777233 0.771257 0.784665 0.789536 0.783277 0.768278 0.775663 0.782527 0.801327 0.773798 0.783786 0.782585 0.777043 0.765654 0.757504 0.75159 0.745206 0.745253 0.694793 0.657977 0.627305 0.587667 0.558428 0.507883 0.429494 0.354626 0.276308 0.221929 0.216919 0.222186 0.2257 0.21829 0.21727 0.220388 0.234087 0.271358 0.365188 0.397868 0.464485 0.436238 0.473228 0.516054 0.581783 0.66704 0.702402 0.772848 0.836386 0.870549 0.883748 0.890929 0.9015 0.933377 0.993732 1.01464 1.01678 1.01107 1.00868 1.01584 1.0091 1.01149 1.00289 0.992969 1.00104 0.999807 1.00424 1.00244 1.00696 0.999669 0.989008 0.995648 0.977475 0.976959 0.986356 0.969375 0.973117 0.970714 0.96257 0.956754 0.954274 0.92185 0.935669 0.933797 0.918595 0.86588 0.831554 0.800549 0.77171 0.756608 0.738918 0.707971 0.681183 0.654234 0.644363 0.619473 0.607539 0.589974 0.569724 0.538563 0.524551 0.521722 0.497904 0.486697 0.453492 0.43853 0.449554 0.472133 0.481396 0.487145 0.49219 0.520493 0.56075 0.603805 0.616122 0.687576 0.726017 0.756402 0.862825 0.926746 0.974142 0.960781 0.925374 0.932887 0.938424 0.948403 0.924803 0.914451 0.920006 0.874211 0.873257 0.888927 0.905825 0.908478 0.932547 0.975009 1.03873 1.0638 1.06204 1.07596 1.07522 1.06945 1.05537 1.07162 1.03632 1.03053 1.02946 1.01674 1.0092 0.98566 0.979956 0.946627 0.933132 0.904111 0.826381 0.789019 0.738187 0.717317 0.656708 0.490356 0.434063 0.3134 0.213561 0.18305 0.174464 0.13647 0.127878 0.0663346 0.018491 -0.00133265 0.000999137 -0.00121855 0.000119434 0.000319056 3.22909e-05 -0.000269401 -0.000233193 0.00030071 -0.000469815 -5.96254e-05 0.000130806 9.13643e-05 2.98268e-05 4.07632e-05 7.35067e-05 0.0153078 0.0186766 0.0295282 0.0543363 0.0719927 0.086474 0.126864 0.161484 0.19345 0.300308 0.404477 0.422215 0.514335 0.513278 0.625498 0.93727 0.993653 1.03992 1.08643 1.1112 1.23404 1.22048 1.19735 1.20285 1.17902 1.17133 1.16618 1.13694 1.12139 1.10714 1.09385 1.08936 1.08 1.04605 1.03821 1.03905 1.02285 0.989178 0.935135 0.865405 0.723314 0.641447 0.602483 0.522911 0.491991 0.462587 0.500009 0.543585 0.668132 0.752387 0.782115 0.783924 0.772362 0.763335 0.683723 0.644116 0.627112 0.614968 0.57313 0.523073 0.435806 0.335873 0.269904 0.25352 0.260491 0.24324 0.325414 0.359984 0.411085 0.348953 0.272724 0.0950071 -0.00396737]; +%filter +x_filtered = pf_filter(x, 400, 0.07, 15); +%show +figure +index = 1:length(x); +plot(index,x,'b', index,x_filtered,'r'); +legend('x', 'x filtered') + +% rotation (yaw) +yaw=[0 0.367344 0.423404 0.640698 0.914954 1.12181 1.25882 1.36415 1.53533 1.68386 1.7685 1.984 1.98521 2.16754 2.31386 2.71056 2.92328 3.16512 3.30591 3.3121 3.39057 3.4986 3.38722 3.27962 2.94052 2.71029 2.14679 1.77782 1.1752 0.750571 0.374997 0.252442 0.0770365 -0.0714351 -0.0788259 -0.0948595 -0.0976391 -0.102182 -0.0721881 -0.0533192 -0.0312146 0.0153472 0.00734632 0.0203816 0.0274327 0.0244179 0.01258 -0.00401542 -0.0257598 -0.0261856 -0.030745 0.00809133 -0.0201015 -0.0173277 0.0202852 0.0392073 0.0418929 0.102132 0.114319 0.0860487 0.0639182 0.0709067 0.0550139 0.0589345 0.059498 0.0386182 0.0237123 0.0133304 0.0453462 -0.00725487 -0.0839458 -0.148932 -0.241809 -0.337385 -0.408145 -0.668709 -1.03016 -1.23355 -1.41645 -1.8998 -2.29763 -2.56527 -2.92972 -3.28132 -3.29212 -3.18606 -3.06397 -3.11467 -3.21917 -3.28397 -3.31198 -3.25927 -2.7551 -2.5298 -1.79784 -1.23766 -1.06653 -0.8034 -0.850659 -0.844026 -0.723554 -0.553498 -0.479462 -0.31188 -0.265967 -0.21797 -0.156605 -0.123484 -0.131099 -0.10232 -0.0613497 -0.109752 -0.125175 -0.129335 -0.0552938 -0.0475251 -0.0330476 -0.0433853 -0.000880761 0.115343 0.170772 0.131315 0.141837 0.0999629 0.121093 0.10641 0.0607733 -0.0309408 -0.109561 -0.066559 -0.0824674 -0.0320398 -0.0439917 -0.0623439 -0.0695493 -0.0444161 -0.0065631 0.057401 0.0976348 0.139955 0.19723 0.229909 0.191066 0.220196 0.264731 0.360177 0.346012 0.34643 0.351766 0.316502 0.367959 0.361346 0.403335 0.473158 0.519924 0.61397 0.64618 0.700117 0.700964 0.66132 0.5243 0.429438 0.456057 0.476463 0.407447 0.330845 0.331997 0.299632 0.234435 0.336388 0.294559 0.293602 0.27388 0.288828 0.275381 0.296273 0.263594 0.225263 0.186247 0.206763 0.172048 0.151194 0.154786 0.148777 0.132994 0.35366 0.471686 0.949712 1.25971 1.26477 1.3436 1.51096 1.752 1.74347 2.02086 2.12265 2.47172 3.06266 3.02326 3.0401 2.83478 2.6832 2.43665 1.51546 0.690584 0.442643 0.168241 0.0486004 0.0388517 0.063886 0.0594586 0.0671994 0.0716274 -0.00795655 0.00190975 0.0219664 0.0227349 0.0180559 0.026195 0.0434311 0.0426099 0.0737965 0.0520878 0.00251581 -0.057547 -0.0536053 -0.0872076 -0.0905081 -0.0275865 0.0106225 0.00939888 0.0564355 0.0535977 0.0664939 0.0494566 0.0104787 -0.0241714 -0.026226 -0.0377078 -0.0367821 -0.0307445 -0.00809363 -0.00967068 0.0169084 0.0220944 0.0305258 0.0240909 0.0441721 0.0568962 0.0938898 0.181372 0.425365 0.828993 0.970071 1.29186 1.49596 1.72977 1.9131 2.33526 2.67046 2.65711 2.77644 2.87891 2.92561 2.84321 2.86918 2.53234 2.38821 1.90655 1.70474 0.774424 0.242343 0.101759 0.0606305 -0.0168209 -0.0321093 0.0145107 0.0541553 0.0516316 0.0220795 -0.00039793 -0.0337128 -0.0595012 -0.0539847 -0.0388872 -0.635682 -1.3613 -1.84956 -1.77388 -1.23053 -1.08225 -1.05091 -1.00551 -0.779717 -0.0594095 0.0348849 0.0411509 -0.0188255 -0.0742438 -0.0677669 -0.0538894 -0.105211 -0.146425 -0.172514 -0.127626 -0.0157509 0.066493 0.055035 0.165349 0.141456 -0.0243789 -0.0492561 -0.108507 -0.436607 -0.421971 -0.397179 -0.342693 -0.298114 -0.136957 -0.0635116 -0.00672385 0.145058 0.134837 0.140056 0.299669 0.37401 0.256271 0.103959 0.00974883 -0.0162673 0.0043005 -0.00114822 0.011008 0.00846304 0.0198297 0.0207307 0.0143213 -0.000866516 0.00788459 0.0133711 -0.00467761 -0.000142124 -0.000778014 0.00123566 0.171455 0.267658 0.34448 0.719087 0.900269 0.958507 1.051 1.20626 1.3786 2.5974 3.06579 3.01829 3.05114 3.09404 2.95468 0.52717 0.138967 0.0976445 0.248206 0.247692 -0.0151054 -0.0351626 -0.0324181 -0.0302425 0.00564108 0.0495676 0.151858 0.0565908 -0.0921944 -0.0842249 0.0416879 0.0400479 0.0786246 -0.048536 -0.0493043 -0.0457081 -0.03454 -0.0353161 0.00897072 0.182576 0.686738 0.942024 1.22062 2.96135 2.9593 2.90432 1.59589 0.838234 0.57694 0.164348 0.107916 0.0222738 0.00653589 -0.0789186 -0.0262052 0.0161459 0.0682029 0.10532 0.00317246 -0.0800566 -0.0553356 -0.0542734 -0.0175716 0.344492 0.239568 0.117746 -0.269454 -0.184009 -0.436715 0.097191 -0.532786 -0.30076 -0.00929552]; +yaw = yaw * pi / 180; % to radian +%filter +yaw_filtered = pf_filter(yaw, 400, 0.005, 150); +%show +figure +index = 1:length(yaw); +plot(index,yaw,'b', index,yaw_filtered,'r'); +legend('yaw', 'yaw filtered') + + diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index ec575282..4e1dea32 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -107,6 +107,7 @@ public: virtual bool init(); std::string getPath() const {return _path;} + unsigned int imagesCount() const; protected: virtual cv::Mat captureImage(); diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 5687d8d1..11a872db 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -110,11 +110,11 @@ public: } virtual ~StereoCameraModel() {} - bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();} + bool isValid() const {return left_.isValid() && right_.isValid();} const std::string & name() const {return name_;} - bool load(const std::string & directory, const std::string & cameraName); - bool save(const std::string & directory, const std::string & cameraName); + bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true); + bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true); double baseline() const {return -right_.Tx()/right_.fx();} diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 5762a92e..fd68a216 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -86,7 +86,7 @@ class RTABMAP_EXP CameraRGBD { public: virtual ~CameraRGBD(); - void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); virtual bool init(const std::string & calibrationFolder = ".") = 0; virtual bool isCalibrated() const = 0; @@ -116,7 +116,7 @@ protected: /** * returned rgb and depth images should be already rectified */ - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0; + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) = 0; private: float _imageRate; @@ -152,7 +152,7 @@ public: virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); private: pcl::Grabber* interface_; @@ -186,7 +186,7 @@ public: virtual std::string getSerial() const {return "";} // unknown with OpenCV protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); private: bool _asus; @@ -222,7 +222,7 @@ public: bool setMirroring(bool enabled); protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); private: openni::Device * _device; @@ -257,7 +257,7 @@ public: virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); private: int deviceId_; @@ -295,7 +295,7 @@ public: virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); + virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); private: int deviceId_; @@ -328,7 +328,7 @@ public: virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy); + virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); private: DC1394Device *device_; @@ -353,11 +353,46 @@ public: virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy); + virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); private: FlyCapture2::Camera * camera_; void * triclopsCtx_; // TriclopsContext }; +///////////////////////// +// CameraStereoImages +///////////////////////// +class CameraImages; +class RTABMAP_EXP CameraStereoImages : + public CameraRGBD +{ +public: + static bool available(); + +public: + CameraStereoImages( + const std::string & path, + const std::string & cameraName = "stereo_images", // calibration file name + const std::string & timestampsPath = "", // "times.txt" + float imageRate=0.0f, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoImages(); + + virtual bool init(const std::string & calibrationFolder = "."); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); + +private: + CameraImages * camera_; + CameraImages * camera2_; + std::string cameraName_; + std::string timestampsPath_; + std::list stamps_; + StereoCameraModel stereoModel_; +}; + } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index fa18b9de..c51b55f2 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -70,6 +70,7 @@ public: int iterations() const {return iterations_;} bool isSlam2d() const {return slam2d_;} bool isCovarianceIgnored() const {return covarianceIgnored_;} + double epsilon() const {return epsilon_;} virtual std::map optimize( int rootId, @@ -80,13 +81,18 @@ public: virtual void parseParameters(const ParametersMap & parameters); protected: - Optimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false); + Optimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon()); Optimizer(const ParametersMap & parameters); private: int iterations_; bool slam2d_; bool covarianceIgnored_; + double epsilon_; }; class RTABMAP_EXP TOROOptimizer : public Optimizer diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 7810cb55..458c6292 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -76,6 +76,27 @@ public: transVariance_ = transVariance; } + Link merge(const Link & link) const + { + UASSERT(to_ == link.from()); + UASSERT(type_ == link.type()); + UASSERT(!transform_.isNull()); + UASSERT(!link.transform().isNull()); + UASSERT(rotVariance_ > 0 && link.rotVariance() > 0 && transVariance_ > 0 && link.transVariance() > 0); + return Link( + from_, + link.to(), + type_, + transform_ * link.transform(), + 1.0f/(1.0f/rotVariance_ + 1.0f/link.rotVariance()), + 1.0f/(1.0f/transVariance_ + 1.0f/link.transVariance())); + } + + Link inverse() const + { + return Link(to_, from_, type_, transform_.inverse(), rotVariance_, transVariance_); + } + private: int from_; int to_; diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index fedb271f..679ec6f7 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -91,6 +91,7 @@ public: int maxCheckedInDatabase = -1, bool incrementMarginOnLoop = false, bool ignoreLoopIds = false, + bool ignoreBadSignatures = false, double * dbAccessTime = 0) const; std::map getNeighborsIdRadius( int signatureId, @@ -264,6 +265,9 @@ private: bool _bowForce2D; bool _bowEpipolarGeometry; float _bowEpipolarGeometryVar; + bool _bowPnPEstimation; + double _bowPnPReprojError; + int _bowPnPFlags; float _icpMaxTranslation; float _icpMaxRotation; int _icpDecimation; diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 7ccca9cb..da84db11 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -42,11 +42,12 @@ namespace rtabmap { class Feature2D; class OdometryInfo; +class ParticleFilter; class RTABMAP_EXP Odometry { public: - virtual ~Odometry() {} + virtual ~Odometry(); Transform process(const SensorData & data, OdometryInfo * info = 0); virtual void reset(const Transform & initialPose = Transform::getIdentity()); @@ -75,12 +76,21 @@ private: float _maxDepth; int _resetCountdown; bool _force2D; + bool _particleFiltering; + int _particleSize; + float _particleNoiseT; + float _particleLambdaT; + float _particleNoiseR; + float _particleLambdaR; bool _fillInfoData; bool _pnpEstimation; double _pnpReprojError; int _pnpFlags; Transform _pose; int _resetCurrentCount; + double previousStamp_; + + std::vector filters_; protected: Odometry(const rtabmap::ParametersMap & parameters); diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index a5f27adb..2409c8c6 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -40,7 +40,9 @@ public: variance(-1), features(-1), localMapSize(-1), - time(-1), + timeEstimation(-1), + stamp(0), + interval(0), type(-1) {} bool lost; @@ -49,7 +51,12 @@ public: float variance; int features; int localMapSize; - float time; + float timeEstimation; + float timeParticleFiltering; + double stamp; + double interval; + Transform transform; + Transform transformFiltered; int type; // 0=BOW, 1=Optical Flow, 2=ICP diff --git a/corelib/include/rtabmap/core/OdometryThread.h b/corelib/include/rtabmap/core/OdometryThread.h index f58bedd0..b6b1f430 100644 --- a/corelib/include/rtabmap/core/OdometryThread.h +++ b/corelib/include/rtabmap/core/OdometryThread.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -40,7 +41,7 @@ class Odometry; class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler { public: // take ownership of Odometry - OdometryThread(Odometry * odometry); + OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1); virtual ~OdometryThread(); protected: @@ -54,13 +55,14 @@ private: //============================================================ void mainLoop(); void addData(const SensorData & data); - void getData(SensorData & data); + bool getData(SensorData & data); private: USemaphore _dataAdded; UMutex _dataMutex; - SensorData _dataBuffer; + std::list _dataBuffer; Odometry * _odometry; + unsigned int _dataBufferMaxSize; bool _resetOdometry; }; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index a3608b10..bce3b2fe 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -169,7 +169,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity)."); RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity)."); RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate."); - RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 1, "Data buffer size (0 min inf)."); + RTABMAP_PARAM(Rtabmap, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); + RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0."); RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory."); RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM."); RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration."); @@ -307,6 +308,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations."); RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses."); RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); + RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); @@ -314,16 +316,23 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits."); RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences."); RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform."); - RTABMAP_PARAM(Odom, Iterations, int, 30, "Maximum iterations to compute the transform from visual words."); + RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words."); RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit)."); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); + RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); - RTABMAP_PARAM(Odom, PnPReprojError, double, 8.0, "PnP reprojection error."); - RTABMAP_PARAM(Odom, PnPFlags, int, 0, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); + RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error."); + RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); + RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory."); + RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter."); + RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z)."); + RTABMAP_PARAM(Odom, ParticleLambdaT, float, 100, "Lambda of translation components (x,y,z)."); + RTABMAP_PARAM(Odom, ParticleNoiseR, float, 0.002, "Noise (rad) of rotational components (roll,pitch,yaw)."); + RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw)."); // Odometry Bag-of-words RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); @@ -359,6 +368,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false, "Use epipolar geometry to compute the loop closure transform."); RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure."); + RTABMAP_PARAM(LccBow, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); + RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error."); + RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure."); RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4."); RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio."); @@ -371,13 +383,13 @@ 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, "Max iterations."); - RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform."); + RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.0, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane."); RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences."); RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations."); - RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform."); + RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.0, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation."); // Stereo disparity diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 2b5b0bab..fd7c99fe 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -106,9 +106,11 @@ public: bool setUserData(int id, const std::vector & data); void generateDOTGraph(const std::string & path, int id=0, int margin=5); void generateTOROGraph(const std::string & path, bool optimized, bool global); + void exportPoses(const std::string & path, bool optimized, bool global); void resetMemory(); void dumpPrediction() const; void dumpData() const; + void dumpPoses(const std::string & path, const std::map & poses) const; void parseParameters(const ParametersMap & parameters); void setWorkingDirectory(std::string path); void rejectLoopClosure(int oldId, int newId); diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index 980874d4..a49081c7 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -67,6 +67,8 @@ public: kCmdGenerateDOTLocalGraph, // params: path, id, margin kCmdGenerateTOROGraphLocal, // params: path, optimized kCmdGenerateTOROGraphGlobal, // params: path, optimized + kCmdExportPosesGlobal, + kCmdExportPosesLocal, kCmdCleanDataBuffer, kCmdPublish3DMapLocal, // params: optimized kCmdPublish3DMapGlobal, // params: optimized diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index 6702fef4..93c22965 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -64,6 +64,8 @@ public: kStateGeneratingDOTLocalGraph, kStateGeneratingTOROGraphLocal, kStateGeneratingTOROGraphGlobal, + kStateExportingPosesLocal, + kStateExportingPosesGlobal, kStateCleanDataBuffer, kStatePublishingMapLocal, kStatePublishingMapGlobal, @@ -81,7 +83,8 @@ public: void clearBufferedData(); void setDetectorRate(float rate); - void setBufferSize(int bufferSize); + void setDataBufferSize(unsigned int bufferSize); + void createIntermediateNodes(bool enabled); protected: virtual void handleEvent(UEvent * anEvent); @@ -91,9 +94,8 @@ private: virtual void mainLoopKill(); void process(); void addData(const SensorData & data); - void getData(SensorData & data); + bool getData(SensorData & data); void pushNewState(State newState, const ParametersMap & parameters = ParametersMap()); - void setDataBufferSize(int size); void publishMap(bool optimized, bool full) const; void publishGraph(bool optimized, bool full) const; @@ -105,8 +107,9 @@ private: std::list _dataBuffer; UMutex _dataMutex; USemaphore _dataAdded; - int _dataBufferMaxSize; + unsigned int _dataBufferMaxSize; float _rate; + bool _createIntermediateNodes; UTimer * _frameRateTimer; Rtabmap * _rtabmap; diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index 28c80b0c..56e9485b 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -38,6 +38,7 @@ namespace rtabmap { BayesFilter::BayesFilter(const ParametersMap & parameters) : _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()), _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()), + _badSignaturesIgnored(Parameters::defaultRtabmapCreateIntermediateNodes()), _totalPredictionLCValues(0.0f) { this->setPredictionLC(Parameters::defaultBayesPredictionLC()); @@ -56,6 +57,7 @@ void BayesFilter::parseParameters(const ParametersMap & parameters) } Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior); Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate); + Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _badSignaturesIgnored); UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f); } @@ -161,7 +163,7 @@ const std::map & BayesFilter::computePosterior(const Memory * memory // STEP 1 - Prediction : Prior*lastPosterior _prediction = this->generatePrediction(memory, uKeys(likelihood)); - ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols); + UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols); //std::cout << "Prediction=" << _prediction << std::endl; // Adjust the last posterior if some images were @@ -260,7 +262,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector // Set high values (gaussians curves) to loop closure neighbors // ADD prob for each neighbors - std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0); + std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); std::list idsLoopMargin; //filter neighbors in STM for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) @@ -474,7 +476,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, } if(i neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0); + std::map neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap); this->normalize(prediction, i, sum, newIds[0]<0); ++added; @@ -494,7 +496,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, int modified = 0; for(std::set::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0); + std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); int index = newIdToIndexMap.at(*iter); float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); this->normalize(prediction, index, sum, newIds[0]<0); diff --git a/corelib/src/BayesFilter.h b/corelib/src/BayesFilter.h index 24d85a88..4d00cd76 100644 --- a/corelib/src/BayesFilter.h +++ b/corelib/src/BayesFilter.h @@ -58,6 +58,7 @@ public: float getVirtualPlacePrior() const {return _virtualPlacePrior;} const std::vector & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...} std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...} + bool isBadSignaturesIgnored() const {return _badSignaturesIgnored;} cv::Mat generatePrediction(const Memory * memory, const std::vector & ids) const; @@ -79,6 +80,7 @@ private: float _virtualPlacePrior; std::vector _predictionLC; // {Vp, Lc, l1, l2, l3, l4...} bool _fullPredictionUpdate; + bool _badSignaturesIgnored; float _totalPredictionLCValues; }; diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index 43e5efaa..d95ddc7e 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -237,9 +237,22 @@ bool CameraImages::init() { UWARN("Directory is empty \"%s\"", _path.c_str()); } + else + { + UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount()); + } return _dir->isValid(); } +unsigned int CameraImages::imagesCount() const +{ + if(_dir) + { + return _dir->getFileNames().size(); + } + return 0; +} + cv::Mat CameraImages::captureImage() { cv::Mat img; diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 401f5b40..702dac75 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -125,6 +125,10 @@ bool CameraModel::load(const std::string & filePath) return true; } + else + { + UWARN("Could not load calibration file \"%s\".", filePath.c_str()); + } return false; } @@ -241,11 +245,15 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const // //StereoCameraModel // -bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName) +bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) { name_ = cameraName; if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml")) { + if(ignoreStereoTransform) + { + return true; + } //load rotation, translation R_ = cv::Mat(); T_ = cv::Mat(); @@ -299,13 +307,21 @@ bool StereoCameraModel::load(const std::string & directory, const std::string & return true; } + else + { + UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str()); + } } return false; } -bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName) +bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) { if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml")) { + if(ignoreStereoTransform) + { + return true; + } std::string filePath = directory+"/"+cameraName+"_pose.yaml"; if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty()) { diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 7f2faffd..797954be 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -70,6 +70,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#include + namespace rtabmap { @@ -90,7 +92,7 @@ CameraRGBD::~CameraRGBD() } } -void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { bool warnFrameRateTooHigh = false; float actualFrameRate = 0; @@ -119,7 +121,7 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f } UTimer timer; - this->captureImage(rgb, depth, fx, fy, cx, cy); + this->captureImage(rgb, depth, fx, fy, cx, cy, stamp); if(_colorOnly) { depth = cv::Mat(); @@ -258,7 +260,7 @@ std::string CameraOpenni::getSerial() const return ""; } -void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { rgb = cv::Mat(); depth = cv::Mat(); @@ -266,6 +268,7 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa fy=0.0f; cx=0.0f; cy=0.0f; + stamp = 0.0; if(interface_ && interface_->isRunning()) { if(!dataReady_.acquire(1, 2000)) @@ -283,6 +286,7 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa fy = 1.0f/depthConstant_; cx = float(depth_.cols/2) - 0.5f; cy = float(depth_.rows/2) - 0.5f; + stamp = UTimer::now(); } depth_ = cv::Mat(); @@ -369,7 +373,7 @@ bool CameraOpenNICV::isCalibrated() const return true; } -void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { if(_capture.isOpened()) { @@ -384,6 +388,7 @@ void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl fy = _depthFocal; cx = float(depth.cols/2) - 0.5f; cy = float(depth.rows/2) - 0.5f; + stamp = UTimer::now(); } else { @@ -710,7 +715,7 @@ std::string CameraOpenNI2::getSerial() const return ""; } -void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { #ifdef WITH_OPENNI2 rgb = cv::Mat(); @@ -719,6 +724,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo fy = 0.0f; cx = 0.0f; cy = 0.0f; + stamp = 0.0; int readyStream = -1; if(_device->isValid() && @@ -755,6 +761,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo fy = _depthFy; cx = float(depth.cols/2) - 0.5f; cy = float(depth.rows/2) - 0.5f; + stamp = UTimer::now(); } } else @@ -1061,7 +1068,7 @@ std::string CameraFreenect::getSerial() const return ""; } -void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { #ifdef WITH_FREENECT rgb = cv::Mat(); @@ -1070,6 +1077,7 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl fy = 0.0f; cx = 0.0f; cy = 0.0f; + stamp = 0.0; if(ctx_ && freenectDevice_) { if(freenectDevice_->isRunning()) @@ -1082,6 +1090,7 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl fy = freenectDevice_->getDepthFocal(); cx = float(depth.cols/2) - 0.5f; cy = float(depth.rows/2) - 0.5f; + stamp = UTimer::now(); } } else @@ -1234,7 +1243,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder) // look for calibration files if(!calibrationFolder.empty()) { - if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber())) + if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber(), false)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.", dev_->getSerialNumber().c_str(), calibrationFolder.c_str()); @@ -1300,7 +1309,7 @@ std::string CameraFreenect2::getSerial() const return ""; } -void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) +void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) { #ifdef WITH_FREENECT2 rgb = cv::Mat(); @@ -1309,11 +1318,13 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f fy = 0.0f; cx = 0.0f; cy = 0.0f; + stamp = 0.0; if(dev_ && listener_) { libfreenect2::FrameMap frames; if(listener_->waitForNewFrame(frames, 1000)) { + stamp = UTimer::now(); libfreenect2::Frame *rgbFrame = 0; libfreenect2::Frame *irFrame = 0; libfreenect2::Frame *depthFrame = 0; @@ -1757,7 +1768,7 @@ public: return false; } dc1394video_frame_t frame1 = *frame; - // deinterlace frame into two images one on top the other + // deinterlace frame into two imagesCount one on top the other size_t frame1_size = frame->total_bytes; frame1.image = (unsigned char *) malloc(frame1_size); frame1.allocated_image_bytes = frame1_size; @@ -1876,7 +1887,7 @@ std::string CameraStereoDC1394::getSerial() const return ""; } -void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy) +void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) { #ifdef WITH_DC1394 left = cv::Mat(); @@ -1885,6 +1896,7 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f baseline = 0.0f; cx = 0.0f; cy = 0.0f; + stamp = 0.0; if(device_) { device_->getImages(left, right); @@ -1896,6 +1908,7 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f cx = stereoModel_.left().cx(); cy = stereoModel_.left().cy(); baseline = stereoModel_.baseline(); + stamp = UTimer::now(); } #else UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!"); @@ -2056,7 +2069,7 @@ struct ImageContainer } ; #endif -void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy) +void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) { #ifdef WITH_FLYCAPTURE2 left = cv::Mat(); @@ -2065,14 +2078,17 @@ void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, floa baseline = 0.0f; cx = 0.0f; cy = 0.0f; + stamp = 0.0; if(camera_ && triclopsCtx_ && camera_->IsConnected()) { // grab image from camera. - // this image contains both right and left images + // this image contains both right and left imagesCount FlyCapture2::Image grabbedImage; if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK) { + stamp = UTimer::now(); + // right and left image extracted from grabbed image ImageContainer imageCont; @@ -2174,4 +2190,195 @@ void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, floa #endif } +// +// CameraStereoImages +// +bool CameraStereoImages::available() +{ + return true; +} + +CameraStereoImages::CameraStereoImages( + const std::string & path, + const std::string & cameraName, + const std::string & timestampsPath, + float imageRate, + const Transform & localTransform) : + CameraRGBD(imageRate, localTransform), + camera_(0), + camera2_(0), + cameraName_(cameraName), + timestampsPath_(timestampsPath) +{ + std::vector paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';')); + if(paths.size() >= 1) + { + camera_ = new CameraImages(paths[0]); + + if(paths.size() >= 2) + { + camera2_ = new CameraImages(paths[1]); + } + } + else + { + UERROR("The path is empty!"); + } +} + +CameraStereoImages::~CameraStereoImages() +{ + if(camera_) + { + delete camera_; + } + if(camera2_) + { + delete camera2_; + } +} + +bool CameraStereoImages::init(const std::string & calibrationFolder) +{ + // look for calibration files + if(!calibrationFolder.empty() && !cameraName_.empty()) + { + if(!stereoModel_.load(calibrationFolder, cameraName_)) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName_.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", + stereoModel_.left().fx(), + stereoModel_.left().cx(), + stereoModel_.left().cy(), + stereoModel_.baseline()); + } + } + bool success = false; + if(camera_ == 0) + { + UERROR("Cannot initialize the camera."); + } + else if(camera_->init()) + { + if(camera2_) + { + if(camera2_->init()) + { + if(camera_->imagesCount() == camera2_->imagesCount()) + { + success = true; + } + else + { + UERROR("Cameras don't have the same number of images (%d vs %d)", + camera_->imagesCount(), camera2_->imagesCount()); + } + } + else + { + UERROR("Cannot initialize the second camera."); + } + } + else + { + success = true; + } + } + + stamps_.clear(); + if(success && timestampsPath_.size()) + { + FILE * file = 0; +#ifdef _MSC_VER + fopen_s(&file, timestampsPath_.c_str(), "r"); +#else + file = fopen(timestampsPath_.c_str(), "r"); +#endif + if(file) + { + char line[16]; + while ( fgets (line , 16 , file) != NULL ) + { + stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0))); + } + fclose(file); + } + if(stamps_.size() != camera_->imagesCount()) + { + UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove " + "the timestamps file path if you don't want to use them (current file path=%s).", + (int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str()); + stamps_.clear(); + success = false; + } + } + + return success; +} + +bool CameraStereoImages::isCalibrated() const +{ + return stereoModel_.isValid(); +} + +std::string CameraStereoImages::getSerial() const +{ + return "stereo_images"; +} + +void CameraStereoImages::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) +{ + left = cv::Mat(); + right = cv::Mat(); + fx = 0.0f; + baseline = 0.0f; + cx = 0.0f; + cy = 0.0f; + stamp = 0.0; + + if(camera_) + { + if(stamps_.size()) + { + stamp = stamps_.front(); + stamps_.pop_front(); + } + else + { + stamp = UTimer::now(); + } + left = camera_->takeImage(); + if(!left.empty()) + { + if(camera2_) + { + right = camera2_->takeImage(); + } + else + { + right = camera_->takeImage(); + } + + if(!right.empty()) + { + // Rectification + //left = stereoModel_.left().rectifyImage(left); + //right = stereoModel_.right().rectifyImage(right); + fx = stereoModel_.left().fx(); + cx = stereoModel_.left().cx(); + cy = stereoModel_.left().cy(); + baseline = stereoModel_.baseline(); + } + else + { + left = cv::Mat(); + } + } + } +} + } // namespace rtabmap diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 6d0dbb4a..95549d59 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -112,25 +112,26 @@ void CameraThread::mainLoop() float fy = 0.0f; float cx = 0.0f; float cy = 0.0f; + double stamp = UTimer::now(); if(_cameraRGBD) { - _cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy); + _cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy, stamp); } else { rgb = _camera->takeImage(); } - if(!rgb.empty() && !this->isKilled()) + if(!rgb.empty()) { if(_cameraRGBD) { - SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now()); + SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, stamp); this->post(new CameraEvent(data, _cameraRGBD->getSerial())); } else { - this->post(new CameraEvent(rgb, ++_seq, UTimer::now())); + this->post(new CameraEvent(rgb, ++_seq, stamp)); } } else if(!this->isKilled()) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index f228c6d3..7f1ffa1b 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -110,17 +110,19 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para return optimizer; } -Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored) : +Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) : iterations_(iterations), slam2d_(slam2d), - covarianceIgnored_(covarianceIgnored) + covarianceIgnored_(covarianceIgnored), + epsilon_(epsilon) { } Optimizer::Optimizer(const ParametersMap & parameters) : - iterations_(100), - slam2d_(false), - covarianceIgnored_(false) + iterations_(Parameters::defaultRGBDOptimizeIterations()), + slam2d_(Parameters::defaultRGBDOptimizeSlam2D()), + covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()), + epsilon_(Parameters::defaultRGBDOptimizeEpsilon()) { parseParameters(parameters); } @@ -130,6 +132,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), iterations_); Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_); Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_); + Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_); } void Optimizer::getConnectedGraph( @@ -350,6 +353,7 @@ std::map TOROOptimizer::optimize( } UINFO("TORO iterate begin (iterations=%d)", iterations()); + double lasterror = 0; for (int i=0; i0) @@ -382,12 +386,14 @@ std::map TOROOptimizer::optimize( } intermediateGraphes->push_back(tmpPoses); } + + double error = 0; if(isSlam2d()) { pg2.iterate(); // compute the error and dump it - double error=pg2.error(); + error=pg2.error(); UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size()); } else @@ -396,10 +402,19 @@ std::map TOROOptimizer::optimize( // compute the error and dump it double mte, mre, are, ate; - double error=pg3.error(&mre, &mte, &are, &ate); + error=pg3.error(&mre, &mte, &are, &ate); UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f", i, pg3.getRotGain(), error, error/pg3.edges.size()); } + + // early stop condition + double errorDelta = lasterror - error; + if(i>0 && errorDelta < this->epsilon()) + { + UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); + break; + } + lasterror = error; } UINFO("TORO iterate end"); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 609d8659..2d89e97a 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_correspondences.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_surface.h" +#include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Compression.h" @@ -103,6 +104,9 @@ Memory::Memory(const ParametersMap & parameters) : _bowForce2D(Parameters::defaultLccBowForce2D()), _bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()), _bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()), + _bowPnPEstimation(Parameters::defaultLccBowPnPEstimation()), + _bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()), + _bowPnPFlags(Parameters::defaultLccBowPnPFlags()), _icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()), _icpMaxRotation(Parameters::defaultLccIcpMaxRotation()), @@ -440,6 +444,9 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D); Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry); Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar); + Parameters::parse(parameters, Parameters::kLccBowPnPEstimation(), _bowPnPEstimation); + Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError); + Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags); Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation); Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation); Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation); @@ -604,13 +611,23 @@ bool Memory::update(const SensorData & data, Statistics * stats) //============================================================ // Transfer the oldest signature of the short-term memory to the working memory //============================================================ - while(_stMem.size() && _maxStMemSize>0 && (int)_stMem.size() > _maxStMemSize) + int validSignaturesCount = 0; + for(std::set::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter) + { + const Signature * s = this->getSignature(*iter); + UASSERT(s != 0); + if(!s->isBadSignature()) + { + ++validSignaturesCount; + } + } + while(_stMem.size() && _maxStMemSize>0 && validSignaturesCount > _maxStMemSize) { UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin()); + Signature * s = this->_getSignature(*_stMem.begin()); if(!_localSpaceLinksKeptInWM) { // remove local space links outside STM - Signature * s = this->_getSignature(*_stMem.begin()); UASSERT(s!=0); std::map links = s->getLinks(); // get a copy because we will remove some links in "s" for(std::map::iterator iter=links.begin(); iter!=links.end(); ++iter) @@ -630,6 +647,10 @@ bool Memory::update(const SensorData & data, Statistics * stats) } } } + if(!s->isBadSignature()) + { + --validSignaturesCount; + } _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); ++_signaturesAdded; @@ -851,6 +872,7 @@ std::map Memory::getNeighborsId(int signatureId, int maxCheckedInDatabase, // default -1 (no limit) bool incrementMarginOnLoop, // default false bool ignoreLoopIds, // default false + bool ignoreBadSignatures, // default false double * dbAccessTime ) const { @@ -871,6 +893,7 @@ std::map Memory::getNeighborsId(int signatureId, std::set nextMargin; nextMargin.insert(signatureId); int m = 0; + std::set ignoredIds; while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size()) { // insert more recent first (priority to be loaded first from the database below if set) @@ -888,7 +911,14 @@ std::map Memory::getNeighborsId(int signatureId, const std::map * links = &tmpLinks; if(s) { - ids.insert(std::pair(*jter, m)); + if(!ignoreBadSignatures || !s->isBadSignature()) + { + ids.insert(std::pair(*jter, m)); + } + else + { + ignoredIds.insert(*jter); + } links = &s->getLinks(); } @@ -908,12 +938,23 @@ std::map Memory::getNeighborsId(int signatureId, // links for(std::map::const_iterator iter=links->begin(); iter!=links->end(); ++iter) { - if( !uContains(ids, iter->first)) + if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end()) { UASSERT(iter->second.type() != Link::kUndef); if(iter->second.type() == Link::kNeighbor) { - nextMargin.insert(iter->first); + if(ignoreBadSignatures && s->isBadSignature()) + { + // stay on the same margin + if(currentMargin.insert(iter->first).second) + { + curentMarginList.push_back(iter->first); + } + } + else + { + nextMargin.insert(iter->first); + } } else if(!ignoreLoopIds) { @@ -1915,7 +1956,134 @@ Transform Memory::computeVisualTransform( std::string msg; // Guess transform from visual words - if(_bowEpipolarGeometry) + if(_bowPnPEstimation) + { + if(_bowEpipolarGeometry) + { + UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); + } + + // 2D -> 3D + if(!oldS.getWords3().empty() && !newS.getWords().empty()) + { + // find correspondences + std::vector ids = uListToVector(uUniqueKeys(newS.getWords())); + std::vector objectPoints(ids.size()); + std::vector imagePoints(ids.size()); + int oi=0; + std::vector matches(ids.size()); + for(unsigned int i=0; isecond; + if(pcl::isFinite(pt)) + { + objectPoints[oi].x = pt.x; + objectPoints[oi].y = pt.y; + objectPoints[oi].z = pt.z; + imagePoints[oi] = newS.getWords().find(ids[i])->second.pt; + matches[oi++] = ids[i]; + } + } + } + + objectPoints.resize(oi); + imagePoints.resize(oi); + matches.resize(oi); + + if((int)matches.size() >= _bowMinInliers) + { + //PnPRansac + cv::Mat K = (cv::Mat_(3,3) << + newS.getFx(), 0, newS.getCx(), + 0, newS.getFy()>1?newS.getFy():newS.getFx(), newS.getCy(), + 0, 0, 1); + + Transform guess = (newS.getLocalTransform()).inverse(); + cv::Mat R = (cv::Mat_(3,3) << + (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), + (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), + (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); + cv::Mat rvec(1,3, CV_64FC1); + cv::Rodrigues(R, rvec); + cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); + std::vector inliersV; + cv::solvePnPRansac(objectPoints, + imagePoints, + K, + cv::Mat(), + rvec, + tvec, + true, + _bowIterations, + _bowPnPReprojError, + 0, + inliersV, + _bowPnPFlags); + + if(inliers) + { + *inliers = (int)inliersV.size(); + } + if((int)inliersV.size() >= _bowMinInliers) + { + cv::Rodrigues(rvec, R); + Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), + R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), + R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); + + transform = newS.getLocalTransform() * pnp; + + UDEBUG("Odom transform = %s", transform.prettyPrint().c_str()); + + // compute variance (like in PCL computeVariance() method of sac_model.h) + if(varianceOut) + { + std::vector errorSqrdDists(inliersV.size()); + oi = 0; + for(unsigned int i=0; i::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]); + if(iter != newS.getWords3().end() && pcl::isFinite(iter->second)) + { + const cv::Point3f & objPt = objectPoints[inliersV[i]]; + pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform); + errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); + } + } + errorSqrdDists.resize(oi); + *varianceOut= 0; + if(errorSqrdDists.size()) + { + std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; + *varianceOut = 2.1981 * median_error_sqr; + } + } + } + else + { + msg = uFormat("PnP not enough inliers (%d[%d] < %d), rejecting the transform...", + (int)inliersV.size(), (int)matches.size(), _bowMinInliers); + UINFO(msg.c_str()); + } + } + else + { + msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers); + UINFO(msg.c_str()); + } + } + else + { + msg = uFormat("Not enough features in the new image (old=%d new=%d min=%d)", + (int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers); + UINFO(msg.c_str()); + } + + } + else if(_bowEpipolarGeometry) { // we only need the camera transform, send guess words3 for scale estimation if(oldS.getWords3().size()) @@ -1988,6 +2156,7 @@ Transform Memory::computeVisualTransform( } else { + // 3D -> 3D if(!oldS.getWords3().empty() && !newS.getWords3().empty()) { pcl::PointCloud::Ptr inliersOld(new pcl::PointCloud); @@ -2887,72 +3056,97 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) } //============================================================ - // Compare with the last + // Compare with the last (not null) //============================================================ - int id = signature->getLinks().begin()->first; - UDEBUG("Comparing with last signature (%d)...", id); - Signature * sB = this->_getSignature(id); - if(!sB) + Signature * sB = 0; + for(std::set::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter) { - UFATAL("Signature %d null?!?", id); - } - float sim = signature->compareTo(*sB); - - int merged = 0; - if(sim >= _similarityThreshold) - { - if(_incrementalMemory) + Signature * s = this->_getSignature(*iter); + UASSERT(s!=0); + if(!s->isBadSignature() && s->id() != signature->id()) { - if(signature->getLinks().begin()->second.transform().isNull()) + sB = s; + break; + } + } + if(sB) + { + int id = sB->id(); + UDEBUG("Comparing with signature (%d)...", id); + + float sim = signature->compareTo(*sB); + + int merged = 0; + if(sim >= _similarityThreshold) + { + if(_incrementalMemory) { - if(this->rehearsalMerge(id, signature->id())) + if(signature->hasLink(id)) { - merged = id; + if(signature->getLinks().begin()->second.transform().isNull()) + { + if(this->rehearsalMerge(id, signature->id())) + { + merged = id; + } + } + else + { + float x,y,z, roll,pitch,yaw; + signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + if((_rehearsalMaxDistance>0.0f && ( + fabs(x) > _rehearsalMaxDistance || + fabs(y) > _rehearsalMaxDistance || + fabs(z) > _rehearsalMaxDistance)) || + (_rehearsalMaxAngle>0.0f && ( + fabs(roll) > _rehearsalMaxAngle || + fabs(pitch) > _rehearsalMaxAngle || + fabs(yaw) > _rehearsalMaxAngle))) + { + if(_rehearsalWeightIgnoredWhileMoving) + { + UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad", + _rehearsalMaxDistance, _rehearsalMaxAngle); + } + else + { + // if the robot has moved, increase only weight of the new one + signature->setWeight(sB->getWeight() + signature->getWeight() + 1); + sB->setWeight(0); + UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)", + signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle); + } + } + else if(this->rehearsalMerge(id, signature->id())) + { + merged = id; + } + } + } + else + { + // cannot merge not neighbor signatures, just update weight + signature->setWeight(sB->getWeight() + signature->getWeight() + 1); + sB->setWeight(0); + UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.", + signature->getWeight(), signature->id(), sB->id()); } } else { - float x,y,z, roll,pitch,yaw; - signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if((_rehearsalMaxDistance>0.0f && ( - fabs(x) > _rehearsalMaxDistance || - fabs(y) > _rehearsalMaxDistance || - fabs(z) > _rehearsalMaxDistance)) || - (_rehearsalMaxAngle>0.0f && ( - fabs(roll) > _rehearsalMaxAngle || - fabs(pitch) > _rehearsalMaxAngle || - fabs(yaw) > _rehearsalMaxAngle))) - { - if(_rehearsalWeightIgnoredWhileMoving) - { - UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad", - _rehearsalMaxDistance, _rehearsalMaxAngle); - } - else - { - // if the robot has moved, increase only weight of the new one - signature->setWeight(sB->getWeight() + signature->getWeight() + 1); - sB->setWeight(0); - UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)", - signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle); - } - } - else if(this->rehearsalMerge(id, signature->id())) - { - merged = id; - } + signature->setWeight(signature->getWeight() + 1 + sB->getWeight()); } } - else - { - signature->setWeight(signature->getWeight() + 1 + sB->getWeight()); - } + + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged); + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim); + UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks()); + } + else + { + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0); + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0); } - - if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged); - if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim); - - UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks()); } bool Memory::rehearsalMerge(int oldId, int newId) @@ -3608,7 +3802,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) pcl::PointCloud::Ptr keypoints3D(new pcl::PointCloud); if(data.keypoints().size() == 0) { - if(_feature2D->getMaxFeatures() >= 0) + if(_feature2D->getMaxFeatures() >= 0 && !data.image().empty()) { // Extract features cv::Mat imageMono; @@ -3812,6 +4006,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) descriptors = cv::Mat(); } } + else if(data.image().empty()) + { + UDEBUG("Empty image, cannot extract features..."); + } else { UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures()); @@ -4061,11 +4259,11 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) rtabmap::compressData2(laserScan), cv::Mat(), cv::Mat(), - 0, - 0, - 0, - 0, - Transform(), + fx, + fyOrBaseline, + cx, + cy, + data.localTransform(), data.laserScanMaxPts()); } if(this->isRawDataKept()) @@ -4309,7 +4507,42 @@ void Memory::getMetricConstraints( uContains(poses, jter->first) && graph::findLink(links, *iter, jter->first) == links.end()) { - links.insert(std::make_pair(*iter, jter->second)); + // Remove bad signatures from the graph (Intermediate nodes) + if(!lookInDatabase) + { + Link link = jter->second; + const Signature * s = this->getSignature(jter->first); + UASSERT(s!=0); + while(s && s->isBadSignature()) + { + // skip to next neighbor, well we assume that bad signatures + // are only linked by max 2 neighbor links. + std::map n = this->getNeighborLinks(s->id(), false); + UASSERT(n.size() <= 2); + std::map::iterator uter = n.upper_bound(s->id()); + if(uter != n.end()) + { + const Signature * s2 = this->getSignature(uter->first); + if(s2) + { + link = link.merge(uter->second); + poses.erase(s->id()); + s = s2; + } + + } + else + { + break; + } + } + + links.insert(std::make_pair(*iter, link)); + } + else + { + links.insert(std::make_pair(*iter, jter->second)); + } } } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ac7777e4..f249b946 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" +#include "ParticleFilter.h" namespace rtabmap { @@ -41,11 +42,18 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _maxDepth(Parameters::defaultOdomMaxDepth()), _resetCountdown(Parameters::defaultOdomResetCountdown()), _force2D(Parameters::defaultOdomForce2D()), + _particleFiltering(Parameters::defaultOdomParticleFiltering()), + _particleSize(Parameters::defaultOdomParticleSize()), + _particleNoiseT(Parameters::defaultOdomParticleNoiseT()), + _particleLambdaT(Parameters::defaultOdomParticleLambdaT()), + _particleNoiseR(Parameters::defaultOdomParticleNoiseR()), + _particleLambdaR(Parameters::defaultOdomParticleLambdaR()), _fillInfoData(Parameters::defaultOdomFillInfoData()), _pnpEstimation(Parameters::defaultOdomPnPEstimation()), _pnpReprojError(Parameters::defaultOdomPnPReprojError()), _pnpFlags(Parameters::defaultOdomPnPFlags()), - _resetCurrentCount(0) + _resetCurrentCount(0), + previousStamp_(0) { Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers); @@ -60,21 +68,78 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError); Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags); UASSERT(_pnpFlags>=0 && _pnpFlags <=2); + Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering); + Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize); + Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT); + Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT); + Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR); + Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR); + UASSERT(_particleNoiseT>0); + UASSERT(_particleLambdaT>0); + UASSERT(_particleNoiseR>0); + UASSERT(_particleLambdaR>0); + if(_particleFiltering) + { + filters_.resize(6); + for(unsigned int i = 0; iinit(x); + filters_[1]->init(y); + filters_[2]->init(z); + filters_[3]->init(roll); + filters_[4]->init(pitch); + filters_[5]->init(yaw); } - Transform pose(x, y, 0, 0, 0, yaw); - _pose = pose; } else { @@ -107,19 +172,48 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) if(info) { - info->time = time.elapsed(); + info->timeEstimation = time.ticks(); info->lost = t.isNull(); + info->stamp = data.stamp(); + info->interval = data.stamp() - previousStamp_; + info->transform = t; } + previousStamp_ = data.stamp(); if(!t.isNull()) { _resetCurrentCount = _resetCountdown; - if(_force2D) + if(_force2D || filters_.size()) { float x,y,z, roll,pitch,yaw; t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); - t = Transform(x,y,0, 0,0,yaw); + + if(filters_.size()) + { + UASSERT(filters_.size()==6); + x = filters_[0]->filter(x); + y = filters_[1]->filter(y); + yaw = filters_[5]->filter(yaw); + + if(!_force2D) + { + z = filters_[2]->filter(z); + roll = filters_[3]->filter(roll); + pitch = filters_[4]->filter(pitch); + } + + if(info) + { + info->timeParticleFiltering = time.ticks(); + } + } + t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw); + + if(info) + { + info->transformFiltered = t; + } } return _pose *= t; // updated diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 003b249f..9b5a0674 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -235,17 +235,24 @@ Transform OdometryBOW::computeTransform( // compute variance (like in PCL computeVariance() method of sac_model.h) std::vector errorSqrdDists(inliersV.size()); + oi = 0; for(unsigned int i=0; i::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]); - UASSERT(iter != newSignature->getWords3().end()); - const cv::Point3f & objPt = objectPoints[inliersV[i]]; - pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform); - errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); + if(iter != newSignature->getWords3().end() && pcl::isFinite(iter->second)) + { + const cv::Point3f & objPt = objectPoints[inliersV[i]]; + pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform); + errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); + } + } + errorSqrdDists.resize(oi); + if(errorSqrdDists.size()) + { + std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; + variance = 2.1981 * median_error_sqr; } - std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; - variance = 2.1981 * median_error_sqr; } else { diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 23d7e2fe..4bccf25c 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -34,8 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -OdometryThread::OdometryThread(Odometry * odometry) : +OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) : _odometry(odometry), + _dataBufferMaxSize(dataBufferMaxSize), _resetOdometry(false) { UASSERT(_odometry != 0); @@ -92,8 +93,7 @@ void OdometryThread::mainLoop() } SensorData data; - getData(data); - if(data.isValid()) + if(getData(data)) { OdometryInfo info; Transform pose = _odometry->process(data, &info); @@ -124,8 +124,13 @@ void OdometryThread::addData(const SensorData & data) bool notify = true; _dataMutex.lock(); { - notify = !_dataBuffer.isValid(); - _dataBuffer = data; + _dataBuffer.push_back(data); + while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) + { + ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one."); + _dataBuffer.pop_front(); + notify = false; + } } _dataMutex.unlock(); @@ -135,18 +140,21 @@ void OdometryThread::addData(const SensorData & data) } } -void OdometryThread::getData(SensorData & data) +bool OdometryThread::getData(SensorData & data) { + bool dataFilled = false; _dataAdded.acquire(); _dataMutex.lock(); { - if(_dataBuffer.isValid()) + if(!_dataBuffer.empty()) { - data = _dataBuffer; - _dataBuffer = SensorData(); + data = _dataBuffer.front(); + _dataBuffer.pop_front(); + dataFilled = true; } } _dataMutex.unlock(); + return dataFilled; } } // namespace rtabmap diff --git a/corelib/src/ParticleFilter.h b/corelib/src/ParticleFilter.h new file mode 100644 index 00000000..ad9ffaef --- /dev/null +++ b/corelib/src/ParticleFilter.h @@ -0,0 +1,173 @@ +/* +Copyright (c) 2010-2015, 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 PARTICLEFILTER_H_ +#define PARTICLEFILTER_H_ + +#include +#include + + +namespace rtabmap { + +// taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/ +#define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */ + +/* + RAND is a macro which returns a pseudo-random numbers from a uniform + distribution on the interval [0 1] +*/ +#define RAND (rand())/((double) RAND_MAX) + +/* + RANDN is a macro which returns a pseudo-random numbers from a normal + distribution with mean zero and standard deviation one. This macro uses Box + Muller's algorithm +*/ +#define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND)) + +std::vector cumSum(const std::vector & v) +{ + std::vector cum(v.size()); + double sum = 0; + for(unsigned int i=0; i resample(const std::vector & p, // particles + const std::vector & w, // weights + bool normalizeWeights = false) +{ + std::vector np; //new particles + if(p.size() != w.size() || p.size() == 0) + { + UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size()); + return np; + } + + std::vector cs; + if(normalizeWeights) + { + double wSum = uSum(w); + std::vector wNorm(w.size()); + for(unsigned int i=0; i(particles_.size(), initValue); + } + + double filter(double val) + { + std::vector weights(particles_.size()); + double sumWeights = 0; + for(unsigned int i=0; i particles_; + double noise_; + double lambda_; +}; + +} + + +#endif /* PARTICLEFILTER_H_ */ diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 703ef5ee..1551ae2a 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -737,6 +737,27 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g } } +void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global) +{ + if(_memory && _memory->getLastWorkingSignature()) + { + std::map poses; + std::multimap constraints; + + if(optimized) + { + this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints); + } + else + { + std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); + _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); + } + + this->dumpPoses(path, poses); + } +} + void Rtabmap::resetMemory() { _highestHypothesis = std::make_pair(0,0.0f); @@ -829,11 +850,6 @@ bool Rtabmap::process(const SensorData & data) // Wait for an image... //============================================================ ULOGGER_INFO("getting data..."); - if(!data.isValid()) - { - ULOGGER_INFO("image is not valid..."); - return false; - } timer.start(); timerTotal.start(); @@ -1002,18 +1018,25 @@ bool Rtabmap::process(const SensorData & data) if(signature->getLinks().size() == 1) { // link should be old to new - if(signature->id() > signature->getLinks().begin()->second.to()) + UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(), + "Only forward links should be added."); + + Link tmp = signature->getLinks().begin()->second.inverse(); + + // if the previous signature is a bad signature, remove it from the local graph + if(_constraints.size() && + _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to()) { - Link tmp = signature->getLinks().begin()->second; - tmp.setFrom(tmp.to()); - tmp.setTo(signature->id()); - tmp.setTransform(tmp.transform().inverse()); - _constraints.insert(std::make_pair(tmp.from(), tmp)); - } - else - { - _constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second)); + const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to()); + UASSERT(s!=0); + if(s->isBadSignature()) + { + tmp = _constraints.rbegin()->second.merge(tmp); + _optimizedPoses.erase(s->id()); + _constraints.erase(--_constraints.end()); + } } + _constraints.insert(std::make_pair(tmp.from(), tmp)); } //============================================================ @@ -1090,7 +1113,29 @@ bool Rtabmap::process(const SensorData & data) // with all images contained in the working memory + reactivated. //============================================================ ULOGGER_INFO("computing likelihood..."); - std::list signaturesToCompare = uKeysList(_memory->getWorkingMem()); + + // select only not empty signatures (may happen often if intermediate nodes are created) + std::list signaturesToCompare; + for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); + iter!=_memory->getWorkingMem().end(); + ++iter) + { + if(iter->first > 0) + { + const Signature * s = _memory->getSignature(iter->first); + UASSERT(s!=0); + if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + { + signaturesToCompare.push_back(iter->first); + } + } + else + { + // virtual signature should be added + signaturesToCompare.push_back(iter->first); + } + } + rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare); // Adjust the likelihood (with mean and std dev) @@ -1228,6 +1273,7 @@ bool Rtabmap::process(const SensorData & data) _maxRetrieved, true, true, + false, &timeGetNeighborsTimeDb); ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size()); //Priority to locations near in time (direct neighbor) then by space (loop closure) @@ -1281,6 +1327,7 @@ bool Rtabmap::process(const SensorData & data) _maxRetrieved, true, false, + false, &timeGetNeighborsSpaceDb); ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size()); firstPassDone = false; @@ -2426,6 +2473,37 @@ void Rtabmap::dumpData() const } } +void Rtabmap::dumpPoses( + const std::string & path, + const std::map & poses) const +{ + UDEBUG(""); + FILE* fout = 0; +#ifdef _MSC_VER + fopen_s(&fout, path.c_str(), "w"); +#else + fout = fopen(path.c_str(), "w"); +#endif + if(fout) + { + Transform localTransformInv = Transform(0,0,0, -CV_PI/2, 0, -CV_PI/2).inverse(); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + Transform t = localTransformInv * (*iter).second; + // in camera frame + const float * p = (const float *)t.data(); + + fprintf(fout, "%f", p[0]); + for(int i=1; i<(*iter).second.size(); i++) + { + fprintf(fout, " %f", p[i]); + } + fprintf(fout, "\n"); + } + fclose(fout); + } +} + // fromId must be in _memory and in _optimizedPoses // Get poses in front of the robot, return optimized poses std::map Rtabmap::getForwardWMPoses( @@ -2586,7 +2664,7 @@ void Rtabmap::optimizeCurrentMap( if(_memory && id > 0) { UTimer timer; - std::map ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true); + std::map ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false); if(!_optimizeFromGraphEnd && ids.size() > 1) { id = ids.begin()->first; @@ -2713,7 +2791,27 @@ void Rtabmap::dumpPrediction() const { if(_memory && _bayesFilter) { - cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uKeys(_memory->getWorkingMem())); + std::list signaturesToCompare; + for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); + iter!=_memory->getWorkingMem().end(); + ++iter) + { + if(iter->first > 0) + { + const Signature * s = _memory->getSignature(iter->first); + UASSERT(s!=0); + if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + { + signaturesToCompare.push_back(iter->first); + } + } + else + { + // virtual signature should be added + signaturesToCompare.push_back(iter->first); + } + } + cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare)); FILE* fout = 0; std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt"; diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 63717acd..80fe155a 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -46,6 +46,7 @@ namespace rtabmap { RtabmapThread::RtabmapThread(Rtabmap * rtabmap) : _dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()), _rate(Parameters::defaultRtabmapDetectionRate()), + _createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()), _frameRateTimer(new UTimer()), _rtabmap(rtabmap), _paused(false), @@ -106,10 +107,14 @@ void RtabmapThread::setDetectorRate(float rate) _rate = rate; } -void RtabmapThread::setBufferSize(int bufferSize) +void RtabmapThread::setDataBufferSize(unsigned int size) { - UASSERT(bufferSize >= 0); - _dataBufferMaxSize = bufferSize; + _dataBufferMaxSize = size; +} + +void RtabmapThread::createIntermediateNodes(bool enabled) +{ + enabled = _createIntermediateNodes; } void RtabmapThread::publishMap(bool optimized, bool full) const @@ -206,6 +211,7 @@ void RtabmapThread::mainLoop() UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty()); Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize); Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate); + Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes); UASSERT(_dataBufferMaxSize >= 0); UASSERT(_rate >= 0.0f); _rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath")); @@ -213,6 +219,7 @@ void RtabmapThread::mainLoop() case kStateChangingParameters: Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize); Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate); + Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes); UASSERT(_dataBufferMaxSize >= 0); UASSERT(_rate >= 0.0f); _rtabmap->parseParameters(parameters); @@ -247,6 +254,12 @@ void RtabmapThread::mainLoop() case kStateGeneratingTOROGraphGlobal: _rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true); break; + case kStateExportingPosesLocal: + _rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false); + break; + case kStateExportingPosesGlobal: + _rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true); + break; case kStateCleanDataBuffer: this->clearBufferedData(); break; @@ -418,6 +431,28 @@ void RtabmapThread::handleEvent(UEvent* event) param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStateGeneratingTOROGraphGlobal, param); + } + else if(cmd == RtabmapEventCmd::kCmdExportPosesLocal) + { + UASSERT(!rtabmapEvent->getStr().empty()); + + ULOGGER_DEBUG("CMD_EXPORT_POSES_LOCAL"); + ParametersMap param; + param.insert(ParametersPair("path", rtabmapEvent->getStr())); + param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); + pushNewState(kStateExportingPosesLocal, param); + + } + else if(cmd == RtabmapEventCmd::kCmdExportPosesGlobal) + { + UASSERT(!rtabmapEvent->getStr().empty()); + + ULOGGER_DEBUG("CMD_EXPORT_POSES_GLOBAL"); + ParametersMap param; + param.insert(ParametersPair("path", rtabmapEvent->getStr())); + param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); + pushNewState(kStateExportingPosesGlobal, param); + } else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer) { @@ -488,8 +523,7 @@ void RtabmapThread::handleEvent(UEvent* event) void RtabmapThread::process() { SensorData data; - getData(data); - if(data.isValid() && _state.empty()) + if(_state.empty() && getData(data)) { if(_rtabmap->getMemory()) { @@ -518,20 +552,14 @@ void RtabmapThread::addData(const SensorData & sensorData) return; } + bool ignoreFrame = false; if(_rate>0.0f) { if(_frameRateTimer->getElapsedTime() < 1.0f/_rate) { - if(!lastPose_.isIdentity() && sensorData.pose().isIdentity()) - { - UWARN("Odometry is reset (identity pose detected). Increment map id!"); - pushNewState(kStateTriggeringMap); - _rotVariance = 0; - _transVariance = 0; - } - - return; + ignoreFrame = true; } + } if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity()) { @@ -540,7 +568,15 @@ void RtabmapThread::addData(const SensorData & sensorData) _rotVariance = 0; _transVariance = 0; } - _frameRateTimer->start(); + + if(ignoreFrame && !_createIntermediateNodes) + { + return; + } + else if(!ignoreFrame) + { + _frameRateTimer->start(); + } lastPose_ = sensorData.pose(); if(sensorData.poseRotVariance() > _rotVariance) @@ -555,7 +591,26 @@ void RtabmapThread::addData(const SensorData & sensorData) bool notify = true; _dataMutex.lock(); { - _dataBuffer.push_back(sensorData); + if(ignoreFrame) + { + // remove data from the frame, keeping only constraints + SensorData tmp( + cv::Mat(), + cv::Mat(), + 0,0,0,0, + sensorData.localTransform(), + sensorData.pose(), + sensorData.poseRotVariance(), + sensorData.poseTransVariance(), + sensorData.id(), + sensorData.stamp(), + sensorData.userData()); + _dataBuffer.push_back(tmp); + } + else + { + _dataBuffer.push_back(sensorData); + } if(_rotVariance <= 0) { _rotVariance = 1.0f; @@ -567,7 +622,7 @@ void RtabmapThread::addData(const SensorData & sensorData) _dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance); _rotVariance = 0; _transVariance = 0; - while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize) + while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) { ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one."); _dataBuffer.pop_front(); @@ -583,7 +638,7 @@ void RtabmapThread::addData(const SensorData & sensorData) } } -void RtabmapThread::getData(SensorData & image) +bool RtabmapThread::getData(SensorData & image) { ULOGGER_DEBUG(""); @@ -591,28 +646,18 @@ void RtabmapThread::getData(SensorData & image) _dataAdded.acquire(); ULOGGER_INFO("wake-up"); + bool dataFilled = false; _dataMutex.lock(); { if(!_dataBuffer.empty()) { image = _dataBuffer.front(); _dataBuffer.pop_front(); + dataFilled = true; } } _dataMutex.unlock(); -} - -void RtabmapThread::setDataBufferSize(int size) -{ - if(size < 0) - { - ULOGGER_WARN("size < 0, then setting it to 0 (inf)."); - _dataBufferMaxSize = 0; - } - else - { - _dataBufferMaxSize = size; - } + return dataFilled; } } /* namespace rtabmap */ diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index 46bbf2ba..ab3f575a 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -210,7 +210,7 @@ float getDepth( if(!(u >=0 && u=0 && v=0 && x=0 && y=0 && x=0 && y::Ptr cloudFromDisparity( UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1); UASSERT(imageDisparity.rows % decimation == 0); UASSERT(imageDisparity.cols % decimation == 0); + UASSERT(decimation >= 1); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - if(decimation < 1) - { - return cloud; - } //cloud.header = cameraInfo.header; cloud->height = imageDisparity.rows/decimation; @@ -396,30 +393,25 @@ pcl::PointCloud::Ptr cloudFromDisparityRGB( float fx, float baseline, int decimation) { + UASSERT(!imageRgb.empty() && !imageDisparity.empty()); UASSERT(imageRgb.rows == imageDisparity.rows && imageRgb.cols == imageDisparity.cols && (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1)); - UASSERT(imageDisparity.rows % decimation == 0); - UASSERT(imageDisparity.cols % decimation == 0); + UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1); + UASSERT(decimation >= 1); + UASSERT(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - if(decimation < 1) - { - return cloud; - } bool mono; if(imageRgb.channels() == 3) // BGR { mono = false; } - else if(imageRgb.channels() == 1) // Mono + else // Mono { mono = true; } - else - { - return cloud; - } //cloud.header = cameraInfo.header; cloud->height = imageRgb.rows/decimation; @@ -463,20 +455,40 @@ pcl::PointCloud::Ptr cloudFromStereoImages( float fx, float baseline, int decimation) { + UASSERT(!imageLeft.empty() && !imageRight.empty()); UASSERT(imageRight.type() == CV_8UC1); + UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1); + UASSERT(imageLeft.rows == imageRight.rows && + imageLeft.cols == imageRight.cols); + UASSERT(decimation >= 1); + + cv::Mat leftColor = imageLeft; + cv::Mat rightMono = imageRight; + + if(leftColor.rows % decimation != 0 || + leftColor.cols % decimation != 0) + { + leftColor = util2d::decimate(leftColor, decimation); + rightMono = util2d::decimate(rightMono, decimation); + fx /= float(decimation); + cx /= float(decimation); + cy /= float(decimation); + decimation = 1; + } cv::Mat leftMono; - if(imageLeft.channels() == 3) + if(leftColor.channels() == 3) { - cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY); + cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY); } else { - leftMono = imageLeft; + leftMono = leftColor; } + return cloudFromDisparityRGB( - imageLeft, - util2d::disparityFromStereoImages(leftMono, imageRight), + leftColor, + util2d::disparityFromStereoImages(leftMono, rightMono), cx, cy, fx, baseline, decimation); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index e55e3c6e..789d88bc 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -131,10 +131,12 @@ private slots: void startDetection(); void pauseDetection(); void stopDetection(); + void notifyNoMoreImages(); void printLoopClosureIds(); void generateMap(); void generateLocalMap(); void generateTOROMap(); + void exportPoses(); void postProcessing(); void deleteMemory(); void openWorkingDirectory(); @@ -308,6 +310,7 @@ private: QString _graphSavingFileName; QString _toroSavingFileName; + QString _posesSavingFileName; bool _autoScreenCaptureOdomSync; QVector _refIds; diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h index ff295613..d39f9f2d 100644 --- a/guilib/include/rtabmap/gui/OdometryViewer.h +++ b/guilib/include/rtabmap/gui/OdometryViewer.h @@ -49,7 +49,7 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler Q_OBJECT public: - OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, int qualityWarningThr=0, QWidget * parent = 0); + OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0); virtual ~OdometryViewer(); public slots: @@ -76,6 +76,7 @@ private: QSpinBox * maxCloudsSpin_; QDoubleSpinBox * voxelSpin_; QSpinBox * decimationSpin_; + QDoubleSpinBox * maxDepthSpin_; QLabel * timeLabel_; int validDecimationValue_; }; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 83729912..895efd77 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -90,7 +90,8 @@ public: kSrcOpenNI2, kSrcFreenect2, kSrcStereoDC1394, - kSrcStereoFlyCapture2 + kSrcStereoFlyCapture2, + kSrcStereoImages }; public: @@ -205,6 +206,7 @@ public: double getLoopThr() const; double getVpThr() const; int getOdomStrategy() const; + int getOdomBufferSize() const; QString getCameraInfoDir() const; // "workinfDir/camera_info" // @@ -248,6 +250,8 @@ private slots: void setupTreeView(); void updateBasicParameter(); void openDatabaseViewer(); + void selectSourceStereoImagesStamps(); + void selectSourceStereoImagesPath(); void updateRGBDCameraGroupBoxVisibility(); void testOdometry(); void testRGBDCamera(); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 4e409e4e..fd2b61a0 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1064,7 +1064,7 @@ void DatabaseViewer::view3DMap() 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); + double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok); if(ok) { std::map optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); @@ -1188,7 +1188,7 @@ void DatabaseViewer::generate3DMap() 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); + double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok); if(ok) { QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_); @@ -2957,6 +2957,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0")); @@ -3075,6 +3076,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked()))); parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0")); @@ -3112,6 +3115,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked()))); memory_->parseParameters(parameters); t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index c2587873..dc1a4c25 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -301,6 +301,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap())); connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap())); connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap())); + connect(_ui->actionExport_poses_txt, SIGNAL(triggered()), this , SLOT(exportPoses())); connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory())); connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph())); @@ -441,7 +442,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : 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())); + connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages())); // Apply state this->changeState(kIdle); @@ -673,6 +674,11 @@ void MainWindow::handleEvent(UEvent* anEvent) _processingOdometry = true; // if we receive too many odometry events! emit odometryReceived(odomEvent->data(), odomEvent->info()); } + else + { + // we receive too many odometry events! just send without data + emit odometryReceived(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->info()); + } } } else if(anEvent->getClassName().compare("ULogEvent") == 0) @@ -699,37 +705,214 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap { _processingOdometry = true; UTimer time; - Transform pose = data.pose(); - bool lost = false; - bool lostStateChanged = false; + // Process Data + if(data.isValid()) + { + Transform pose = data.pose(); + bool lost = false; + bool lostStateChanged = false; - if(pose.isNull()) - { - UDEBUG("odom lost"); // use last pose - lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed; - _ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed); - _ui->imageView_odometry->setBackgroundColor(Qt::darkRed); + if(pose.isNull()) + { + UDEBUG("odom lost"); // use last pose + lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed; + _ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed); + _ui->imageView_odometry->setBackgroundColor(Qt::darkRed); - pose = _lastOdomPose; - lost = true; - } - else if(info.inliers>0 && - _preferencesDialog->getOdomQualityWarnThr() && - info.inliers < _preferencesDialog->getOdomQualityWarnThr()) - { - UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr()); - lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed; - _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow); - _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow); - } - else - { - UDEBUG("odom ok"); - lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed; - _ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor()); - _ui->imageView_odometry->setBackgroundColor(Qt::black); + pose = _lastOdomPose; + lost = true; + } + else if(info.inliers>0 && + _preferencesDialog->getOdomQualityWarnThr() && + info.inliers < _preferencesDialog->getOdomQualityWarnThr()) + { + UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr()); + lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed; + _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow); + _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow); + } + else + { + UDEBUG("odom ok"); + lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed; + _ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor()); + _ui->imageView_odometry->setBackgroundColor(Qt::black); + } + + if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible())) + { + _lastOdomPose = pose; + _odometryReceived = true; + } + + if(_ui->dockWidget_cloudViewer->isVisible()) + { + if(!pose.isNull()) + { + // 3d cloud + if(data.depthOrRightImage().cols == data.image().cols && + data.depthOrRightImage().rows == data.image().rows && + !data.depthOrRightImage().empty() && + data.fx() > 0.0f && + data.fyOrBaseline() > 0.0f && + _preferencesDialog->isCloudsShown(1)) + { + pcl::PointCloud::Ptr cloud; + cloud = createCloud(0, + data.image(), + data.depthOrRightImage(), + data.fx(), + data.fyOrBaseline(), + data.cx(), + data.cy(), + data.localTransform(), + pose, + _preferencesDialog->getCloudVoxelSize(1), + _preferencesDialog->getCloudDecimation(1), + _preferencesDialog->getCloudMaxDepth(1)); + + if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); + _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); + _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); + } + + // 2d cloud + if(!data.laserScan().empty() && + _preferencesDialog->isScansShown(1)) + { + pcl::PointCloud::Ptr cloud; + cloud = util3d::laserScanToPointCloud(data.laserScan()); + cloud = util3d::transformPointCloud(cloud, pose); + if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) + { + UERROR("Adding scanOdom to viewer failed!"); + } + _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true); + _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1)); + _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); + } + + if(!data.pose().isNull()) + { + // update camera position + _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose()); + } + } + _ui->widget_cloudViewer->update(); + } + + if(_ui->graphicsView_graphView->isVisible()) + { + if(!pose.isNull() && !data.pose().isNull()) + { + _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose()); + _ui->graphicsView_graphView->update(); + } + } + + if(_ui->dockWidget_odometry->isVisible() && + !data.image().empty()) + { + if(_ui->imageView_odometry->isFeaturesShown()) + { + if(info.type == 0) + { + _ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow); + } + else if(info.type == 1) + { + std::vector kpts; + cv::KeyPoint::convert(info.refCorners, kpts); + _ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red); + } + } + + _ui->imageView_odometry->clearLines(); + if(lost) + { + if(lostStateChanged) + { + // save state + _odomImageShow = _ui->imageView_odometry->isImageShown(); + _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown(); + } + _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image())); + _ui->imageView_odometry->setImageShown(true); + _ui->imageView_odometry->setImageDepthShown(true); + } + else + { + if(lostStateChanged) + { + // restore state + _ui->imageView_odometry->setImageShown(_odomImageShow); + _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow); + } + + _ui->imageView_odometry->setImage(uCvMat2QImage(data.image())); + if(_ui->imageView_odometry->isImageDepthShown()) + { + _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage())); + } + + if(info.type == 0) + { + if(_ui->imageView_odometry->isFeaturesShown()) + { + for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers + } + for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers + } + } + } + } + if(info.type == 1 && info.cornerInliers.size()) + { + if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) + { + //draw lines + UASSERT(info.refCorners.size() == info.newCorners.size()); + for(unsigned int i=0; iimageView_odometry->isFeaturesShown()) + { + _ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers + } + if(_ui->imageView_odometry->isLinesShown()) + { + _ui->imageView_odometry->addLine( + info.refCorners[info.cornerInliers[i]].x, + info.refCorners[info.cornerInliers[i]].y, + info.newCorners[info.cornerInliers[i]].x, + info.newCorners[info.cornerInliers[i]].y, + Qt::blue); + } + } + } + } + if(!data.image().empty()) + { + _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows)); + } + + _ui->imageView_odometry->update(); + } + + if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync) + { + this->captureScreen(); + } } + //Process info if(info.inliers >= 0) { _ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers); @@ -746,9 +929,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap { _ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance); } - if(info.time > 0) + if(info.timeEstimation > 0) { - _ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f); + _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)data.id(), (float)info.timeEstimation*1000.0f); + } + if(info.timeParticleFiltering > 0) + { + _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)data.id(), (float)info.timeParticleFiltering*1000.0f); } if(info.features >=0) { @@ -761,188 +948,35 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id()); 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(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible())) + if(!info.transform.isNull()) { - _lastOdomPose = pose; - _odometryReceived = true; + info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + _ui->statsToolBox->updateStat("Odometry/Tx/m", (float)data.id(), x); + _ui->statsToolBox->updateStat("Odometry/Ty/m", (float)data.id(), y); + _ui->statsToolBox->updateStat("Odometry/Tz/m", (float)data.id(), z); + _ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)data.id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)data.id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)data.id(), yaw*180.0/CV_PI); } - if(_ui->dockWidget_cloudViewer->isVisible()) + if(!info.transformFiltered.isNull()) { - if(!pose.isNull()) - { - // 3d cloud - if(data.depthOrRightImage().cols == data.image().cols && - data.depthOrRightImage().rows == data.image().rows && - !data.depthOrRightImage().empty() && - data.fx() > 0.0f && - data.fyOrBaseline() > 0.0f && - _preferencesDialog->isCloudsShown(1)) - { - pcl::PointCloud::Ptr cloud; - cloud = createCloud(0, - data.image(), - data.depthOrRightImage(), - data.fx(), - data.fyOrBaseline(), - data.cx(), - data.cy(), - data.localTransform(), - pose, - _preferencesDialog->getCloudVoxelSize(1), - _preferencesDialog->getCloudDecimation(1), - _preferencesDialog->getCloudMaxDepth(1)); - - if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) - { - UERROR("Adding cloudOdom to viewer failed!"); - } - _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); - _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); - _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); - } - - // 2d cloud - if(!data.laserScan().empty() && - _preferencesDialog->isScansShown(1)) - { - pcl::PointCloud::Ptr cloud; - cloud = util3d::laserScanToPointCloud(data.laserScan()); - cloud = util3d::transformPointCloud(cloud, pose); - if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) - { - UERROR("Adding scanOdom to viewer failed!"); - } - _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true); - _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1)); - _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); - } - - if(!data.pose().isNull()) - { - // update camera position - _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose()); - } - } - _ui->widget_cloudViewer->update(); + info.transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + _ui->statsToolBox->updateStat("Odometry/Fx/m", (float)data.id(), x); + _ui->statsToolBox->updateStat("Odometry/Fy/m", (float)data.id(), y); + _ui->statsToolBox->updateStat("Odometry/Fz/m", (float)data.id(), z); + _ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)data.id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)data.id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)data.id(), yaw*180.0/CV_PI); } - if(_ui->graphicsView_graphView->isVisible()) + if(info.interval > 0) { - if(!pose.isNull() && !data.pose().isNull()) - { - _ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose()); - _ui->graphicsView_graphView->update(); - } - } - - if(_ui->dockWidget_odometry->isVisible() && - !data.image().empty()) - { - if(_ui->imageView_odometry->isFeaturesShown()) - { - if(info.type == 0) - { - _ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow); - } - else if(info.type == 1) - { - std::vector kpts; - cv::KeyPoint::convert(info.refCorners, kpts); - _ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red); - } - } - - _ui->imageView_odometry->clearLines(); - if(lost) - { - if(lostStateChanged) - { - // save state - _odomImageShow = _ui->imageView_odometry->isImageShown(); - _odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown(); - } - _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image())); - _ui->imageView_odometry->setImageShown(true); - _ui->imageView_odometry->setImageDepthShown(true); - } - else - { - if(lostStateChanged) - { - // restore state - _ui->imageView_odometry->setImageShown(_odomImageShow); - _ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow); - } - - _ui->imageView_odometry->setImage(uCvMat2QImage(data.image())); - if(_ui->imageView_odometry->isImageDepthShown()) - { - _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage())); - } - - if(info.type == 0) - { - if(_ui->imageView_odometry->isFeaturesShown()) - { - for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers - } - for(unsigned int i=0; iimageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers - } - } - } - } - if(info.type == 1 && info.cornerInliers.size()) - { - if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) - { - //draw lines - UASSERT(info.refCorners.size() == info.newCorners.size()); - for(unsigned int i=0; iimageView_odometry->isFeaturesShown()) - { - _ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers - } - if(_ui->imageView_odometry->isLinesShown()) - { - _ui->imageView_odometry->addLine( - info.refCorners[info.cornerInliers[i]].x, - info.refCorners[info.cornerInliers[i]].y, - info.newCorners[info.cornerInliers[i]].x, - info.newCorners[info.cornerInliers[i]].y, - Qt::blue); - } - } - } - } - if(!data.image().empty()) - { - _ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows)); - } - - _ui->imageView_odometry->update(); - } - - if(_ui->actionAuto_screen_capture->isChecked() && _autoScreenCaptureOdomSync) - { - this->captureScreen(); + _ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f); + _ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f); } _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0); - _processingOdometry = false; } @@ -973,21 +1007,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) int highestHypothesisId = static_cast(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f)); bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f); - // Loop closure info - _ui->imageView_source->clear(); - _ui->imageView_loopClosure->clear(); - _ui->imageView_source->setBackgroundColor(Qt::black); - _ui->imageView_loopClosure->setBackgroundColor(Qt::black); - // update cache Signature signature = stat.getSignature(); signature.uncompressData(); // make sure data are uncompressed _cachedSignatures.insert(stat.getSignature().id(), signature); + // For intermediate empty nodes, keep latest image shown + if(!signature.getImageRaw().empty() || signature.getWords().size()) + { + _ui->imageView_source->clear(); + _ui->imageView_loopClosure->clear(); + + _ui->imageView_source->setBackgroundColor(Qt::black); + _ui->imageView_loopClosure->setBackgroundColor(Qt::black); + + _ui->label_matchId->clear(); + } + int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f); bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f); - _ui->label_matchId->clear(); _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId)); if(rehearsed > 0) @@ -1119,10 +1158,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible()) { UDEBUG(""); - if(stat.weights().size() != stat.posterior().size()) - { - UWARN("%d %d", stat.weights().size(), stat.posterior().size()); - } _posteriorCurve->setData(QMap(stat.posterior()), QMap(stat.weights())); ULOGGER_DEBUG(""); @@ -2747,7 +2782,7 @@ void MainWindow::startDetection() { odom = new OdometryBOW(parameters); } - _odomThread = new OdometryThread(odom); + _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize()); UEventsManager::addHandler(_odomThread); UEventsManager::createPipe(_camera, _odomThread, "CameraEvent"); @@ -2988,6 +3023,13 @@ void MainWindow::stopDetection() emit stateChanged(kInitialized); } +void MainWindow::notifyNoMoreImages() +{ + QMessageBox::information(this, + tr("No more images..."), + tr("The camera has reached the end of the stream.")); +} + void MainWindow::printLoopClosureIds() { _ui->dockWidget_console->show(); @@ -3126,6 +3168,66 @@ void MainWindow::generateTOROMap() } } +void MainWindow::exportPoses() +{ + if(_posesSavingFileName.isEmpty()) + { + _posesSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "poses.txt"; + } + + QStringList items; + items.append("Local map optimized"); + items.append("Local map not optimized"); + items.append("Global map optimized"); + items.append("Global map not optimized"); + bool ok; + QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok); + if(ok) + { + bool optimized=false, global=false; + if(item.compare("Local map optimized") == 0) + { + optimized = true; + } + else if(item.compare("Local map not optimized") == 0) + { + + } + else if(item.compare("Global map optimized") == 0) + { + global=true; + optimized=true; + } + else if(item.compare("Global map not optimized") == 0) + { + global=true; + } + else + { + UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str()); + } + + QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _posesSavingFileName, tr("Text file (*.txt)")); + if(!path.isEmpty()) + { + _posesSavingFileName = path; + if(global) + { + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesGlobal, path.toStdString(), optimized?1:0)); + } + else + { + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesLocal, path.toStdString(), optimized?1:0)); + } + + _ui->dockWidget_console->show(); + _ui->widget_console->appendMsg(QString("Poses saved (global=%1, optimized=%2)... %3") + .arg(global?"true":"false").arg(optimized?"true":"false").arg(_posesSavingFileName)); + } + + } +} + void MainWindow::postProcessing() { if(_cachedSignatures.size() == 0) @@ -5109,6 +5211,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setVisible(!monitoring); _ui->actionGenerate_local_map->setVisible(!monitoring); _ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring); + _ui->actionExport_poses_txt->setVisible(!monitoring); _ui->actionOpen_working_directory->setVisible(!monitoring); _ui->actionData_recorder->setVisible(!monitoring); _ui->menuSelect_source->menuAction()->setVisible(!monitoring); @@ -5188,6 +5291,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_local_map->setEnabled(false); _ui->actionGenerate_TORO_graph_graph->setEnabled(false); + _ui->actionExport_poses_txt->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); _ui->menuSelect_source->setEnabled(false); @@ -5235,6 +5339,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setEnabled(true); _ui->actionGenerate_local_map->setEnabled(true); _ui->actionGenerate_TORO_graph_graph->setEnabled(true); + _ui->actionExport_poses_txt->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_graph->setEnabled(true); _ui->menuSelect_source->setEnabled(true); @@ -5271,6 +5376,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_local_map->setEnabled(false); _ui->actionGenerate_TORO_graph_graph->setEnabled(false); + _ui->actionExport_poses_txt->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); _ui->menuSelect_source->setEnabled(false); @@ -5309,6 +5415,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setEnabled(false); _ui->actionGenerate_local_map->setEnabled(false); _ui->actionGenerate_TORO_graph_graph->setEnabled(false); + _ui->actionExport_poses_txt->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); _state = kDetecting; @@ -5337,6 +5444,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionGenerate_map->setEnabled(true); _ui->actionGenerate_local_map->setEnabled(true); _ui->actionGenerate_TORO_graph_graph->setEnabled(true); + _ui->actionExport_poses_txt->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_graph->setEnabled(true); _state = kPaused; diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 5125720f..c74b81b5 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -48,7 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) : +OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, float maxDepth, int qualityWarningThr, QWidget * parent) : QDialog(parent), imageView_(new ImageView(this)), cloudView_(new CloudViewer(this)), @@ -66,12 +66,14 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i imageView_->setImageDepthShown(false); imageView_->setMinimumSize(320, 240); + imageView_->setAlpha(255); - cloudView_->setCameraFree(); + cloudView_->setCameraTargetLocked(); cloudView_->setGridShown(true); QLabel * maxCloudsLabel = new QLabel("Max clouds", this); QLabel * voxelLabel = new QLabel("Voxel", this); + QLabel * maxDepthLabel = new QLabel("Max depth", this); QLabel * decimationLabel = new QLabel("Decimation", this); maxCloudsSpin_ = new QSpinBox(this); maxCloudsSpin_->setMinimum(0); @@ -84,6 +86,13 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i voxelSpin_->setSingleStep(0.01); voxelSpin_->setSuffix(" m"); voxelSpin_->setValue(voxelSize); + maxDepthSpin_ = new QDoubleSpinBox(this); + maxDepthSpin_->setMinimum(0); + maxDepthSpin_->setMaximum(100); + maxDepthSpin_->setDecimals(0); + maxDepthSpin_->setSingleStep(1); + maxDepthSpin_->setSuffix(" m"); + maxDepthSpin_->setValue(maxDepth); decimationSpin_ = new QSpinBox(this); decimationSpin_->setMinimum(1); decimationSpin_->setMaximum(16); @@ -107,6 +116,8 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i hlayout2->addWidget(maxCloudsSpin_); hlayout2->addWidget(voxelLabel); hlayout2->addWidget(voxelSpin_); + hlayout2->addWidget(maxDepthLabel); + hlayout2->addWidget(maxDepthSpin_); hlayout2->addWidget(decimationLabel); hlayout2->addWidget(decimationSpin_); hlayout2->addWidget(timeLabel_); @@ -170,25 +181,32 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap cloudView_->setBackgroundColor(Qt::black); } - timeLabel_->setText(QString("%1 s").arg(info.time)); + timeLabel_->setText(QString("%1 s").arg(info.timeEstimation)); if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f) { UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality); - if(data.image().cols % decimationSpin_->value() == 0 && - data.image().rows % decimationSpin_->value() == 0) + if(!data.depth().empty()) { - validDecimationValue_ = decimationSpin_->value(); + if(data.image().cols % decimationSpin_->value() == 0 && + data.image().rows % decimationSpin_->value() == 0) + { + validDecimationValue_ = decimationSpin_->value(); + } + else + { + UWARN("Decimation (%d) must be a denominator of the width and height of " + "the image (%d/%d). Using last valid decimation value (%d).", + decimationSpin_->value(), + data.image().cols, + data.image().rows, + validDecimationValue_); + } } else { - UWARN("Decimation (%d) must be a denominator of the width and height of " - "the image (%d/%d). Using last valid decimation value (%d).", - decimationSpin_->value(), - data.image().cols, - data.image().rows, - validDecimationValue_); + validDecimationValue_ = decimationSpin_->value(); } @@ -214,6 +232,11 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap validDecimationValue_); } + if(maxDepthSpin_->value() > 0.0f && cloud->size()) + { + cloud = util3d::passThrough(cloud, "z", 0, maxDepthSpin_->value()); + } + if(voxelSpin_->value() > 0.0f && cloud->size()) { cloud = util3d::voxelize(cloud, voxelSpin_->value()); diff --git a/guilib/src/PdfPlot.cpp b/guilib/src/PdfPlot.cpp index 59630be8..0b7b9eaa 100644 --- a/guilib/src/PdfPlot.cpp +++ b/guilib/src/PdfPlot.cpp @@ -159,10 +159,9 @@ void PdfPlotCurve::setData(const QMap & dataMap, const QMap::iterator iter = _items.begin(); - QMap::const_iterator j=weightsMap.begin(); - for(QMap::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i, ++j) + for(QMap::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i) { - ((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), j!=weightsMap.end()?j.value():-1); + ((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), weightsMap.value(i.key(), -1)); //2 times... ++iter; ++iter; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index d14b1f00..bc9d9650 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -311,6 +311,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_cameraStereoImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesStamps())); + connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_cameraStereoImages_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPath())); + connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); @@ -351,6 +355,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->general_spinBox_memoryThr->setObjectName(Parameters::kRtabmapMemoryThr().c_str()); _ui->general_doubleSpinBox_detectionRate->setObjectName(Parameters::kRtabmapDetectionRate().c_str()); _ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str()); + _ui->general_checkBox_createIntermediateNodes->setObjectName(Parameters::kRtabmapCreateIntermediateNodes().c_str()); _ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str()); _ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str()); _ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str()); @@ -483,6 +488,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str()); _ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str()); _ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); + _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str()); @@ -503,6 +509,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_bowForce2D->setObjectName(Parameters::kLccBowForce2D().c_str()); _ui->loopClosure_bowEpipolarGeometry->setObjectName(Parameters::kLccBowEpipolarGeometry().c_str()); _ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str()); + _ui->loopClosure_pnpEstimation->setObjectName(Parameters::kLccBowPnPEstimation().c_str()); + _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str()); + _ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str()); _ui->groupBox_reextract->setObjectName(Parameters::kLccReextractActivated().c_str()); _ui->reextract_nn->setObjectName(Parameters::kLccReextractNNType().c_str()); @@ -542,6 +551,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str()); _ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str()); _ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str()); + _ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str()); _ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str()); _ui->odom_pnpEstimation->setObjectName(Parameters::kOdomPnPEstimation().c_str()); _ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str()); @@ -567,6 +577,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_minTranslation->setObjectName(Parameters::kOdomMonoMinTranslation().c_str()); _ui->doubleSpinBox_maxVariance->setObjectName(Parameters::kOdomMonoMaxVariance().c_str()); + //Odometry particle filter + _ui->odom_particleFiltering->setObjectName(Parameters::kOdomParticleFiltering().c_str()); + _ui->spinBox_particleSize->setObjectName(Parameters::kOdomParticleSize().c_str()); + _ui->doubleSpinBox_particleNoiseT->setObjectName(Parameters::kOdomParticleNoiseT().c_str()); + _ui->doubleSpinBox_particleLambdaT->setObjectName(Parameters::kOdomParticleLambdaT().c_str()); + _ui->doubleSpinBox_particleNoiseR->setObjectName(Parameters::kOdomParticleNoiseR().c_str()); + _ui->doubleSpinBox_particleLambdaR->setObjectName(Parameters::kOdomParticleLambdaR().c_str()); + //Stereo _ui->stereo_flow_winSize->setObjectName(Parameters::kStereoWinSize().c_str()); _ui->stereo_flow_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str()); @@ -972,6 +990,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->openni2_gain->setValue(100); _ui->openni2_mirroring->setChecked(false); _ui->comboBox_freenect2Format->setCurrentIndex(0); + _ui->lineEdit_cameraStereoImages_timestamps->setText(""); + _ui->lineEdit_cameraStereoImages_path->setText(""); _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->lineEdit_openniDevice->setText(""); _ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2"); @@ -1237,6 +1257,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt()); _ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool()); _ui->comboBox_freenect2Format->setCurrentIndex(settings.value("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()).toInt()); + _ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString()); + _ui->lineEdit_cameraStereoImages_path->setText(settings.value("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text()).toString()); _ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool()); _ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString()); _ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString()); @@ -1507,6 +1529,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("openni2Gain", _ui->openni2_gain->value()); settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked()); settings.setValue("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()); + settings.setValue("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text()); + settings.setValue("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text()); settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()); settings.setValue("device", _ui->lineEdit_openniDevice->text()); settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text()); @@ -2111,6 +2135,12 @@ void PreferencesDialog::selectSourceRGBD(Src src) _ui->groupBox_sourceDatabase->setChecked(false); } + if(src == kSrcStereoImages) + { + _ui->lineEdit_cameraStereoImages_timestamps->setText(""); + _ui->lineEdit_cameraStereoImages_path->setText(""); + } + if(validateForm()) { // Even if there is no change, MainWindow should be notified @@ -2140,6 +2170,34 @@ void PreferencesDialog::openDatabaseViewer() } } +void PreferencesDialog::selectSourceStereoImagesStamps() +{ + QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)")); + if(path.size()) + { + _ui->lineEdit_cameraStereoImages_timestamps->setText(path); + } +} + +void PreferencesDialog::selectSourceStereoImagesPath() +{ + QString dir = _ui->lineEdit_cameraStereoImages_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getExistingDirectory(this, tr("Select stereo images directory"), dir); + if(path.size()) + { + _ui->lineEdit_cameraStereoImages_path->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -2837,6 +2895,7 @@ void PreferencesDialog::updateRGBDCameraGroupBoxVisibility() { _ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL); _ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL); + _ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcStereoImages-kSrcOpenNI_PCL); } /*** GETTERS ***/ @@ -3245,6 +3304,15 @@ CameraRGBD * PreferencesDialog::createCameraRGBD(bool forCalibration) this->getGeneralInputRate(), this->getSourceOpenniLocalTransform()); } + else if(this->getSourceRGBD() == kSrcStereoImages) + { + return new CameraStereoImages( + _ui->lineEdit_cameraStereoImages_path->text().toStdString(), + this->getSourceOpenniDevice().toStdString(), + _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(), + this->getGeneralInputRate(), + this->getSourceOpenniLocalTransform()); + } else { UFATAL("RGBD Source type undefined!"); @@ -3268,6 +3336,10 @@ int PreferencesDialog::getOdomStrategy() const { return _ui->odom_strategy->currentIndex(); } +int PreferencesDialog::getOdomBufferSize() const +{ + return _ui->odom_dataBufferSize->value(); +} QString PreferencesDialog::getCameraInfoDir() const { @@ -3409,12 +3481,15 @@ void PreferencesDialog::testOdometry(int type) odometry = new OdometryBOW(parameters); } - OdometryThread odomThread(odometry); // take ownership of odometry + OdometryThread odomThread( + odometry, // take ownership of odometry + _ui->odom_dataBufferSize->value()); odomThread.registerToEventsManager(); OdometryViewer * odomViewer = new OdometryViewer(10, _ui->spinBox_decimation_odom->value(), _ui->doubleSpinBox_voxelSize_odom->value(), + _ui->doubleSpinBox_maxDepth_odom->value(), this->getOdomQualityWarnThr(), this); odomViewer->setWindowTitle(tr("Odometry viewer")); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index ba510639..4d331934 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -783,14 +783,14 @@ - 2 + 1 0 0 - 312 + 315 314 @@ -1009,9 +1009,9 @@ 0 - 0 + -12 351 - 331 + 355 @@ -1020,7 +1020,7 @@ - + 1 @@ -1040,7 +1040,7 @@ - + m @@ -1056,47 +1056,28 @@ - - + + - NNDR + Max feature depth - - + + - Min correspondences + Max correspondence distance - - + + - 2D transform (x,y,yaw) + Iteration - - - - m - - - 3 - - - 0.001000000000000 - - - 1.000000000000000 - - - 0.020000000000000 - - - - + m @@ -1118,14 +1099,21 @@ - - + + - Max feature depth + NNDR - + + + + Min correspondences + + + + 3 @@ -1138,17 +1126,43 @@ - - + + - Max correspondence distance + 2D transform (x,y,yaw) - - + + + + m + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.020000000000000 + + + + + - Iteration + PnP (2D->3D estimation) + + + + + + + @@ -1278,7 +1292,7 @@ 0 - -118 + 0 330 304 @@ -1496,7 +1510,7 @@ 0 0 - 248 + 315 319 @@ -1690,8 +1704,8 @@ 0 0 - 201 - 126 + 330 + 186 diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 13c6d8af..91aa6443 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 25 + 22 @@ -49,7 +49,7 @@ - Edit + Edit @@ -60,6 +60,7 @@ + @@ -1203,6 +1204,11 @@ Send a goal... + + + Export poses (*.txt)... + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 3d457a19..2ac9eb01 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -7,7 +7,7 @@ 0 0 1058 - 725 + 858 @@ -63,9 +63,9 @@ 0 - 0 + -613 755 - 1557 + 1591 @@ -86,7 +86,7 @@ QFrame::Raised - 19 + 3 @@ -1502,18 +1502,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Images dataset - - - + + + - ... - - - - - - - false + @@ -1527,17 +1520,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Start position (default 1, 0=start from the last). + + + + false - - + + - + ... @@ -1548,6 +1541,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Start position (default 1, 0=start from the last). + + + @@ -1787,6 +1787,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki StereoFlyCapture2 + + + StereoImages + + @@ -1984,6 +1989,63 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + CameraStereoImages + + + + + + + + + + + + + ... + + + + + + + Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. + + + true + + + + + + + + + + + + + + Path to directory containing stereo images. The images order should be left/right/left/right... and so on. You can also set two directories (separated by ';'), one for left images and one for right images. + + + true + + + + + + + ... + + + + + + @@ -1999,7 +2061,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. In case of OpenNI and OpenNI2 drivers, this can be a path to an ONI file. For StereoImages, this is the name used when looking for calibration files. true @@ -2323,16 +2385,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Hz + + + + - - 1 - - - 1.000000000000000 + + true @@ -2356,6 +2415,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created. + + + true + + + @@ -2366,26 +2435,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + Hz - - true + + 1 + + + 1.000000000000000 - + - Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created. + Create intermediate nodes if odometry is faster than the detection rate. true + + + + + + + false + + + @@ -4600,6 +4682,9 @@ When set to false, no new words are added to dictionary, so no more updates are + + 3 + 1.000000000000000 @@ -4657,7 +4742,7 @@ When set to false, no new words are added to dictionary, so no more updates are - K. + K. Harris detector free parameter. true @@ -4674,7 +4759,7 @@ When set to false, no new words are added to dictionary, so no more updates are - Quality level. + Quality level. Parameter characterizing the minimal accepted quality of image corners. The parameter value is multiplied by the best corner quality measure, which is the minimal eigenvalue (see cornerMinEigenVal() ) or the Harris function response. The corners with the quality measure less than the product are rejected. For example, if the best corner has the quality measure = 1500, and the qualityLevel=0.01 , then all the corners with the quality measure less than 15 are rejected. true @@ -4684,7 +4769,7 @@ When set to false, no new words are added to dictionary, so no more updates are - Mininum distance. + Minimum possible Euclidean distance between the returned corners. true @@ -4694,7 +4779,7 @@ When set to false, no new words are added to dictionary, so no more updates are - Block size. + Block size. Size of an average block for computing a derivative covariation matrix over each pixel neighborhood. true @@ -5181,13 +5266,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Iterations. - - - @@ -5201,31 +5279,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - - - - Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances). - - - true - - - - + - + Optimize graph from the newest node. @@ -5235,21 +5303,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Qt::Horizontal - + - + 2d SLAM: use fast 3DoF (x, y, theta) optimization instead of 6DoF (x, y, z, roll, pitch, yaw) optimization. @@ -5266,7 +5334,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: 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). @@ -5276,7 +5344,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. @@ -5286,6 +5354,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Iterations. + + + + + + + Stop optimizing when the error improvement is less than this value. + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances). + + + true + + + @@ -5552,29 +5663,111 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + m - - 3 - - 0.001000000000000 - - - 0.010000000000000 + 0.000000000000000 - 0.020000000000000 + 5.000000000000000 - - + + - Maximum distance for visual word correspondences. + Max feature depth. Note that parameter "Visual Word"->"Max words depth" is applied before this. + + + true + + + + + + + Use epipolar geometry to compute the loop closure transform. "Maximum distance for visual word correspondences" is not used in this mode. + + + true + + + + + + + PnP: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses "Minimum visual word correspondences" and "Maximum iterations" above. + + + true + + + + + + + + + + + + + + PnP reprojection error. + + + true + + + + + + + PnP flags. + + + true + + + + + + + + Iterative + + + + + EPNP + + + + + P3P + + + + + + + + pix + + + 1 + + + 0.100000000000000 + + + 1.000000000000000 + + + 8.000000000000000 @@ -5601,47 +5794,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 0.000000000000000 - - - 5.000000000000000 - - - - - - - Max feature depth. Note that parameter "Visual Word"->"Max words depth" is applied before this. - - - true - - - - - - - - - - - - - - Force 2D transform (3DoF: x,y and yaw). - - - true - - - - + QComboBox::AdjustToContents @@ -5663,34 +5816,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - When enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See "ICP" panel for parameters. - - - true - - - - - - - Use epipolar geometry to compute the loop closure transform. "Maximum distance for visual word correspondences" is not used in this mode. - - - true - - - - + - + Epipolar geometry maximum variance to accept the loop closure. @@ -5700,7 +5833,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -5719,6 +5852,59 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Maximum distance for visual word correspondences. + + + + + + + + + + + + + + Force 2D transform (3DoF: x,y and yaw). + + + true + + + + + + + m + + + 3 + + + 0.001000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + When enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See "ICP" panel for parameters. + + + true + + + @@ -6576,16 +6762,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - 3-Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. - - - true - - - @@ -6625,7 +6801,108 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + + + Feature detector. In BOW/Mono modes, the related descriptor is also used. In Optical flow mode, only the keypoint detector is used. + + + true + + + + + + + Odometry strategy: + + + false + + + + + + + + + + + + + + Force 2D transform (3DoF: x,y and yaw). + + + true + + + + + + + Fill info with data (inliers/outliers features to be shown in Odometry view). + + + true + + + + + + + Test selected odometry + + + + + + + 2-Optical flow estimate the location of 2D features from last frame to new frame, then computes RANSAC transformation with corresponding 3D features. + + + true + + + + + + + 1-BOW matches features extracted from both frames using nearest neighbor with descriptors, then computes RANSAC transformation estimation with corresponding 3D features. + + + true + + + + + + + Data buffer size (0 means inf). + + + true + + + + + + + 999999 + + + + + + + 3-Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. + + + true + + + + @@ -6679,80 +6956,23 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - + + - Feature detector. In BOW/Mono modes, the related descriptor is also used. In Optical flow mode, only the keypoint detector is used. + Particle filtering to smooth the odometry trajectory. See "Particle Filter" panel for the related parameters. true - - - - Odometry strategy: - - - false - - - - - + + - - - - Force 2D transform (3DoF: x,y and yaw). - - - true - - - - - - - Fill info with data (inliers/outliers features to be shown in Odometry view). - - - true - - - - - - - Test selected odometry - - - - - - - 2-Optical flow estimate the location of 2D features from last frame to new frame, then computes RANSAC transformation with corresponding 3D features. - - - true - - - - - - - 1-BOW matches features extracted from both frames using nearest neighbor with descriptors, then computes RANSAC transformation estimation with corresponding 3D features. - - - true - - - @@ -6806,7 +7026,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - RANSAC: Maximum distance for 3D feature correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost). + Maximum distance for 3D feature correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost). true @@ -6832,7 +7052,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - RANSAC: Maximum iterations to compute the transform from 3D features. + Maximum iterations to compute the transform from 3D features. true @@ -6904,7 +7124,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - PnP RANSAC: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses "Minimum feature correspondences" and "Maximum iterations" above. + PnP: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses "Minimum feature correspondences" and "Maximum iterations" above. true @@ -7572,6 +7792,197 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + + + Particle Filter + + + + + + Parameters for the particle filter when used to smooth the odometry trajectory. + + + true + + + + + + + + + + + + 1 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 1.000000000000000 + + + 15.000000000000000 + + + + + + + rad + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + m + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + Noise of translation components (x,y,z). + + + true + + + + + + + Noise of rotation components (roll, pitch, yaw). + + + true + + + + + + + Lambda of translation components (x,y,z). + + + true + + + + + + + Lambda of rotation components (roll, pitch, yaw). + + + true + + + + + + + + + + 1 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 1.000000000000000 + + + 15.000000000000000 + + + + + + + Particle size. + + + true + + + + + + + 1 + + + 10000 + + + 400 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 1324 + + + + + + diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 04efc6f7..98b11f33 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -378,6 +378,7 @@ void UPlotCurve::_addValue(UPlotItem * data) { float x = data->data().x(); float y = data->data().y(); + if(_minMax.size() != 4) { _minMax = QVector(4); @@ -428,6 +429,16 @@ void UPlotCurve::addValue(UPlotItem * data) void UPlotCurve::addValue(float x, float y) { + if(_items.size() && + dynamic_cast(_items.back()) && + x < ((UPlotItem*)_items.back())->data().x()) + { + UWARN("New value (%f) added to curve \"%s\" is smaller " + "than the last added (%f). Clearing the curve.", + x, this->name().toStdString().c_str(), _items.back()->pos().x()); + this->clear(); + } + float width = 2; // TODO warn : hard coded value! this->addValue(new UPlotItem(x,y,width)); } diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 7c20e38f..80914435 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -159,7 +159,8 @@ int main(int argc, char * argv[]) } cv::Mat rgb, depth; float fx, fy, cx, cy; - camera->takeImage(rgb, depth, fx, fy, cx, cy); + double stamp = 0.0; + camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp); if(rgb.cols != depth.cols || rgb.rows != depth.rows) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", @@ -228,7 +229,7 @@ int main(int argc, char * argv[]) rgb = cv::Mat(); depth = cv::Mat(); - camera->takeImage(rgb, depth, fx, fy, cx, cy); + camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp); } cv::destroyWindow("Video"); cv::destroyWindow("Depth"); From dfbf6e721e1e50044cd3f7ada72b77b9a82f04f1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 15 Jun 2015 14:44:44 -0400 Subject: [PATCH 06/45] Refactored OdometryOpticalFlow (added optical flow guess using previous odometry transform, merged stereo/depth stuff) --- corelib/include/rtabmap/core/Odometry.h | 8 +- corelib/include/rtabmap/core/OdometryInfo.h | 2 + .../include/rtabmap/core/util3d_features.h | 17 +- corelib/src/Odometry.cpp | 48 +- corelib/src/OdometryOpticalFlow.cpp | 807 +++++------------- corelib/src/ParticleFilter.h | 8 +- corelib/src/Rtabmap.cpp | 4 +- corelib/src/util2d.cpp | 2 +- corelib/src/util3d_features.cpp | 47 +- guilib/include/rtabmap/gui/DatabaseViewer.h | 1 + guilib/src/DatabaseViewer.cpp | 122 ++- guilib/src/MainWindow.cpp | 35 +- guilib/src/ui/DatabaseViewer.ui | 346 +++++++- guilib/src/ui/preferencesDialog.ui | 9 +- 14 files changed, 785 insertions(+), 671 deletions(-) diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index da84db11..dedb85bf 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -63,6 +63,7 @@ public: bool isPnPEstimationUsed() const {return _pnpEstimation;} double getPnPReprojError() const {return _pnpReprojError;} int getPnPFlags() const {return _pnpFlags;} + const Transform & previousTransform() const {return previousTransform_;} private: virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0; @@ -89,6 +90,8 @@ private: Transform _pose; int _resetCurrentCount; double previousStamp_; + Transform previousTransform_; + float distanceTravelled_; std::vector filters_; @@ -133,9 +136,7 @@ public: private: 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_; @@ -156,7 +157,6 @@ private: Feature2D * feature2D_; cv::Mat refFrame_; - cv::Mat refRightFrame_; std::vector refCorners_; pcl::PointCloud::Ptr refCorners3D_; }; diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index 2409c8c6..67d36676 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -43,6 +43,7 @@ public: timeEstimation(-1), stamp(0), interval(0), + distanceTravelled(0), type(-1) {} bool lost; @@ -57,6 +58,7 @@ public: double interval; Transform transform; Transform transformFiltered; + float distanceTravelled; int type; // 0=BOW, 1=Optical Flow, 2=ICP diff --git a/corelib/include/rtabmap/core/util3d_features.h b/corelib/include/rtabmap/core/util3d_features.h index 6670f321..1970d2d0 100644 --- a/corelib/include/rtabmap/core/util3d_features.h +++ b/corelib/include/rtabmap/core/util3d_features.h @@ -73,7 +73,22 @@ pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( int flowWinSize = 9, int flowMaxLevel = 4, int flowIterations = 20, - double flowEps = 0.02); + double flowEps = 0.02, + double maxCorrespondencesSlope = 0.0); +pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( + const std::vector & leftCorners, + const cv::Mat & leftImage, + const cv::Mat & rightImage, + float fx, + float baseline, + float cx, + float cy, + const Transform & transform = Transform::getIdentity(), + int flowWinSize = 9, + int flowMaxLevel = 4, + int flowIterations = 20, + double flowEps = 0.02, + double maxCorrespondencesSlope = 0.0); std::multimap RTABMAP_EXP generateWords3DMono( const std::multimap & kpts, diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index f249b946..ce5f5c08 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" +#include "rtabmap/utilite/UConversion.h" #include "ParticleFilter.h" namespace rtabmap { @@ -53,7 +54,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _pnpReprojError(Parameters::defaultOdomPnPReprojError()), _pnpFlags(Parameters::defaultOdomPnPFlags()), _resetCurrentCount(0), - previousStamp_(0) + previousStamp_(0), + previousTransform_(Transform::getIdentity()), + distanceTravelled_(0) { Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers); @@ -106,8 +109,10 @@ Odometry::~Odometry() void Odometry::reset(const Transform & initialPose) { + previousTransform_.setIdentity(); _resetCurrentCount = 0; previousStamp_ = 0; + distanceTravelled_ = 0; if(_force2D || filters_.size()) { float x,y,z, roll,pitch,yaw; @@ -178,6 +183,8 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) info->interval = data.stamp() - previousStamp_; info->transform = t; } + + previousTransform_.setIdentity(); previousStamp_ = data.stamp(); if(!t.isNull()) @@ -192,15 +199,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) if(filters_.size()) { UASSERT(filters_.size()==6); - x = filters_[0]->filter(x); - y = filters_[1]->filter(y); - yaw = filters_[5]->filter(yaw); - - if(!_force2D) + if(_pose.isIdentity()) { - z = filters_[2]->filter(z); - roll = filters_[3]->filter(roll); - pitch = filters_[4]->filter(pitch); + filters_[0]->init(x); + filters_[1]->init(y); + filters_[2]->init(z); + filters_[3]->init(roll); + filters_[4]->init(pitch); + filters_[5]->init(yaw); + } + else + { + x = filters_[0]->filter(x); + y = filters_[1]->filter(y); + yaw = filters_[5]->filter(yaw); + + if(!_force2D) + { + z = filters_[2]->filter(z); + roll = filters_[3]->filter(roll); + pitch = filters_[4]->filter(pitch); + } } if(info) @@ -208,6 +227,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) info->timeParticleFiltering = time.ticks(); } } + UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && + uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw), + uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s", + x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str()); t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw); if(info) @@ -216,6 +239,13 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) } } + previousTransform_ = t; + if(info) + { + distanceTravelled_ += t.getNorm(); + info->distanceTravelled = distanceTravelled_; + } + return _pose *= t; // updated } else if(_resetCurrentCount > 0) diff --git a/corelib/src/OdometryOpticalFlow.cpp b/corelib/src/OdometryOpticalFlow.cpp index 36ea6d0c..cf4a3f1c 100644 --- a/corelib/src/OdometryOpticalFlow.cpp +++ b/corelib/src/OdometryOpticalFlow.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_registration.h" +#include "rtabmap/core/util3d_features.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UConversion.h" @@ -120,29 +121,6 @@ void OdometryOpticalFlow::reset(const Transform & initialPose) Transform OdometryOpticalFlow::computeTransform( const SensorData & data, OdometryInfo * info) -{ - UDEBUG(""); - - if(info) - { - info->type = 1; - } - - if(!data.rightImage().empty()) - { - //stereo - return computeTransformStereo(data, info); - } - else - { - //rgbd - return computeTransformRGBD(data, info); - } -} - -Transform OdometryOpticalFlow::computeTransformStereo( - const SensorData & data, - OdometryInfo * info) { UTimer timer; Transform output; @@ -151,6 +129,11 @@ Transform OdometryOpticalFlow::computeTransformStereo( int inliers = 0; int correspondences = 0; + if(info) + { + info->type = 1; + } + cv::Mat newLeftFrame; // convert to grayscale if(data.image().channels() > 1) @@ -161,17 +144,50 @@ Transform OdometryOpticalFlow::computeTransformStereo( { newLeftFrame = data.image().clone(); } - cv::Mat newRightFrame = data.rightImage().clone(); std::vector newCorners; - UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1); - if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size()) + UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d", + (int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightImage().empty()?0:1); + if(!refFrame_.empty() && + !data.depthOrRightImage().empty() && + refCorners_.size() && + refCorners3D_->size()) { - UDEBUG(""); + UASSERT_MSG(refCorners_.size() == refCorners3D_->size(), + uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str()); + + // make guess + bool flowGuessByMotion = true; + cv::Mat K = (cv::Mat_(3,3) << + data.fx(), 0, data.cx(), + 0, data.fx(), data.cy(), + 0, 0, 1); + Transform guess = (this->previousTransform() * data.localTransform()).inverse(); + cv::Mat R = (cv::Mat_(3,3) << + (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), + (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), + (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); + cv::Mat rvec(1,3, CV_64FC1); + cv::Rodrigues(R, rvec); + cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); + std::vector objectPoints(refCorners3D_->size()); + for(unsigned int i=0; iat(i).x; + objectPoints[i].y = refCorners3D_->at(i).y; + objectPoints[i].z = refCorners3D_->at(i).z; + } + if(flowGuessByMotion && !this->previousTransform().isIdentity()) + { + UDEBUG("project points to new image"); + cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners); + } + // Find features in the new left image std::vector status; std::vector err; UDEBUG("cv::calcOpticalFlowPyrLK() begin"); + int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2); cv::calcOpticalFlowPyrLK( refFrame_, newLeftFrame, @@ -179,155 +195,54 @@ Transform OdometryOpticalFlow::computeTransformStereo( newCorners, status, err, - cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_, + cv::Size(winSize, winSize), + (newCorners.size()||!flowGuessByMotion)?flowMaxLevel_:flowMaxLevel_*2, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4); + cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4); UDEBUG("cv::calcOpticalFlowPyrLK() end"); - std::vector lastCornersKept(status.size()); + pcl::PointCloud::Ptr refCorners3DKept(new pcl::PointCloud); + refCorners3DKept->resize(status.size()); + std::vector objectPointsKept(status.size()); + std::vector refCornersKept(status.size()); std::vector newCornersKept(status.size()); int ki = 0; for(unsigned int i=0; iat(ki) = refCorners3D_->at(i); + objectPointsKept[ki] = objectPoints[i]; + refCornersKept[ki] = refCorners_[i]; newCornersKept[ki] = newCorners[i]; ++ki; } } - lastCornersKept.resize(ki); + refCorners3DKept->resize(ki); + objectPointsKept.resize(ki); + refCornersKept.resize(ki); newCornersKept.resize(ki); if(ki && ki >= this->getMinInliers()) { - std::vector statusLast; - std::vector errLast; - std::vector lastCornersKeptRight; - UDEBUG("previous stereo disparity"); - cv::calcOpticalFlowPyrLK( - refFrame_, - refRightFrame_, - lastCornersKept, - lastCornersKeptRight, - statusLast, - errLast, - cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_, - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4); - - UDEBUG("new stereo disparity"); - std::vector statusNew; - std::vector errNew; - std::vector newCornersKeptRight; - cv::calcOpticalFlowPyrLK( - newLeftFrame, - newRightFrame, - newCornersKept, - newCornersKeptRight, - statusNew, - errNew, - cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_, - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4); - if(this->isPnPEstimationUsed()) { // find correspondences if(this->isInfoDataFilled() && info) { - info->refCorners.resize(statusLast.size()); - info->newCorners.resize(statusLast.size()); + info->refCorners = refCornersKept; + info->newCorners = newCornersKept; } - int flowInliers = 0; - std::vector objectPoints(statusLast.size()); - std::vector imagePoints(statusLast.size()); - std::vector image3DPoints(statusLast.size()); - int oi=0; - float bad_point = std::numeric_limits::quiet_NaN (); - for(unsigned int i=0; i 0.0f && lastSlope < stereoMaxSlope_) - { - pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D( - lastCornersKept[i], - lastDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); - - if(pcl::isFinite(lastPt3D) && - (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth()))) - { - //Add 3D correspondences! - lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); - objectPoints[oi].x = lastPt3D.x; - objectPoints[oi].y = lastPt3D.y; - objectPoints[oi].z = lastPt3D.z; - imagePoints[oi] = newCornersKept.at(i); - - // new 3D points, used to compute variance - image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point); - if(newDisparity > 0.0f && newSlope < stereoMaxSlope_) - { - pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D( - newCornersKept[i], - newDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); - if(pcl::isFinite(newPt3D) && - (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))) - { - image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform()); - } - } - - if(this->isInfoDataFilled() && info) - { - info->refCorners[oi] = lastCornersKept[i]; - info->newCorners[oi] = newCornersKept[i]; - } - ++oi; - } - } - ++flowInliers; - } - } - objectPoints.resize(oi); - imagePoints.resize(oi); - image3DPoints.resize(oi); - UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi); - - if(this->isInfoDataFilled() && info) - { - info->refCorners.resize(oi); - info->newCorners.resize(oi); - } - - correspondences = oi; + correspondences = refCornersKept.size(); if(correspondences >= this->getMinInliers()) { //PnPRansac - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fx(), data.cy(), - 0, 0, 1); - Transform guess = (data.localTransform()).inverse(); - cv::Mat R = (cv::Mat_(3,3) << - (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), - (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), - (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); - cv::Mat rvec(1,3, CV_64FC1); - cv::Rodrigues(R, rvec); - cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); std::vector inliersV; - cv::solvePnPRansac(objectPoints, - imagePoints, + cv::solvePnPRansac( + objectPointsKept, + newCornersKept, K, cv::Mat(), rvec, @@ -339,39 +254,17 @@ Transform OdometryOpticalFlow::computeTransformStereo( inliersV, this->getPnPFlags()); + cv::Rodrigues(rvec, R); + Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), + R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), + R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); + inliers = (int)inliersV.size(); if((int)inliersV.size() >= this->getMinInliers()) { - cv::Rodrigues(rvec, R); - Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), - R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), - R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - // make it incremental output = (data.localTransform() * pnp).inverse(); - - UDEBUG("Odom transform = %s", output.prettyPrint().c_str()); - - // compute variance (like in PCL computeVariance() method of sac_model.h) - std::vector errorSqrdDists(inliersV.size()); - int ii=0; - for(unsigned int i=0; i> 1]; - variance = 2.1981 * median_error_sqr; - } + variance = 1; // FIXME, is there a way to compute a variance from the PNP approach? } else { @@ -390,57 +283,79 @@ Transform OdometryOpticalFlow::computeTransformStereo( } else { - UDEBUG("Getting correspondences begin"); // Get 3D correspondences - pcl::PointCloud::Ptr correspondencesLast(new pcl::PointCloud); + pcl::PointCloud::Ptr correspondencesRef(new pcl::PointCloud); pcl::PointCloud::Ptr correspondencesNew(new pcl::PointCloud); - correspondencesLast->resize(statusLast.size()); - correspondencesNew->resize(statusLast.size()); - int oi = 0; + correspondencesRef->resize(newCornersKept.size()); + correspondencesNew->resize(newCornersKept.size()); if(this->isInfoDataFilled() && info) { - info->refCorners.resize(statusLast.size()); - info->newCorners.resize(statusLast.size()); + info->refCorners.resize(newCornersKept.size()); + info->newCorners.resize(newCornersKept.size()); } - for(unsigned int i=0; i 0.0f && newDisparity > 0.0f && - lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_) - { - pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D( - lastCornersKept[i], - lastDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); - pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D( - newCornersKept[i], - newDisparity, - data.cx(), data.cy(), data.fx(), data.baseline()); + // stereo + pcl::PointCloud::Ptr newCorners3D = util3d::generateKeypoints3DStereo( + newCornersKept, + newLeftFrame, + data.rightImage(), + data.fx(), + data.baseline(), + data.cx(), + data.cy(), + Transform::getIdentity(), + stereoWinSize_, + stereoMaxLevel_, + stereoIterations_, + stereoEps_, + stereoMaxSlope_); - if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) && - pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))) + UASSERT(newCorners3D->size() == refCorners3DKept->size()); + for(unsigned int i=0; isize(); ++i) + { + if(pcl::isFinite(newCorners3D->at(i)) && (this->getMaxDepth() <= 0.0f || newCorners3D->at(i).z < this->getMaxDepth())) + { + //Add 3D correspondences! + correspondencesRef->at(oi) = refCorners3DKept->at(i); + correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), data.localTransform()); + if(this->isInfoDataFilled() && info) + { + info->refCorners[oi] = refCornersKept[i]; + info->newCorners[oi] = newCornersKept[i]; + } + ++oi; + } + }// end loop + } + else + { + //depth + for(unsigned int i=0; igetMaxDepth() == 0.0f || pt.z < this->getMaxDepth())) { //Add 3D correspondences! - lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); - newPt3D = util3d::transformPoint(newPt3D, data.localTransform()); - correspondencesLast->at(oi) = lastPt3D; - correspondencesNew->at(oi) = newPt3D; + correspondencesRef->at(oi) = refCorners3DKept->at(i); + correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform()); if(this->isInfoDataFilled() && info) { - info->refCorners[oi] = lastCornersKept[i]; + info->refCorners[oi] = refCornersKept[i]; info->newCorners[oi] = newCornersKept[i]; } ++oi; } } } - }// end loop - correspondencesLast->resize(oi); + } + correspondencesRef->resize(oi); correspondencesNew->resize(oi); if(this->isInfoDataFilled() && info) { @@ -448,8 +363,7 @@ Transform OdometryOpticalFlow::computeTransformStereo( info->newCorners.resize(oi); } correspondences = oi; - refCorners3D_ = correspondencesNew; - UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size()); + UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size()); if(correspondences >= this->getMinInliers()) { @@ -457,7 +371,7 @@ Transform OdometryOpticalFlow::computeTransformStereo( UTimer timerRANSAC; Transform t = util3d::transformFromXYZCorrespondences( correspondencesNew, - correspondencesLast, + correspondencesRef, this->getInlierDistance(), this->getIterations(), this->getRefineIterations()>0, 3.0, this->getRefineIterations(), @@ -499,11 +413,7 @@ Transform OdometryOpticalFlow::computeTransformStereo( // Copy or generate new keypoints if(data.keypoints().size()) { - newCorners.resize(data.keypoints().size()); - for(unsigned int i=0; i this->getMinInliers()) + if((int)newCorners.size() >= this->getMinInliers()) { - refFrame_ = newLeftFrame; - refRightFrame_ = newRightFrame; - refCorners_ = newCorners; + pcl::PointCloud::Ptr newCorners3D(new pcl::PointCloud); + newCorners3D->resize(newCorners.size()); + std::vector newCornersFiltered(newCorners.size()); + int oi=0; + if(!data.rightImage().empty()) + { + /// stereo + pcl::PointCloud::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo( + newCorners, + newLeftFrame, + data.rightImage(), + data.fx(), + data.baseline(), + data.cx(), + data.cy(), + Transform::getIdentity(), + stereoWinSize_, + stereoMaxLevel_, + stereoIterations_, + stereoEps_, + stereoMaxSlope_); + UASSERT(refCorners3DTmp->size() == newCorners.size()); + for(unsigned int i=0; iat(i)) && + (this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth())) + { + newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.localTransform()); + newCornersFiltered[oi] = newCorners[i]; + ++oi; + } + } + } + else + { + // depth + for(unsigned int i=0; igetMaxDepth() == 0.0f || pt.z < this->getMaxDepth())) + { + newCorners3D->at(oi) = util3d::transformPoint(pt, data.localTransform()); + newCornersFiltered[oi] = newCorners[i]; + ++oi; + } + } + } + } + newCornersFiltered.resize(oi); + newCorners3D->resize(oi); + + if((int)newCornersFiltered.size() >= this->getMinInliers()) + { + refFrame_ = newLeftFrame; + refCorners_ = newCornersFiltered; + refCorners3D_ = newCorners3D; + } + else + { + UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...", + (int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers()); + output.setNull(); + } } else { @@ -562,394 +537,4 @@ Transform OdometryOpticalFlow::computeTransformStereo( return output; } -Transform OdometryOpticalFlow::computeTransformRGBD( - const SensorData & data, - OdometryInfo * info) -{ - UTimer timer; - Transform output; - - double variance = 0; - int inliers = 0; - int correspondences = 0; - - cv::Mat newFrame; - // convert to grayscale - if(data.image().channels() > 1) - { - cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY); - } - else - { - newFrame = data.image().clone(); - } - - std::vector newCorners; - if(!refFrame_.empty() && - (int)refCorners_.size() >= this->getMinInliers() && - (int)refCorners3D_->size() >= this->getMinInliers()) - { - std::vector status; - std::vector err; - UDEBUG("cv::calcOpticalFlowPyrLK() begin"); - cv::calcOpticalFlowPyrLK( - refFrame_, - newFrame, - refCorners_, - newCorners, - status, - err, - cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_, - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_), - cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4); - UDEBUG("cv::calcOpticalFlowPyrLK() end"); - - if(this->isPnPEstimationUsed()) - { - // find correspondences - if(this->isInfoDataFilled() && info) - { - info->refCorners.resize(refCorners_.size()); - info->newCorners.resize(refCorners_.size()); - } - - UASSERT(refCorners_.size() == refCorners3D_->size()); - UDEBUG("lastCorners3D_ = %d", refCorners3D_->size()); - int flowInliers = 0; - std::vector objectPoints(refCorners_.size()); - std::vector imagePoints(refCorners_.size()); - std::vector image3DPoints(refCorners_.size()); - int oi=0; - float bad_point = std::numeric_limits::quiet_NaN (); - for(unsigned int i=0; iat(i))) - { - objectPoints[oi].x = refCorners3D_->at(i).x; - objectPoints[oi].y = refCorners3D_->at(i).y; - objectPoints[oi].z = refCorners3D_->at(i).z; - imagePoints[oi] = newCorners.at(i); - - // new 3D points, used to compute variance - image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point); - if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) && - uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows))) - { - pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, - data.cx(), data.cy(), data.fx(), data.fy(), true); - if(pcl::isFinite(pt) && - (this->getMaxDepth() == 0.0f || ( - uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) - { - image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform()); - } - } - - if(this->isInfoDataFilled() && info) - { - info->refCorners[oi] = refCorners_[i]; - info->newCorners[oi] = newCorners[i]; - } - - ++oi; - } - ++flowInliers; - } - } - objectPoints.resize(oi); - imagePoints.resize(oi); - image3DPoints.resize(oi); - UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi); - - if(this->isInfoDataFilled() && info) - { - info->refCorners.resize(oi); - info->newCorners.resize(oi); - } - - correspondences = oi; - - if(correspondences >= this->getMinInliers()) - { - //PnPRansac - cv::Mat K = (cv::Mat_(3,3) << - data.fx(), 0, data.cx(), - 0, data.fy(), data.cy(), - 0, 0, 1); - Transform guess = (data.localTransform()).inverse(); - cv::Mat R = (cv::Mat_(3,3) << - (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), - (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), - (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); - cv::Mat rvec(1,3, CV_64FC1); - cv::Rodrigues(R, rvec); - cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); - std::vector inliersV; - cv::solvePnPRansac(objectPoints, - imagePoints, - K, - cv::Mat(), - rvec, - tvec, - true, - this->getIterations(), - this->getPnPReprojError(), - 0, - inliersV, - this->getPnPFlags()); - - inliers = (int)inliersV.size(); - if((int)inliersV.size() >= this->getMinInliers()) - { - cv::Rodrigues(rvec, R); - Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), - R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), - R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - - // make it incremental - output = (data.localTransform() * pnp).inverse(); - - UDEBUG("Odom transform = %s", output.prettyPrint().c_str()); - - // compute variance (like in PCL computeVariance() method of sac_model.h) - std::vector errorSqrdDists(inliersV.size()); - int ii=0; - for(unsigned int i=0; i> 1]; - variance = 2.1981 * median_error_sqr; - } - } - else - { - UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers()); - } - - if(this->isInfoDataFilled() && info) - { - info->cornerInliers = inliersV; - } - } - else - { - UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers()); - } - } - else - { - pcl::PointCloud::Ptr correspondencesLast(new pcl::PointCloud); - pcl::PointCloud::Ptr correspondencesNew(new pcl::PointCloud); - correspondencesLast->resize(refCorners_.size()); - correspondencesNew->resize(refCorners_.size()); - int oi=0; - - if(this->isInfoDataFilled() && info) - { - info->refCorners.resize(refCorners_.size()); - info->newCorners.resize(refCorners_.size()); - } - - UASSERT(refCorners_.size() == refCorners3D_->size()); - UDEBUG("lastCorners3D_ = %d", refCorners3D_->size()); - int flowInliers = 0; - for(unsigned int i=0; iat(i)) && - uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) && - uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows))) - { - pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y, - data.cx(), data.cy(), data.fx(), data.fy(), true); - if(pcl::isFinite(pt) && - (this->getMaxDepth() == 0.0f || ( - uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) - { - pt = util3d::transformPoint(pt, data.localTransform()); - correspondencesLast->at(oi) = refCorners3D_->at(i); - correspondencesNew->at(oi) = pt; - - if(this->isInfoDataFilled() && info) - { - info->refCorners[oi] = refCorners_[i]; - info->newCorners[oi] = newCorners[i]; - } - - ++oi; - } - ++flowInliers; - } - else if(status[i]) - { - ++flowInliers; - } - } - UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi); - - if(this->isInfoDataFilled() && info) - { - info->refCorners.resize(oi); - info->newCorners.resize(oi); - } - correspondencesLast->resize(oi); - correspondencesNew->resize(oi); - correspondences = oi; - if(correspondences >= this->getMinInliers()) - { - std::vector inliersV; - UTimer timerRANSAC; - output = util3d::transformFromXYZCorrespondences( - correspondencesNew, - correspondencesLast, - this->getInlierDistance(), - this->getIterations(), - this->getRefineIterations()>0, 3.0, this->getRefineIterations(), - &inliersV, - &variance); - UDEBUG("time RANSAC = %fs", timerRANSAC.ticks()); - - inliers = (int)inliersV.size(); - if(inliers < this->getMinInliers()) - { - output.setNull(); - UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences); - } - - if(this->isInfoDataFilled() && info) - { - info->cornerInliers = inliersV; - } - } - else - { - UWARN("Not enough correspondences (%d)", correspondences); - } - } - } - else - { - //return Identity - output = Transform::getIdentity(); - } - - newCorners.clear(); - if(!output.isNull()) - { - // Copy or generate new keypoints - if(data.keypoints().size()) - { - newCorners.resize(data.keypoints().size()); - for(unsigned int i=0; i newKtps; - cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios()); - newKtps = feature2D_->generateKeypoints(newFrame, roi); - Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth()); - - if(newKtps.size()) - { - cv::KeyPoint::convert(newKtps, newCorners); - - if(subPixWinSize_ > 0 && subPixIterations_ > 0) - { - cv::cornerSubPix(newFrame, newCorners, - cv::Size( subPixWinSize_, subPixWinSize_ ), - cv::Size( -1, -1 ), - cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) ); - } - } - } - - if((int)newCorners.size() > this->getMinInliers()) - { - // get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow) - pcl::PointCloud::Ptr newCorners3D(new pcl::PointCloud); - newCorners3D->resize(newCorners.size()); - std::vector newCornersFiltered(newCorners.size()); - int oi=0; - for(unsigned int i=0; igetMaxDepth() == 0.0f || ( - uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) && - uIsInBounds(pt.z, 0.0f, this->getMaxDepth())))) - { - pt = util3d::transformPoint(pt, data.localTransform()); - newCorners3D->at(oi) = pt; - newCornersFiltered[oi] = newCorners[i]; - ++oi; - } - } - } - newCornersFiltered.resize(oi); - newCorners3D->resize(oi); - if((int)newCornersFiltered.size() > this->getMinInliers()) - { - refFrame_ = newFrame; - refCorners_ = newCornersFiltered; - refCorners3D_ = newCorners3D; - } - else - { - UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...", - (int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers()); - output.setNull(); - } - } - else - { - UWARN("Too low 2D corners (%d), ignoring new frame...", - (int)newCorners.size()); - output.setNull(); - } - } - - if(info) - { - info->type = 1; - info->variance = variance; - info->inliers = inliers; - info->features = (int)newCorners.size(); - info->matches = correspondences; - } - - UINFO("Odom update time = %fs lost=%s inliers=%d/%d, variance=%f, new corners=%d", - timer.elapsed(), - output.isNull()?"true":"false", - inliers, - correspondences, - variance, - (int)newCorners.size()); - return output; -} - } // namespace rtabmap diff --git a/corelib/src/ParticleFilter.h b/corelib/src/ParticleFilter.h index ad9ffaef..c03ff79f 100644 --- a/corelib/src/ParticleFilter.h +++ b/corelib/src/ParticleFilter.h @@ -132,7 +132,7 @@ public: double filter(double val) { - std::vector weights(particles_.size()); + std::vector weights(particles_.size(), 1); double sumWeights = 0; for(unsigned int i=0; i 0) + { + weights[i] = w; + } sumWeights += weights[i]; } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 1551ae2a..66a1e643 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2486,12 +2486,10 @@ void Rtabmap::dumpPoses( #endif if(fout) { - Transform localTransformInv = Transform(0,0,0, -CV_PI/2, 0, -CV_PI/2).inverse(); for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - Transform t = localTransformInv * (*iter).second; // in camera frame - const float * p = (const float *)t.data(); + const float * p = (const float *)(*iter).second.data(); fprintf(fout, "%f", p[0]); for(int i=1; i<(*iter).second.size(); i++) diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index ab3f575a..4a48544d 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -162,7 +162,7 @@ cv::Mat disparityFromStereoCorrespondences( { float d = leftCorners[i].x - rightCorners[i].x; float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x)); - if(d > 0.0f && slope < maxSlope) + if(d > 0.0f && (maxSlope <= 0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxSlope)) { disparity.at(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d; } diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index 5352a7cf..b6e623a4 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -124,15 +124,46 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( int flowWinSize, int flowMaxLevel, int flowIterations, - double flowEps) + double flowEps, + double maxCorrespondencesSlope) +{ + std::vector leftCorners; + cv::KeyPoint::convert(keypoints, leftCorners); + return generateKeypoints3DStereo( + leftCorners, + leftImage, + rightImage, + fx, + baseline, + cx, + cy, + transform, + flowWinSize, + flowMaxLevel, + flowIterations, + flowEps, + maxCorrespondencesSlope); +} + +pcl::PointCloud::Ptr generateKeypoints3DStereo( + const std::vector & leftCorners, + const cv::Mat & leftImage, + const cv::Mat & rightImage, + float fx, + float baseline, + float cx, + float cy, + const Transform & transform, + int flowWinSize, + int flowMaxLevel, + int flowIterations, + double flowEps, + double maxCorrespondencesSlope) { UASSERT(!leftImage.empty() && !rightImage.empty() && leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 && leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols); - std::vector leftCorners; - cv::KeyPoint::convert(keypoints, leftCorners); - // Find features in the new left image std::vector status; std::vector err; @@ -151,16 +182,18 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( UDEBUG("cv::calcOpticalFlowPyrLK() end"); pcl::PointCloud::Ptr keypoints3d(new pcl::PointCloud); - keypoints3d->resize(keypoints.size()); + keypoints3d->resize(leftCorners.size()); float bad_point = std::numeric_limits::quiet_NaN (); - UASSERT(status.size() == keypoints.size()); + UASSERT(status.size() == leftCorners.size()); for(unsigned int i=0; i 0.0f) + float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)); + if(disparity > 0.0f && + (maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope)) { pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D( leftCorners[i], diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 2ffe609b..f8da2747 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -105,6 +105,7 @@ private slots: void resetConstraint(); void rejectConstraint(); void updateConstraintView(); + void updateStereo(); private: QString getIniFilePath() const; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index fd2b61a0..4215fcf1 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -102,6 +102,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : ui_->constraintsViewer->setCameraLockZ(false); ui_->constraintsViewer->setCameraFree(); + ui_->graphicsView_stereo->setAlpha(255); + this->readSettings(); if(RTABMAP_NONFREE == 0) @@ -215,6 +217,20 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid())); connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid())); + connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(updateStereo())); + connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(updateStereo())); + connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo())); + connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo())); + connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(updateStereo())); + connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(updateStereo())); + connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(updateStereo())); + connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(updateStereo())); + connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(updateStereo())); + ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }"); + ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }"); + ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }"); + ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }"); + // connect configuration changed connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified())); @@ -263,6 +279,16 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified())); connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); + //stereo parameters + connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); + connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(configModified())); + connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(configModified())); + connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(configModified())); + connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(configModified())); + connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified())); + connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(configModified())); + connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(configModified())); + connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(configModified())); // dockwidget QList dockWidgets = this->findChildren(); for(int i=0; ispinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt()); settings.endGroup(); + //Stereo parameters + settings.beginGroup("stereo"); + ui_->spinBox_stereo_flowIterations->setValue(settings.value("flowIterations", ui_->spinBox_stereo_flowIterations->value()).toInt()); + ui_->spinBox_stereo_flowMaxLevel->setValue(settings.value("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value()).toInt()); + ui_->spinBox_stereo_flowWinSize->setValue(settings.value("flowWinSize", ui_->spinBox_stereo_flowWinSize->value()).toInt()); + ui_->spinBox_stereo_gfttBlockSize->setValue(settings.value("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value()).toInt()); + ui_->doubleSpinBox_stereo_flowEps->setValue(settings.value("flowEps", ui_->doubleSpinBox_stereo_flowEps->value()).toDouble()); + ui_->doubleSpinBox_stereo_gfttMinDistance->setValue(settings.value("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value()).toDouble()); + ui_->doubleSpinBox_stereo_gfttQuality->setValue(settings.value("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value()).toDouble()); + ui_->doubleSpinBox_stereo_maxSlope->setValue(settings.value("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value()).toDouble()); + ui_->checkBox_stereo_subpix->setChecked(settings.value("subpix", ui_->checkBox_stereo_subpix->isChecked()).toBool()); + settings.endGroup(); + settings.endGroup(); // DatabaseViewer } @@ -468,6 +507,19 @@ void DatabaseViewer::writeSettings() settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()); settings.endGroup(); + //Stereo parameters + settings.beginGroup("stereo"); + settings.setValue("flowIterations", ui_->spinBox_stereo_flowIterations->value()); + settings.setValue("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value()); + settings.setValue("flowWinSize", ui_->spinBox_stereo_flowWinSize->value()); + settings.setValue("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value()); + settings.setValue("flowEps", ui_->doubleSpinBox_stereo_flowEps->value()); + settings.setValue("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value()); + settings.setValue("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value()); + settings.setValue("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value()); + settings.setValue("subpix", ui_->checkBox_stereo_subpix->isChecked()); + settings.endGroup(); + settings.endGroup(); // DatabaseViewer this->setWindowModified(false); @@ -1535,6 +1587,11 @@ void DatabaseViewer::update(int value, { this->updateStereo(&data); } + else + { + ui_->stereoViewer->clear(); + ui_->graphicsView_stereo->clear(); + } // 3d view if(view3D->isVisible() && !data.getDepthRaw().empty()) @@ -1688,6 +1745,16 @@ void DatabaseViewer::update(int value, } } +void DatabaseViewer::updateStereo() +{ + if(ui_->horizontalSlider_A->maximum()) + { + int id = ids_.at(ui_->horizontalSlider_A->value()); + Signature data = memory_->getSignatureData(id, true); + updateStereo(&data); + } +} + void DatabaseViewer::updateStereo(const Signature * data) { if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1) @@ -1708,8 +1775,10 @@ void DatabaseViewer::updateStereo(const Signature * data) std::vector kpts; cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04"); ParametersMap parameters; - parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "1000")); - parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), "5")); + parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0")); + parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttMinDistance->value()))); + parameters.insert(ParametersPair(Parameters::kGFTTQualityLevel(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttQuality->value()))); + parameters.insert(ParametersPair(Parameters::kGFTTBlockSize(), uNumber2Str(ui_->spinBox_stereo_gfttBlockSize->value()))); Feature2D::Type type = Feature2D::kFeatureGfttBrief; Feature2D * kptDetector = Feature2D::create(type, parameters); kpts = kptDetector->generateKeypoints(leftMono, roi); @@ -1720,6 +1789,19 @@ void DatabaseViewer::updateStereo(const Signature * data) std::vector leftCorners; cv::KeyPoint::convert(kpts, leftCorners); + int subPixWinSize = 3; + int subPixIterations = 30; + double subPixEps = 0.02; + if(ui_->checkBox_stereo_subpix->isChecked()) + { + UDEBUG("cv::cornerSubPix() begin"); + cv::cornerSubPix(leftMono, leftCorners, + cv::Size( subPixWinSize, subPixWinSize ), + cv::Size( -1, -1 ), + cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) ); + UDEBUG("cv::cornerSubPix() end"); + } + // Find features in the new left image std::vector status; std::vector err; @@ -1731,8 +1813,8 @@ void DatabaseViewer::updateStereo(const Signature * data) rightCorners, status, err, - cv::Size(Parameters::defaultStereoWinSize(), Parameters::defaultStereoWinSize()), Parameters::defaultStereoMaxLevel(), - cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, Parameters::defaultStereoIterations(), Parameters::defaultStereoEps())); + cv::Size(ui_->spinBox_stereo_flowWinSize->value(), ui_->spinBox_stereo_flowWinSize->value()), ui_->spinBox_stereo_flowMaxLevel->value(), + cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value())); float timeFlow = timer.ticks(); @@ -1741,6 +1823,10 @@ void DatabaseViewer::updateStereo(const Signature * data) float bad_point = std::numeric_limits::quiet_NaN (); UASSERT(status.size() == kpts.size()); int oi = 0; + int inliers = 0; + int flowOutliers= 0; + int slopeOutliers= 0; + int negativeDisparityOutliers = 0; for(unsigned int i=0; i 0.0f) { - if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < Parameters::defaultStereoMaxSlope()) + if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < ui_->doubleSpinBox_stereo_maxSlope->value()) { pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D( leftCorners[i], @@ -1759,23 +1845,32 @@ void DatabaseViewer::updateStereo(const Signature * data) if(pcl::isFinite(tmpPt)) { pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f()); - if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2) - { - status[i] = 100; //blue - } + status[i] = 100; //blue + ++inliers; cloud->at(oi++) = pt; } } + else if(fabs(leftCorners[i].y-rightCorners[i].y) <=1.0f) + { + status[i] = 110; //cyan + ++inliers; + } else { status[i] = 101; //yellow + ++slopeOutliers; } } else { status[i] = 102; //magenta + ++negativeDisparityOutliers; } } + else + { + ++flowOutliers; + } } cloud->resize(oi); @@ -1786,6 +1881,11 @@ void DatabaseViewer::updateStereo(const Signature * data) ui_->stereoViewer->addOrUpdateCloud("stereo", cloud); ui_->stereoViewer->update(); + ui_->label_stereo_inliers->setNum(inliers); + ui_->label_stereo_flowOutliers->setNum(flowOutliers); + ui_->label_stereo_slopeOutliers->setNum(slopeOutliers); + ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers); + std::vector rightKpts; cv::KeyPoint::convert(rightCorners, rightKpts); std::vector good_matches(kpts.size()); @@ -1832,6 +1932,10 @@ void DatabaseViewer::updateStereo(const Signature * data) { c = Qt::magenta; } + else if(status[i] == 110) + { + c = Qt::cyan; + } ui_->graphicsView_stereo->addLine( kpts[i].pt.x, kpts[i].pt.y, diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index dc1a4c25..14595e06 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -880,20 +880,21 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap { //draw lines UASSERT(info.refCorners.size() == info.newCorners.size()); - for(unsigned int i=0; i inliers(info.cornerInliers.begin(), info.cornerInliers.end()); + for(unsigned int i=0; iimageView_odometry->isFeaturesShown()) + if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) { - _ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers + _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers } if(_ui->imageView_odometry->isLinesShown()) { _ui->imageView_odometry->addLine( - info.refCorners[info.cornerInliers[i]].x, - info.refCorners[info.cornerInliers[i]].y, - info.newCorners[info.cornerInliers[i]].x, - info.newCorners[info.cornerInliers[i]].y, - Qt::blue); + info.refCorners[i].x, + info.refCorners[i].y, + info.newCorners[i].x, + info.newCorners[i].y, + inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); } } } @@ -975,6 +976,10 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap _ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f); _ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f); } + if(info.distanceTravelled > 0) + { + _ui->statsToolBox->updateStat("Odometry/Distance/m", (float)data.id(), info.distanceTravelled); + } _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0); _processingOdometry = false; @@ -993,7 +998,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1)); _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId)); - _ui->label_matchId->clear(); if(stat.extended()) { @@ -1252,6 +1256,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1)); _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId)); } + else + { + _ui->label_matchId->clear(); + } float elapsedTime = static_cast(totalTime.elapsed()); UINFO("Updating GUI time = %fs", elapsedTime/1000.0f); _ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime); @@ -2902,7 +2910,14 @@ void MainWindow::startDetection() if(_dataRecorder) { - UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + if(_camera) + { + UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + } + else if(_dbReader) + { + UEventsManager::createPipe(_dbReader, _dataRecorder, "CameraEvent"); + } } _lastOdomPose.setNull(); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 4d331934..16723161 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -50,7 +50,7 @@ 0 0 - 172 + 196 184 @@ -200,6 +200,9 @@ + + Qt::ClickFocus + Qt::Horizontal @@ -233,7 +236,7 @@ 0 0 - 172 + 195 184 @@ -383,6 +386,9 @@ + + Qt::ClickFocus + Qt::Horizontal @@ -479,6 +485,9 @@ + + Qt::ClickFocus + Qt::Horizontal @@ -496,6 +505,9 @@ + + Qt::ClickFocus + Qt::Horizontal @@ -654,6 +666,9 @@ + + Qt::ClickFocus + Qt::Horizontal @@ -783,14 +798,14 @@ - 1 + 5 0 0 - 315 + 312 314 @@ -1009,7 +1024,7 @@ 0 - -12 + 0 351 355 @@ -1510,7 +1525,7 @@ 0 0 - 315 + 248 319 @@ -1704,8 +1719,8 @@ 0 0 - 330 - 186 + 201 + 126 @@ -1799,6 +1814,217 @@ + + + + 0 + 0 + 283 + 322 + + + + Stereo correspondences + + + + + + pixels + + + 1 + + + 3 + + + + + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.010000000000000 + + + + + + + Optical Flow eps + + + + + + + GFTT block size + + + + + + + pixels + + + 3 + + + 16 + + + + + + + Optical Flow win size + + + + + + + 1 + + + 3 + + + + + + + GFTT min distance + + + + + + + 4 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Optical Flow max level + + + + + + + Optical Flow iterations + + + + + + + GFTT quality level + + + + + + + 1 + + + 1000 + + + 30 + + + + + + + 2 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + pixels + + + 1 + + + 5.000000000000000 + + + + + + + Sub pixel + + + + + + + Matches max slope + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + @@ -1812,12 +2038,110 @@ 4 - + + + 0 + + + 0 + - + + + + + + + + - + + + 6 + + + + + Inliers: + + + Qt::RichText + + + + + + + 0 + + + + + + + Slope outliers: + + + Qt::RichText + + + + + + + 0 + + + + + + + Negative disparity outliers: + + + Qt::RichText + + + + + + + 0 + + + + + + + Flow outliers: + + + Qt::RichText + + + + + + + 0 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 2ac9eb01..160414e7 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -613 + -450 755 1591 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 23 @@ -7222,11 +7222,14 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare m - 2 + 0 0.000000000000000 + + 999.000000000000000 + 1.000000000000000 From 53f7719655c544d66c41c11f96be7b5a0b4d9af6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 15 Jun 2015 20:53:31 -0400 Subject: [PATCH 07/45] Added odometry honolomic parameter --- corelib/include/rtabmap/core/Odometry.h | 1 + corelib/include/rtabmap/core/Parameters.h | 1 + corelib/src/Odometry.cpp | 19 +++++++-- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/mainWindow.ui | 2 +- guilib/src/ui/preferencesDialog.ui | 47 +++++++++++++++-------- 6 files changed, 52 insertions(+), 19 deletions(-) diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index dedb85bf..4a805652 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -77,6 +77,7 @@ private: float _maxDepth; int _resetCountdown; bool _force2D; + bool _holonomic; bool _particleFiltering; int _particleSize; float _particleNoiseT; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index bce3b2fe..dc3bb3f7 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -322,6 +322,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); + RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ce5f5c08..6608dead 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -43,6 +43,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _maxDepth(Parameters::defaultOdomMaxDepth()), _resetCountdown(Parameters::defaultOdomResetCountdown()), _force2D(Parameters::defaultOdomForce2D()), + _holonomic(Parameters::defaultOdomHolonomic()), _particleFiltering(Parameters::defaultOdomParticleFiltering()), _particleSize(Parameters::defaultOdomParticleSize()), _particleNoiseT(Parameters::defaultOdomParticleNoiseT()), @@ -66,6 +67,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth); Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios); Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D); + Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic); Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData); Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation); Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError); @@ -191,7 +193,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) { _resetCurrentCount = _resetCountdown; - if(_force2D || filters_.size()) + if(_force2D || !_holonomic || filters_.size()) { float x,y,z, roll,pitch,yaw; t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); @@ -211,8 +213,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) else { x = filters_[0]->filter(x); - y = filters_[1]->filter(y); yaw = filters_[5]->filter(yaw); + if(_holonomic) + { + y = filters_[1]->filter(y); + } + else + { + y = x * tan(yaw); + } if(!_force2D) { @@ -227,13 +236,17 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) info->timeParticleFiltering = time.ticks(); } } + else if(!_holonomic) + { + y = x * tan(yaw); + } UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw), uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s", x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str()); t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw); - if(info) + if(info && filters_.size()) { info->transformFiltered = t; } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index bc9d9650..09943ff3 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -550,6 +550,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str()); _ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str()); _ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str()); + _ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str()); _ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str()); _ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str()); _ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str()); diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 91aa6443..3b84d7a0 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -55,6 +55,7 @@ Advanced + @@ -73,7 +74,6 @@ - diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 160414e7..0acaa790 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -450 + 0 755 1591 @@ -6794,13 +6794,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - 999999 - - - @@ -6838,7 +6831,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Fill info with data (inliers/outliers features to be shown in Odometry view). @@ -6848,7 +6841,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Test selected odometry @@ -6875,7 +6868,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Data buffer size (0 means inf). @@ -6885,7 +6878,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 999999 @@ -6902,7 +6895,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + @@ -6956,7 +6949,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Particle filtering to smooth the odometry trajectory. See "Particle Filter" panel for the related parameters. @@ -6966,13 +6959,37 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + + + + 999999 + + + + + + + If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). + + + true + + + + + + + + + + From bc18d4bf7db75829eef2395e618a16d710139248 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 16 Jun 2015 08:31:38 -0400 Subject: [PATCH 08/45] updated pf_filter.m --- Matlab/ParticleFilter/pf_filter.m | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/Matlab/ParticleFilter/pf_filter.m b/Matlab/ParticleFilter/pf_filter.m index 37a7339a..bc03c0db 100644 --- a/Matlab/ParticleFilter/pf_filter.m +++ b/Matlab/ParticleFilter/pf_filter.m @@ -1,17 +1,24 @@ function filtered = pf_filter(x, nParticles, noise, lambda) -particles = zeros(nParticles,1) ; -weights = zeros(nParticles,1); +particles = ones(nParticles,1)*x(1) ; +weights = ones(nParticles,1); filtered=zeros(1,length(x)); for i = 1:length(x); for j = 1:nParticles rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++ - particles(j) = particles(j) + noise*rn ; - dist = abs(particles(j) - x(i)); - weights(j) = exp(-lambda*dist); + noisyP = particles(j) + noise*rn ; + dist = abs(noisyP - x(i)); + tmp = exp(-lambda*dist); + if isfinite(tmp) && tmp > 0 + particles(j) = noisyP; + weights(j) = tmp; + end end - weights = weights ./(sum(weights(:))); + if sum(weights(:)) > 0 + weights = weights ./sum(weights(:)); + end + filtered(i) = weights'*particles; particles = pf_resample(particles, weights); end \ No newline at end of file From 7cd0d0cd53be72d5a37cd460674fb8e0eb2c82ab Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 16 Jun 2015 08:50:32 -0400 Subject: [PATCH 09/45] holonomic option: makeing z depending on x and pitch too if nonholonomic --- corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/Odometry.cpp | 13 ++++++++++++- guilib/src/ui/preferencesDialog.ui | 2 +- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index dc3bb3f7..80a37d62 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -322,7 +322,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); - RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); + RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 6608dead..4dfb26ef 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -225,9 +225,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) if(!_force2D) { - z = filters_[2]->filter(z); roll = filters_[3]->filter(roll); pitch = filters_[4]->filter(pitch); + if(_holonomic) + { + z = filters_[2]->filter(z); + } + else + { + z = x * tan(pitch); + } } } @@ -239,6 +246,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) else if(!_holonomic) { y = x * tan(yaw); + if(!_force2D) + { + z = x * tan(pitch); + } } UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw), diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 0acaa790..fc3b5f42 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6976,7 +6976,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). + If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)). true From 6a7a9fb9b0b4e86d9671f1fffeb7c2e93966ced2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 16 Jun 2015 10:50:49 -0400 Subject: [PATCH 10/45] reverted modif on z when honolonomic but added modif on yaw depending on the estimated y value --- corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/Odometry.cpp | 37 ++++++++++++----------- guilib/src/PreferencesDialog.cpp | 23 +++++++++----- guilib/src/ui/preferencesDialog.ui | 10 +++--- 4 files changed, 42 insertions(+), 30 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 80a37d62..dc3bb3f7 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -322,7 +322,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); - RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch))."); + RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 4dfb26ef..d4f6c95b 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -213,28 +213,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) else { x = filters_[0]->filter(x); + y = filters_[1]->filter(y); yaw = filters_[5]->filter(yaw); - if(_holonomic) + + if(!_holonomic) { - y = filters_[1]->filter(y); - } - else - { - y = x * tan(yaw); + float tmpY = x * tan(yaw); + if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) + { + y = tmpY; + } + else + { + yaw = atan(y/x); + } } if(!_force2D) { + z = filters_[2]->filter(z); roll = filters_[3]->filter(roll); pitch = filters_[4]->filter(pitch); - if(_holonomic) - { - z = filters_[2]->filter(z); - } - else - { - z = x * tan(pitch); - } } } @@ -245,10 +244,14 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) } else if(!_holonomic) { - y = x * tan(yaw); - if(!_force2D) + float tmpY = x * tan(yaw); + if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) { - z = x * tan(pitch); + y = tmpY; + } + else + { + yaw = atan(y/x); } } UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 09943ff3..b31b8fea 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -3188,13 +3188,9 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const QString str = _ui->lineEdit_openniLocalTransform->text(); str.replace("PI_2", QString::number(3.141592/2.0)); QStringList list = str.split(' '); - if(list.size() != 6) + if(list.size() == 6 || list.size() == 9) { - UERROR("Local transform is wrong! must have 6 items (%s)", str.toStdString().c_str()); - } - else - { - std::vector numbers(6); + std::vector numbers(list.size()); bool ok = false; for(int i=0; i 0 - 0 + -837 755 1591 @@ -86,7 +86,7 @@ QFrame::Raised - 23 + 3 @@ -2071,14 +2071,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 0 0 -PI_2 0 -PI_2 + 0 0 1 -1 0 0 0 -1 0 - Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. + Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33. true @@ -6976,7 +6976,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)). + If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). true From 82943e85e8d5db089c9081724bedc2fa90a20ee5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 18 Jun 2015 01:24:19 -0400 Subject: [PATCH 11/45] Fixed PnP camera matrix empty when computing loop closure. Updated odometry nonholomic motion estimation (using arc around ICR). --- corelib/src/CameraThread.cpp | 4 ++- corelib/src/Memory.cpp | 48 +++++++++++++++++++++++++----------- corelib/src/Odometry.cpp | 10 +++++--- 3 files changed, 43 insertions(+), 19 deletions(-) diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 5ed3cb80..aa988f9d 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -127,7 +127,9 @@ void CameraThread::mainLoop() if(_cameraRGBD) { SensorData data; - if(dynamic_cast(_cameraRGBD) || dynamic_cast(_cameraRGBD)) + if(dynamic_cast(_cameraRGBD) || + dynamic_cast(_cameraRGBD) || + dynamic_cast(_cameraRGBD)) { //stereo data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index aaa4e488..8be2f96a 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1975,20 +1975,29 @@ Transform Memory::computeVisualTransform( UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); } - if(!newS.sensorData().rightRaw().empty() && !newS.sensorData().stereoCameraModel().isValid()) - { - UERROR("Calibrated stereo camera required"); - } - else if(!newS.sensorData().depthRaw().empty() && - (newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid())) + if((!newS.sensorData().rightRaw().empty() || + !newS.sensorData().stereoCameraModel().isValid()) && + (!newS.sensorData().depthRaw().empty() || + newS.sensorData().cameraModels().size() != 1 || + !newS.sensorData().cameraModels()[0].isValid())) { UERROR("Calibrated camera required (multi-cameras not supported)."); } else { - cv::Mat K = newS.sensorData().cameraModels().size()?newS.sensorData().cameraModels()[0].K():newS.sensorData().stereoCameraModel().left().K(); - Transform localTransform = newS.sensorData().cameraModels().size()?newS.sensorData().cameraModels()[0].localTransform():newS.sensorData().stereoCameraModel().left().localTransform(); - + cv::Mat K; + Transform localTransform; + if(newS.sensorData().cameraModels().size()) + { + K = newS.sensorData().cameraModels()[0].K(); + localTransform = newS.sensorData().cameraModels()[0].localTransform(); + } + else + { + K = newS.sensorData().stereoCameraModel().left().K(); + localTransform = newS.sensorData().stereoCameraModel().left().localTransform(); + } + UASSERT(!K.empty() && !localTransform.isNull()); // 2D -> 3D if(!oldS.getWords3().empty() && !newS.getWords().empty()) { @@ -4268,11 +4277,22 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p data.stamp(), "", pose, - data.userData(), - SensorData( - rtabmap::compressData2(laserScan), - data.laserScanMaxPts(), - cv::Mat(), cv::Mat(), CameraModel(), id)); + data.userData(), + stereoCameraModel.isValid()? + SensorData( + rtabmap::compressData2(laserScan), + data.laserScanMaxPts(), + cv::Mat(), + cv::Mat(), + stereoCameraModel, + id): + SensorData( + rtabmap::compressData2(laserScan), + data.laserScanMaxPts(), + cv::Mat(), + cv::Mat(), + cameraModels, + id)); } s->setWords(words); s->setWords3(words3D); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 434a67f3..f55d11d6 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -218,14 +218,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) if(!_holonomic) { - float tmpY = x * tan(yaw); + // arc trajectory around ICR + float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f; if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) { y = tmpY; } else { - yaw = atan(y/x); + yaw = (atan(x/y)*2.0f-CV_PI)*-1; } } @@ -244,14 +245,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) } else if(!_holonomic) { - float tmpY = x * tan(yaw); + // arc trajectory around ICR + float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f; if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) { y = tmpY; } else { - yaw = atan(y/x); + yaw = (atan(x/y)*2.0f-CV_PI)*-1; } } UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && From a5efee20bc7aa9178262c5f41dd98dea03446a94 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jun 2015 18:29:12 -0400 Subject: [PATCH 12/45] Using Dijkstra for global planning for a significative performance boost (no need to optimize the graph before computing the path) --- corelib/include/rtabmap/core/DBDriver.h | 2 + corelib/include/rtabmap/core/Graph.h | 17 ++ corelib/include/rtabmap/core/Memory.h | 3 + corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/include/rtabmap/core/Rtabmap.h | 2 +- corelib/src/DBDriver.cpp | 27 +++ corelib/src/DBDriverSqlite3.cpp | 89 ++++++++ corelib/src/DBDriverSqlite3.h | 1 + corelib/src/Graph.cpp | 123 +++++++++++ corelib/src/Memory.cpp | 48 +++++ corelib/src/Rtabmap.cpp | 245 ++++++++++++---------- 11 files changed, 441 insertions(+), 118 deletions(-) diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 8d18dbb6..23b255d6 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -101,6 +101,7 @@ public: 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 getAllLinks(std::multimap & links, bool ignoreNullLinks = true) const; void getLastNodeId(int & id) const; void getLastWordId(int & id) const; void getInvertedIndexNi(int signatureId, int & ni) const; @@ -137,6 +138,7 @@ private: virtual void getNodeDataQuery(int signatureId, SensorData & data) const = 0; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const = 0; + virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const = 0; virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0; virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0; virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0; diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index c51b55f2..648283ae 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include namespace rtabmap { +class Memory; namespace graph { @@ -198,6 +199,22 @@ std::list > RTABMAP_EXP computePath( int to, bool updateNewCosts = false); +/** + * Perform Dijkstra path planning in the graph. + * @param fromId initial node + * @param toId final node + * @param memory The graph's memory + * @param lookInDatabase check links in database + * @param updateNewCosts Keep up-to-date costs while traversing the graph. + * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node). + */ +std::list > RTABMAP_EXP computePath( + int fromId, + int toId, + const Memory * memory, + bool lookInDatabase = true, + bool updateNewCosts = false); + int RTABMAP_EXP findNearestNode( const std::map & nodes, const rtabmap::Transform & targetPose); diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 554d7310..d6595d51 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -115,6 +115,9 @@ public: bool lookInDatabase = false) const; std::map getLoopClosureLinks(int signatureId, bool lookInDatabase = false) const; + std::map getLinks(int signatureId, + bool lookInDatabase = false) const; + std::multimap getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const; bool isRawDataKept() const {return _rawDataKept;} bool isBinDataKept() const {return _binDataKept;} float getSimilarityThreshold() const {return _similarityThreshold;} diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index dc3bb3f7..253e0a3d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -291,7 +291,7 @@ class RTABMAP_EXP Parameters 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)."); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter."); - RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\"."); + RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\"."); RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 730cfafc..8a39d18e 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -129,7 +129,7 @@ public: std::map * signatures = 0); void clearPath(); bool computePath(int targetNode, bool global); - bool computePath(const Transform & targetPose, bool global); + bool computePath(const Transform & targetPose); // only in current optimized map const std::vector > & getPath() const {return _path;} std::vector > getPathNextPoses() const; std::vector getPathNextNodes() const; diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 6e9ecf5c..b4f6dba5 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -556,6 +556,33 @@ void DBDriver::getAllNodeIds(std::set & ids, bool ignoreChildren) const _dbSafeAccessMutex.unlock(); } +void DBDriver::getAllLinks(std::multimap & links, bool ignoreNullLinks) const +{ + _dbSafeAccessMutex.lock(); + this->getAllLinksQuery(links, ignoreNullLinks); + _dbSafeAccessMutex.unlock(); + + // look in the trash + _trashesMutex.lock(); + if(_trashSignatures.size()) + { + for(std::map::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter) + { + links.erase(iter->first); + for(std::multimap::const_iterator jter=iter->second->getLinks().begin(); + jter!=iter->second->getLinks().end(); + ++jter) + { + if(!ignoreNullLinks || jter->second.isValid()) + { + links.insert(std::make_pair(iter->first, jter->second)); + } + } + } + } + _trashesMutex.unlock(); +} + void DBDriver::getLastNodeId(int & id) const { // look in the trash diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 2dbd4d07..a2ea085d 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -1055,6 +1055,95 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set & ids, bool ignoreChildre } } +void DBDriverSqlite3::getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const +{ + links.clear(); + if(_ppDb) + { + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::stringstream query; + + if(uStrNumCmp(_version, "0.8.4") >= 0) + { + query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance FROM Link ORDER BY from_id, to_id"; + } + else if(uStrNumCmp(_version, "0.7.4") >= 0) + { + query << "SELECT from_id, to_id, type, transform, variance FROM Link ORDER BY from_id, to_id"; + } + else + { + query << "SELECT from_id, to_id, type, transform FROM Link ORDER BY from_id, 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 fromId = -1; + int toId = -1; + int type = Link::kUndef; + float rotVariance = 1.0f; + float transVariance = 1.0f; + const void * data = 0; + int dataSize = 0; + + // Process the result if one + rc = sqlite3_step(ppStmt); + while(rc == SQLITE_ROW) + { + int index = 0; + + fromId = sqlite3_column_int(ppStmt, index++); + 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); + } + else if(dataSize) + { + UERROR("Error while loading link transform from %d to %d! Setting to null...", fromId, toId); + } + + if(!ignoreNullLinks || !transform.isNull()) + { + if(uStrNumCmp(_version, "0.8.4") >= 0) + { + rotVariance = sqlite3_column_double(ppStmt, index++); + transVariance = sqlite3_column_double(ppStmt, index++); + links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance))); + } + else if(uStrNumCmp(_version, "0.7.4") >= 0) + { + rotVariance = transVariance = sqlite3_column_double(ppStmt, index++); + links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance))); + } + else + { + // neighbor is 0, loop closures are 1 and 2 (child) + links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance))); + } + } + + 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()); + } +} + void DBDriverSqlite3::getLastIdQuery(const std::string & tableName, int & id) const { if(_ppDb) diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index d530eb72..2cb99426 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -74,6 +74,7 @@ private: virtual void getNodeDataQuery(int signatureId, SensorData & data) const; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const; + virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const; virtual void getLastIdQuery(const std::string & tableName, int & id) const; virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const; virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const; diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 058e1a2c..a3b0bcf7 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -1258,6 +1260,127 @@ std::list > computePath( return path; } + +// return path starting from "fromId" (Identity pose for the first node) +std::list > computePath( + int fromId, + int toId, + const Memory * memory, + bool lookInDatabase, + bool updateNewCosts) +{ + UASSERT(memory!=0); + UASSERT(fromId>=0); + UASSERT(toId>=0); + std::list > path; + + std::multimap allLinks; + if(lookInDatabase) + { + // Faster to load all links in one query + //UTimer t; + allLinks = memory->getAllLinks(lookInDatabase); + //UWARN("getting all %d links time = %f s", (int)allLinks.size(), t.ticks()); + } + + //dijkstra + int startNode = fromId; + int endNode = toId; + std::map nodes; + nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity()))); + std::priority_queue, Order> pq; + std::multimap pqmap; + if(updateNewCosts) + { + pqmap.insert(std::make_pair(0, startNode)); + } + else + { + pq.push(Pair(startNode, 0)); + } + + while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size())) + { + Node * currentNode; + if(updateNewCosts) + { + currentNode = &nodes.find(pqmap.begin()->second)->second; + pqmap.erase(pqmap.begin()); + } + else + { + currentNode = &nodes.find(pq.top().first)->second; + pq.pop(); + } + + currentNode->setClosed(true); + + if(currentNode->id() == endNode) + { + while(currentNode->id()!=startNode) + { + path.push_front(std::make_pair(currentNode->id(), currentNode->pose())); + currentNode = &nodes.find(currentNode->fromId())->second; + } + path.push_front(std::make_pair(startNode, currentNode->pose())); + break; + } + + // lookup neighbors + std::map links; + if(allLinks.size() == 0) + { + links = memory->getLinks(currentNode->id(), lookInDatabase); + } + else + { + for(std::multimap::const_iterator iter = allLinks.lower_bound(currentNode->id()); + iter!=allLinks.end() && iter->first == currentNode->id(); + ++iter) + { + links.insert(std::make_pair(iter->second.to(), iter->second)); + } + } + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + { + std::map::iterator nodeIter = nodes.find(iter->first); + if(nodeIter == nodes.end()) + { + Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform()); + n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm()); + nodes.insert(std::make_pair(iter->second.to(), n)); + if(updateNewCosts) + { + pqmap.insert(std::make_pair(n.totalCost(), n.id())); + } + else + { + pq.push(Pair(n.id(), n.totalCost())); + } + } + else if(updateNewCosts && nodeIter->second.isOpened()) + { + float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose()); + if(nodeIter->second.costSoFar() > newCostSoFar) + { + // update the cost in the priority queue + for(std::multimap::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter) + { + if(mapIter->second == nodeIter->first) + { + pqmap.erase(mapIter); + nodeIter->second.setCostSoFar(newCostSoFar); + pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first)); + break; + } + } + } + } + } + } + return path; +} + int findNearestNode( const std::map & nodes, const rtabmap::Transform & targetPose) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index b1a20ba4..0f85cd4a 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -876,6 +876,54 @@ std::map Memory::getLoopClosureLinks( return loopClosures; } +std::map Memory::getLinks( + int signatureId, + bool lookInDatabase) const +{ + std::map links; + Signature * s = uValue(_signatures, signatureId, (Signature*)0); + if(s) + { + links = s->getLinks(); + } + else if(lookInDatabase && _dbDriver) + { + _dbDriver->loadLinks(signatureId, links, Link::kUndef); + } + else + { + UWARN("Cannot find signature %d in memory", signatureId); + } + return links; +} + +std::multimap Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const +{ + std::multimap links; + + if(lookInDatabase && _dbDriver) + { + _dbDriver->getAllLinks(links, ignoreNullLinks); + } + + for(std::map::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) + { + links.erase(iter->first); + for(std::multimap::const_iterator jter=iter->second->getLinks().begin(); + jter!=iter->second->getLinks().end(); + ++jter) + { + if(!ignoreNullLinks || jter->second.isValid()) + { + links.insert(std::make_pair(iter->first, jter->second)); + } + } + } + + return links; +} + + // return map, including signatureId // maxCheckedInDatabase = -1 means no limit to check in database (default) // maxCheckedInDatabase = 0 means don't check in database diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3fff7b98..9b3cda7d 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2965,14 +2965,25 @@ void Rtabmap::clearPath() } } -bool Rtabmap::computePath( - int targetNode, - std::map nodes, - const std::multimap & constraints) +// return true if path is updated +bool Rtabmap::computePath(int targetNode, bool global) { + UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0); + this->clearPath(); + + if(!_rgbdSlamMode) + { + UWARN("A path can only be computed in RGBD-SLAM mode"); + return false; + } + + UTimer totalTimer; + UTimer timer; + + // No need to optimize the graph if(_memory) { - int currentNode; + int currentNode = 0; if(_memory->isIncremental()) { if(!_memory->getLastWorkingSignature()) @@ -2991,123 +3002,63 @@ bool Rtabmap::computePath( } currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); } - - if(!uContains(nodes, currentNode)) + if(currentNode && targetNode) { - UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode); - return false; - } + std::list > path = graph::computePath( + currentNode, + targetNode, + _memory, + global); - if(!uContains(nodes, targetNode)) - { - UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode); - return false; - } - - // transform nodes into current referential - if(_optimizedPoses.size()) - { - if(uContains(nodes, currentNode) && uContains(_optimizedPoses, currentNode)) + //transform in current referential + Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity()); + _path.resize(path.size()); + int oi = 0; + for(std::list >::iterator iter=path.begin(); iter!=path.end();++iter) { - Transform t = _optimizedPoses.at(currentNode) * nodes.at(currentNode).inverse(); - for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) - { - iter->second = t * iter->second; - } + _path[oi].first = iter->first; + _path[oi++].second = t * iter->second; } } - - std::multimap links; - for(std::multimap::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) - { - links.insert(std::make_pair(iter->first, iter->second.to())); - links.insert(std::make_pair(iter->second.to(), iter->first)); // <-> - } - // Add links between neighbor nodes in the goal radius. - if(_planVirtualLinks) - { - std::multimap clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI); - for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) - { - if(graph::findLink(links, iter->first, iter->second) == links.end()) - { - links.insert(*iter); - links.insert(std::make_pair(iter->second, iter->first)); // <-> - } - } - } - - UINFO("Computing path from location %d to %d", currentNode, targetNode); - UTimer timer; - _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, targetNode)); - UINFO("A* time = %fs", timer.ticks()); - - if(_path.size() == 0) - { - _path.clear(); - UWARN("Cannot compute a path!"); - } - else - { - UINFO("Path generated! Size=%d", (int)_path.size()); - if(ULogger::level() == ULogger::kInfo) - { - std::stringstream stream; - for(unsigned int i=0; i<_path.size(); ++i) - { - stream << _path[i].first; - if(i+1 < _path.size()) - { - stream << " "; - } - } - UINFO("Path = [%s]", stream.str().c_str()); - } - if(_goalsSavedInUserData) - { - // set goal to latest signature - std::string goalStr = uFormat("GOAL:%d", targetNode); - setUserData(0, uStr2Bytes(goalStr)); - } - } - - return _path.size()>0; } - return false; -} + UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path)); -// return true if path is updated -bool Rtabmap::computePath(int targetNode, bool global) -{ - UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0); - this->clearPath(); - - if(!_rgbdSlamMode) + if(_path.size() == 0) { - UWARN("A path can only be computed in RGBD-SLAM mode"); - return false; + _path.clear(); + UWARN("Cannot compute a path!"); } - - UTimer totalTimer; - UTimer timer; - std::map nodes; - std::multimap constraints; - this->getGraph(nodes, constraints, true, global); - UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks()); - - if(computePath(targetNode, nodes, constraints)) + else { + UINFO("Path generated! Size=%d", (int)_path.size()); + if(ULogger::level() == ULogger::kInfo) + { + std::stringstream stream; + for(unsigned int i=0; i<_path.size(); ++i) + { + stream << _path[i].first; + if(i+1 < _path.size()) + { + stream << " "; + } + } + UINFO("Path = [%s]", stream.str().c_str()); + } + if(_goalsSavedInUserData) + { + // set goal to latest signature + std::string goalStr = uFormat("GOAL:%d", targetNode); + setUserData(0, uStr2Bytes(goalStr)); + } updateGoalIndex(); } - UINFO("Time computing path (A*) = %fs", timer.ticks()); - UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path)); return _path.size()>0; } -bool Rtabmap::computePath(const Transform & targetPose, bool global) +bool Rtabmap::computePath(const Transform & targetPose) { - UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0); + UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str()); this->clearPath(); std::list > pathPoses; @@ -3120,14 +3071,19 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global) //Find the nearest node UTimer timer; - std::map nodes; - std::multimap constraints; - std::map mapIds; - std::map stamps; - std::map labels; - std::map > userDatas; - this->getGraph(nodes, constraints, true, global); - UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks()); + std::map nodes = _optimizedPoses; + std::multimap links; + for(std::map::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter) + { + const Signature * s = _memory->getSignature(iter->first); + UASSERT(s); + for(std::map::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter) + { + links.insert(std::make_pair(jter->second.from(), jter->second.to())); + links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> + } + } + UINFO("Time getting links = %fs", timer.ticks()); int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks()); @@ -3140,15 +3096,72 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global) } else { - if(computePath(nearestId, nodes, constraints)) + int currentNode = 0; + if(_memory->isIncremental()) { - UASSERT(_path.size() > 0); + if(!_memory->getLastWorkingSignature()) + { + UWARN("Working memory is empty... cannot compute a path"); + return false; + } + currentNode = _memory->getLastWorkingSignature()->id(); + } + else + { + if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0) + { + UWARN("Last localization pose is null... cannot compute a path"); + return false; + } + currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); + } + + // Add links between neighbor nodes in the goal radius. + if(_planVirtualLinks) + { + std::multimap clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI); + for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) + { + if(graph::findLink(links, iter->first, iter->second) == links.end()) + { + links.insert(*iter); + links.insert(std::make_pair(iter->second, iter->first)); // <-> + } + } + } + + UINFO("Computing path from location %d to %d", currentNode, nearestId); + UTimer timer; + _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId)); + UINFO("A* time = %fs", timer.ticks()); + + if(_path.size() == 0) + { + _path.clear(); + UWARN("Cannot compute a path!"); + } + else + { + UINFO("Path generated! Size=%d", (int)_path.size()); + if(ULogger::level() == ULogger::kInfo) + { + std::stringstream stream; + for(unsigned int i=0; i<_path.size(); ++i) + { + stream << _path[i].first; + if(i+1 < _path.size()) + { + stream << " "; + } + } + UINFO("Path = [%s]", stream.str().c_str()); + } + UASSERT(uContains(nodes, _path.back().first)); _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose; updateGoalIndex(); } - UINFO("Time computing path = %fs", timer.ticks()); } } else From 87063cf3578dbcffedbdc392936e8b9c6ab4dff5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jun 2015 20:16:56 -0400 Subject: [PATCH 13/45] Added "Cancel goal" action --- corelib/include/rtabmap/core/RtabmapEvent.h | 3 ++- corelib/include/rtabmap/core/RtabmapThread.h | 3 ++- corelib/src/RtabmapThread.cpp | 8 ++++++++ guilib/include/rtabmap/gui/MainWindow.h | 1 + guilib/src/MainWindow.cpp | 7 +++++++ guilib/src/ui/mainWindow.ui | 8 +++++++- 6 files changed, 27 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index 865a2c6d..9cd7075f 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -76,7 +76,8 @@ public: kCmdPublishTOROGraphLocal, // params: optimized kCmdTriggerNewMap, kCmdPause, - kCmdGoal}; // params: label or location ID + kCmdGoal, // params: label or location ID + kCmdCancelGoal}; public: RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) : UEvent(0), diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index 5ce197c1..cc364091 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -74,7 +74,8 @@ public: kStatePublishingTOROGraphGlobal, kStateTriggeringMap, kStateAddingUserData, - kStateSettingGoal + kStateSettingGoal, + kStateCancellingGoal }; public: diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index b16b1ecb..ad2dd815 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -286,6 +286,9 @@ void RtabmapThread::mainLoop() } this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath())); break; + case kStateCancellingGoal: + _rtabmap->clearPath(); + break; default: UFATAL("Invalid state !?!?"); break; @@ -492,6 +495,11 @@ void RtabmapThread::handleEvent(UEvent* event) param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStateSettingGoal, param); } + else if(cmd == RtabmapEventCmd::kCmdCancelGoal) + { + ULOGGER_DEBUG("CMD_CANCEL_GOAL"); + pushNewState(kStateCancellingGoal); + } else { UWARN("Cmd %d unknown!", cmd); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index bcd93ae9..6caa5239 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -156,6 +156,7 @@ private slots: void dumpTheMemory(); void dumpThePrediction(); void sendGoal(); + void cancelGoal(); void downloadAllClouds(); void downloadPoseGraph(); void clearTheCache(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 9025b9f6..294bffaf 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -294,6 +294,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory())); connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction())); connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal())); + connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal())); connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache())); connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec())); connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds())); @@ -3870,6 +3871,12 @@ void MainWindow::sendGoal() } } +void MainWindow::cancelGoal() +{ + UINFO("Cancelling goal..."); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal)); +} + void MainWindow::downloadAllClouds() { QStringList items; diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 3b84d7a0..971327f6 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 22 + 25 @@ -64,6 +64,7 @@ + @@ -1209,6 +1210,11 @@ Export poses (*.txt)... + + + Cancel goal + + From 785d2e45dd6ae7949f638faed3bab36dbd0ff955 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jun 2015 20:40:26 -0400 Subject: [PATCH 14/45] added refresh icon and added "Donload all clouds" action to toolbar --- guilib/src/GuiLib.qrc | 1 + guilib/src/images/view-refresh.png | Bin 0 -> 2024 bytes guilib/src/ui/mainWindow.ui | 5 +++++ 3 files changed, 6 insertions(+) create mode 100644 guilib/src/images/view-refresh.png diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 4288d1ab..307cc2f2 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -22,6 +22,7 @@ images/document-new.png images/document-save.png images/document-properties.png + images/view-refresh.png images/system-log-out.png images/kinect_xbox_360.png images/kinect_xbox_one.png diff --git a/guilib/src/images/view-refresh.png b/guilib/src/images/view-refresh.png new file mode 100644 index 0000000000000000000000000000000000000000..606ea9eba46b82eea04678e64369b97e595f9da5 GIT binary patch literal 2024 zcmVP)3&2703Vg-nY!=84tGEaSdr|grO`3Ts7naC?zFPi$WDz z4_xq^mdg&AI3QJ9o}K?+x&OHZ~Kd)lWAr^ZC{9C>)PF4plg=Tf{kY z7+97I)^5AV9LFGb0B@UKShkwLDQn&w;rtP8XvFvna0iFc(3KaQ}=Z5 z3$U)f>AQYi`NQ_FRRxzWE@ec3Bmzl-O9Ux%eguFsK;aCjjE&YCefH_|owh@6lOwUW zW&e&L09gCI#)lL|oU~mxFfG8khNf)+T|M&rw`&5$VIO2(kU$`r192pQh#)0F5+ErD zqN^OPBz$z~wtc##({O+WCh58m{9ju)RfXO-c?DhoWxV+MU4Jg&j2~)vqB<}u;)6>B z!*UQyWMNni6d_O))DQ{jAOdVx!g3@?5^PB@Z5M(wJiNL>t*j_h|8nxmfp{_-cw*Bs zS~xd6A=wE46j1BeFQ2c^D=Wg)_5phT<6Cktk>QMi5mC;END9vE#q*=)s-^Qol@%r2 zr!u5X7a7w=GGif?Ng=2!*s*0*AZysrbd|Z1rU%dk-&0eyz<22Fi|(zv2{@93;M}w< zr<=IDqhDi~l~jJvLd*ta!F5>Ua6G zg0r7|W#uB@%F21lVA4c9V?#|9HZ%- zNG7>qInaE*;gH|=K|t3(a2)3*kvri`PiC!$YI^|sSW&p8C<@}E7Ki{MfH8oxiHB}X z4JbBTM*y>wH-10t*MD65*eb0*X&{ldAd%}8g&mj@ zAnV93Rd9@EvLE($JMFe2Mw8iu>ALplbmBD)Oq@kk=ru7s4iLh9&EV&94LDcf?Z zrxR~(?(l8gxY#Jn+l#GlGU9)QCviU6LP>e;Uh-Yx%&6c#| zI-4&Z-1+&G=@KzFs+Kpm4b1(Y7`Z^MiA@PG-e!d~sDj~-$IeN^cIyh=a4P%7FTt+g z%|0w3t4>|*f1tU!Sy5GOYwlZP8URnBG6s|sX~<-40K^4&ZS|g`rPBd)MlOZpY4@uD zU#4)XYKza=NF;YK23SIjObOt9W)J~4dq*HS#}9Wtgbi!Tw+W8+b@fM{uc>RC*xd7$ zmiD8n!m2o@DCB#N7#72N6 zi^|kl(IWNor@b$Ab`Sk(eZ$ccwj*ED44nU@>8Vupi|B5b%*N;8PQc)CI5Bq{r}gKiZ6}LCzg(nWON}EopA+}e|Ag9K_IBR(P&uF1Ag4;>KVzJ<_|8uvZHaz9Uua3-bu*A zZ?*6T%S*CbHazH81P5aT0+2Bsqzwl`FhrwaF}o}hCJBtB4Wtblj!WR2F|KHENh*#D zq>^b^hB0c!)ni_*@c;|}ZuKV6`1jV4;oIjQwNn+FI(lM1U$b;xA@0`|FoyhlAwV*Bl_c|d2W4q#)E^A^n5L-!V{jy3nl{pgg=8}ACS!@LZD#({eetcI_Fs9Y1AsMdn9P&C z20;IE?aO!g273O|R6vDuCnE)-KCMgh10JsC(r_+GD_?o~_V zg$oee1K<@e0C*w1!cP9)1e?*j-Xv?hqaf~unD`ImKK5UPJ;I1>GIMqS0000 false + @@ -988,6 +989,10 @@ + + + :/images/view-refresh.png:/images/view-refresh.png + Download all clouds (update cache) From 290df19cc62a806376e3603f17c6519532303ec3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Jun 2015 20:47:41 -0400 Subject: [PATCH 15/45] Added "Window->Default views" action --- guilib/include/rtabmap/gui/MainWindow.h | 1 + guilib/src/MainWindow.cpp | 18 ++++++++++++++++++ guilib/src/ui/mainWindow.ui | 6 ++++++ 3 files changed, 25 insertions(+) diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 6caa5239..a9144251 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -161,6 +161,7 @@ private slots: void downloadPoseGraph(); void clearTheCache(); void openPreferences(); + void setDefaultViews(); void selectScreenCaptureFormat(bool checked); void takeScreenshot(); void updateElapsedTime(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 294bffaf..43d17a2f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -306,6 +306,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph())); connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu())); + connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews())); connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool))); connect(_ui->actionScreenshot, SIGNAL(triggered()), this, SLOT(takeScreenshot())); connect(_ui->action16_9, SIGNAL(triggered()), this, SLOT(setAspectRatio16_9())); @@ -4093,6 +4094,23 @@ void MainWindow::openPreferences() _preferencesDialog->exec(); } +void MainWindow::setDefaultViews() +{ + _ui->dockWidget_posterior->setVisible(false); + _ui->dockWidget_likelihood->setVisible(false); + _ui->dockWidget_rawlikelihood->setVisible(false); + _ui->dockWidget_statsV2->setVisible(false); + _ui->dockWidget_console->setVisible(false); + _ui->dockWidget_loopClosureViewer->setVisible(false); + _ui->dockWidget_mapVisibility->setVisible(false); + _ui->dockWidget_graphViewer->setVisible(false); + _ui->dockWidget_odometry->setVisible(true); + _ui->dockWidget_cloudViewer->setVisible(true); + _ui->dockWidget_imageView->setVisible(true); + _ui->toolBar->setVisible(_state != kMonitoring && _state != kMonitoringPaused); + _ui->toolBar_2->setVisible(true); +} + void MainWindow::selectScreenCaptureFormat(bool checked) { if(checked) diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index dbb27337..5e2a71e9 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -212,6 +212,7 @@ + @@ -1220,6 +1221,11 @@ Cancel goal + + + Default views + + From 91a4506956d4527178e9353ff38cf2628d60b57c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 22 Jun 2015 13:55:15 -0400 Subject: [PATCH 16/45] Small refactoring of the Bayes filter --- corelib/src/BayesFilter.cpp | 8 +++----- corelib/src/BayesFilter.h | 2 -- corelib/src/Rtabmap.cpp | 5 ++--- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index 56e9485b..e80f142f 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -38,7 +38,6 @@ namespace rtabmap { BayesFilter::BayesFilter(const ParametersMap & parameters) : _virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()), _fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()), - _badSignaturesIgnored(Parameters::defaultRtabmapCreateIntermediateNodes()), _totalPredictionLCValues(0.0f) { this->setPredictionLC(Parameters::defaultBayesPredictionLC()); @@ -57,7 +56,6 @@ void BayesFilter::parseParameters(const ParametersMap & parameters) } Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior); Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate); - Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _badSignaturesIgnored); UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f); } @@ -262,7 +260,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector // Set high values (gaussians curves) to loop closure neighbors // ADD prob for each neighbors - std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true); std::list idsLoopMargin; //filter neighbors in STM for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end();) @@ -476,7 +474,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, } if(i neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true); float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap); this->normalize(prediction, i, sum, newIds[0]<0); ++added; @@ -496,7 +494,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, int modified = 0; for(std::set::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored); + std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); int index = newIdToIndexMap.at(*iter); float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); this->normalize(prediction, index, sum, newIds[0]<0); diff --git a/corelib/src/BayesFilter.h b/corelib/src/BayesFilter.h index 4d00cd76..24d85a88 100644 --- a/corelib/src/BayesFilter.h +++ b/corelib/src/BayesFilter.h @@ -58,7 +58,6 @@ public: float getVirtualPlacePrior() const {return _virtualPlacePrior;} const std::vector & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...} std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...} - bool isBadSignaturesIgnored() const {return _badSignaturesIgnored;} cv::Mat generatePrediction(const Memory * memory, const std::vector & ids) const; @@ -80,7 +79,6 @@ private: float _virtualPlacePrior; std::vector _predictionLC; // {Vp, Lc, l1, l2, l3, l4...} bool _fullPredictionUpdate; - bool _badSignaturesIgnored; float _totalPredictionLCValues; }; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 9b3cda7d..fbab3e72 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1116,7 +1116,6 @@ bool Rtabmap::process( //============================================================ ULOGGER_INFO("computing likelihood..."); - // select only not empty signatures (may happen often if intermediate nodes are created) std::list signaturesToCompare; for(std::map::const_iterator iter=_memory->getWorkingMem().begin(); iter!=_memory->getWorkingMem().end(); @@ -1126,7 +1125,7 @@ bool Rtabmap::process( { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); - if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + if(s->getWeight() != -1) // ignore intermediate nodes { signaturesToCompare.push_back(iter->first); } @@ -2780,7 +2779,7 @@ void Rtabmap::dumpPrediction() const { const Signature * s = _memory->getSignature(iter->first); UASSERT(s!=0); - if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature()) + if(s->getWeight() != -1) // ignore intermediate nodes { signaturesToCompare.push_back(iter->first); } From cdb59371d7d3e415a5335a7624913a537b3e847f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 22 Jun 2015 14:55:32 -0400 Subject: [PATCH 17/45] Added debug info for time required to get links from database when planning --- corelib/src/Graph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index a3b0bcf7..9a463cb7 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1278,9 +1278,9 @@ std::list > computePath( if(lookInDatabase) { // Faster to load all links in one query - //UTimer t; + UTimer t; allLinks = memory->getAllLinks(lookInDatabase); - //UWARN("getting all %d links time = %f s", (int)allLinks.size(), t.ticks()); + UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks()); } //dijkstra From 817906d60897823ef9d8185fc45c824803a44bfc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Jun 2015 20:20:27 -0400 Subject: [PATCH 18/45] DBViewer: added stereo images extraction --- corelib/include/rtabmap/core/CameraModel.h | 4 +- corelib/src/CameraModel.cpp | 4 +- guilib/include/rtabmap/gui/DatabaseViewer.h | 1 + guilib/src/DatabaseViewer.cpp | 65 +++++++++++++++++++-- 4 files changed, 64 insertions(+), 10 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 5c536b36..1f1c8da1 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -87,7 +87,7 @@ public: int imageWeight() const {return imageSize_.height;} bool load(const std::string & filePath); - bool save(const std::string & filePath); + bool save(const std::string & filePath) const; void scale(double scale); @@ -146,7 +146,7 @@ public: const std::string & name() const {return name_;} bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true); - bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true); + bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true) const; double baseline() const {return -right_.Tx()/right_.fx();} diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 65a60e97..8063ef67 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -170,7 +170,7 @@ bool CameraModel::load(const std::string & filePath) return false; } -bool CameraModel::save(const std::string & filePath) +bool CameraModel::save(const std::string & filePath) const { if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty()) { @@ -368,7 +368,7 @@ bool StereoCameraModel::load(const std::string & directory, const std::string & } return false; } -bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) +bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) const { if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml")) { diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 0509d474..b3aac23e 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -156,6 +156,7 @@ private: QList loopLinks_; rtabmap::Memory * memory_; QString pathDatabase_; + std::string databaseFileName_; std::list > graphes_; std::multimap graphLinks_; std::map poses_; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index c4dec3b7..d4aa1a48 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "rtabmap/core/Memory.h" #include "rtabmap/core/DBDriver.h" #include "rtabmap/gui/KeypointItem.h" @@ -557,6 +558,7 @@ bool DatabaseViewer::openDatabase(const QString & path) localMaps_.clear(); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); ui_->checkBox_showOptimized->setEnabled(false); + databaseFileName_.clear(); } std::string driverType = "sqlite3"; @@ -576,6 +578,7 @@ bool DatabaseViewer::openDatabase(const QString & path) else { pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str(); + databaseFileName_ = UFile::getName(path.toStdString()); updateIds(); return true; } @@ -880,15 +883,65 @@ void DatabaseViewer::extractImages() QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath()); if(!path.isNull()) { - for(int i=0; igetNodeData(id, true); + if(!data.imageRaw().empty() && !data.rightRaw().empty()) + { + QDir dir; + dir.mkdir(QString("%1/left").arg(path)); + dir.mkdir(QString("%1/right").arg(path)); + if(databaseFileName_.empty()) + { + UERROR("Cannot save calibration file, database name is empty!"); + } + else + { + std::string cameraName = uSplit(databaseFileName_, '.').front(); + StereoCameraModel model( + cameraName, + data.imageRaw().size(), + data.stereoCameraModel().left().K(), + data.stereoCameraModel().left().D(), + data.stereoCameraModel().left().R(), + data.stereoCameraModel().left().P(), + data.rightRaw().size(), + data.stereoCameraModel().right().K(), + data.stereoCameraModel().right().D(), + data.stereoCameraModel().right().R(), + data.stereoCameraModel().right().P(), + data.stereoCameraModel().R(), + data.stereoCameraModel().T(), + data.stereoCameraModel().E(), + data.stereoCameraModel().F(), + data.stereoCameraModel().left().localTransform()); + if(model.save(path.toStdString(), cameraName)) + { + UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str()); + } + else + { + UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str()); + } + } + } + } + + for(int i=0; igetImageCompressed(id); - if(!compressedRgb.empty()) + SensorData data = memory_->getNodeData(id, true); + if(!data.imageRaw().empty() && !data.rightRaw().empty()) { - cv::Mat imageMat = rtabmap::uncompressImage(compressedRgb); - cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat); - UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str()); + cv::imwrite(QString("%1/left/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw()); + cv::imwrite(QString("%1/right/%2.jpg").arg(path).arg(id).toStdString(), data.rightRaw()); + UINFO(QString("Saved left/%1.jpg and right/%1.jpg").arg(id).toStdString().c_str()); + } + else if(!data.imageRaw().empty()) + { + cv::imwrite(QString("%1/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw()); + UINFO(QString("Saved %1.jpg").arg(id).toStdString().c_str()); } } } From ad23421c9b80afa2e46d46e771aeff488df47834 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Jun 2015 20:32:46 -0400 Subject: [PATCH 19/45] Removed odometry warning when some frames are ignored (when camera rate is faster than odometry) --- corelib/src/OdometryThread.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index f74dd9be..7f3383f2 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -128,7 +128,7 @@ void OdometryThread::addData(const SensorData & data) _dataBuffer.push_back(data); while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) { - ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one."); + UDEBUG("Data buffer is full, the oldest data is removed to add the new one."); _dataBuffer.pop_front(); notify = false; } From 01f2f1348c94ae2f200f384614dadfe16f340477 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 25 Jun 2015 16:04:42 -0400 Subject: [PATCH 20/45] Added OdomBow/FixedLocalMapPath parameter --- corelib/include/rtabmap/core/Memory.h | 2 +- corelib/include/rtabmap/core/Odometry.h | 1 + corelib/include/rtabmap/core/Parameters.h | 1 + corelib/src/Memory.cpp | 8 +- corelib/src/Odometry.cpp | 5 +- corelib/src/OdometryBOW.cpp | 106 +++++++++++++++--- corelib/src/OdometryThread.cpp | 3 +- corelib/src/Rtabmap.cpp | 47 ++++---- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/MainWindow.cpp | 30 ++++- guilib/src/PreferencesDialog.cpp | 19 ++++ guilib/src/ui/preferencesDialog.ui | 42 +++++-- 12 files changed, 209 insertions(+), 56 deletions(-) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index d6595d51..2d79db19 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -83,7 +83,7 @@ public: std::list forget(const std::set & ignoredIds = std::set()); std::set reactivateSignatures(const std::list & ids, unsigned int maxLoaded, double & timeDbAccess); - std::list cleanup(const std::list & ignoredIds = std::list()); + int cleanup(); void emptyTrash(); void joinTrashThread(); bool addLink(const Link & link); diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 4a805652..10f46f5a 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -118,6 +118,7 @@ private: private: //Parameters int _localHistoryMaxSize; + std::string _fixedLocalMapPath; Memory * _memory; std::multimap localMap_; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 253e0a3d..4a82e332 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -339,6 +339,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); RTABMAP_PARAM(OdomBow, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); RTABMAP_PARAM(OdomBow, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio."); + RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.") // Odometry Mono RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step."); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 0f85cd4a..f2a0c3ed 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1487,10 +1487,10 @@ std::list Memory::forget(const std::set & ignoredIds) } -std::list Memory::cleanup(const std::list & ignoredIds) +int Memory::cleanup() { UDEBUG(""); - std::list signaturesRemoved; + int signatureRemoved = 0; // bad signature if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory)) @@ -1499,11 +1499,11 @@ std::list Memory::cleanup(const std::list & ignoredIds) { UDEBUG("Bad signature! %d", _lastSignature->id()); } - signaturesRemoved.push_back(_lastSignature->id()); + signatureRemoved = _lastSignature->id(); moveToTrash(_lastSignature, _incrementalMemory); } - return signaturesRemoved; + return signatureRemoved; } void Memory::emptyTrash() diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index f55d11d6..54db7178 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -162,9 +162,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) } UASSERT(!data.imageRaw().empty()); - if(dynamic_cast(this) == 0) + if(dynamic_cast(this) == 0 && dynamic_cast(this) == 0) { - UASSERT(!data.depthOrRightRaw().empty()); + UERROR("Depth or stereo images required with the odometry selected!"); + return Transform(); } if(!data.stereoCameraModel().isValid() && diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 7235f08e..27d856e6 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_correspondences.h" +#include "rtabmap/core/Graph.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" @@ -49,9 +50,12 @@ namespace rtabmap { OdometryBOW::OdometryBOW(const ParametersMap & parameters) : Odometry(parameters), _localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()), + _fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()), _memory(0) { + UDEBUG(""); Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize); + Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath); ParametersMap customParameters; customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth()))); @@ -101,10 +105,71 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) : } } - _memory = new Memory(customParameters); - if(!_memory->init("", false, ParametersMap())) + if(_fixedLocalMapPath.empty()) { - UERROR("Error initializing the memory for BOW Odometry."); + _memory = new Memory(customParameters); + if(!_memory->init("", false, ParametersMap())) + { + UERROR("Error initializing the memory for BOW Odometry."); + } + } + else + { + UINFO("Init odometry from a fixed database: \"%s\"", _fixedLocalMapPath.c_str()); + // init the local map with a all 3D features contained in the database + customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false")); + customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); + _memory = new Memory(customParameters); + if(!_memory->init(_fixedLocalMapPath, false, ParametersMap())) + { + UERROR("Error initializing the memory for BOW Odometry."); + } + else + { + // get the graph + std::map ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, -1); + std::map poses; + std::multimap links; + _memory->getMetricConstraints(uKeysSet(ids), poses, links, true); + + if(poses.size()) + { + //optimize the graph + graph::TOROOptimizer optimizer; + std::map optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links); + + // fill the local map + for(std::map::iterator posesIter=optimizedPoses.begin(); + posesIter!=optimizedPoses.end(); + ++posesIter) + { + const Signature * s = _memory->getSignature(posesIter->first); + if(s) + { + // Transform 3D points accordingly to pose and add them to local map + const std::multimap & words3D = s->getWords3(); + for(std::multimap::const_iterator pointsIter=words3D.begin(); + pointsIter!=words3D.end(); + ++pointsIter) + { + if(!uContains(localMap_, pointsIter->first)) + { + localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second))); + } + } + } + } + } + else + { + UERROR("No pose loaded from database \"%s\"", _fixedLocalMapPath.c_str()); + } + } + if((int)localMap_.size() < this->getMinInliers() || localMap_.size() == 0) + { + UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!", + _fixedLocalMapPath.c_str(), (int)localMap_.size()); + } } } @@ -117,9 +182,16 @@ OdometryBOW::~OdometryBOW() void OdometryBOW::reset(const Transform & initialPose) { - Odometry::reset(initialPose); - _memory->init("", false, ParametersMap()); - localMap_.clear(); + if(_fixedLocalMapPath.empty()) + { + Odometry::reset(initialPose); + _memory->init("", false, ParametersMap()); + localMap_.clear(); + } + else + { + UWARN("Odometry cannot be reset when a fixed local map is set."); + } } // return not null transform if odometry is correctly computed @@ -140,7 +212,6 @@ Transform OdometryBOW::computeTransform( int correspondences = 0; int nFeatures = 0; - const Signature * previousSignature = _memory->getLastWorkingSignature(); if(_memory->update(data)) { const Signature * newSignature = _memory->getLastWorkingSignature(); @@ -153,7 +224,7 @@ Transform OdometryBOW::computeTransform( } } - if(previousSignature && newSignature) + if(localMap_.size() && newSignature) { Transform transform; if((int)localMap_.size() >= this->getMinInliers()) @@ -257,6 +328,10 @@ Transform OdometryBOW::computeTransform( double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; variance = 2.1981 * median_error_sqr; } + else + { + variance = 1; + } } else { @@ -364,9 +439,10 @@ Transform OdometryBOW::computeTransform( { _memory->deleteLocation(newSignature->id()); } - else + else if(_fixedLocalMapPath.empty()) { output = transform; + // remove words if history max size is reached while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1) { @@ -410,14 +486,18 @@ Transform OdometryBOW::computeTransform( } } } + else + { + // fixed local map, just delete the new signature + output = transform; + _memory->deleteLocation(newSignature->id()); + } } - else if(!previousSignature && newSignature) + else if(newSignature) { - localMap_.clear(); - int count = 0; std::list uniques = uUniqueKeys(newSignature->getWords3()); - if((int)uniques.size() >= this->getMinInliers()) + if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers()) { output.setIdentity(); diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index f74dd9be..812431de 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -105,7 +105,7 @@ void OdometryThread::mainLoop() void OdometryThread::addData(const SensorData & data) { - if(dynamic_cast(_odometry) == 0) + if(dynamic_cast(_odometry) == 0 && dynamic_cast(_odometry) == 0) { if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) { @@ -115,6 +115,7 @@ void OdometryThread::addData(const SensorData & data) } else { + // Mono and BOW can accept RGB only if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid())) { ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?"); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index fbab3e72..3e68668d 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2141,32 +2141,39 @@ bool Rtabmap::process( lastSignatureData = *signature; } - //By default, remove all signatures with a loop closure link if they are not in reactivateIds - //This will also remove rehearsed signatures - std::list signaturesRemoved = _memory->cleanup(); + // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored) + std::list signaturesRemoved; + int signatureRemoved = _memory->cleanup(); + if(signatureRemoved) + { + signaturesRemoved.push_back(signatureRemoved); + } // If this option activated, add new nodes only if there are linked with a previous map. // Used when rtabmap is first started, it will wait a // global loop closure detection before starting the new map, // otherwise it deletes the current node. - if(_startNewMapOnLoopClosure && - _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 + if(signatureRemoved != lastSignatureData.id()) { - UWARN("Ignoring location %d because a global loop closure is required before starting a new map!", - signature->id()); - signaturesRemoved.push_back(signature->id()); - _memory->deleteLocation(signature->id()); - } - else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0) - { - // Don't delete the location if a loop closure is detected - UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)", - signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate); - // If there is a too small displacement, remove the node - signaturesRemoved.push_back(signature->id()); - _memory->deleteLocation(signature->id()); + if(_startNewMapOnLoopClosure && + _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 + { + UWARN("Ignoring location %d because a global loop closure is required before starting a new map!", + signature->id()); + signaturesRemoved.push_back(signature->id()); + _memory->deleteLocation(signature->id()); + } + else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0) + { + // Don't delete the location if a loop closure is detected + UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)", + signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate); + // If there is a too small displacement, remove the node + signaturesRemoved.push_back(signature->id()); + _memory->deleteLocation(signature->id()); + } } // Pass this point signature should not be used, since it could have been transferred... diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 895efd77..18960b49 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -246,6 +246,7 @@ private slots: void updateKpROI(); void changeWorkingDirectory(); void changeDictionaryPath(); + void changeOdomBowFixedLocalMapPath(); void readSettingsEnd(); void setupTreeView(); void updateBasicParameter(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 43d17a2f..de17f657 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1738,14 +1738,36 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int pcl::PointCloud::Ptr cloud(new pcl::PointCloud); cloud->resize(iter->getWords3().size()); int oi=0; - for(std::multimap::const_iterator jter=iter->getWords3().begin(); jter!=iter->getWords3().end(); ++jter) + UASSERT(iter->getWords().size() == iter->getWords3().size()); + std::multimap::const_iterator kter=iter->getWords().begin(); + for(std::multimap::const_iterator jter=iter->getWords3().begin(); + jter!=iter->getWords3().end(); ++jter, ++kter, ++oi) { (*cloud)[oi].x = jter->second.x; (*cloud)[oi].y = jter->second.y; (*cloud)[oi].z = jter->second.z; - (*cloud)[oi].r = 255; - (*cloud)[oi].g = 255; - (*cloud)[oi++].b = 255; + int u = kter->second.pt.x+0.5; + int v = kter->second.pt.x+0.5; + if(!iter->sensorData().imageRaw().empty() && + uIsInBounds(u, 0, iter->sensorData().imageRaw().cols-1) && + uIsInBounds(v, 0, iter->sensorData().imageRaw().rows-1)) + { + if(iter->sensorData().imageRaw().channels() == 1) + { + (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = iter->sensorData().imageRaw().at(u, v); + } + else + { + cv::Vec3b bgr = iter->sensorData().imageRaw().at(u, v); + (*cloud)[oi].r = bgr.val[0]; + (*cloud)[oi].g = bgr.val[1]; + (*cloud)[oi].b = bgr.val[2]; + } + } + else + { + (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255; + } } if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color)) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index bf1e08ec..80f44e78 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -597,6 +597,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str()); _ui->odom_bin_nn->setObjectName(Parameters::kOdomBowNNType().c_str()); _ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomBowNNDR().c_str()); + _ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomBowFixedLocalMapPath().c_str()); + connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath())); //Odometry Optical Flow _ui->odom_flow_winSize->setObjectName(Parameters::kOdomFlowWinSize().c_str()); @@ -2927,6 +2929,23 @@ void PreferencesDialog::changeDictionaryPath() } } +void PreferencesDialog::changeOdomBowFixedLocalMapPath() +{ + QString path; + if(_ui->odom_fixedLocalMapPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Database"), this->getWorkingDirectory(), tr("RTAB-Map database files (*.db)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Database"), _ui->odom_fixedLocalMapPath->text(), tr("RTAB-Map database files (*.db)")); + } + if(!path.isEmpty()) + { + _ui->odom_fixedLocalMapPath->setText(path); + } +} + void PreferencesDialog::updateRGBDCameraGroupBoxVisibility() { _ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f199e7e7..1b1efee4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -837 - 755 - 1591 + 0 + 760 + 1570 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 24 @@ -7387,8 +7387,8 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare BOW - - + + 0 @@ -7404,7 +7404,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving. @@ -7414,7 +7414,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + QComboBox::AdjustToContents @@ -7446,7 +7446,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector. @@ -7456,7 +7456,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 1 @@ -7475,7 +7475,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + NNDR ratio @@ -7487,6 +7487,26 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + + + + Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated. + + + true + + + + + + + ... + + + From 6df403ed4260c75aeb576fe31c97df2d3a8a0ba9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 26 Jun 2015 18:21:32 -0400 Subject: [PATCH 21/45] Refactored Camera classes and Preferences->Source menu --- corelib/include/rtabmap/core/Camera.h | 112 +- corelib/include/rtabmap/core/CameraEvent.h | 7 +- corelib/include/rtabmap/core/CameraRGB.h | 124 ++ corelib/include/rtabmap/core/CameraRGBD.h | 180 +- corelib/include/rtabmap/core/CameraStereo.h | 130 ++ corelib/include/rtabmap/core/CameraThread.h | 10 +- corelib/include/rtabmap/core/SensorData.h | 3 + corelib/src/CMakeLists.txt | 2 + corelib/src/Camera.cpp | 399 +--- corelib/src/CameraRGB.cpp | 328 ++++ corelib/src/CameraRGBD.cpp | 1106 +---------- corelib/src/CameraStereo.cpp | 910 +++++++++ corelib/src/CameraThread.cpp | 114 +- corelib/src/DBReader.cpp | 6 +- corelib/src/OdometryThread.cpp | 2 +- corelib/src/RtabmapThread.cpp | 2 +- examples/BOWMapping/main.cpp | 10 +- examples/RGBDMapping/main.cpp | 9 +- examples/WifiMapping/main.cpp | 14 +- guilib/include/rtabmap/gui/MainWindow.h | 14 +- guilib/include/rtabmap/gui/OdometryViewer.h | 1 + .../include/rtabmap/gui/PreferencesDialog.h | 71 +- guilib/src/AboutDialog.cpp | 1 + guilib/src/CalibrationDialog.cpp | 4 +- guilib/src/CameraViewer.cpp | 30 +- guilib/src/DataRecorder.cpp | 3 +- guilib/src/GuiLib.qrc | 1 + guilib/src/MainWindow.cpp | 214 +- guilib/src/OdometryViewer.cpp | 8 + guilib/src/PreferencesDialog.cpp | 1134 ++++++----- guilib/src/images/webcam.png | Bin 0 -> 11049 bytes guilib/src/ui/mainWindow.ui | 74 +- guilib/src/ui/preferencesDialog.ui | 1731 ++++++++++------- tools/Calibration/main.cpp | 20 +- tools/Camera/main.cpp | 22 +- tools/CameraRGBD/main.cpp | 66 +- tools/ConsoleApp/main.cpp | 61 +- tools/DataRecorder/main.cpp | 5 +- tools/OdometryViewer/main.cpp | 3 +- 39 files changed, 3543 insertions(+), 3388 deletions(-) create mode 100644 corelib/include/rtabmap/core/CameraRGB.h create mode 100644 corelib/include/rtabmap/core/CameraStereo.h create mode 100644 corelib/src/CameraRGB.cpp create mode 100644 corelib/src/CameraStereo.cpp create mode 100644 guilib/src/images/webcam.png diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index 4e1dea32..91defefa 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -50,118 +50,40 @@ class RTABMAP_EXP Camera { public: virtual ~Camera(); - cv::Mat takeImage(); - virtual bool init() = 0; + SensorData takeImage(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0; + virtual bool isCalibrated() const = 0; + virtual std::string getSerial() const = 0; + int getNextSeqID() {return ++_seq;} //getters - void getImageSize(unsigned int & width, unsigned int & height); float getImageRate() const {return _imageRate;} - bool isMirroringEnabled() const {return _mirroring;} + const Transform & getLocalTransform() const {return _localTransform;} //setters void setImageRate(float imageRate) {_imageRate = imageRate;} - void setImageSize(unsigned int width, unsigned int height); - void setMirroringEnabled(bool enabled) {_mirroring = enabled;} - - void setCalibration(const std::string & fileName); - void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients); - void resetCalibration(); + void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} protected: /** * Constructor * - * @param imageRate : image/second , 0 for fast as the camera can + * @param imageRate : image/second , 0 for fast as the camera can */ - Camera(float imageRate = 0, - unsigned int imageWidth = 0, - unsigned int imageHeight = 0); + Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); - virtual cv::Mat captureImage() = 0; + /** + * returned rgb and depth images should be already rectified if calibration was loaded + */ + virtual SensorData captureImage() = 0; private: float _imageRate; - unsigned int _imageWidth; - unsigned int _imageHeight; - bool _mirroring; + Transform _localTransform; + cv::Size _targetImageSize; UTimer * _frameRateTimer; - cv::Mat _k; // camera_matrix - cv::Mat _d; // distorsion_coefficients -}; - - -///////////////////////// -// CameraImages -///////////////////////// -class RTABMAP_EXP CameraImages : - public Camera -{ -public: - CameraImages(const std::string & path, - int startAt = 1, - bool refreshDir = false, - float imageRate = 0, - unsigned int imageWidth = 0, - unsigned int imageHeight = 0); - virtual ~CameraImages(); - - virtual bool init(); - std::string getPath() const {return _path;} - unsigned int imagesCount() const; - -protected: - virtual cv::Mat captureImage(); - -private: - std::string _path; - int _startAt; - // If the list of files in the directory is refreshed - // on each call of takeImage() - bool _refreshDir; - int _count; - UDirectory * _dir; - std::string _lastFileName; -}; - - - - -///////////////////////// -// CameraVideo -///////////////////////// -class RTABMAP_EXP CameraVideo : - public Camera -{ -public: - enum Source{kVideoFile, kUsbDevice}; - -public: - CameraVideo(int usbDevice = 0, - float imageRate = 0, - unsigned int imageWidth = 0, - unsigned int imageHeight = 0); - CameraVideo(const std::string & filePath, - float imageRate = 0, - unsigned int imageWidth = 0, - unsigned int imageHeight = 0); - virtual ~CameraVideo(); - - virtual bool init(); - int getUsbDevice() const {return _usbDevice;} - const std::string & getFilePath() const {return _filePath;} - -protected: - virtual cv::Mat captureImage(); - -private: - // File type - std::string _filePath; - - cv::VideoCapture _capture; - Source _src; - - // Usb camera - int _usbDevice; + int _seq; }; diff --git a/corelib/include/rtabmap/core/CameraEvent.h b/corelib/include/rtabmap/core/CameraEvent.h index b1077db0..470be8ee 100644 --- a/corelib/include/rtabmap/core/CameraEvent.h +++ b/corelib/include/rtabmap/core/CameraEvent.h @@ -38,14 +38,13 @@ class CameraEvent : { public: enum Code { - kCodeImage, - kCodeImageDepth, + kCodeData, kCodeNoMoreImages }; public: CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") : - UEvent(kCodeImage), + UEvent(kCodeData), data_(image, seq, stamp), cameraName_(cameraName) { @@ -57,7 +56,7 @@ public: } CameraEvent(const SensorData & data, const std::string & cameraName = "") : - UEvent(kCodeImageDepth), + UEvent(kCodeData), data_(data), cameraName_(cameraName) { diff --git a/corelib/include/rtabmap/core/CameraRGB.h b/corelib/include/rtabmap/core/CameraRGB.h new file mode 100644 index 00000000..e1bb899a --- /dev/null +++ b/corelib/include/rtabmap/core/CameraRGB.h @@ -0,0 +1,124 @@ +/* +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. +*/ + +#pragma once + +#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines + +#include +#include "rtabmap/core/Camera.h" +#include +#include +#include +#include + +class UDirectory; +class UTimer; + +namespace rtabmap +{ + +///////////////////////// +// CameraImages +///////////////////////// +class RTABMAP_EXP CameraImages : + public Camera +{ +public: + CameraImages(const std::string & path, + int startAt = 1, + bool refreshDir = false, + float imageRate = 0, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraImages(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + std::string getPath() const {return _path;} + unsigned int imagesCount() const; + +protected: + virtual SensorData captureImage(); + +private: + std::string _path; + int _startAt; + // If the list of files in the directory is refreshed + // on each call of takeImage() + bool _refreshDir; + int _count; + UDirectory * _dir; + std::string _lastFileName; +}; + + + + +///////////////////////// +// CameraVideo +///////////////////////// +class RTABMAP_EXP CameraVideo : + public Camera +{ +public: + enum Source{kVideoFile, kUsbDevice}; + +public: + CameraVideo(int usbDevice = 0, + float imageRate = 0, + const Transform & localTransform = Transform::getIdentity()); + CameraVideo(const std::string & filePath, + float imageRate = 0, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraVideo(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + int getUsbDevice() const {return _usbDevice;} + const std::string & getFilePath() const {return _filePath;} + +protected: + virtual SensorData captureImage(); + +private: + // File type + std::string _filePath; + + cv::VideoCapture _capture; + Source _src; + + // Usb camera + int _usbDevice; + std::string _guid; + + CameraModel _model; +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index fd68a216..7bc61257 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -29,24 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines -#include -#include "rtabmap/core/SensorData.h" #include "rtabmap/utilite/UMutex.h" #include "rtabmap/utilite/USemaphore.h" #include "rtabmap/core/CameraModel.h" -#include -#include -#include -#include +#include "rtabmap/core/Camera.h" #include #include #include -class UDirectory; -class UTimer; - namespace openni { class Device; @@ -67,70 +59,17 @@ class Registration; class PacketPipeline; } -namespace FlyCapture2 -{ -class Camera; -} - typedef struct _freenect_context freenect_context; typedef struct _freenect_device freenect_device; namespace rtabmap { -/** - * Class CameraRGBD - * - */ -class RTABMAP_EXP CameraRGBD -{ -public: - virtual ~CameraRGBD(); - void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); - - virtual bool init(const std::string & calibrationFolder = ".") = 0; - virtual bool isCalibrated() const = 0; - virtual std::string getSerial() const = 0; - - //getters - float getImageRate() const {return _imageRate;} - const Transform & getLocalTransform() const {return _localTransform;} - bool isMirroringEnabled() const {return _mirroring;} - bool isColorOnly() const {return _colorOnly;} - - //setters - void setImageRate(float imageRate) {_imageRate = imageRate;} - void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} - void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;} - void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} - -protected: - /** - * Constructor - * - * @param imageRate : image/second , 0 for fast as the camera can - */ - CameraRGBD(float imageRate = 0, - const Transform & localTransform = Transform::getIdentity()); - - /** - * returned rgb and depth images should be already rectified - */ - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) = 0; - -private: - float _imageRate; - Transform _localTransform; - bool _mirroring; - bool _colorOnly; - UTimer * _frameRateTimer; -}; - ///////////////////////// // CameraOpenNIPCL ///////////////////////// class RTABMAP_EXP CameraOpenni : - public CameraRGBD + public Camera { public: static bool available() {return true;} @@ -147,12 +86,12 @@ public: const boost::shared_ptr& depth, float constant); - virtual bool init(const std::string & calibrationFolder = "."); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); + virtual SensorData captureImage(); private: pcl::Grabber* interface_; @@ -169,7 +108,7 @@ private: // CameraOpenNICV ///////////////////////// class RTABMAP_EXP CameraOpenNICV : - public CameraRGBD + public Camera { public: @@ -181,12 +120,12 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~CameraOpenNICV(); - virtual bool init(const std::string & calibrationFolder = "."); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const {return "";} // unknown with OpenCV protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); + virtual SensorData captureImage(); private: bool _asus; @@ -198,7 +137,7 @@ private: // CameraOpenNI2 ///////////////////////// class RTABMAP_EXP CameraOpenNI2 : - public CameraRGBD + public Camera { public: @@ -211,7 +150,7 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~CameraOpenNI2(); - virtual bool init(const std::string & calibrationFolder = "."); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; @@ -222,7 +161,7 @@ public: bool setMirroring(bool enabled); protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); + virtual SensorData captureImage(); private: openni::Device * _device; @@ -240,7 +179,7 @@ private: class FreenectDevice; class RTABMAP_EXP CameraFreenect : - public CameraRGBD + public Camera { public: static bool available(); @@ -252,12 +191,12 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~CameraFreenect(); - virtual bool init(const std::string & calibrationFolder = "."); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); + virtual SensorData captureImage(); private: int deviceId_; @@ -270,7 +209,7 @@ private: ///////////////////////// class RTABMAP_EXP CameraFreenect2 : - public CameraRGBD + public Camera { public: static bool available(); @@ -290,12 +229,12 @@ public: const Transform & localTransform = Transform::getIdentity()); virtual ~CameraFreenect2(); - virtual bool init(const std::string & calibrationFolder = "."); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; protected: - virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp); + virtual SensorData captureImage(); private: int deviceId_; @@ -308,91 +247,4 @@ private: libfreenect2::Registration * reg_; }; -///////////////////////// -// CameraStereoDC1394 -///////////////////////// -class DC1394Device; - -class RTABMAP_EXP CameraStereoDC1394 : - public CameraRGBD -{ -public: - static bool available(); - -public: - CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); - virtual ~CameraStereoDC1394(); - - virtual bool init(const std::string & calibrationFolder = "."); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); - -private: - DC1394Device *device_; - StereoCameraModel stereoModel_; -}; - -///////////////////////// -// CameraStereoFlyCapture2 -///////////////////////// -class RTABMAP_EXP CameraStereoFlyCapture2 : - public CameraRGBD -{ -public: - static bool available(); - -public: - CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); - virtual ~CameraStereoFlyCapture2(); - - virtual bool init(const std::string & calibrationFolder = "."); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); - -private: - FlyCapture2::Camera * camera_; - void * triclopsCtx_; // TriclopsContext -}; - -///////////////////////// -// CameraStereoImages -///////////////////////// -class CameraImages; -class RTABMAP_EXP CameraStereoImages : - public CameraRGBD -{ -public: - static bool available(); - -public: - CameraStereoImages( - const std::string & path, - const std::string & cameraName = "stereo_images", // calibration file name - const std::string & timestampsPath = "", // "times.txt" - float imageRate=0.0f, - const Transform & localTransform = Transform::getIdentity()); - virtual ~CameraStereoImages(); - - virtual bool init(const std::string & calibrationFolder = "."); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp); - -private: - CameraImages * camera_; - CameraImages * camera2_; - std::string cameraName_; - std::string timestampsPath_; - std::list stamps_; - StereoCameraModel stereoModel_; -}; - } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/CameraStereo.h b/corelib/include/rtabmap/core/CameraStereo.h new file mode 100644 index 00000000..ef897f98 --- /dev/null +++ b/corelib/include/rtabmap/core/CameraStereo.h @@ -0,0 +1,130 @@ +/* +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. +*/ + +#pragma once + +#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines + +#include "rtabmap/core/CameraModel.h" +#include "rtabmap/core/Camera.h" +#include + +namespace FlyCapture2 +{ +class Camera; +} + +namespace rtabmap +{ + +///////////////////////// +// CameraStereoDC1394 +///////////////////////// +class DC1394Device; + +class RTABMAP_EXP CameraStereoDC1394 : + public Camera +{ +public: + static bool available(); + +public: + CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoDC1394(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(); + +private: + DC1394Device *device_; + StereoCameraModel stereoModel_; +}; + +///////////////////////// +// CameraStereoFlyCapture2 +///////////////////////// +class RTABMAP_EXP CameraStereoFlyCapture2 : + public Camera +{ +public: + static bool available(); + +public: + CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoFlyCapture2(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(); + +private: + FlyCapture2::Camera * camera_; + void * triclopsCtx_; // TriclopsContext +}; + +///////////////////////// +// CameraStereoImages +///////////////////////// +class CameraImages; +class RTABMAP_EXP CameraStereoImages : + public Camera +{ +public: + static bool available(); + +public: + CameraStereoImages( + const std::string & path, + const std::string & timestampsPath = "", // "times.txt" + float imageRate=0.0f, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoImages(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(); + +private: + CameraImages * camera_; + CameraImages * camera2_; + std::string timestampsPath_; + std::list stamps_; + StereoCameraModel stereoModel_; + std::string cameraName_; +}; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 13df9b99..2768d2f2 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -36,7 +36,6 @@ namespace rtabmap { class Camera; -class CameraRGBD; /** * Class CameraThread @@ -49,10 +48,10 @@ class RTABMAP_EXP CameraThread : public: // ownership transferred CameraThread(Camera * camera); - CameraThread(CameraRGBD * camera); virtual ~CameraThread(); - bool init(); // call camera->init() + void setMirroringEnabled(bool enabled) {_mirroring = enabled;} + void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} //getters bool isPaused() const {return !this->isRunning();} @@ -60,15 +59,14 @@ public: void setImageRate(float imageRate); Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted - CameraRGBD * cameraRGBD() {return _cameraRGBD;} // return null if not set, valid until CameraThread is deleted private: virtual void mainLoop(); private: Camera * _camera; - CameraRGBD * _cameraRGBD; - int _seq; + bool _mirroring; + bool _colorOnly; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 48a1085d..0d3bd123 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -157,6 +157,9 @@ public: void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;} void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;} void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;} + void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);} + void setCameraModels(const std::vector & models) {_cameraModels = models;} + void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;} //for convenience cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();} diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 757a160c..0f431b8e 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -13,7 +13,9 @@ SET(SRC_FILES Camera.cpp CameraThread.cpp + CameraRGB.cpp CameraRGBD.cpp + CameraStereo.cpp CameraModel.cpp EpipolarGeometry.cpp diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index b2b16b6f..9e5eebdc 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -44,14 +44,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -Camera::Camera(float imageRate, - unsigned int imageWidth, - unsigned int imageHeight) : +Camera::Camera(float imageRate, const Transform & localTransform) : _imageRate(imageRate), - _imageWidth(imageWidth), - _imageHeight(imageHeight), - _mirroring(false), - _frameRateTimer(new UTimer()) + _localTransform(localTransform), + _targetImageSize(0,0), + _frameRateTimer(new UTimer()), + _seq(0) { } @@ -63,397 +61,46 @@ Camera::~Camera() } } -void Camera::setImageSize(unsigned int width, unsigned int height) +SensorData Camera::takeImage() { - _imageWidth = width; - _imageHeight = height; -} - -void Camera::getImageSize(unsigned int & width, unsigned int & height) -{ - width = _imageWidth; - height = _imageHeight; -} - -void Camera::setCalibration(const std::string & fileName) -{ - if(UFile::getExtension(fileName).compare("yaml") == 0) + bool warnFrameRateTooHigh = false; + float actualFrameRate = 0; + if(_imageRate>0) { - cv::FileStorage fs; - fs.open(fileName, cv::FileStorage::READ); - - if (!fs.isOpened()) - { - UERROR("Failed to open file \"%s\"", fileName.c_str()); - return; - } - - cv::Mat k,d; - - cv::FileNode n = fs["camera_matrix"]; - int rows = n["rows"]; - int cols = n["cols"]; - std::vector data; - n["data"] >> data; - if(rows > 0 && cols > 0 && (int)data.size() == rows*cols) - { - k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone(); - } - - cv::FileNode nd = fs["distortion_coefficients"]; - rows = nd["rows"]; - cols = nd["cols"]; - data.clear(); - nd["data"] >> data; - if(rows > 0 && cols > 0 && (int)data.size() == rows*cols) - { - d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone(); - } - - if(k.empty()) - { - UERROR("Failed to load \"camera_matrix\" matrix."); - } - if(d.empty()) - { - UERROR("Failed to load \"distortion_coefficients\" matrix."); - } - if(!k.empty() && !d.empty()) - { - this->setCalibration(k, d); - } - } - else - { - UERROR("Calibration file must be in \"*.yaml\" format"); - } -} - -void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients) -{ - UASSERT(cameraMatrix.type() == CV_64FC1 && - cameraMatrix.rows == 3 && - cameraMatrix.cols == 3); - UASSERT(distorsionCoefficients.type() == CV_64FC1 && - distorsionCoefficients.rows ==1 && - (distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8)); - - _k = cameraMatrix; - _d = distorsionCoefficients; -} - -void Camera::resetCalibration() -{ - _k = cv::Mat(); - _d = cv::Mat(); -} - -cv::Mat Camera::takeImage() -{ - cv::Mat img; - float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity - if(imageRate>0) - { - int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); + int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); if(sleepTime > 2) { uSleep(sleepTime-2); } + else if(sleepTime < 0) + { + warnFrameRateTooHigh = true; + actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); + } // Add precision at the cost of a small overhead - while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001) + while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001) { // } double slept = _frameRateTimer->getElapsedTime(); _frameRateTimer->start(); - UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate)); + UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate)); } UTimer timer; - img = this->captureImage(); - if(!img.empty() && !_k.empty() && !_d.empty()) + SensorData data = this->captureImage(); + if(warnFrameRateTooHigh) { - cv::Mat temp = img.clone(); - cv::undistort(temp, img, _k, _d); - } - if(!img.empty() && _mirroring) - { - cv::flip(img,img,1); - } - UDEBUG("Time capturing image = %fs", timer.ticks()); - return img; -} - -///////////////////////// -// CameraImages -///////////////////////// -CameraImages::CameraImages(const std::string & path, - int startAt, - bool refreshDir, - float imageRate, - unsigned int imageWidth, - unsigned int imageHeight) : - Camera(imageRate, imageWidth, imageHeight), - _path(path), - _startAt(startAt), - _refreshDir(refreshDir), - _count(0), - _dir(0) -{ - -} - -CameraImages::~CameraImages(void) -{ - if(_dir) - { - delete _dir; - } -} - -bool CameraImages::init() -{ - UDEBUG(""); - if(_dir) - { - _dir->setPath(_path, "jpg ppm png bmp pnm tiff"); + UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.", + _imageRate, actualFrameRate, timer.ticks()); } else { - _dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff"); + UDEBUG("Time capturing image = %fs", timer.ticks()); } - _count = 0; - if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/') - { - _path.append("/"); - } - if(!_dir->isValid()) - { - ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str()); - } - else if(_dir->getFileNames().size() == 0) - { - UWARN("Directory is empty \"%s\"", _path.c_str()); - } - else - { - UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount()); - } - return _dir->isValid(); -} - -unsigned int CameraImages::imagesCount() const -{ - if(_dir) - { - return _dir->getFileNames().size(); - } - return 0; -} - -cv::Mat CameraImages::captureImage() -{ - cv::Mat img; - UDEBUG(""); - if(_dir->isValid()) - { - if(_refreshDir) - { - _dir->update(); - } - if(_startAt == 0) - { - const std::list & fileNames = _dir->getFileNames(); - if(fileNames.size()) - { - if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0) - { - _lastFileName = *fileNames.rbegin(); - std::string fullPath = _path + _lastFileName; - img = cv::imread(fullPath.c_str()); - } - } - } - else - { - std::string fileName; - std::string fullPath; - fileName = _dir->getNextFileName(); - if(fileName.size()) - { - fullPath = _path + fileName; - while(++_count < _startAt && (fileName = _dir->getNextFileName()).size()) - { - fullPath = _path + fileName; - } - if(fileName.size()) - { - ULOGGER_DEBUG("Loading image : %s", fullPath.c_str()); - -#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) - img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED); -#else - img = cv::imread(fullPath.c_str(), -1); -#endif - UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", - img.cols, img.rows, img.channels(), img.elemSize(), img.total()); - -#if CV_MAJOR_VERSION < 3 - // FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works... - if(img.depth() != CV_8U) - { - // The depth should be 8U - UWARN("Cannot read the image correctly, falling back to old OpenCV C interface..."); - IplImage * i = cvLoadImage(fullPath.c_str()); - img = cv::Mat(i, true); - cvReleaseImage(&i); - } -#endif - - if(img.channels()>3) - { - UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str()); - cv::Mat out; - cv::cvtColor(img, out, CV_BGRA2BGR); - img = out; - } - } - } - } - } - else - { - UWARN("Directory is not set, camera must be initialized."); - } - - unsigned int w; - unsigned int h; - this->getImageSize(w, h); - - if(!img.empty() && - w && - h && - w != (unsigned int)img.cols && - h != (unsigned int)img.rows) - { - cv::Mat resampled; - cv::resize(img, resampled, cv::Size(w, h)); - img = resampled; - } - return img; -} - - - -///////////////////////// -// CameraVideo -///////////////////////// -CameraVideo::CameraVideo(int usbDevice, - float imageRate, - unsigned int imageWidth, - unsigned int imageHeight) : - Camera(imageRate, imageWidth, imageHeight), - _src(kUsbDevice), - _usbDevice(usbDevice) -{ - -} - -CameraVideo::CameraVideo(const std::string & filePath, - float imageRate, - unsigned int imageWidth, - unsigned int imageHeight) : - Camera(imageRate, imageWidth, imageHeight), - _filePath(filePath), - _src(kVideoFile), - _usbDevice(0) -{ -} - -CameraVideo::~CameraVideo() -{ - _capture.release(); -} - -bool CameraVideo::init() -{ - if(_capture.isOpened()) - { - _capture.release(); - } - - if(_src == kUsbDevice) - { - unsigned int w; - unsigned int h; - this->getImageSize(w, h); - - ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h); - _capture.open(_usbDevice); - - if(w && h) - { - _capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w)); - _capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h)); - } - } - else if(_src == kVideoFile) - { - ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str()); - _capture.open(_filePath.c_str()); - } - else - { - ULOGGER_ERROR("Camera: Unknown source..."); - } - if(!_capture.isOpened()) - { - ULOGGER_ERROR("Camera: Failed to create a capture object!"); - _capture.release(); - return false; - } - return true; -} - -cv::Mat CameraVideo::captureImage() -{ - cv::Mat img; - if(_capture.isOpened()) - { - if(_capture.read(img)) - { - unsigned int w; - unsigned int h; - this->getImageSize(w, h); - - if(!img.empty() && - w && - h && - w != (unsigned int)img.cols && - h != (unsigned int)img.rows) - { - cv::Mat resampled; - cv::resize(img, resampled, cv::Size(w, h)); - img = resampled; - } - else - { - // clone required - img = img.clone(); - } - } - else if(_usbDevice) - { - UERROR("Camera has been disconnected!"); - } - } - else - { - ULOGGER_WARN("The camera must be initialized before requesting an image."); - } - return img; + return data; } } // namespace rtabmap diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp new file mode 100644 index 00000000..aa5664cb --- /dev/null +++ b/corelib/src/CameraRGB.cpp @@ -0,0 +1,328 @@ +/* +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. +*/ + +#include "rtabmap/core/CameraRGB.h" +#include "rtabmap/core/DBDriver.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace rtabmap +{ + +///////////////////////// +// CameraImages +///////////////////////// +CameraImages::CameraImages(const std::string & path, + int startAt, + bool refreshDir, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + _path(path), + _startAt(startAt), + _refreshDir(refreshDir), + _count(0), + _dir(0) +{ + +} + +CameraImages::~CameraImages(void) +{ + if(_dir) + { + delete _dir; + } +} + +bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName) +{ + UDEBUG(""); + if(_dir) + { + _dir->setPath(_path, "jpg ppm png bmp pnm tiff"); + } + else + { + _dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff"); + } + _count = 0; + if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/') + { + _path.append("/"); + } + if(!_dir->isValid()) + { + ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str()); + } + else if(_dir->getFileNames().size() == 0) + { + UWARN("Directory is empty \"%s\"", _path.c_str()); + } + else + { + UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount()); + } + return _dir->isValid(); +} + +bool CameraImages::isCalibrated() const +{ + return false; +} + +std::string CameraImages::getSerial() const +{ + return ""; +} + +unsigned int CameraImages::imagesCount() const +{ + if(_dir) + { + return _dir->getFileNames().size(); + } + return 0; +} + +SensorData CameraImages::captureImage() +{ + cv::Mat img; + UDEBUG(""); + if(_dir->isValid()) + { + if(_refreshDir) + { + _dir->update(); + } + if(_startAt == 0) + { + const std::list & fileNames = _dir->getFileNames(); + if(fileNames.size()) + { + if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0) + { + _lastFileName = *fileNames.rbegin(); + std::string fullPath = _path + _lastFileName; + img = cv::imread(fullPath.c_str()); + } + } + } + else + { + std::string fileName; + std::string fullPath; + fileName = _dir->getNextFileName(); + if(fileName.size()) + { + fullPath = _path + fileName; + while(++_count < _startAt && (fileName = _dir->getNextFileName()).size()) + { + fullPath = _path + fileName; + } + if(fileName.size()) + { + ULOGGER_DEBUG("Loading image : %s", fullPath.c_str()); + +#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4) + img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED); +#else + img = cv::imread(fullPath.c_str(), -1); +#endif + UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", + img.cols, img.rows, img.channels(), img.elemSize(), img.total()); + +#if CV_MAJOR_VERSION < 3 + // FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works... + if(img.depth() != CV_8U) + { + // The depth should be 8U + UWARN("Cannot read the image correctly, falling back to old OpenCV C interface..."); + IplImage * i = cvLoadImage(fullPath.c_str()); + img = cv::Mat(i, true); + cvReleaseImage(&i); + } +#endif + + if(img.channels()>3) + { + UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str()); + cv::Mat out; + cv::cvtColor(img, out, CV_BGRA2BGR); + img = out; + } + } + } + } + } + else + { + UWARN("Directory is not set, camera must be initialized."); + } + + return SensorData(img); +} + + + +///////////////////////// +// CameraVideo +///////////////////////// +CameraVideo::CameraVideo(int usbDevice, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + _src(kUsbDevice), + _usbDevice(usbDevice) +{ + +} + +CameraVideo::CameraVideo(const std::string & filePath, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + _filePath(filePath), + _src(kVideoFile), + _usbDevice(0) +{ +} + +CameraVideo::~CameraVideo() +{ + _capture.release(); +} + +bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName) +{ + _guid.clear(); + if(_capture.isOpened()) + { + _capture.release(); + } + + if(_src == kUsbDevice) + { + ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice); + _capture.open(_usbDevice); + } + else if(_src == kVideoFile) + { + ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str()); + _capture.open(_filePath.c_str()); + } + else + { + ULOGGER_ERROR("Camera: Unknown source..."); + } + if(!_capture.isOpened()) + { + ULOGGER_ERROR("Camera: Failed to create a capture object!"); + _capture.release(); + return false; + } + else + { + uint32_t guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID); + if(guid != 0 && guid != 0xffffffff) + { + _guid = uFormat("%08x", guid); + } + + // look for calibration files + if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty())) + { + if(!_model.load(calibrationFolder + "/" + (cameraName.empty()?_guid:cameraName) + ".yaml")) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Camera parameters: fx=%f cx=%f cy=%f cy=%f", + _model.fx(), + _model.cx(), + _model.cy(), + _model.cy()); + } + } + } + return true; +} + +bool CameraVideo::isCalibrated() const +{ + return _model.isValid(); +} + +std::string CameraVideo::getSerial() const +{ + return _guid; +} + +SensorData CameraVideo::captureImage() +{ + cv::Mat img; + if(_capture.isOpened()) + { + if(_capture.read(img)) + { + if(_model.isValid()) + { + img = _model.rectifyImage(img); + } + else + { + // clone required + img = img.clone(); + } + } + else if(_usbDevice) + { + UERROR("Camera has been disconnected!"); + } + } + else + { + ULOGGER_WARN("The camera must be initialized before requesting an image."); + } + + return SensorData(img); +} + +} // namespace rtabmap diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 86179cad..f0f3b5e1 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/util2d.h" +#include "rtabmap/core/CameraRGB.h" #include #include @@ -70,93 +71,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif -#include - namespace rtabmap { -CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) : - _imageRate(imageRate), - _localTransform(localTransform), - _mirroring(false), - _colorOnly(false), - _frameRateTimer(new UTimer()) -{ -} - -CameraRGBD::~CameraRGBD() -{ - if(_frameRateTimer) - { - delete _frameRateTimer; - } -} - -void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) -{ - bool warnFrameRateTooHigh = false; - float actualFrameRate = 0; - if(_imageRate>0) - { - int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); - if(sleepTime > 2) - { - uSleep(sleepTime-2); - } - else if(sleepTime < 0) - { - warnFrameRateTooHigh = true; - actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); - } - - // Add precision at the cost of a small overhead - while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001) - { - // - } - - double slept = _frameRateTimer->getElapsedTime(); - _frameRateTimer->start(); - UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate)); - } - - UTimer timer; - this->captureImage(rgb, depth, fx, fy, cx, cy, stamp); - if(_colorOnly) - { - depth = cv::Mat(); - } - if(_mirroring) - { - if(!rgb.empty()) - { - cv::flip(rgb,rgb,1); - if(cx != 0.0f) - { - cx = float(rgb.cols) - cx; - } - } - if(!depth.empty()) - { - cv::flip(depth,depth,1); - } - } - if(warnFrameRateTooHigh) - { - UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.", - _imageRate, actualFrameRate, timer.ticks()); - } - else - { - UDEBUG("Time capturing image = %fs", timer.ticks()); - } -} - ///////////////////////// // CameraOpenNIPCL ///////////////////////// CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), + Camera(imageRate, localTransform), interface_(0), deviceId_(deviceId), depthConstant_(0.0f) @@ -204,7 +126,7 @@ void CameraOpenni::image_cb ( } } -bool CameraOpenni::init(const std::string & calibrationFolder) +bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName) { if(interface_) { @@ -260,15 +182,9 @@ std::string CameraOpenni::getSerial() const return ""; } -void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) +SensorData CameraOpenni::captureImage() { - rgb = cv::Mat(); - depth = cv::Mat(); - fx=0.0f; - fy=0.0f; - cx=0.0f; - cy=0.0f; - stamp = 0.0; + SensorData data; if(interface_ && interface_->isRunning()) { if(!dataReady_.acquire(1, 2000)) @@ -278,15 +194,15 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa else { UScopeMutex s(dataMutex_); - if(depthConstant_) + if(depthConstant_ && !rgb_.empty() && !depth_.empty()) { - depth = depth_; - rgb = rgb_; - fx = 1.0f/depthConstant_; - fy = 1.0f/depthConstant_; - cx = float(depth_.cols/2) - 0.5f; - cy = float(depth_.rows/2) - 0.5f; - stamp = UTimer::now(); + CameraModel model( + 1.0f/depthConstant_, //fx + 1.0f/depthConstant_, //fy + float(rgb_.cols/2) - 0.5f, //cx + float(rgb_.rows/2) - 0.5f, //cy + this->getLocalTransform()); + data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now()); } depth_ = cv::Mat(); @@ -294,6 +210,7 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa depthConstant_ = 0.0f; } } + return data; } @@ -307,7 +224,7 @@ bool CameraOpenNICV::available() } CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) : - CameraRGBD(imageRate, localTransform), + Camera(imageRate, localTransform), _asus(asus), _depthFocal(0.0f) { @@ -319,14 +236,14 @@ CameraOpenNICV::~CameraOpenNICV() _capture.release(); } -bool CameraOpenNICV::init(const std::string & calibrationFolder) +bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName) { if(_capture.isOpened()) { _capture.release(); } - ULOGGER_DEBUG("CameraRGBD::init()"); + ULOGGER_DEBUG("Camera::init()"); _capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI ); if(_capture.isOpened()) { @@ -354,14 +271,14 @@ bool CameraOpenNICV::init(const std::string & calibrationFolder) } else { - UERROR("CameraRGBD: Device doesn't contain image generator."); + UERROR("Camera: Device doesn't contain image generator."); _capture.release(); return false; } } else { - ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!"); + ULOGGER_ERROR("Camera: Failed to create a capture object!"); _capture.release(); return false; } @@ -373,27 +290,36 @@ bool CameraOpenNICV::isCalibrated() const return true; } -void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) +SensorData CameraOpenNICV::captureImage() { + SensorData data; if(_capture.isOpened()) { _capture.grab(); - _capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP ); - _capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE ); + cv::Mat depth, rgb; + _capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP ); + _capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE ); depth = depth.clone(); rgb = rgb.clone(); - UASSERT(_depthFocal > 0.0f); - fx = _depthFocal; - fy = _depthFocal; - cx = float(depth.cols/2) - 0.5f; - cy = float(depth.rows/2) - 0.5f; - stamp = UTimer::now(); + + UASSERT(_depthFocal>0.0f); + if(!rgb.empty() && !depth.empty()) + { + CameraModel model( + _depthFocal, //fx + _depthFocal, //fy + float(rgb.cols/2) - 0.5f, //cx + float(rgb.rows/2) - 0.5f, //cy + this->getLocalTransform()); + data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + } } else { ULOGGER_WARN("The camera must be initialized before requesting an image."); } + return data; } @@ -422,7 +348,7 @@ CameraOpenNI2::CameraOpenNI2( const std::string & deviceId, float imageRate, const rtabmap::Transform & localTransform) : - CameraRGBD(imageRate, localTransform), + Camera(imageRate, localTransform), #ifdef WITH_OPENNI2 _device(new openni::Device()), _color(new openni::VideoStream()), @@ -526,7 +452,7 @@ bool CameraOpenNI2::setMirroring(bool enabled) return false; } -bool CameraOpenNI2::init(const std::string & calibrationFolder) +bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName) { #ifdef WITH_OPENNI2 openni::OpenNI::initialize(); @@ -715,17 +641,10 @@ std::string CameraOpenNI2::getSerial() const return ""; } -void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) +SensorData CameraOpenNI2::captureImage() { + SensorData data; #ifdef WITH_OPENNI2 - rgb = cv::Mat(); - depth = cv::Mat(); - fx = 0.0f; - fy = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; - int readyStream = -1; if(_device->isValid() && _depth->isValid() && @@ -745,6 +664,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo openni::VideoFrameRef depthFrame, colorFrame; _depth->readFrame(&depthFrame); _color->readFrame(&colorFrame); + cv::Mat depth, rgb; if(depthFrame.isValid() && colorFrame.isValid()) { int h=depthFrame.getHeight(); @@ -757,11 +677,16 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo cv::cvtColor(tmp, rgb, CV_RGB2BGR); } UASSERT(_depthFx != 0.0f && _depthFy != 0.0f); - fx = _depthFx; - fy = _depthFy; - cx = float(depth.cols/2) - 0.5f; - cy = float(depth.rows/2) - 0.5f; - stamp = UTimer::now(); + if(!rgb.empty() && !depth.empty()) + { + CameraModel model( + _depthFx, //fx + _depthFy, //fy + float(rgb.cols/2) - 0.5f, //cx + float(rgb.rows/2) - 0.5f, //cy + this->getLocalTransform()); + data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + } } } else @@ -771,6 +696,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo #else UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!"); #endif + return data; } #ifdef WITH_FREENECT @@ -988,7 +914,7 @@ bool CameraFreenect::available() } CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), + Camera(imageRate, localTransform), deviceId_(deviceId), ctx_(0), freenectDevice_(0) @@ -1016,7 +942,7 @@ CameraFreenect::~CameraFreenect() #endif } -bool CameraFreenect::init(const std::string & calibrationFolder) +bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName) { #ifdef WITH_FREENECT if(freenectDevice_) @@ -1068,29 +994,29 @@ std::string CameraFreenect::getSerial() const return ""; } -void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) +SensorData CameraFreenect::captureImage() { + SensorData data; #ifdef WITH_FREENECT - rgb = cv::Mat(); - depth = cv::Mat(); - fx = 0.0f; - fy = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; if(ctx_ && freenectDevice_) { if(freenectDevice_->isRunning()) { + cv::Mat depth,rgb; freenectDevice_->getData(rgb, depth); if(!rgb.empty() && !depth.empty()) { UASSERT(freenectDevice_->getDepthFocal() != 0.0f); - fx = freenectDevice_->getDepthFocal(); - fy = freenectDevice_->getDepthFocal(); - cx = float(depth.cols/2) - 0.5f; - cy = float(depth.rows/2) - 0.5f; - stamp = UTimer::now(); + if(!rgb.empty() && !depth.empty()) + { + CameraModel model( + freenectDevice_->getDepthFocal(), //fx + freenectDevice_->getDepthFocal(), //fy + float(rgb.cols/2) - 0.5f, //cx + float(rgb.rows/2) - 0.5f, //cy + this->getLocalTransform()); + data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + } } } else @@ -1103,6 +1029,7 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl #else UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!"); #endif + return data; } // @@ -1118,7 +1045,7 @@ bool CameraFreenect2::available() } CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), + Camera(imageRate, localTransform), deviceId_(deviceId), type_(type), freenect2_(0), @@ -1200,7 +1127,7 @@ CameraFreenect2::~CameraFreenect2() #endif } -bool CameraFreenect2::init(const std::string & calibrationFolder) +bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName) { #ifdef WITH_FREENECT2 if(dev_) @@ -1243,10 +1170,15 @@ bool CameraFreenect2::init(const std::string & calibrationFolder) // look for calibration files if(!calibrationFolder.empty()) { - if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber(), false)) + std::string calibrationName = dev_->getSerialNumber(); + if(!cameraName.empty()) + { + calibrationName = cameraName; + } + if(!stereoModel_.load(calibrationFolder, calibrationName, false)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.", - dev_->getSerialNumber().c_str(), calibrationFolder.c_str()); + calibrationName.c_str(), calibrationFolder.c_str()); } else { @@ -1309,16 +1241,10 @@ std::string CameraFreenect2::getSerial() const return ""; } -void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) +SensorData CameraFreenect2::captureImage() { + SensorData data; #ifdef WITH_FREENECT2 - rgb = cv::Mat(); - depth = cv::Mat(); - fx = 0.0f; - fy = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; if(dev_ && listener_) { libfreenect2::FrameMap frames; @@ -1337,7 +1263,7 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f else #endif { - stamp = UTimer::now(); + double stamp = UTimer::now(); libfreenect2::Frame *rgbFrame = 0; libfreenect2::Frame *irFrame = 0; libfreenect2::Frame *depthFrame = 0; @@ -1360,6 +1286,8 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f break; } + cv::Mat rgb, depth; + float fx=0,fy=0,cx=0,cy=0; if(irFrame && depthFrame) { cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data); @@ -1377,8 +1305,10 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f } cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); + cv::flip(rgb, rgb, 1); cv::flip(depth, depth, 1); + if(stereoModel_.isValid()) { //rectify @@ -1432,7 +1362,7 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f depth, stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K - stereoModel_.transform()); + stereoModel_.stereoTransform()); util2d::fillRegisteredDepthHoles(depth, true, false); fx = stereoModel_.right().fx(); fy = stereoModel_.right().fy(); @@ -1528,866 +1458,22 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f cv::flip(depth, depth, 1); } } + + CameraModel model( + fx, //fx + fy, //fy + cx, //cx + cy, // cy + this->getLocalTransform()); + data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp); + listener_->release(frames); } } #else UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!"); #endif -} - -// -// CameraStereoDC1394 -// Inspired from ROS camera1394stereo package -// - -#ifdef WITH_DC1394 -class DC1394Device -{ -public: - DC1394Device() : - camera_(0), - context_(0) - { - - } - ~DC1394Device() - { - if (camera_) - { - if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) || - DC1394_SUCCESS != dc1394_capture_stop(camera_)) - { - UWARN("unable to stop camera"); - } - - // Free resources - dc1394_capture_stop(camera_); - dc1394_camera_free(camera_); - camera_ = NULL; - } - if(context_) - { - dc1394_free(context_); - context_ = NULL; - } - } - - const std::string & guid() const {return guid_;} - - bool init() - { - if(camera_) - { - // Free resources - dc1394_capture_stop(camera_); - dc1394_camera_free(camera_); - camera_ = NULL; - } - - // look for a camera - int err; - if(context_ == NULL) - { - context_ = dc1394_new (); - if (context_ == NULL) - { - UERROR( "Could not initialize dc1394_context.\n" - "Make sure /dev/raw1394 exists, you have access permission,\n" - "and libraw1394 development package is installed."); - return false; - } - } - - dc1394camera_list_t *list; - err = dc1394_camera_enumerate(context_, &list); - if (err != DC1394_SUCCESS) - { - UERROR("Could not get camera list"); - return false; - } - - if (list->num == 0) - { - UERROR("No cameras found"); - dc1394_camera_free_list (list); - return false; - } - uint64_t guid = list->ids[0].guid; - dc1394_camera_free_list (list); - - // Create a camera - camera_ = dc1394_camera_new (context_, guid); - if (!camera_) - { - UERROR("Failed to initialize camera with GUID [%016lx]", guid); - return false; - } - - uint32_t value[3]; - value[0]= camera_->guid & 0xffffffff; - value[1]= (camera_->guid >>32) & 0x000000ff; - value[2]= (camera_->guid >>40) & 0xfffff; - guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]); - - UINFO("camera model: %s %s", camera_->vendor, camera_->model); - - // initialize camera - // Enable IEEE1394b mode if the camera and bus support it - bool bmode = camera_->bmode_capable; - if (bmode - && (DC1394_SUCCESS != - dc1394_video_set_operation_mode(camera_, - DC1394_OPERATION_MODE_1394B))) - { - bmode = false; - UWARN("failed to set IEEE1394b mode"); - } - - // start with highest speed supported - dc1394speed_t request = DC1394_ISO_SPEED_3200; - int rate = 3200; - if (!bmode) - { - // not IEEE1394b capable: so 400Mb/s is the limit - request = DC1394_ISO_SPEED_400; - rate = 400; - } - - // round requested speed down to next-lower defined value - while (rate > 400) - { - if (request <= DC1394_ISO_SPEED_MIN) - { - // get current ISO speed of the device - dc1394speed_t curSpeed; - if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX) - { - // Translate curSpeed back to an int for the parameter - // update, works as long as any new higher speeds keep - // doubling. - request = curSpeed; - rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN); - } - else - { - UWARN("Unable to get ISO speed; assuming 400Mb/s"); - rate = 400; - request = DC1394_ISO_SPEED_400; - } - break; - } - // continue with next-lower possible value - request = (dc1394speed_t) ((int) request - 1); - rate = rate / 2; - } - - // set the requested speed - if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request)) - { - UERROR("Failed to set iso speed"); - return false; - } - - // set video mode - dc1394video_modes_t vmodes; - err = dc1394_video_get_supported_modes(camera_, &vmodes); - if (err != DC1394_SUCCESS) - { - UERROR("unable to get supported video modes"); - return (dc1394video_mode_t) 0; - } - - // see if requested mode is available - bool found = false; - dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee - for (uint32_t i = 0; i < vmodes.num; ++i) - { - if (vmodes.modes[i] == videoMode) - { - found = true; - } - } - if(!found) - { - UERROR("unable to get video mode %d", videoMode); - return false; - } - - if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode)) - { - UERROR("Failed to set video mode %d", videoMode); - return false; - } - - // special handling for Format7 modes - if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE) - { - if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16)) - { - UERROR("Could not set color coding"); - return false; - } - uint32_t packetSize; - if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize)) - { - UERROR("Could not get default packet size"); - return false; - } - - if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize)) - { - UERROR("Could not set packet size"); - return false; - } - } - else - { - UERROR("Video is not in mode scalable"); - } - - // start the device streaming data - // Set camera to use DMA, improves performance. - if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT)) - { - UERROR("Failed to open device!"); - return false; - } - - // Start transmitting camera data - if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON)) - { - UERROR("Failed to start device!"); - return false; - } - - return true; - } - - bool getImages(cv::Mat & left, cv::Mat & right) - { - if(camera_) - { - dc1394video_frame_t * frame = NULL; - UDEBUG("[%016lx] waiting camera", camera_->guid); - dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame); - if (!frame) - { - UERROR("Unable to capture frame"); - return false; - } - dc1394video_frame_t frame1 = *frame; - // deinterlace frame into two imagesCount one on top the other - size_t frame1_size = frame->total_bytes; - frame1.image = (unsigned char *) malloc(frame1_size); - frame1.allocated_image_bytes = frame1_size; - frame1.color_coding = DC1394_COLOR_CODING_RAW8; - int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED); - if (err != DC1394_SUCCESS) - { - free(frame1.image); - dc1394_capture_enqueue(camera_, frame); - UERROR("Could not extract stereo frames"); - return false; - } - - uint8_t* capture_buffer = reinterpret_cast(frame1.image); - UASSERT(capture_buffer); - - cv::Mat image(frame->size[1], frame->size[0], CV_8UC3); - cv::Mat image2 = image.clone(); - - //DC1394_COLOR_CODING_RAW16: - //DC1394_COLOR_FILTER_BGGR - cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR); - cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY); - - dc1394_capture_enqueue(camera_, frame); - - free(frame1.image); - - return true; - } - return false; - } - -private: - dc1394camera_t *camera_; - dc1394_t *context_; - std::string guid_; -}; -#endif - -bool CameraStereoDC1394::available() -{ -#ifdef WITH_DC1394 - return true; -#else - return false; -#endif -} - -CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), - device_(0) -{ -#ifdef WITH_DC1394 - device_ = new DC1394Device(); -#endif -} - -CameraStereoDC1394::~CameraStereoDC1394() -{ -#ifdef WITH_DC1394 - if(device_) - { - delete device_; - } -#endif -} - -bool CameraStereoDC1394::init(const std::string & calibrationFolder) -{ -#ifdef WITH_DC1394 - if(device_) - { - bool ok = device_->init(); - if(ok) - { - // look for calibration files - if(!calibrationFolder.empty()) - { - if(!stereoModel_.load(calibrationFolder, device_->guid())) - { - UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", - device_->guid().c_str(), calibrationFolder.c_str()); - } - else - { - UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", - stereoModel_.left().fx(), - stereoModel_.left().cx(), - stereoModel_.left().cy(), - stereoModel_.baseline()); - } - } - } - return ok; - } -#else - UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!"); -#endif - return false; -} - -bool CameraStereoDC1394::isCalibrated() const -{ - return stereoModel_.isValid(); -} - -std::string CameraStereoDC1394::getSerial() const -{ -#ifdef WITH_DC1394 - if(device_) - { - return device_->guid(); - } -#endif - return ""; -} - -void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) -{ -#ifdef WITH_DC1394 - left = cv::Mat(); - right = cv::Mat(); - fx = 0.0f; - baseline = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; - if(device_) - { - device_->getImages(left, right); - - // Rectification - left = stereoModel_.left().rectifyImage(left); - right = stereoModel_.right().rectifyImage(right); - fx = stereoModel_.left().fx(); - cx = stereoModel_.left().cx(); - cy = stereoModel_.left().cy(); - baseline = stereoModel_.baseline(); - stamp = UTimer::now(); - } -#else - UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!"); -#endif -} - -// -// CameraTriclops -// -CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), - camera_(0), - triclopsCtx_(0) -{ -#ifdef WITH_FLYCAPTURE2 - camera_ = new FlyCapture2::Camera(); -#endif -} - -CameraStereoFlyCapture2::~CameraStereoFlyCapture2() -{ -#ifdef WITH_FLYCAPTURE2 - // Close the camera - camera_->StopCapture(); - camera_->Disconnect(); - - // Destroy the Triclops context - triclopsDestroyContext( triclopsCtx_ ) ; - - delete camera_; -#endif -} - -bool CameraStereoFlyCapture2::available() -{ -#ifdef WITH_FLYCAPTURE2 - return true; -#else - return false; -#endif -} - -bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder) -{ -#ifdef WITH_FLYCAPTURE2 - if(camera_) - { - // Close the camera - camera_->StopCapture(); - camera_->Disconnect(); - } - if(triclopsCtx_) - { - triclopsDestroyContext(triclopsCtx_); - triclopsCtx_ = 0; - } - - // connect camera - FlyCapture2::Error fc2Error = camera_->Connect(); - if(fc2Error != FlyCapture2::PGRERROR_OK) - { - UERROR("Failed to connect the camera."); - return false; - } - - // configure camera - Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW; - if(Fc2Triclops::setStereoMode(*camera_, mode )) - { - UERROR("Failed to set stereo mode."); - return false; - } - - // generate the Triclops context - FlyCapture2::CameraInfo camInfo; - if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK) - { - UERROR("Failed to get camera info."); - return false; - } - - float dummy; - unsigned packetSz; - FlyCapture2::Format7ImageSettings imageSettings; - int maxWidth = 640; - int maxHeight = 480; - if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK) - { - maxHeight = imageSettings.height; - maxWidth = imageSettings.width; - } - - // Get calibration from th camera - if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_)) - { - UERROR("Failed to get calibration from the camera."); - return false; - } - - float fx, cx, cy, baseline; - triclopsGetFocalLength(triclopsCtx_, &fx); - triclopsGetImageCenter(triclopsCtx_, &cy, &cx); - triclopsGetBaseline(triclopsCtx_, &baseline); - UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline); - - triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW ); - UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK); - - if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK) - { - UERROR("Failed to start capture."); - return false; - } - - return true; -#else - UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!"); -#endif - return false; -} - -bool CameraStereoFlyCapture2::isCalibrated() const -{ -#ifdef WITH_FLYCAPTURE2 - if(triclopsCtx_) - { - float fx, cx, cy, baseline; - triclopsGetFocalLength(triclopsCtx_, &fx); - triclopsGetImageCenter(triclopsCtx_, &cy, &cx); - triclopsGetBaseline(triclopsCtx_, &baseline); - return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f; - } -#endif - return false; -} - -std::string CameraStereoFlyCapture2::getSerial() const -{ -#ifdef WITH_FLYCAPTURE2 - if(camera_ && camera_->IsConnected()) - { - FlyCapture2::CameraInfo camInfo; - if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK) - { - return uNumber2Str(camInfo.serialNumber); - } - } -#endif - return ""; -} - -// struct containing image needed for processing -#ifdef WITH_FLYCAPTURE2 -struct ImageContainer -{ - FlyCapture2::Image tmp[2]; - FlyCapture2::Image unprocessed[2]; -} ; -#endif - -void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) -{ -#ifdef WITH_FLYCAPTURE2 - left = cv::Mat(); - right = cv::Mat(); - fx = 0.0f; - baseline = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; - - if(camera_ && triclopsCtx_ && camera_->IsConnected()) - { - // grab image from camera. - // this image contains both right and left imagesCount - FlyCapture2::Image grabbedImage; - if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK) - { - stamp = UTimer::now(); - - // right and left image extracted from grabbed image - ImageContainer imageCont; - - // generate triclops input from grabbed image - FlyCapture2::Image imageRawRight; - FlyCapture2::Image imageRawLeft; - FlyCapture2::Image * unprocessedImage = imageCont.unprocessed; - - // Convert the pixel interleaved raw data to de-interleaved and color processed data - if(Fc2Triclops::unpackUnprocessedRawOrMono16Image( - grabbedImage, - true /*assume little endian*/, - imageRawLeft /* right */, - imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK) - { - // convert to color - FlyCapture2::Image srcImgRightRef(imageRawRight); - FlyCapture2::Image srcImgLeftRef(imageRawLeft); - - bool ok = true;; - if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK || - srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK) - { - ok = false; - } - - if(ok) - { - FlyCapture2::Image imageColorRight; - FlyCapture2::Image imageColorLeft; - if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK || - srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK) - { - ok = false; - } - - if(ok) - { - //RECTIFY RIGHT - TriclopsInput triclopsColorInputs; - triclopsBuildRGBTriclopsInput( - grabbedImage.GetCols(), - grabbedImage.GetRows(), - imageColorRight.GetStride(), - (unsigned long)grabbedImage.GetTimeStamp().seconds, - (unsigned long)grabbedImage.GetTimeStamp().microSeconds, - imageColorRight.GetData(), - imageColorRight.GetData(), - imageColorRight.GetData(), - &triclopsColorInputs); - - triclopsRectify(triclopsCtx_, const_cast(&triclopsColorInputs) ); - // Retrieve the rectified image from the triclops context - TriclopsImage rectifiedImage; - triclopsGetImage( triclopsCtx_, - TriImg_RECTIFIED, - TriCam_REFERENCE, - &rectifiedImage ); - - right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone(); - - //RECTIFY LEFT COLOR - triclopsBuildPackedTriclopsInput( - grabbedImage.GetCols(), - grabbedImage.GetRows(), - imageColorLeft.GetStride(), - (unsigned long)grabbedImage.GetTimeStamp().seconds, - (unsigned long)grabbedImage.GetTimeStamp().microSeconds, - imageColorLeft.GetData(), - &triclopsColorInputs ); - - cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4); - TriclopsPackedColorImage colorImage; - triclopsSetPackedColorImageBuffer( - triclopsCtx_, - TriCam_LEFT, - (TriclopsPackedColorPixel*)pixelsLeftBuffer.data ); - - triclopsRectifyPackedColorImage( - triclopsCtx_, - TriCam_LEFT, - &triclopsColorInputs, - &colorImage ); - - cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB); - - // Set calibration stuff - triclopsGetFocalLength(triclopsCtx_, &fx); - triclopsGetImageCenter(triclopsCtx_, &cy, &cx); - triclopsGetBaseline(triclopsCtx_, &baseline); - } - } - } - } - } - -#else - UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!"); -#endif -} - -// -// CameraStereoImages -// -bool CameraStereoImages::available() -{ - return true; -} - -CameraStereoImages::CameraStereoImages( - const std::string & path, - const std::string & cameraName, - const std::string & timestampsPath, - float imageRate, - const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), - camera_(0), - camera2_(0), - cameraName_(cameraName), - timestampsPath_(timestampsPath) -{ - std::vector paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';')); - if(paths.size() >= 1) - { - camera_ = new CameraImages(paths[0]); - - if(paths.size() >= 2) - { - camera2_ = new CameraImages(paths[1]); - } - } - else - { - UERROR("The path is empty!"); - } -} - -CameraStereoImages::~CameraStereoImages() -{ - if(camera_) - { - delete camera_; - } - if(camera2_) - { - delete camera2_; - } -} - -bool CameraStereoImages::init(const std::string & calibrationFolder) -{ - // look for calibration files - if(!calibrationFolder.empty() && !cameraName_.empty()) - { - if(!stereoModel_.load(calibrationFolder, cameraName_)) - { - UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", - cameraName_.c_str(), calibrationFolder.c_str()); - } - else - { - UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", - stereoModel_.left().fx(), - stereoModel_.left().cx(), - stereoModel_.left().cy(), - stereoModel_.baseline()); - } - } - bool success = false; - if(camera_ == 0) - { - UERROR("Cannot initialize the camera."); - } - else if(camera_->init()) - { - if(camera2_) - { - if(camera2_->init()) - { - if(camera_->imagesCount() == camera2_->imagesCount()) - { - success = true; - } - else - { - UERROR("Cameras don't have the same number of images (%d vs %d)", - camera_->imagesCount(), camera2_->imagesCount()); - } - } - else - { - UERROR("Cannot initialize the second camera."); - } - } - else - { - success = true; - } - } - - stamps_.clear(); - if(success && timestampsPath_.size()) - { - FILE * file = 0; -#ifdef _MSC_VER - fopen_s(&file, timestampsPath_.c_str(), "r"); -#else - file = fopen(timestampsPath_.c_str(), "r"); -#endif - if(file) - { - char line[16]; - while ( fgets (line , 16 , file) != NULL ) - { - stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0))); - } - fclose(file); - } - if(stamps_.size() != camera_->imagesCount()) - { - UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove " - "the timestamps file path if you don't want to use them (current file path=%s).", - (int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str()); - stamps_.clear(); - success = false; - } - } - - return success; -} - -bool CameraStereoImages::isCalibrated() const -{ - return stereoModel_.isValid(); -} - -std::string CameraStereoImages::getSerial() const -{ - return "stereo_images"; -} - -void CameraStereoImages::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp) -{ - left = cv::Mat(); - right = cv::Mat(); - fx = 0.0f; - baseline = 0.0f; - cx = 0.0f; - cy = 0.0f; - stamp = 0.0; - - if(camera_) - { - if(stamps_.size()) - { - stamp = stamps_.front(); - stamps_.pop_front(); - } - else - { - stamp = UTimer::now(); - } - left = camera_->takeImage(); - if(!left.empty()) - { - if(camera2_) - { - right = camera2_->takeImage(); - } - else - { - right = camera_->takeImage(); - } - - if(!right.empty()) - { - // Rectification - //left = stereoModel_.left().rectifyImage(left); - //right = stereoModel_.right().rectifyImage(right); - fx = stereoModel_.left().fx(); - cx = stereoModel_.left().cx(); - cy = stereoModel_.left().cy(); - baseline = stereoModel_.baseline(); - } - else - { - left = cv::Mat(); - } - } - } + return data; } } // namespace rtabmap diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp new file mode 100644 index 00000000..00e43296 --- /dev/null +++ b/corelib/src/CameraStereo.cpp @@ -0,0 +1,910 @@ +/* +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. +*/ + +#include "rtabmap/core/CameraStereo.h" +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/CameraRGB.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef WITH_DC1394 +#include +#endif + +#ifdef WITH_FLYCAPTURE2 +#include +#include +#endif + +namespace rtabmap +{ + +// +// CameraStereoDC1394 +// Inspired from ROS camera1394stereo package +// + +#ifdef WITH_DC1394 +class DC1394Device +{ +public: + DC1394Device() : + camera_(0), + context_(0) + { + + } + ~DC1394Device() + { + if (camera_) + { + if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) || + DC1394_SUCCESS != dc1394_capture_stop(camera_)) + { + UWARN("unable to stop camera"); + } + + // Free resources + dc1394_capture_stop(camera_); + dc1394_camera_free(camera_); + camera_ = NULL; + } + if(context_) + { + dc1394_free(context_); + context_ = NULL; + } + } + + const std::string & guid() const {return guid_;} + + bool init() + { + if(camera_) + { + // Free resources + dc1394_capture_stop(camera_); + dc1394_camera_free(camera_); + camera_ = NULL; + } + + // look for a camera + int err; + if(context_ == NULL) + { + context_ = dc1394_new (); + if (context_ == NULL) + { + UERROR( "Could not initialize dc1394_context.\n" + "Make sure /dev/raw1394 exists, you have access permission,\n" + "and libraw1394 development package is installed."); + return false; + } + } + + dc1394camera_list_t *list; + err = dc1394_camera_enumerate(context_, &list); + if (err != DC1394_SUCCESS) + { + UERROR("Could not get camera list"); + return false; + } + + if (list->num == 0) + { + UERROR("No cameras found"); + dc1394_camera_free_list (list); + return false; + } + uint64_t guid = list->ids[0].guid; + dc1394_camera_free_list (list); + + // Create a camera + camera_ = dc1394_camera_new (context_, guid); + if (!camera_) + { + UERROR("Failed to initialize camera with GUID [%016lx]", guid); + return false; + } + + uint32_t value[3]; + value[0]= camera_->guid & 0xffffffff; + value[1]= (camera_->guid >>32) & 0x000000ff; + value[2]= (camera_->guid >>40) & 0xfffff; + guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]); + + UINFO("camera model: %s %s", camera_->vendor, camera_->model); + + // initialize camera + // Enable IEEE1394b mode if the camera and bus support it + bool bmode = camera_->bmode_capable; + if (bmode + && (DC1394_SUCCESS != + dc1394_video_set_operation_mode(camera_, + DC1394_OPERATION_MODE_1394B))) + { + bmode = false; + UWARN("failed to set IEEE1394b mode"); + } + + // start with highest speed supported + dc1394speed_t request = DC1394_ISO_SPEED_3200; + int rate = 3200; + if (!bmode) + { + // not IEEE1394b capable: so 400Mb/s is the limit + request = DC1394_ISO_SPEED_400; + rate = 400; + } + + // round requested speed down to next-lower defined value + while (rate > 400) + { + if (request <= DC1394_ISO_SPEED_MIN) + { + // get current ISO speed of the device + dc1394speed_t curSpeed; + if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX) + { + // Translate curSpeed back to an int for the parameter + // update, works as long as any new higher speeds keep + // doubling. + request = curSpeed; + rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN); + } + else + { + UWARN("Unable to get ISO speed; assuming 400Mb/s"); + rate = 400; + request = DC1394_ISO_SPEED_400; + } + break; + } + // continue with next-lower possible value + request = (dc1394speed_t) ((int) request - 1); + rate = rate / 2; + } + + // set the requested speed + if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request)) + { + UERROR("Failed to set iso speed"); + return false; + } + + // set video mode + dc1394video_modes_t vmodes; + err = dc1394_video_get_supported_modes(camera_, &vmodes); + if (err != DC1394_SUCCESS) + { + UERROR("unable to get supported video modes"); + return (dc1394video_mode_t) 0; + } + + // see if requested mode is available + bool found = false; + dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee + for (uint32_t i = 0; i < vmodes.num; ++i) + { + if (vmodes.modes[i] == videoMode) + { + found = true; + } + } + if(!found) + { + UERROR("unable to get video mode %d", videoMode); + return false; + } + + if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode)) + { + UERROR("Failed to set video mode %d", videoMode); + return false; + } + + // special handling for Format7 modes + if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE) + { + if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16)) + { + UERROR("Could not set color coding"); + return false; + } + uint32_t packetSize; + if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize)) + { + UERROR("Could not get default packet size"); + return false; + } + + if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize)) + { + UERROR("Could not set packet size"); + return false; + } + } + else + { + UERROR("Video is not in mode scalable"); + } + + // start the device streaming data + // Set camera to use DMA, improves performance. + if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT)) + { + UERROR("Failed to open device!"); + return false; + } + + // Start transmitting camera data + if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON)) + { + UERROR("Failed to start device!"); + return false; + } + + return true; + } + + bool getImages(cv::Mat & left, cv::Mat & right) + { + if(camera_) + { + dc1394video_frame_t * frame = NULL; + UDEBUG("[%016lx] waiting camera", camera_->guid); + dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame); + if (!frame) + { + UERROR("Unable to capture frame"); + return false; + } + dc1394video_frame_t frame1 = *frame; + // deinterlace frame into two imagesCount one on top the other + size_t frame1_size = frame->total_bytes; + frame1.image = (unsigned char *) malloc(frame1_size); + frame1.allocated_image_bytes = frame1_size; + frame1.color_coding = DC1394_COLOR_CODING_RAW8; + int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED); + if (err != DC1394_SUCCESS) + { + free(frame1.image); + dc1394_capture_enqueue(camera_, frame); + UERROR("Could not extract stereo frames"); + return false; + } + + uint8_t* capture_buffer = reinterpret_cast(frame1.image); + UASSERT(capture_buffer); + + cv::Mat image(frame->size[1], frame->size[0], CV_8UC3); + cv::Mat image2 = image.clone(); + + //DC1394_COLOR_CODING_RAW16: + //DC1394_COLOR_FILTER_BGGR + cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR); + cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY); + + dc1394_capture_enqueue(camera_, frame); + + free(frame1.image); + + return true; + } + return false; + } + +private: + dc1394camera_t *camera_; + dc1394_t *context_; + std::string guid_; +}; +#endif + +bool CameraStereoDC1394::available() +{ +#ifdef WITH_DC1394 + return true; +#else + return false; +#endif +} + +CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) : + Camera(imageRate, localTransform), + device_(0) +{ +#ifdef WITH_DC1394 + device_ = new DC1394Device(); +#endif +} + +CameraStereoDC1394::~CameraStereoDC1394() +{ +#ifdef WITH_DC1394 + if(device_) + { + delete device_; + } +#endif +} + +bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef WITH_DC1394 + if(device_) + { + bool ok = device_->init(); + if(ok) + { + // look for calibration files + if(!calibrationFolder.empty()) + { + if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName)) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", + stereoModel_.left().fx(), + stereoModel_.left().cx(), + stereoModel_.left().cy(), + stereoModel_.baseline()); + } + } + } + return ok; + } +#else + UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!"); +#endif + return false; +} + +bool CameraStereoDC1394::isCalibrated() const +{ + return stereoModel_.isValid(); +} + +std::string CameraStereoDC1394::getSerial() const +{ +#ifdef WITH_DC1394 + if(device_) + { + return device_->guid(); + } +#endif + return ""; +} + +SensorData CameraStereoDC1394::captureImage() +{ + SensorData data; +#ifdef WITH_DC1394 + if(device_) + { + cv::Mat left, right; + device_->getImages(left, right); + + if(!left.empty() && !right.empty()) + { + // Rectification + left = stereoModel_.left().rectifyImage(left); + right = stereoModel_.right().rectifyImage(right); + StereoCameraModel model( + stereoModel_.left().fx(), //fx + stereoModel_.left().fy(), //fy + stereoModel_.left().cx(), //cx + stereoModel_.left().cy(), //cy + stereoModel_.baseline(), + this->getLocalTransform()); + data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now()); + } + } +#else + UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!"); +#endif + return data; +} + +// +// CameraTriclops +// +CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) : + Camera(imageRate, localTransform), + camera_(0), + triclopsCtx_(0) +{ +#ifdef WITH_FLYCAPTURE2 + camera_ = new FlyCapture2::Camera(); +#endif +} + +CameraStereoFlyCapture2::~CameraStereoFlyCapture2() +{ +#ifdef WITH_FLYCAPTURE2 + // Close the camera + camera_->StopCapture(); + camera_->Disconnect(); + + // Destroy the Triclops context + triclopsDestroyContext( triclopsCtx_ ) ; + + delete camera_; +#endif +} + +bool CameraStereoFlyCapture2::available() +{ +#ifdef WITH_FLYCAPTURE2 + return true; +#else + return false; +#endif +} + +bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef WITH_FLYCAPTURE2 + if(camera_) + { + // Close the camera + camera_->StopCapture(); + camera_->Disconnect(); + } + if(triclopsCtx_) + { + triclopsDestroyContext(triclopsCtx_); + triclopsCtx_ = 0; + } + + // connect camera + FlyCapture2::Error fc2Error = camera_->Connect(); + if(fc2Error != FlyCapture2::PGRERROR_OK) + { + UERROR("Failed to connect the camera."); + return false; + } + + // configure camera + Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW; + if(Fc2Triclops::setStereoMode(*camera_, mode )) + { + UERROR("Failed to set stereo mode."); + return false; + } + + // generate the Triclops context + FlyCapture2::CameraInfo camInfo; + if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK) + { + UERROR("Failed to get camera info."); + return false; + } + + float dummy; + unsigned packetSz; + FlyCapture2::Format7ImageSettings imageSettings; + int maxWidth = 640; + int maxHeight = 480; + if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK) + { + maxHeight = imageSettings.height; + maxWidth = imageSettings.width; + } + + // Get calibration from th camera + if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_)) + { + UERROR("Failed to get calibration from the camera."); + return false; + } + + float fx, cx, cy, baseline; + triclopsGetFocalLength(triclopsCtx_, &fx); + triclopsGetImageCenter(triclopsCtx_, &cy, &cx); + triclopsGetBaseline(triclopsCtx_, &baseline); + UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline); + + triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW ); + UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK); + + if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK) + { + UERROR("Failed to start capture."); + return false; + } + + return true; +#else + UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!"); +#endif + return false; +} + +bool CameraStereoFlyCapture2::isCalibrated() const +{ +#ifdef WITH_FLYCAPTURE2 + if(triclopsCtx_) + { + float fx, cx, cy, baseline; + triclopsGetFocalLength(triclopsCtx_, &fx); + triclopsGetImageCenter(triclopsCtx_, &cy, &cx); + triclopsGetBaseline(triclopsCtx_, &baseline); + return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f; + } +#endif + return false; +} + +std::string CameraStereoFlyCapture2::getSerial() const +{ +#ifdef WITH_FLYCAPTURE2 + if(camera_ && camera_->IsConnected()) + { + FlyCapture2::CameraInfo camInfo; + if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK) + { + return uNumber2Str(camInfo.serialNumber); + } + } +#endif + return ""; +} + +// struct containing image needed for processing +#ifdef WITH_FLYCAPTURE2 +struct ImageContainer +{ + FlyCapture2::Image tmp[2]; + FlyCapture2::Image unprocessed[2]; +} ; +#endif + +SensorData CameraStereoFlyCapture2::captureImage() +{ + SensorData data; +#ifdef WITH_FLYCAPTURE2 + if(camera_ && triclopsCtx_ && camera_->IsConnected()) + { + // grab image from camera. + // this image contains both right and left imagesCount + FlyCapture2::Image grabbedImage; + if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK) + { + stamp = UTimer::now(); + + // right and left image extracted from grabbed image + ImageContainer imageCont; + + // generate triclops input from grabbed image + FlyCapture2::Image imageRawRight; + FlyCapture2::Image imageRawLeft; + FlyCapture2::Image * unprocessedImage = imageCont.unprocessed; + + // Convert the pixel interleaved raw data to de-interleaved and color processed data + if(Fc2Triclops::unpackUnprocessedRawOrMono16Image( + grabbedImage, + true /*assume little endian*/, + imageRawLeft /* right */, + imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK) + { + // convert to color + FlyCapture2::Image srcImgRightRef(imageRawRight); + FlyCapture2::Image srcImgLeftRef(imageRawLeft); + + bool ok = true;; + if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK || + srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK) + { + ok = false; + } + + if(ok) + { + FlyCapture2::Image imageColorRight; + FlyCapture2::Image imageColorLeft; + if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK || + srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK) + { + ok = false; + } + + if(ok) + { + //RECTIFY RIGHT + TriclopsInput triclopsColorInputs; + triclopsBuildRGBTriclopsInput( + grabbedImage.GetCols(), + grabbedImage.GetRows(), + imageColorRight.GetStride(), + (unsigned long)grabbedImage.GetTimeStamp().seconds, + (unsigned long)grabbedImage.GetTimeStamp().microSeconds, + imageColorRight.GetData(), + imageColorRight.GetData(), + imageColorRight.GetData(), + &triclopsColorInputs); + + triclopsRectify(triclopsCtx_, const_cast(&triclopsColorInputs) ); + // Retrieve the rectified image from the triclops context + TriclopsImage rectifiedImage; + triclopsGetImage( triclopsCtx_, + TriImg_RECTIFIED, + TriCam_REFERENCE, + &rectifiedImage ); + + cv::Mat left,right; + right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone(); + + //RECTIFY LEFT COLOR + triclopsBuildPackedTriclopsInput( + grabbedImage.GetCols(), + grabbedImage.GetRows(), + imageColorLeft.GetStride(), + (unsigned long)grabbedImage.GetTimeStamp().seconds, + (unsigned long)grabbedImage.GetTimeStamp().microSeconds, + imageColorLeft.GetData(), + &triclopsColorInputs ); + + cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4); + TriclopsPackedColorImage colorImage; + triclopsSetPackedColorImageBuffer( + triclopsCtx_, + TriCam_LEFT, + (TriclopsPackedColorPixel*)pixelsLeftBuffer.data ); + + triclopsRectifyPackedColorImage( + triclopsCtx_, + TriCam_LEFT, + &triclopsColorInputs, + &colorImage ); + + cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB); + + // Set calibration stuff + float fx, cy, cx, baseline; + triclopsGetFocalLength(triclopsCtx_, &fx); + triclopsGetImageCenter(triclopsCtx_, &cy, &cx); + triclopsGetBaseline(triclopsCtx_, &baseline); + + StereoCameraModel model( + fx + fx, + cx + cy + baseline, + this->getLocalTransform()); + data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now()); + } + } + } + } + } + +#else + UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!"); +#endif + return data; +} + +// +// CameraStereoImages +// +bool CameraStereoImages::available() +{ + return true; +} + +CameraStereoImages::CameraStereoImages( + const std::string & path, + const std::string & timestampsPath, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + camera_(0), + camera2_(0), + timestampsPath_(timestampsPath) +{ + std::vector paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';')); + if(paths.size() >= 1) + { + camera_ = new CameraImages(paths[0]); + + if(paths.size() >= 2) + { + camera2_ = new CameraImages(paths[1]); + } + } + else + { + UERROR("The path is empty!"); + } +} + +CameraStereoImages::~CameraStereoImages() +{ + if(camera_) + { + delete camera_; + } + if(camera2_) + { + delete camera2_; + } +} + +bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName) +{ + // look for calibration files + cameraName_.clear(); + if(!calibrationFolder.empty() && !cameraName.empty()) + { + cameraName_ = cameraName; + if(!stereoModel_.load(calibrationFolder, cameraName)) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", + stereoModel_.left().fx(), + stereoModel_.left().cx(), + stereoModel_.left().cy(), + stereoModel_.baseline()); + } + } + bool success = false; + if(camera_ == 0) + { + UERROR("Cannot initialize the camera."); + } + else if(camera_->init()) + { + if(camera2_) + { + if(camera2_->init()) + { + if(camera_->imagesCount() == camera2_->imagesCount()) + { + success = true; + } + else + { + UERROR("Cameras don't have the same number of images (%d vs %d)", + camera_->imagesCount(), camera2_->imagesCount()); + } + } + else + { + UERROR("Cannot initialize the second camera."); + } + } + else + { + success = true; + } + } + + stamps_.clear(); + if(success && timestampsPath_.size()) + { + FILE * file = 0; +#ifdef _MSC_VER + fopen_s(&file, timestampsPath_.c_str(), "r"); +#else + file = fopen(timestampsPath_.c_str(), "r"); +#endif + if(file) + { + char line[16]; + while ( fgets (line , 16 , file) != NULL ) + { + stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0))); + } + fclose(file); + } + if(stamps_.size() != camera_->imagesCount()) + { + UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove " + "the timestamps file path if you don't want to use them (current file path=%s).", + (int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str()); + stamps_.clear(); + success = false; + } + } + + return success; +} + +bool CameraStereoImages::isCalibrated() const +{ + return stereoModel_.isValid(); +} + +std::string CameraStereoImages::getSerial() const +{ + return cameraName_; +} + +SensorData CameraStereoImages::captureImage() +{ + SensorData data; + if(camera_) + { + double stamp; + if(stamps_.size()) + { + stamp = stamps_.front(); + stamps_.pop_front(); + } + else + { + stamp = UTimer::now(); + } + SensorData left, right; + left = camera_->takeImage(); + if(!left.imageRaw().empty()) + { + if(camera2_) + { + right = camera2_->takeImage(); + } + else + { + right = camera_->takeImage(); + } + + if(!right.imageRaw().empty()) + { + // Rectification + //left = stereoModel_.left().rectifyImage(left); + //right = stereoModel_.right().rectifyImage(right); + StereoCameraModel model( + stereoModel_.left().fx(), //fx + stereoModel_.left().fy(), //fy + stereoModel_.left().cx(), //cx + stereoModel_.left().cy(), //cy + stereoModel_.baseline(), + this->getLocalTransform()); + data = SensorData(left.imageRaw(), right.imageRaw(), model, this->getNextSeqID(), stamp); + } + } + } + return data; +} + +} // namespace rtabmap diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index aa988f9d..cdb1aa62 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Camera.h" -#include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraEvent.h" #include @@ -39,21 +38,12 @@ namespace rtabmap // ownership transferred CameraThread::CameraThread(Camera * camera) : _camera(camera), - _cameraRGBD(0), - _seq(0) + _mirroring(false), + _colorOnly(false) { UASSERT(_camera != 0); } -// ownership transferred -CameraThread::CameraThread(CameraRGBD * camera) : - _camera(0), - _cameraRGBD(camera), - _seq(0) -{ - UASSERT(_cameraRGBD != 0); -} - CameraThread::~CameraThread() { join(true); @@ -61,10 +51,6 @@ CameraThread::~CameraThread() { delete _camera; } - if(_cameraRGBD) - { - delete _cameraRGBD; - } } void CameraThread::setImageRate(float imageRate) @@ -73,86 +59,48 @@ void CameraThread::setImageRate(float imageRate) { _camera->setImageRate(imageRate); } - if(_cameraRGBD) - { - _cameraRGBD->setImageRate(imageRate); - } -} - -bool CameraThread::init() -{ - if(!this->isRunning()) - { - _seq = 0; - if(_cameraRGBD) - { - return _cameraRGBD->init(); - } - else - { - return _camera->init(); - } - - // Added sleep time to ignore first frames (which are darker) - uSleep(1000); - } - else - { - UERROR("Cannot initialize the camera because it is already running..."); - } - return false; } void CameraThread::mainLoop() { UTimer timer; UDEBUG(""); - cv::Mat rgb, depth; - float fx = 0.0f; - float fyOrBaseline = 0.0f; - float cx = 0.0f; - float cy = 0.0f; - double stamp = UTimer::now(); - if(_cameraRGBD) - { - _cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp); - } - else - { - rgb = _camera->takeImage(); - } + SensorData data = _camera->takeImage(); - if(!rgb.empty()) + if(!data.imageRaw().empty()) { - if(_cameraRGBD) - { - SensorData data; - if(dynamic_cast(_cameraRGBD) || - dynamic_cast(_cameraRGBD) || - dynamic_cast(_cameraRGBD)) - { - //stereo - data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp); - UASSERT(data.stereoCameraModel().isValid()); - } - else - { - data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp); - UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()); - } - this->post(new CameraEvent(data, _cameraRGBD->getSerial())); - } - else + if(_colorOnly && !data.depthRaw().empty()) { - this->post(new CameraEvent(rgb, ++_seq, stamp)); + data.setDepthOrRightRaw(cv::Mat()); } + if(_mirroring && data.cameraModels().size() == 1) + { + cv::Mat tmpRgb; + cv::flip(data.imageRaw(), tmpRgb, 1); + data.setImageRaw(tmpRgb); + if(data.cameraModels()[0].cx()) + { + CameraModel tmpModel( + data.cameraModels()[0].fx(), + data.cameraModels()[0].fy(), + float(data.imageRaw().cols) - data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), + data.cameraModels()[0].localTransform()); + data.setCameraModel(tmpModel); + } + if(!data.depthRaw().empty()) + { + cv::Mat tmpDepth; + cv::flip(data.depthRaw(), tmpDepth, 1); + data.setDepthOrRightRaw(tmpDepth); + } + } + + this->post(new CameraEvent(data, _camera->getSerial())); } else if(!this->isKilled()) { - if(_cameraRGBD) - { - UWARN("no more images..."); - } + UWARN("no more images..."); this->kill(); this->post(new CameraEvent()); } diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index e1478778..48f96eeb 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -51,7 +51,8 @@ DBReader::DBReader(const std::string & databasePath, _odometryIgnored(odometryIgnored), _ignoreGoalDelay(ignoreGoalDelay), _dbDriver(0), - _currentId(_ids.end()) + _currentId(_ids.end()), + _previousStamp(0) { } @@ -64,7 +65,8 @@ DBReader::DBReader(const std::list & databasePaths, _odometryIgnored(odometryIgnored), _ignoreGoalDelay(ignoreGoalDelay), _dbDriver(0), - _currentId(_ids.end()) + _currentId(_ids.end()), + _previousStamp(0) { } diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 8720b948..e8a5cc77 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -60,7 +60,7 @@ void OdometryThread::handleEvent(UEvent * event) if(event->getClassName().compare("CameraEvent") == 0) { CameraEvent * cameraEvent = (CameraEvent*)event; - if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth) + if(cameraEvent->getCode() == CameraEvent::kCodeData) { this->addData(cameraEvent->data()); } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index ad2dd815..7b57adb5 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -302,7 +302,7 @@ void RtabmapThread::handleEvent(UEvent* event) { UDEBUG("CameraEvent"); CameraEvent * e = (CameraEvent*)event; - if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth) + if(e->getCode() == CameraEvent::kCodeData) { this->addData(OdometryEvent(e->data(), Transform(), 1, 1)); } diff --git a/examples/BOWMapping/main.cpp b/examples/BOWMapping/main.cpp index 799f0cb5..f2a2e8bd 100644 --- a/examples/BOWMapping/main.cpp +++ b/examples/BOWMapping/main.cpp @@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/Rtabmap.h" -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" #include #include "rtabmap/utilite/UFile.h" #include @@ -118,12 +118,12 @@ int main(int argc, char * argv[]) int countLoopDetected=0; int i=0; - cv::Mat img = camera.takeImage(); + rtabmap::SensorData data = camera.takeImage(); int nextIndex = rtabmap.getLastLocationId()+1; - while(!img.empty()) + while(!data.imageRaw().empty()) { // Process image : Main loop of RTAB-Map - rtabmap.process(img, nextIndex); + rtabmap.process(data.imageRaw(), nextIndex); // Check if a loop closure is detected and print some info if(rtabmap.getLoopClosureId()) @@ -157,7 +157,7 @@ int main(int argc, char * argv[]) ++nextIndex; //Get next image - img = camera.takeImage(); + data = camera.takeImage(); } printf("Processing images completed. Loop closures found = %d\n", countLoopDetected); diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index 2623d570..7d1b51ef 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -71,7 +71,7 @@ int main(int argc, char * argv[]) // Create the OpenNI camera, it will send a CameraEvent at the rate specified. // Set transform to camera so z is up, y is left and x going forward - CameraRGBD * camera = 0; + Camera * camera = 0; Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); if(driver == 1) { @@ -114,13 +114,14 @@ int main(int argc, char * argv[]) camera = new rtabmap::CameraOpenni("", 0, opticalRotation); } - CameraThread cameraThread(camera); - if(!cameraThread.init()) + if(!camera->init()) { UERROR("Camera init failed!"); - exit(1); } + CameraThread cameraThread(camera); + + // GUI stuff, there the handler will receive RtabmapEvent and construct the map // We give it the camera so the GUI can pause/resume the camera QApplication app(argc, argv); diff --git a/examples/WifiMapping/main.cpp b/examples/WifiMapping/main.cpp index 9ddcdc75..05ad15c6 100644 --- a/examples/WifiMapping/main.cpp +++ b/examples/WifiMapping/main.cpp @@ -109,7 +109,7 @@ int main(int argc, char * argv[]) // Create the OpenNI camera, it will send a CameraEvent at the rate specified. // Set transform to camera so z is up, y is left and x going forward - CameraRGBD * camera = 0; + Camera * camera = 0; Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); if(driver == 1) { @@ -152,17 +152,17 @@ int main(int argc, char * argv[]) camera = new rtabmap::CameraOpenni("", 0, opticalRotation); } - if(mirroring) - { - camera->setMirroringEnabled(true); - } - CameraThread cameraThread(camera); - if(!cameraThread.init()) + if(!camera->init()) { UERROR("Camera init failed!"); //exit(1); } + CameraThread cameraThread(camera); + if(mirroring) + { + cameraThread.setMirroringEnabled(true); + } // GUI stuff, there the handler will receive RtabmapEvent and construct the map // We give it the camera so the GUI can pause/resume the camera diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index a9144251..28886777 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -86,13 +86,6 @@ public: kMonitoringPaused }; - enum SrcType { - kSrcUndefined, - kSrcVideo, - kSrcImages, - kSrcStream - }; - public: /** * @param prefDialog If NULL, a default dialog is created. This @@ -141,10 +134,7 @@ private slots: void deleteMemory(); void openWorkingDirectory(); void updateEditMenu(); - void selectImages(); - void selectVideo(); void selectStream(); - void selectDatabase(); void selectOpenni(); void selectFreenect(); void selectOpenniCv(); @@ -161,6 +151,7 @@ private slots: void downloadPoseGraph(); void clearTheCache(); void openPreferences(); + void openPreferencesSource(); void setDefaultViews(); void selectScreenCaptureFormat(bool checked); void takeScreenshot(); @@ -252,9 +243,6 @@ private: rtabmap::DBReader * _dbReader; rtabmap::OdometryThread * _odomThread; - SrcType _srcType; - QString _srcPath; - //Dialogs PreferencesDialog * _preferencesDialog; AboutDialog * _aboutDialog; diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h index 976ea83c..38aea98f 100644 --- a/guilib/include/rtabmap/gui/OdometryViewer.h +++ b/guilib/include/rtabmap/gui/OdometryViewer.h @@ -58,6 +58,7 @@ protected: virtual void handleEvent(UEvent * event); private slots: + void reset(); void processData(const rtabmap::OdometryEvent & odom); private: diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 18960b49..83cd5d3e 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -59,7 +59,7 @@ namespace rtabmap { class Signature; class LoopClosureViewer; -class CameraRGBD; +class Camera; class CalibrationDialog; class RTABMAPGUI_EXP PreferencesDialog : public QDialog @@ -79,19 +79,27 @@ public: Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag); enum Src { - kSrcUndef, - kSrcUsbDevice, - kSrcImages, - kSrcVideo, - kSrcOpenNI_PCL, - kSrcFreenect, - kSrcOpenNI_CV, - kSrcOpenNI_CV_ASUS, - kSrcOpenNI2, - kSrcFreenect2, - kSrcStereoDC1394, - kSrcStereoFlyCapture2, - kSrcStereoImages + kSrcUndef = -1, + + kSrcRGBD = 0, + kSrcOpenNI_PCL = 0, + kSrcFreenect = 1, + kSrcOpenNI_CV = 2, + kSrcOpenNI_CV_ASUS = 3, + kSrcOpenNI2 = 4, + kSrcFreenect2 = 5, + + kSrcStereo = 100, + kSrcDC1394 = 100, + kSrcFlyCapture2 = 101, + kSrcStereoImages = 102, + + kSrcRGB = 200, + kSrcUsbDevice = 200, + kSrcImages = 201, + kSrcVideo = 202, + + kSrcDatabase = 300 }; public: @@ -100,6 +108,7 @@ public: virtual QString getIniFilePath() const; void init(); + void setCurrentPanelToSource(); // save stuff void saveSettings(); @@ -162,26 +171,23 @@ public: // source panel double getGeneralInputRate() const; bool isSourceMirroring() const; - bool isSourceImageUsed() const; - bool isSourceDatabaseUsed() const; - bool isSourceRGBDUsed() const; - PreferencesDialog::Src getSourceImageType() const; - QString getSourceImageTypeStr() const; - int getSourceWidth() const; - int getSourceHeight() const; + QString getCalibrationName() const; + PreferencesDialog::Src getSourceType() const; + PreferencesDialog::Src getSourceDriver() const; + QString getSourceDriverStr() const; + QString getSourceDevice() const; + QString getSourceImagesPath() const; //Images group QString getSourceImagesSuffix() const; //Images group int getSourceImagesSuffixIndex() const; //Images group int getSourceImagesStartPos() const; //Images group bool getSourceImagesRefreshDir() const; //Images group QString getSourceVideoPath() const; //Video group - int getSourceUsbDeviceId() const; //UsbDevice group QString getSourceDatabasePath() const; //Database group bool getSourceDatabaseOdometryIgnored() const; //Database group bool getSourceDatabaseGoalDelayIgnored() const; //Database group int getSourceDatabaseStartPos() const; //Database group bool getSourceDatabaseStampsUsed() const;//Database group - Src getSourceRGBD() const; // Openni group bool getSourceOpenni2AutoWhiteBalance() const; //Openni group bool getSourceOpenni2AutoExposure() const; //Openni group int getSourceOpenni2Exposure() const; //Openni group @@ -189,9 +195,8 @@ public: bool getSourceOpenni2Mirroring() const; //Openni group int getSourceFreenect2Format() const; //Openni group bool isSourceRGBDColorOnly() const; - QString getSourceOpenniDevice() const; //Openni group - Transform getSourceOpenniLocalTransform() const; //Openni group - CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null + Transform getSourceLocalTransform() const; //Openni group + Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null int getIgnoredDCComponents() const; @@ -221,9 +226,7 @@ public slots: void setDetectionRate(double value); void setTimeLimit(float value); void setSLAMMode(bool enabled); - void selectSourceImage(Src src = kSrcUndef); - void selectSourceDatabase(bool user = false); - void selectSourceRGBD(Src src = kSrcUndef); + void selectSourceDriver(Src src); void calibrate(); private slots: @@ -251,11 +254,19 @@ private slots: void setupTreeView(); void updateBasicParameter(); void openDatabaseViewer(); + void selectSourceDatabase(); void selectSourceStereoImagesStamps(); void selectSourceStereoImagesPath(); + void selectSourceImagesPath(); + void selectSourceVideoPath(); + void selectSourceOniPath(); + void selectSourceOni2Path(); + void updateSourceGrpVisibility(); void updateRGBDCameraGroupBoxVisibility(); + void updateRGBCameraGroupBoxVisibility(); + void updateStereoCameraGroupBoxVisibility(); void testOdometry(); - void testRGBDCamera(); + void testCamera(); protected: virtual void showEvent ( QShowEvent * event ); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index c69c7010..65e97fde 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "AboutDialog.h" #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/Graph.h" #include "ui_aboutDialog.h" #include diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index bd43cc49..1ef53713 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -167,6 +167,7 @@ void CalibrationDialog::setStereoMode(bool stereo) ui_->lineEdit_R_2->setVisible(stereo_); ui_->lineEdit_P_2->setVisible(stereo_); ui_->radioButton_stereoRectified->setVisible(stereo_); + ui_->checkBox_switchImages->setVisible(stereo_); } void CalibrationDialog::setBoardWidth(int width) @@ -234,8 +235,7 @@ void CalibrationDialog::handleEvent(UEvent * event) if(event->getClassName().compare("CameraEvent") == 0) { rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event; - if(e->getCode() == rtabmap::CameraEvent::kCodeImage || - e->getCode() == rtabmap::CameraEvent::kCodeImageDepth) + if(e->getCode() == rtabmap::CameraEvent::kCodeData) { processingData_ = true; QMetaObject::invokeMethod(this, "processImages", diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index a8da041d..c5b3d664 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -76,17 +77,33 @@ CameraViewer::~CameraViewer() void CameraViewer::showImage(const rtabmap::SensorData & data) { processingImages_ = true; - imageView_->setImage(uCvMat2QImage(data.imageRaw())); - imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw())); - if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size())) + if(!data.imageRaw().empty()) { - cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data)); + imageView_->setImage(uCvMat2QImage(data.imageRaw())); + } + if(!data.depthOrRightRaw().empty()) + { + imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw())); + } + if((data.stereoCameraModel().isValid() || data.cameraModels().size())) + { + if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty()) + { + cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data)); + cloudView_->setVisible(true); + cloudView_->update(); + } + else if(!data.depthOrRightRaw().empty()) + { + cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data)); + cloudView_->setVisible(true); + cloudView_->update(); + } } else { cloudView_->setVisible(false); } - cloudView_->update(); processingImages_ = false; } @@ -95,8 +112,7 @@ void CameraViewer::handleEvent(UEvent * event) if(event->getClassName().compare("CameraEvent") == 0) { CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeImageDepth || - camEvent->getCode() == CameraEvent::kCodeImage) + if(camEvent->getCode() == CameraEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 7d9734d5..c7909096 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -171,8 +171,7 @@ void DataRecorder::handleEvent(UEvent * event) if(event->getClassName().compare("CameraEvent") == 0) { CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeImageDepth || - camEvent->getCode() == CameraEvent::kCodeImage) + if(camEvent->getCode() == CameraEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 307cc2f2..103266f7 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -29,5 +29,6 @@ images/sense.png images/xtion_pro_live.png images/bumblebee2.png + images/webcam.png diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index de17f657..3fc5def1 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -29,7 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ui_mainWindow.h" -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" +#include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/CameraThread.h" #include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/DBReader.h" @@ -118,7 +119,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _camera(0), _dbReader(0), _odomThread(0), - _srcType(kSrcUndefined), _preferencesDialog(0), _aboutDialog(0), _exportDialog(0), @@ -338,7 +338,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->actionPost_processing->setEnabled(false); QToolButton* toolButton = new QToolButton(this); - toolButton->setMenu(_ui->menuRGB_D_camera); + toolButton->setMenu(_ui->menuSelect_source); toolButton->setPopupMode(QToolButton::InstantPopup); toolButton->setIcon(QIcon(":images/kinect_xbox_360.png")); toolButton->setToolTip("Select sensor driver"); @@ -351,10 +351,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : #endif //Settings menu - connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages())); - connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo())); + connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource())); connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream())); - connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase())); connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni())); connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni())); connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect())); @@ -2113,39 +2111,26 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags) // Camera settings... _ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate()); this->updateSelectSourceMenu(); - QString src; - if(_preferencesDialog->isSourceImageUsed()) - { - src = _preferencesDialog->getSourceImageTypeStr(); - } - else if(_preferencesDialog->isSourceDatabaseUsed()) - { - src = "Database"; - } - _ui->label_stats_source->setText(src); + _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr()); if(_camera) { _camera->setImageRate(_preferencesDialog->getGeneralInputRate()); - if(_camera->cameraRGBD() && dynamic_cast(_camera->cameraRGBD()) != 0) + if(_camera->camera() && dynamic_cast(_camera->camera()) != 0) { - ((CameraOpenNI2*)_camera->cameraRGBD())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance()); - ((CameraOpenNI2*)_camera->cameraRGBD())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)_camera->camera())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance()); + ((CameraOpenNI2*)_camera->camera())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure()); if(CameraOpenNI2::exposureGainAvailable()) { - ((CameraOpenNI2*)_camera->cameraRGBD())->setExposure(_preferencesDialog->getSourceOpenni2Exposure()); - ((CameraOpenNI2*)_camera->cameraRGBD())->setGain(_preferencesDialog->getSourceOpenni2Gain()); + ((CameraOpenNI2*)_camera->camera())->setExposure(_preferencesDialog->getSourceOpenni2Exposure()); + ((CameraOpenNI2*)_camera->camera())->setGain(_preferencesDialog->getSourceOpenni2Gain()); } } - if(_camera->camera()) + if(_camera) { - _camera->camera()->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - } - if(_camera->cameraRGBD()) - { - _camera->cameraRGBD()->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - _camera->cameraRGBD()->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); + _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); + _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); } } if(_dbReader) @@ -2433,23 +2418,25 @@ bool MainWindow::eventFilter(QObject *obj, QEvent *event) void MainWindow::updateSelectSourceMenu() { - _ui->actionUsbCamera->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcUsbDevice); - _ui->actionImageFiles->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages); - _ui->actionVideo->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo); + _ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice); - _ui->actionDatabase->setChecked(_preferencesDialog->isSourceDatabaseUsed()); + _ui->actionMore_options->setChecked( + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages); - _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL); - _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL); - _ui->actionFreenect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect); - _ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV); - _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS); - _ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2); - _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2); - _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2); - _ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2); - _ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394); - _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoFlyCapture2); + _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL); + _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL); + _ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect); + _ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV); + _ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS); + _ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); + _ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); + _ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2); + _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2); + _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394); + _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2); } void MainWindow::changeImgRateSetting() @@ -2674,11 +2661,9 @@ void MainWindow::startDetection() { ParametersMap parameters = _preferencesDialog->getAllParameters(); // verify source with input rates - if((_preferencesDialog->isSourceImageUsed() && - (_preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages || - _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo)) - || - _preferencesDialog->isSourceDatabaseUsed()) + if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase) { float inputRate = _preferencesDialog->getGeneralInputRate(); float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate())); @@ -2748,9 +2733,7 @@ void MainWindow::startDetection() } // Adjust pre-requirements - if( !_preferencesDialog->isSourceImageUsed() && - !_preferencesDialog->isSourceDatabaseUsed() && - !_preferencesDialog->isSourceRGBDUsed()) + if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef) { QMessageBox::warning(this, tr("RTAB-Map"), @@ -2760,41 +2743,19 @@ void MainWindow::startDetection() return; } - if(_preferencesDialog->isSourceRGBDUsed()) - { - CameraRGBD * camera = _preferencesDialog->createCameraRGBD(); - if(!camera->init(_preferencesDialog->getCameraInfoDir().toStdString())) + if(_preferencesDialog->getSourceDriver() < PreferencesDialog::kSrcDatabase) + { + Camera * camera = _preferencesDialog->createCamera(); + if(!camera) { - ULOGGER_WARN("init camera failed... "); - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("Camera initialization failed...")); emit stateChanged(kInitialized); - delete camera; - camera = 0; - if(_odomThread) - { - delete _odomThread; - _odomThread = 0; - } return; } - else if(dynamic_cast(camera) != 0) - { - ((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance()); - ((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure()); - ((CameraOpenNI2*)camera)->setMirroring(_preferencesDialog->getSourceOpenni2Mirroring()); - if(CameraOpenNI2::exposureGainAvailable()) - { - ((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure()); - ((CameraOpenNI2*)camera)->setGain(_preferencesDialog->getSourceOpenni2Gain()); - } - } - camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); _camera = new CameraThread(camera); + _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); + _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); //Create odometry thread if rgbd slam if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str())) @@ -2823,6 +2784,7 @@ void MainWindow::startDetection() { UERROR("OdomThread must be already deleted here?!"); delete _odomThread; + _odomThread = 0; } Odometry * odom; if(_preferencesDialog->getOdomStrategy() == 1) @@ -2845,7 +2807,7 @@ void MainWindow::startDetection() } } } - else if(_preferencesDialog->isSourceDatabaseUsed()) + else if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase) { _dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(), _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(), @@ -2902,58 +2864,6 @@ void MainWindow::startDetection() UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent"); } } - else - { - if(_preferencesDialog->isSourceImageUsed()) - { - Camera * camera = 0; - // Change type of the camera... - // - int sourceType = _preferencesDialog->getSourceImageType(); - UASSERT(sourceType >= PreferencesDialog::kSrcUsbDevice && sourceType <= PreferencesDialog::kSrcVideo); - if(sourceType == PreferencesDialog::kSrcImages) //Images - { - camera = new CameraImages( - _preferencesDialog->getSourceImagesPath().append(QDir::separator()).toStdString(), - _preferencesDialog->getSourceImagesStartPos(), - _preferencesDialog->getSourceImagesRefreshDir(), - _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceWidth(), - _preferencesDialog->getSourceHeight()); - } - else if(sourceType == PreferencesDialog::kSrcVideo) - { - camera = new CameraVideo( - _preferencesDialog->getSourceVideoPath().toStdString(), - _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceWidth(), - _preferencesDialog->getSourceHeight()); - } - else //if(sourceType == PreferencesDialog::kSrcUsbDevice) - { - camera = new CameraVideo( - _preferencesDialog->getSourceUsbDeviceId(), - _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceWidth(), - _preferencesDialog->getSourceHeight()); - } - - if(!camera->init()) - { - ULOGGER_WARN("init camera failed... "); - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("Camera initialization failed...")); - emit stateChanged(kInitialized); - delete camera; - camera = 0; - return; - } - camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - - _camera = new CameraThread(camera); - } - } if(_dataRecorder) { @@ -3810,64 +3720,49 @@ void MainWindow::updateEditMenu() } } -void MainWindow::selectImages() -{ - _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages); -} - -void MainWindow::selectVideo() -{ - _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo); -} - void MainWindow::selectStream() { - _preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice); -} - -void MainWindow::selectDatabase() -{ - _preferencesDialog->selectSourceDatabase(true); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice); } void MainWindow::selectOpenni() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL); } void MainWindow::selectFreenect() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect); } void MainWindow::selectOpenniCv() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV); } void MainWindow::selectOpenniCvAsus() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS); } void MainWindow::selectOpenni2() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2); } void MainWindow::selectFreenect2() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect2); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2); } void MainWindow::selectStereoDC1394() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394); } void MainWindow::selectStereoFlyCapture2() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoFlyCapture2); + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2); } @@ -4116,6 +4011,13 @@ void MainWindow::openPreferences() _preferencesDialog->exec(); } +void MainWindow::openPreferencesSource() +{ + _preferencesDialog->setCurrentPanelToSource(); + openPreferences(); + this->updateSelectSourceMenu(); +} + void MainWindow::setDefaultViews() { _ui->dockWidget_posterior->setVisible(false); diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 4af699b0..2062820a 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -97,8 +97,10 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f decimationSpin_->setMaximum(16); decimationSpin_->setValue(decimation); timeLabel_ = new QLabel(this); + QPushButton * resetButton = new QPushButton("reset", this); QPushButton * clearButton = new QPushButton("clear", this); QPushButton * closeButton = new QPushButton("close", this); + connect(resetButton, SIGNAL(clicked()), this, SLOT(reset())); connect(clearButton, SIGNAL(clicked()), this, SLOT(clear())); connect(closeButton, SIGNAL(clicked()), this, SLOT(reject())); @@ -121,6 +123,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f hlayout2->addWidget(decimationSpin_); hlayout2->addWidget(timeLabel_); hlayout2->addStretch(1); + hlayout2->addWidget(resetButton); hlayout2->addWidget(clearButton); hlayout2->addWidget(closeButton); @@ -140,6 +143,11 @@ OdometryViewer::~OdometryViewer() UDEBUG(""); } +void OdometryViewer::reset() +{ + this->post(new OdometryResetEvent()); +} + void OdometryViewer::clear() { addedClouds_.clear(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 80f44e78..7ae62dd1 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -51,10 +51,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraThread.h" -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" +#include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/Graph.h" +#include "rtabmap/core/DBReader.h" #include "rtabmap/gui/LoopClosureViewer.h" #include "rtabmap/gui/CameraViewer.h" @@ -202,7 +204,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable()); // Default Driver + connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility())); + connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBCameraGroupBoxVisibility())); + connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoCameraGroupBoxVisibility())); this->resetSettings(_ui->groupBox_source0); _ui->predictionPlot->showLegend(false); @@ -219,7 +224,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig())); connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView())); connect(_ui->pushButton_testOdometry, SIGNAL(clicked()), this, SLOT(testOdometry())); - connect(_ui->pushButton_test_rgbd_camera, SIGNAL(clicked()), this, SLOT(testRGBDCamera())); + connect(_ui->pushButton_test_camera, SIGNAL(clicked()), this, SLOT(testCamera())); // General panel connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel())); @@ -310,22 +315,24 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : //Source panel connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_calibrationName, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + _ui->stackedWidget_src->setCurrentIndex(_ui->comboBox_sourceType->currentIndex()); + connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_src, SLOT(setCurrentIndex(int))); + connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_sourceDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_sourceLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + //Image source - connect(_ui->groupBox_sourceImage, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel())); _ui->stackedWidget_image->setCurrentIndex(_ui->source_comboBox_image_type->currentIndex()); connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_image, SLOT(setCurrentIndex(int))); connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - //usbDevice group - connect(_ui->source_usbDevice_spinBox_id, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->source_spinBox_imgWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->source_spinBox_imgheight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //images group - connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImage())); + connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImagesPath())); connect(_ui->source_images_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //video group - connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImage())); + connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath())); connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); //database group connect(_ui->source_database_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceDatabase())); @@ -338,7 +345,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //openni group - connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel())); + _ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex()); + connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_rgbd, SLOT(setCurrentIndex(int))); connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_autoWhiteBalance, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -351,9 +359,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->toolButton_cameraStereoImages_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPath())); connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); + connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); + connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path())); + connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + //Rtabmap basic @@ -672,6 +683,20 @@ void PreferencesDialog::init() _initialized = true; } +void PreferencesDialog::setCurrentPanelToSource() +{ + QList boxes = this->getGroupBoxes(); + for(int i =0;igroupBox_source0) + { + _ui->stackedWidget->setCurrentIndex(i); + _ui->treeView->setCurrentIndex(_indexModel->index(i-2, 0)); + break; + } + } +} + void PreferencesDialog::saveSettings() { writeSettings(); @@ -990,49 +1015,51 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) } else if(groupBox->objectName() == _ui->groupBox_source0->objectName()) { - _ui->general_doubleSpinBox_imgRate->setValue(30.0); + _ui->general_doubleSpinBox_imgRate->setValue(0.0); _ui->source_mirroring->setChecked(false); + _ui->lineEdit_calibrationName->clear(); + _ui->comboBox_sourceType->setCurrentIndex(kSrcRGBD); + _ui->lineEdit_sourceDevice->setText(""); + _ui->lineEdit_sourceLocalTransform->setText("0 0 0 -PI_2 0 -PI_2"); - _ui->groupBox_sourceImage->setChecked(false); - _ui->source_spinBox_imgWidth->setValue(0); - _ui->source_spinBox_imgheight->setValue(0); + _ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice); _ui->source_images_spinBox_startPos->setValue(1); _ui->source_images_refreshDir->setChecked(false); - _ui->groupBox_sourceDatabase->setChecked(false); _ui->source_checkBox_ignoreOdometry->setChecked(false); _ui->source_checkBox_ignoreGoalDelay->setChecked(false); _ui->source_spinBox_databaseStartPos->setValue(0); _ui->source_checkBox_useDbStamps->setChecked(false); - _ui->groupBox_sourceOpenni->setChecked(true); #ifdef _WIN32 - _ui->comboBox_cameraRGBD->setCurrentIndex(4); // openni2 + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2 #else if(CameraFreenect::available()) { - _ui->comboBox_cameraRGBD->setCurrentIndex(1); // freenect + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcFreenect-kSrcOpenNI_PCL); // freenect } else if(CameraOpenNI2::available()) { - _ui->comboBox_cameraRGBD->setCurrentIndex(4); // openni2 + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2 } else { - _ui->comboBox_cameraRGBD->setCurrentIndex(0); // openni-pcl + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI_PCL-kSrcOpenNI_PCL); // openni-pcl } #endif + _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); _ui->openni2_autoExposure->setChecked(true); _ui->openni2_exposure->setValue(0); _ui->openni2_gain->setValue(100); _ui->openni2_mirroring->setChecked(false); _ui->comboBox_freenect2Format->setCurrentIndex(0); + _ui->lineEdit_openniOniPath->clear(); + _ui->lineEdit_openni2OniPath->clear(); + + _ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394); _ui->lineEdit_cameraStereoImages_timestamps->setText(""); _ui->lineEdit_cameraStereoImages_path->setText(""); - _ui->checkbox_rgbd_colorOnly->setChecked(false); - _ui->lineEdit_openniDevice->setText(""); - _ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2"); } else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName()) { @@ -1255,30 +1282,59 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) QSettings settings(path, QSettings::IniFormat); settings.beginGroup("Camera"); - _ui->groupBox_sourceImage->setChecked(settings.value("imageUsed", _ui->groupBox_sourceImage->isChecked()).toBool()); _ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble()); _ui->source_mirroring->setChecked(settings.value("mirroring", _ui->source_mirroring->isChecked()).toBool()); - _ui->source_comboBox_image_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_image_type->currentIndex()).toInt()); - _ui->source_spinBox_imgWidth->setValue(settings.value("imgWidth",_ui->source_spinBox_imgWidth->value()).toInt()); - _ui->source_spinBox_imgheight->setValue(settings.value("imgHeight",_ui->source_spinBox_imgheight->value()).toInt()); - //usbDevice group - settings.beginGroup("usbDevice"); - _ui->source_usbDevice_spinBox_id->setValue(settings.value("id",_ui->source_usbDevice_spinBox_id->value()).toInt()); - settings.endGroup(); // usbDevice - //images group - settings.beginGroup("images"); + _ui->lineEdit_calibrationName->setText(settings.value("calibrationName", _ui->lineEdit_calibrationName->text()).toString()); + _ui->comboBox_sourceType->setCurrentIndex(settings.value("type", _ui->comboBox_sourceType->currentIndex()).toInt()); + _ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString()); + _ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString()); + + settings.beginGroup("rgbd"); + _ui->comboBox_cameraRGBD->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraRGBD->currentIndex()).toInt()); + _ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool()); + settings.endGroup(); // rgbd + + settings.beginGroup("stereo"); + _ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt()); + settings.endGroup(); // stereo + + settings.beginGroup("rgb"); + _ui->source_comboBox_image_type->setCurrentIndex(settings.value("driver", _ui->source_comboBox_image_type->currentIndex()).toInt()); + settings.endGroup(); // rgb + + settings.beginGroup("Openni"); + _ui->lineEdit_openniOniPath->setText(settings.value("oniPath", _ui->lineEdit_openniOniPath->text()).toString()); + settings.endGroup(); // Openni + + settings.beginGroup("Openni2"); + _ui->openni2_autoWhiteBalance->setChecked(settings.value("autoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked()).toBool()); + _ui->openni2_autoExposure->setChecked(settings.value("autoExposure", _ui->openni2_autoExposure->isChecked()).toBool()); + _ui->openni2_exposure->setValue(settings.value("exposure", _ui->openni2_exposure->value()).toInt()); + _ui->openni2_gain->setValue(settings.value("gain", _ui->openni2_gain->value()).toInt()); + _ui->openni2_mirroring->setChecked(settings.value("mirroring", _ui->openni2_mirroring->isChecked()).toBool()); + _ui->lineEdit_openni2OniPath->setText(settings.value("oniPath", _ui->lineEdit_openni2OniPath->text()).toString()); + settings.endGroup(); // Openni2 + + settings.beginGroup("Freenect2"); + _ui->comboBox_freenect2Format->setCurrentIndex(settings.value("format", _ui->comboBox_freenect2Format->currentIndex()).toInt()); + settings.endGroup(); // Freenect2 + + settings.beginGroup("StereoImages"); + _ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString()); + _ui->lineEdit_cameraStereoImages_path->setText(settings.value("path", _ui->lineEdit_cameraStereoImages_path->text()).toString()); + settings.endGroup(); // StereoImages + + settings.beginGroup("Images"); _ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString()); _ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt()); _ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool()); settings.endGroup(); // images - //video group - settings.beginGroup("video"); + + settings.beginGroup("Video"); _ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString()); settings.endGroup(); // video - settings.endGroup(); // Camera settings.beginGroup("Database"); - _ui->groupBox_sourceDatabase->setChecked(settings.value("databaseUsed", _ui->groupBox_sourceDatabase->isChecked()).toBool()); _ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString()); _ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool()); _ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool()); @@ -1286,22 +1342,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool()); settings.endGroup(); // Database - settings.beginGroup("Openni"); - _ui->groupBox_sourceOpenni->setChecked(settings.value("openniUsed", _ui->groupBox_sourceOpenni->isChecked()).toBool()); - _ui->comboBox_cameraRGBD->setCurrentIndex(settings.value("cameraRGBDType", _ui->comboBox_cameraRGBD->currentIndex()).toInt()); - _ui->openni2_autoWhiteBalance->setChecked(settings.value("openni2AutoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked()).toBool()); - _ui->openni2_autoExposure->setChecked(settings.value("openni2AutoExposure", _ui->openni2_autoExposure->isChecked()).toBool()); - _ui->openni2_exposure->setValue(settings.value("openni2Exposure", _ui->openni2_exposure->value()).toInt()); - _ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt()); - _ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool()); - _ui->comboBox_freenect2Format->setCurrentIndex(settings.value("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()).toInt()); - _ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString()); - _ui->lineEdit_cameraStereoImages_path->setText(settings.value("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text()).toString()); - _ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool()); - _ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString()); - _ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString()); + settings.endGroup(); // Camera + _calibrationDialog->loadSettings(settings, "CalibrationDialog"); - settings.endGroup(); // Openni + } bool PreferencesDialog::readCoreSettings(const QString & filePath) @@ -1525,55 +1569,71 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const path = filePath; } QSettings settings(path, QSettings::IniFormat); + settings.beginGroup("Camera"); - settings.setValue("imageUsed", _ui->groupBox_sourceImage->isChecked()); settings.setValue("imgRate", _ui->general_doubleSpinBox_imgRate->value()); settings.setValue("mirroring", _ui->source_mirroring->isChecked()); - settings.setValue("type", _ui->source_comboBox_image_type->currentIndex()); - settings.setValue("imgWidth", _ui->source_spinBox_imgWidth->value()); - settings.setValue("imgHeight", _ui->source_spinBox_imgheight->value()); - //usbDevice group - settings.beginGroup("usbDevice"); - settings.setValue("id", _ui->source_usbDevice_spinBox_id->value()); - settings.endGroup(); //usbDevice - //images group - settings.beginGroup("images"); + settings.setValue("calibrationName", _ui->lineEdit_calibrationName->text()); + settings.setValue("type", _ui->comboBox_sourceType->currentIndex()); + settings.setValue("device", _ui->lineEdit_sourceDevice->text()); + settings.setValue("localTransform", _ui->lineEdit_sourceLocalTransform->text()); + + settings.beginGroup("rgbd"); + settings.setValue("driver", _ui->comboBox_cameraRGBD->currentIndex()); + settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()); + settings.endGroup(); // rgbd + + settings.beginGroup("stereo"); + settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex()); + settings.endGroup(); // stereo + + settings.beginGroup("rgb"); + settings.setValue("driver", _ui->source_comboBox_image_type->currentIndex()); + settings.endGroup(); // rgb + + settings.beginGroup("Openni"); + settings.setValue("oniPath", _ui->lineEdit_openniOniPath->text()); + settings.endGroup(); // Openni + + settings.beginGroup("Openni2"); + settings.setValue("autoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked()); + settings.setValue("autoExposure", _ui->openni2_autoExposure->isChecked()); + settings.setValue("exposure", _ui->openni2_exposure->value()); + settings.setValue("gain", _ui->openni2_gain->value()); + settings.setValue("mirroring", _ui->openni2_mirroring->isChecked()); + settings.setValue("oniPath", _ui->lineEdit_openni2OniPath->text()); + settings.endGroup(); // Openni2 + + settings.beginGroup("Freenect2"); + settings.setValue("format", _ui->comboBox_freenect2Format->currentIndex()); + settings.endGroup(); // Freenect2 + + settings.beginGroup("StereoImages"); + settings.setValue("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()); + settings.setValue("path", _ui->lineEdit_cameraStereoImages_path->text()); + settings.endGroup(); // StereoImages + + settings.beginGroup("Images"); settings.setValue("path", _ui->source_images_lineEdit_path->text()); settings.setValue("startPos", _ui->source_images_spinBox_startPos->value()); settings.setValue("refreshDir", _ui->source_images_refreshDir->isChecked()); - settings.endGroup(); //images - //video group - settings.beginGroup("video"); - settings.setValue("path", _ui->source_video_lineEdit_path->text()); - settings.endGroup(); //video + settings.endGroup(); // images - settings.endGroup(); // Camera + settings.beginGroup("Video"); + settings.setValue("path", _ui->source_video_lineEdit_path->text()); + settings.endGroup(); // video settings.beginGroup("Database"); - settings.setValue("databaseUsed", _ui->groupBox_sourceDatabase->isChecked()); settings.setValue("path", _ui->source_database_lineEdit_path->text()); settings.setValue("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()); settings.setValue("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()); settings.setValue("startPos", _ui->source_spinBox_databaseStartPos->value()); settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()); - settings.endGroup(); + settings.endGroup(); // Database + + settings.endGroup(); // Camera - settings.beginGroup("Openni"); - settings.setValue("openniUsed", _ui->groupBox_sourceOpenni->isChecked()); - settings.setValue("cameraRGBDType", _ui->comboBox_cameraRGBD->currentIndex()); - settings.setValue("openni2AutoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked()); - settings.setValue("openni2AutoExposure", _ui->openni2_autoExposure->isChecked()); - settings.setValue("openni2Exposure", _ui->openni2_exposure->value()); - settings.setValue("openni2Gain", _ui->openni2_gain->value()); - settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked()); - settings.setValue("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()); - settings.setValue("stereoImagesStamps", _ui->lineEdit_cameraStereoImages_timestamps->text()); - settings.setValue("stereoImagesPath", _ui->lineEdit_cameraStereoImages_path->text()); - settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()); - settings.setValue("device", _ui->lineEdit_openniDevice->text()); - settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text()); _calibrationDialog->saveSettings(settings, "CalibrationDialog"); - settings.endGroup(); // Openni } void PreferencesDialog::writeCoreSettings(const QString & filePath) const @@ -1999,184 +2059,36 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri return value; } -void PreferencesDialog::selectSourceImage(Src src) + +void PreferencesDialog::selectSourceDriver(Src src) { - ULOGGER_DEBUG(""); - - bool fromPrefDialog = false; - //bool modified = false; - if(src == kSrcUndef) + if(src >= kSrcRGBD && srcsource_comboBox_image_type->currentIndex() == 1) + _ui->comboBox_sourceType->setCurrentIndex(0); + _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcRGBD); + if(src == kSrcOpenNI_PCL) { - src = kSrcImages; + _ui->lineEdit_openniOniPath->clear(); } - else if(_ui->source_comboBox_image_type->currentIndex() == 2) + else if(src == kSrcOpenNI2) { - src = kSrcVideo; - } - else - { - src = kSrcUsbDevice; + _ui->lineEdit_openni2OniPath->clear(); } } - - if(!fromPrefDialog) + else if(src >= kSrcStereo && srcgeneral_checkBox_activateRGBD->isChecked()) - { - int button = QMessageBox::information(this, - tr("Desactivate RGB-D SLAM?"), - tr("You've selected source input as images only and RGB-D SLAM mode is activated. " - "RGB-D SLAM cannot work with images only so do you want to desactivate it?"), - QMessageBox::Yes | QMessageBox::No); - if(button & QMessageBox::Yes) - { - _ui->general_checkBox_activateRGBD->setChecked(false); - } - } + _ui->comboBox_sourceType->setCurrentIndex(1); + _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcStereo); } - - if(src == kSrcImages) + else if(src >= kSrcRGB && srcsource_images_lineEdit_path->text()); - QDir dir(path); - if(!path.isEmpty() && dir.exists()) - { - QStringList filters; - filters << "*.jpg" << "*.ppm" << "*.bmp" << "*.png" << "*.pnm" << "*.tiff"; - dir.setNameFilters(filters); - QFileInfoList files = dir.entryInfoList(); - if(!files.empty()) - { - _ui->source_comboBox_image_type->setCurrentIndex(1); - _ui->source_images_lineEdit_path->setText(path); - _ui->source_images_spinBox_startPos->setValue(1); - _ui->source_images_refreshDir->setChecked(false); - _ui->groupBox_sourceImage->setChecked(true); - } - else - { - QMessageBox::information(this, - tr("RTAB-Map"), - tr("Images must be one of these formats: ") + filters.join(" ")); - } - } + _ui->comboBox_sourceType->setCurrentIndex(2); + _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcRGB); } - else if(src == kSrcVideo) + else if(src >= kSrcDatabase) { - QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)")); - QFile file(path); - if(!path.isEmpty() && file.exists()) - { - _ui->source_comboBox_image_type->setCurrentIndex(2); - _ui->source_video_lineEdit_path->setText(path); - _ui->groupBox_sourceImage->setChecked(true); - } - } - else // kSrcUsbDevice - { - _ui->source_comboBox_image_type->setCurrentIndex(0); - _ui->groupBox_sourceImage->setChecked(true); - } - - if(_ui->groupBox_sourceImage->isChecked()) - { - _ui->groupBox_sourceDatabase->setChecked(false); - _ui->groupBox_sourceOpenni->setChecked(false); - } - - if(!fromPrefDialog) - { - // Even if there is no change, MainWindow should be notified - makeObsoleteSourcePanel(); - - if(validateForm()) - { - this->writeSettings(getTmpIniFilePath()); - } - else - { - this->readSettingsBegin(); - } - } -} - -void PreferencesDialog::selectSourceDatabase(bool user) -{ - ULOGGER_DEBUG(""); - - QString dir = _ui->source_database_lineEdit_path->text(); - if(dir.isEmpty()) - { - dir = getWorkingDirectory(); - } - QStringList paths = QFileDialog::getOpenFileNames(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)")); - if(paths.size()) - { - int r = QMessageBox::question(this, tr("Odometry in database..."), tr("Use odometry saved in database (if some saved)?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes); - - _ui->groupBox_sourceDatabase->setChecked(true); - _ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes); - _ui->source_checkBox_ignoreGoalDelay->setChecked(false); - _ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";")); - _ui->source_spinBox_databaseStartPos->setValue(0); - _ui->source_checkBox_useDbStamps->setChecked(false); - } - - if(_ui->groupBox_sourceDatabase->isChecked()) - { - _ui->groupBox_sourceImage->setChecked(false); - _ui->groupBox_sourceOpenni->setChecked(false); - } - - if(user) - { - // Even if there is no change, MainWindow should be notified - makeObsoleteSourcePanel(); - - if(validateForm()) - { - this->writeSettings(getTmpIniFilePath()); - } - else - { - this->readSettingsBegin(); - } - } -} - -void PreferencesDialog::selectSourceRGBD(Src src) -{ - ULOGGER_DEBUG(""); - - if(!_ui->general_checkBox_activateRGBD->isChecked()) - { - int button = QMessageBox::information(this, - tr("Activate RGB-D SLAM?"), - tr("You've selected RGB-D camera as source input, " - "would you want to activate RGB-D SLAM mode?"), - QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes); - if(button & QMessageBox::Yes) - { - _ui->general_checkBox_activateRGBD->setChecked(true); - } - } - - _ui->groupBox_sourceOpenni->setChecked(true); - _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcOpenNI_PCL); - - if(_ui->groupBox_sourceOpenni->isChecked()) - { - _ui->groupBox_sourceImage->setChecked(false); - _ui->groupBox_sourceDatabase->setChecked(false); - } - - if(src == kSrcStereoImages) - { - _ui->lineEdit_cameraStereoImages_timestamps->setText(""); - _ui->lineEdit_cameraStereoImages_path->setText(""); + _ui->comboBox_sourceType->setCurrentIndex(3); + _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcDatabase); } if(validateForm()) @@ -2192,6 +2104,24 @@ void PreferencesDialog::selectSourceRGBD(Src src) } } +void PreferencesDialog::selectSourceDatabase() +{ + QString dir = _ui->source_database_lineEdit_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QStringList paths = QFileDialog::getOpenFileNames(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)")); + if(paths.size()) + { + int r = QMessageBox::question(this, tr("Odometry in database..."), tr("Use odometry saved in database (if some saved)?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes); + + _ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes); + _ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";")); + _ui->source_spinBox_databaseStartPos->setValue(0); + } +} + void PreferencesDialog::openDatabaseViewer() { DatabaseViewer * viewer = new DatabaseViewer(this); @@ -2236,6 +2166,63 @@ void PreferencesDialog::selectSourceStereoImagesPath() } } +void PreferencesDialog::selectSourceImagesPath() +{ + QString dir = _ui->source_images_lineEdit_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getExistingDirectory(this, tr("Select images directory"), _ui->source_images_lineEdit_path->text()); + if(!path.isEmpty()) + { + _ui->source_images_lineEdit_path->setText(path); + _ui->source_images_spinBox_startPos->setValue(0); + } +} + +void PreferencesDialog::selectSourceVideoPath() +{ + QString dir = _ui->source_video_lineEdit_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)")); + if(!path.isEmpty()) + { + _ui->source_video_lineEdit_path->setText(path); + } +} + +void PreferencesDialog::selectSourceOniPath() +{ + QString dir = _ui->lineEdit_openniOniPath->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_openniOniPath->text(), tr("OpenNI (*.oni)")); + if(!path.isEmpty()) + { + _ui->lineEdit_openniOniPath->setText(path); + } +} + +void PreferencesDialog::selectSourceOni2Path() +{ + QString dir = _ui->lineEdit_openni2OniPath->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_openni2OniPath->text(), tr("OpenNI (*.oni)")); + if(!path.isEmpty()) + { + _ui->lineEdit_openni2OniPath->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -2748,22 +2735,6 @@ void PreferencesDialog::makeObsoleteLoggingPanel() void PreferencesDialog::makeObsoleteSourcePanel() { - if(sender() == _ui->groupBox_sourceDatabase && _ui->groupBox_sourceDatabase->isChecked()) - { - _ui->groupBox_sourceImage->setChecked(false); - _ui->groupBox_sourceOpenni->setChecked(false); - } - else if(sender() == _ui->groupBox_sourceImage && _ui->groupBox_sourceImage->isChecked()) - { - _ui->groupBox_sourceDatabase->setChecked(false); - _ui->groupBox_sourceOpenni->setChecked(false); - } - else if(sender() == _ui->groupBox_sourceOpenni && _ui->groupBox_sourceOpenni->isChecked()) - { - _ui->groupBox_sourceImage->setChecked(false); - _ui->groupBox_sourceDatabase->setChecked(false); - } - ULOGGER_DEBUG(""); _obsoletePanels = _obsoletePanels | kPanelSource; } @@ -2946,11 +2917,29 @@ void PreferencesDialog::changeOdomBowFixedLocalMapPath() } } +void PreferencesDialog::updateSourceGrpVisibility() +{ + _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); + _ui->groupBox_sourceStereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1); + _ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2); + _ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3); +} + void PreferencesDialog::updateRGBDCameraGroupBoxVisibility() { _ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL); _ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL); - _ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcStereoImages-kSrcOpenNI_PCL); +} + +void PreferencesDialog::updateRGBCameraGroupBoxVisibility() +{ + _ui->source_groupBox_images->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcUsbDevice); + _ui->source_groupBox_video->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcUsbDevice); +} + +void PreferencesDialog::updateStereoCameraGroupBoxVisibility() +{ + _ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcDC1394); } /*** GETTERS ***/ @@ -3117,129 +3106,82 @@ bool PreferencesDialog::isSourceMirroring() const { return _ui->source_mirroring->isChecked(); } -bool PreferencesDialog::isSourceImageUsed() const +QString PreferencesDialog::getCalibrationName() const { - return _ui->groupBox_sourceImage->isChecked(); + return _ui->lineEdit_calibrationName->text(); } -bool PreferencesDialog::isSourceDatabaseUsed() const +PreferencesDialog::Src PreferencesDialog::getSourceType() const { - return _ui->groupBox_sourceDatabase->isChecked(); -} -bool PreferencesDialog::isSourceRGBDUsed() const -{ - return _ui->groupBox_sourceOpenni->isChecked(); -} - - -PreferencesDialog::Src PreferencesDialog::getSourceImageType() const -{ - if(_ui->source_comboBox_image_type->currentIndex() == 1) + int index = _ui->comboBox_sourceType->currentIndex(); + if(index == 0) { - return kSrcImages; + return kSrcRGBD; } - else if(_ui->source_comboBox_image_type->currentIndex() == 2) + else if(index == 1) { - return kSrcVideo; + return kSrcStereo; } - else + else if(index == 2) { - return kSrcUsbDevice; + return kSrcRGB; } + else if(index == 3) + { + return kSrcDatabase; + } + return kSrcUndef; } -QString PreferencesDialog::getSourceImageTypeStr() const +PreferencesDialog::Src PreferencesDialog::getSourceDriver() const { - return _ui->source_comboBox_image_type->currentText(); + PreferencesDialog::Src type = getSourceType(); + if(type==kSrcRGBD) + { + return (PreferencesDialog::Src)(_ui->comboBox_cameraRGBD->currentIndex()+kSrcRGBD); + } + else if(type==kSrcStereo) + { + return (PreferencesDialog::Src)(_ui->comboBox_cameraStereo->currentIndex()+kSrcStereo); + } + else if(type==kSrcRGB) + { + return (PreferencesDialog::Src)(_ui->source_comboBox_image_type->currentIndex()+kSrcRGB); + } + else if(type==kSrcDatabase) + { + return kSrcDatabase; + } + return kSrcUndef; } -int PreferencesDialog::getSourceWidth() const +QString PreferencesDialog::getSourceDriverStr() const { - return _ui->source_spinBox_imgWidth->value(); -} -int PreferencesDialog::getSourceHeight() const -{ - return _ui->source_spinBox_imgheight->value(); -} -QString PreferencesDialog::getSourceImagesPath() const -{ - return _ui->source_images_lineEdit_path->text(); -} -int PreferencesDialog::getSourceImagesStartPos() const -{ - return _ui->source_images_spinBox_startPos->value(); -} -bool PreferencesDialog::getSourceImagesRefreshDir() const -{ - return _ui->source_images_refreshDir->isChecked(); -} -QString PreferencesDialog::getSourceVideoPath() const -{ - return _ui->source_video_lineEdit_path->text(); -} -int PreferencesDialog::getSourceUsbDeviceId() const -{ - return _ui->source_usbDevice_spinBox_id->value(); -} -QString PreferencesDialog::getSourceDatabasePath() const -{ - return _ui->source_database_lineEdit_path->text(); -} -bool PreferencesDialog::getSourceDatabaseOdometryIgnored() const -{ - return _ui->source_checkBox_ignoreOdometry->isChecked(); -} -bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const -{ - return _ui->source_checkBox_ignoreGoalDelay->isChecked(); -} -int PreferencesDialog::getSourceDatabaseStartPos() const -{ - return _ui->source_spinBox_databaseStartPos->value(); -} -bool PreferencesDialog::getSourceDatabaseStampsUsed() const -{ - return _ui->source_checkBox_useDbStamps->isChecked(); + PreferencesDialog::Src type = getSourceType(); + if(type==kSrcRGBD) + { + return _ui->comboBox_cameraRGBD->currentText(); + } + else if(type==kSrcStereo) + { + return _ui->comboBox_cameraStereo->currentText(); + } + else if(type==kSrcRGB) + { + return _ui->source_comboBox_image_type->currentText(); + } + else if(type==kSrcDatabase) + { + return "Database"; + } + return ""; } -PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const +QString PreferencesDialog::getSourceDevice() const { - return (PreferencesDialog::Src)(_ui->comboBox_cameraRGBD->currentIndex()+kSrcOpenNI_PCL); + return _ui->lineEdit_sourceDevice->text(); } -bool PreferencesDialog::getSourceOpenni2AutoWhiteBalance() const -{ - return _ui->openni2_autoWhiteBalance->isChecked(); -} -bool PreferencesDialog::getSourceOpenni2AutoExposure() const -{ - return _ui->openni2_autoExposure->isChecked(); -} -int PreferencesDialog::getSourceOpenni2Exposure() const -{ - return _ui->openni2_exposure->value(); -} -int PreferencesDialog::getSourceOpenni2Gain() const -{ - return _ui->openni2_gain->value(); -} -bool PreferencesDialog::getSourceOpenni2Mirroring() const -{ - return _ui->openni2_mirroring->isChecked(); -} -int PreferencesDialog::getSourceFreenect2Format() const -{ - return _ui->comboBox_freenect2Format->currentIndex(); -} - -bool PreferencesDialog::isSourceRGBDColorOnly() const -{ - return _ui->checkbox_rgbd_colorOnly->isChecked(); -} -QString PreferencesDialog::getSourceOpenniDevice() const -{ - return _ui->lineEdit_openniDevice->text(); -} -Transform PreferencesDialog::getSourceOpenniLocalTransform() const +Transform PreferencesDialog::getSourceLocalTransform() const { Transform t = Transform::getIdentity(); - QString str = _ui->lineEdit_openniLocalTransform->text(); + QString str = _ui->lineEdit_sourceLocalTransform->text(); str.replace("PI_2", QString::number(3.141592/2.0)); QStringList list = str.split(' '); if(list.size() == 6 || list.size() == 9) @@ -3277,111 +3219,246 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const return t; } -CameraRGBD * PreferencesDialog::createCameraRGBD(bool forCalibration) +QString PreferencesDialog::getSourceImagesPath() const { - if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL) + return _ui->source_images_lineEdit_path->text(); +} +int PreferencesDialog::getSourceImagesStartPos() const +{ + return _ui->source_images_spinBox_startPos->value(); +} +bool PreferencesDialog::getSourceImagesRefreshDir() const +{ + return _ui->source_images_refreshDir->isChecked(); +} +QString PreferencesDialog::getSourceVideoPath() const +{ + return _ui->source_video_lineEdit_path->text(); +} +QString PreferencesDialog::getSourceDatabasePath() const +{ + return _ui->source_database_lineEdit_path->text(); +} +bool PreferencesDialog::getSourceDatabaseOdometryIgnored() const +{ + return _ui->source_checkBox_ignoreOdometry->isChecked(); +} +bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const +{ + return _ui->source_checkBox_ignoreGoalDelay->isChecked(); +} +int PreferencesDialog::getSourceDatabaseStartPos() const +{ + return _ui->source_spinBox_databaseStartPos->value(); +} +bool PreferencesDialog::getSourceDatabaseStampsUsed() const +{ + return _ui->source_checkBox_useDbStamps->isChecked(); +} + +bool PreferencesDialog::getSourceOpenni2AutoWhiteBalance() const +{ + return _ui->openni2_autoWhiteBalance->isChecked(); +} +bool PreferencesDialog::getSourceOpenni2AutoExposure() const +{ + return _ui->openni2_autoExposure->isChecked(); +} +int PreferencesDialog::getSourceOpenni2Exposure() const +{ + return _ui->openni2_exposure->value(); +} +int PreferencesDialog::getSourceOpenni2Gain() const +{ + return _ui->openni2_gain->value(); +} +bool PreferencesDialog::getSourceOpenni2Mirroring() const +{ + return _ui->openni2_mirroring->isChecked(); +} +int PreferencesDialog::getSourceFreenect2Format() const +{ + return _ui->comboBox_freenect2Format->currentIndex(); +} + +bool PreferencesDialog::isSourceRGBDColorOnly() const +{ + return _ui->checkbox_rgbd_colorOnly->isChecked(); +} + + +Camera * PreferencesDialog::createCamera(bool useRawImages) +{ + Src driver = this->getSourceDriver(); + Camera * camera = 0; + if(driver == PreferencesDialog::kSrcOpenNI_PCL) { - if(forCalibration) + if(useRawImages) { QMessageBox::warning(this, tr("Calibration"), - tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. " + tr("Using raw images for \"OpenNI\" driver is not yet supported. " "Factory calibration loaded from OpenNI is used."), QMessageBox::Ok); return 0; } else { - return new CameraOpenni( - this->getSourceOpenniDevice().toStdString(), + camera = new CameraOpenni( + _ui->lineEdit_openniOniPath->text().isEmpty()?this->getSourceDevice().toStdString():_ui->lineEdit_openniOniPath->text().toStdString(), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); } } - else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2) + else if(driver == PreferencesDialog::kSrcOpenNI2) { - if(forCalibration) - { - QMessageBox::warning(this, tr("Calibration"), - tr("RTAB-Map calibration for \"OpenNI2\" driver is not yet supported. " - "Factory calibration loaded from OpenNI2 is used."), QMessageBox::Ok); - return 0; - } - else - { - return new CameraOpenNI2( - this->getSourceOpenniDevice().toStdString(), - this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); - } - } - else if(this->getSourceRGBD() == PreferencesDialog::kSrcFreenect) - { - if(forCalibration) + if(useRawImages) { QMessageBox::warning(this, tr("Calibration"), - tr("RTAB-Map calibration for \"Freenect\" driver is not yet supported. " + tr("Using raw images for \"OpenNI2\" driver is not yet supported. " + "Factory calibration loaded from OpenNI2 is used."), QMessageBox::Ok); + return 0; + } + else + { + camera = new CameraOpenNI2( + _ui->lineEdit_openni2OniPath->text().isEmpty()?this->getSourceDevice().toStdString():_ui->lineEdit_openni2OniPath->text().toStdString(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } + } + else if(driver == PreferencesDialog::kSrcFreenect) + { + if(useRawImages) + { + QMessageBox::warning(this, tr("Calibration"), + tr("Using raw images for \"Freenect\" driver is not yet supported. " "Factory calibration loaded from Freenect is used."), QMessageBox::Ok); return 0; } else { - return new CameraFreenect( - this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()), + camera = new CameraFreenect( + this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); } } - else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV || - this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS) + else if(driver == PreferencesDialog::kSrcOpenNI_CV || + driver == PreferencesDialog::kSrcOpenNI_CV_ASUS) { - if(forCalibration) + if(useRawImages) { QMessageBox::warning(this, tr("Calibration"), - tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. " + tr("Using raw images for \"OpenNI\" driver is not yet supported. " "Factory calibration loaded from OpenNI is used."), QMessageBox::Ok); return 0; } else { - return new CameraOpenNICV( - this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS, + camera = new CameraOpenNICV( + driver == PreferencesDialog::kSrcOpenNI_CV_ASUS, this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); } } - else if(this->getSourceRGBD() == kSrcFreenect2) + else if(driver == kSrcFreenect2) { - return new CameraFreenect2( - this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()), - forCalibration?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)getSourceFreenect2Format(), + camera = new CameraFreenect2( + this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()), + useRawImages?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)getSourceFreenect2Format(), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); } - else if(this->getSourceRGBD() == kSrcStereoDC1394) + else if(driver == kSrcDC1394) { - return new CameraStereoDC1394( + camera = new CameraStereoDC1394( this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); } - else if(this->getSourceRGBD() == kSrcStereoFlyCapture2) + else if(driver == kSrcFlyCapture2) { - return new CameraStereoFlyCapture2( - this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + if(useRawImages) + { + QMessageBox::warning(this, tr("Calibration"), + tr("Using raw images for \"FlyCapture2\" driver is not yet supported. " + "Factory calibration loaded from FlyCapture2 is used."), QMessageBox::Ok); + return 0; + } + else + { + camera = new CameraStereoFlyCapture2( + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } } - else if(this->getSourceRGBD() == kSrcStereoImages) + else if(driver == kSrcStereoImages) { - return new CameraStereoImages( - _ui->lineEdit_cameraStereoImages_path->text().toStdString(), - this->getSourceOpenniDevice().toStdString(), + camera = new CameraStereoImages( + _ui->lineEdit_cameraStereoImages_path->text().append(QDir::separator()).toStdString(), _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceLocalTransform()); + } + else if(driver == kSrcUsbDevice) + { + camera = new CameraVideo( + this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } + else if(driver == kSrcVideo) + { + camera = new CameraVideo( + this->getSourceVideoPath().toStdString(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } + else if(driver == kSrcImages) + { + camera = new CameraImages( + this->getSourceImagesPath().toStdString(), + this->getSourceImagesStartPos(), + this->getSourceImagesRefreshDir(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } + else if(driver == kSrcDatabase) + { + UERROR("Call directly DBReader for kSrcDatabase."); + return 0; } else { - UFATAL("RGBD Source type undefined!"); + UFATAL("Source driver undefined (%d)!", driver); } - return 0; + + if(camera) + { + // don't set calibration folder if we want raw images + if(!camera->init(useRawImages?"":this->getCameraInfoDir().toStdString(), this->getCalibrationName().toStdString())) + { + UWARN("init camera failed... "); + QMessageBox::warning(this, + tr("RTAB-Map"), + tr("Camera initialization failed...")); + delete camera; + camera = 0; + } + + //should be after initialization + if(driver == kSrcOpenNI2) + { + ((CameraOpenNI2*)camera)->setAutoWhiteBalance(this->getSourceOpenni2AutoWhiteBalance()); + ((CameraOpenNI2*)camera)->setAutoExposure(this->getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)camera)->setMirroring(this->getSourceOpenni2Mirroring()); + if(CameraOpenNI2::exposureGainAvailable()) + { + ((CameraOpenNI2*)camera)->setExposure(this->getSourceOpenni2Exposure()); + ((CameraOpenNI2*)camera)->setGain(this->getSourceOpenni2Gain()); + } + } + } + + return camera; } bool PreferencesDialog::isStatisticsPublished() const @@ -3503,32 +3580,28 @@ void PreferencesDialog::testOdometry() void PreferencesDialog::testOdometry(int type) { - CameraRGBD * camera = this->createCameraRGBD(); + DBReader dbReader(this->getSourceDatabasePath().toStdString(), + this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(), + true, + true); + Camera * camera = 0; + if(this->getSourceType() == kSrcDatabase) + { + if(!dbReader.init()) + { + QMessageBox::warning(this, tr("Camera viewer"), tr("Failed to initialize the database reader!")); + return; + } + } + else + { + camera = this->createCamera(); + if(!camera) + { + return; + } + } - if(camera == 0 || !camera->init(this->getCameraInfoDir().toStdString())) - { - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("RGBD camera initialization failed!")); - if(camera) - { - delete camera; - } - return; - } - else if(dynamic_cast(camera) != 0) - { - ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); - ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); - ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); - if(CameraOpenNI2::exposureGainAvailable()) - { - ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); - ((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain()); - } - } - camera->setMirroringEnabled(isSourceMirroring()); - camera->setColorOnly(isSourceRGBDColorOnly()); ParametersMap parameters = this->getAllParameters(); Odometry * odometry; @@ -3560,94 +3633,106 @@ void PreferencesDialog::testOdometry(int type) odomViewer->resize(1280, 480+QPushButton().minimumHeight()); odomViewer->registerToEventsManager(); - CameraThread cameraThread(camera); // take ownership of camera - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); - UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent"); + if(camera) + { + CameraThread cameraThread(camera); // take ownership of camera + cameraThread.setMirroringEnabled(isSourceMirroring()); + cameraThread.setColorOnly(isSourceRGBDColorOnly()); + UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent"); + UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent"); - odomThread.start(); - cameraThread.start(); + odomThread.start(); + cameraThread.start(); - odomViewer->exec(); - delete odomViewer; + odomViewer->exec(); + delete odomViewer; - cameraThread.join(true); - odomThread.join(true); + cameraThread.join(true); + odomThread.join(true); + } + else + { + UEventsManager::createPipe(&dbReader, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent"); + UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent"); + + odomThread.start(); + dbReader.start(); + + odomViewer->exec(); + delete odomViewer; + + dbReader.join(true); + odomThread.join(true); + } } -void PreferencesDialog::testRGBDCamera() +void PreferencesDialog::testCamera() { - CameraRGBD * camera = this->createCameraRGBD(); - if(camera == 0 || !camera->init(this->getCameraInfoDir().toStdString())) - { - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("RGBD camera initialization failed!")); - if(camera) - { - delete camera; - } - return; - } - else if(dynamic_cast(camera) != 0) - { - ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); - ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); - ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); - if(CameraOpenNI2::exposureGainAvailable()) - { - ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); - ((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain()); - } - } - camera->setMirroringEnabled(isSourceMirroring()); - - // Create DataRecorder without init it, just to show images... CameraViewer * window = new CameraViewer(this); - window->setWindowTitle(tr("RGBD camera viewer")); + window->setWindowTitle(tr("Camera viewer")); window->resize(1280, 480+QPushButton().minimumHeight()); window->registerToEventsManager(); - CameraThread cameraThread(camera); - UEventsManager::createPipe(&cameraThread, window, "CameraEvent"); + if(this->getSourceType() == kSrcDatabase) + { + DBReader dbReader(this->getSourceDatabasePath().toStdString(), + this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(), + true, + true); + if(!dbReader.init()) + { + QMessageBox::warning(this, tr("Camera viewer"), tr("Failed to initialize the database reader!")); + delete window; + } + else + { + UEventsManager::createPipe(&dbReader, window, "CameraEvent"); + + dbReader.start(); + window->exec(); + delete window; + dbReader.join(true); + } + } + else + { + Camera * camera = this->createCamera(); + if(camera) + { + CameraThread cameraThread(camera); + cameraThread.setMirroringEnabled(isSourceMirroring()); + cameraThread.setColorOnly(isSourceRGBDColorOnly()); + UEventsManager::createPipe(&cameraThread, window, "CameraEvent"); + + cameraThread.start(); + window->exec(); + delete window; + cameraThread.join(true); + } + else + { + delete window; + } + } - cameraThread.start(); - window->exec(); - delete window; - cameraThread.join(true); } void PreferencesDialog::calibrate() { - CameraRGBD * camera = this->createCameraRGBD(true); - if(camera == 0 || !camera->init("")) // don't set calibration folder to use raw images + if(this->getSourceType() == kSrcDatabase) { - if(camera != 0) - { - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("RGBD camera initialization failed!")); - } - //else already warned - - if(camera) - { - delete camera; - } + QMessageBox::warning(this, + tr("Calibration"), + tr("Cannot calibrate database source!")); return; } - else if(dynamic_cast(camera) != 0) + Camera * camera = this->createCamera(true); + if(!camera) { - ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); - ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); - ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); - if(CameraOpenNI2::exposureGainAvailable()) - { - ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); - ((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain()); - } + return; } - camera->setMirroringEnabled(isSourceMirroring()); - if(!this->getCameraInfoDir().isEmpty()) { @@ -3661,7 +3746,7 @@ void PreferencesDialog::calibrate() } } } - _calibrationDialog->setStereoMode(true); + _calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB); // RGB+Depth or left+right _calibrationDialog->setSwitchedImages(dynamic_cast(camera) != 0); _calibrationDialog->setSavingDirectory(this->getCameraInfoDir()); _calibrationDialog->registerToEventsManager(); @@ -3678,3 +3763,4 @@ void PreferencesDialog::calibrate() } } + diff --git a/guilib/src/images/webcam.png b/guilib/src/images/webcam.png new file mode 100644 index 0000000000000000000000000000000000000000..07c8875500d2eaebc12eb8908e52065ef02f452a GIT binary patch literal 11049 zcmWk!1ymGW6kcF~r59YfmIgseTImK!NokN0ke2RlL~QTZWd8Oq#LBWOS=B~ zclYg_Gv~~knfLDf?!Di4G+s_!@)O{ z|Kefb=G>r9D5-BEttj#0{dvRUWE%km4;ptH&a$3>@EjEPy;{%Il909^q+_Bbo3`wvr8uwnBVIZXsbK|hq(_Dfg}^Zi*!%x%+-@{#izl* zopmmA=HIQc2SPT8ME{0PP8w90G#orYK%tIfrB;UvZScsolDwbR-&0-Y+(9+uicLn) z$f)d64g8ocNR(VED<0x>>3s*K%n`yf7>s6D+kVwO{Z3;n4T;>y;IkN1-ia0a24*G0 zdyywapCjsu12T2%#KvS|?WD@#h5QhXO#KRBDv2+IzgL07@YlnL7!n1v)nvJ%cAMSk za1+0WLX0yCkdGB~{_uAPJI$T*v8m3s1zw$mA(4-B8U1jH=`I%)Wo&F%c$?P9cr4oK zKyC+3n;Do{1WO5sQ+2V;R9k9WTie)Q))+^AXqP!9ksL*loG2?QVD@l$D3t8^$&}|H z69m&sBjVuwrLRbiS@2r#d&2;Ke?pLhgM*Bx=jj76W_L1uLTM>G2tEGgrK%9k=U?Iv zf;M|Nau6X6c}GVsHFb4aNSI@Ge)R1a0sxINTghb zEhHTJ*ENfsZVso!3!XD@@7pu@ndIe^3r=Xu!8L`!w^vu~WXHLWs_X`6O#Qb@K%t|f zx>kR(1TqD>54_EMti|Y*g8)WyZ6_q8BYk(7;% zZ3!U{Qv~k$bJhK)wSe28)trcznXMH+z1x!s2zW-<6UG@o>`lMyM)h6=TvAdJ+q1fa=&I%cOfHf7 zDb84_Mi-*VQ^58&3=)ZUH`AUv+eiv$%m^72!}aDe-3fo{DyUVe%Qf|`-B6e8#?fVe ziw7jsK&LDpH1EhJ<3EHJ#qB&)XRo|pnD<$nnwvApF{F>S$}cLyYBDAMZbX=_%io>v zNCWt*QW6ceEHrh%PDkG?N%awj^~Rr1Sx@bW-#9rf>*Dv+cZ&02GGOOZs(oUOOJyZR zcs7nJIyy2Rhznn~8lNgCsv*$-NY3bt`C{yGVEZ4TfpwFHx8L{) zu?jimV2Xrxus*6QC6ZiDEnKDCeN8)sQ~SX8>GF$Tk57JN(veR8f6pYr%o zY5w5{7UQ(}*T{#6XTbaZ>l7`+yj2E$v1(7sQ!_3vFAwGE2j0BzHCYS^V>A+J)ab@U z@#IzH(^4&E!0+F@c>{uGMPqdFqo?zeh6rMDI6%kx(9QcMv|_XjBvvbuGd5L$w2&1i z7z+dmhkFh_L*wfA`~>Cc5%nV@V{EnIb7XEbLMu!jrg5 z$SRAy+?AOEeJL7oYZ!LX$#>7kKpM|1ucP-scK!fUy!95+{UPlZ9BP#nva*zES0nyI zJO&0X(}54sF6sKa-@gNReALdF{5~NH{Plg6$@Hupv+~et-R%{iSPN0 z{jk>_6@ZDRQ)lBzR{0$rjDVG<>y{(qM~a+|-=_q_HgkQ<-_xwPmfkF`^!jq6H!Hab zeLO*Fob}vkf2W7FGwDZI0AGP@OAwCTLUBLMcW-LdrI6bR<8b)Xsj6@ zy<_Ne&^=RQm-qrx@1e4=H*dD#L1raR`WB<*-Tsfx2;7med8{rW5GcE}pt+gmWtnCa z5~*nX%~>QtV@R4r4$Na(3j%{HAd?McV_MjOQhLk3q5_9I(OEukQz7r3hE9|^COeb= z6sOdxt?OuADZm}_|2LvMGCmHTa+d{%pAJfrJhl4+S|z->IP7`PqRj8P@ghAv9qmqM zl!4t}Z8;|*R{)i|+jesAyuGS&xpDcDvD{0I^ryNgru*~3-=I7=&^jVg+ zgY;|o?dq*^@ZHzn6JHIB>gvc*yVm!&SC^F!w|t4T(!@O}jJQnTy1KgQ79YO$o6m>V zQ>b}un0V|imW+uB?Ig-9;W`>;d}5;Wm<8c#eAn8y3-V=LVm=~h-IKW%$F+ZvpJ;)< zRCY*?@uTIKiR7D+`#n#$2C z**TGkp;VK6;=6dmh}vcT_@ba>T}p%7HC0$G*wOtotAsvcAh$`@Bq z@J^`Sc^+R4*XG&#=wIcnM=BI__T4xTFJt+^^LLllkKEiut5rX(9b8T3bd4PAB5Z}H#El!`^VWS zNLC&x9hID{IX;z4tfzRtkUU8Ix`uj<8ybTt&AA?drrhz^h&cT6s6pjA2@%E;LsA4p zTp*jxPY1F9{=w&8_!<{NB9)67hv(<%IWTqDbd*|Iapr{D0>6=@m$Si<0!H(W(~=Zf zAH(Us%9jUYBrdS<4-glp1wov$E7goZ?;d}OeE&4O>P(yC@nPo)rb+wor{5+LLJzYS zZrIHEXTAh_`ijgt(O8v-$`;im_6T=&A+wlOiCm6B+$T(fjbhBoG7_H+z_PH{#?j3E zme|OioB-_5r()Y8+C{j3gfW=ql2amd1u2NvdtZ;HDrym*bme*`dr1(D3C2vHw8J(z zbVSC(sr8&Ju z>*#ADmFoXn=F6@|FN$M7ih}4#?ORiPC}wwSU$CVmsIR@41Ee^rMez8$@l7nN49WJl z>{45DMcKhx%a`#$h>+tfpJx>N2ix=FZgc|u!L}Dc=sl`f(@|iNc(c1RJU>RT)2wK_ zS;&bIdAXG9+&_k?YdVFS*BrJgJf3@jI!0CqYed-2#P&R*(l-xljuxO2M$#c{VrfqM z?EY=VvReLajK)gdJy$6NX9@}lH&`zJz%EB3@loGzhzmjv-(-DmS+Z-f^xjuGPa<-A zsZss~*_|a|Gd#OM++UsWd!%R?0#n3$@kfqNSehB<35Ce`8*xVlw@;!H&i!(JifkTD zbRN-RBo_YEQUPO6IBA7{t3%wSwzz$ZI5s`rIV+GI{h9SPFf`l@hBjuGu9O&>cV->0 zx({$j>mX_!XTsM8afdHvL*R7*Y@-7?(8ax9Hv08-@~r2!_@I>!L-Fwe)J3>&ThCv1$Ff*< z6Y|NA&yTfD_OAQGRFzJTSH;}6_)%@=V~u+&B$Q^Z7xZ$#YxB=qVC)6|$rB~XH21(0 zahZuJR&9Bt{b$(R^{W~zrvl7^yhH3V}F9g+XgGViZRu`!a89+4(|yd3?5Uz|;$deOHT zM1Qam=N8lbF8t=ldx$lg0B1aAx0q?qnUpBMBNvrT2xcg4WsBWfRFVB4A}%?Zct?!b z2Em6!R!kC(DLBpHnFb#Jx@-YTB$ruOBnwc-v1JK6vA=CUuDSd}G{y3vf4L#$o}se! zTF`lp_>b0Hy+H7-uj#uhWo>=^^=}wOuv-4#pG5g^<-6OJ_dYY$rkxy;BQEG`o{L|F zKb!Kt8(LZty1W%}3*6t9mR?WpO)oCw4Bml;mu_nT1=>dS)3Q7+hypt6``P7B4(a6W zu|XLhEe^qLLLoC;86Xi+H@evKub8W1pr_x5DheEVC7BZEH6646J({6q>W`Neh50sO z=byfOKGbuMrB&Z6zp+pkbua5q!Q(q1{&4JK(Rg*9Cs~W%u;6vq>-p1pP6?=?MRIHl zW!m4ho?Y_H)$iH?wbJ>y=JQK5zMg@;^^eG7&bW{H-}b?;Qu1Wf``w-Wt)2o%5l)A} zWX^OzJyy1Q>vx)8{@A&n*8_kYLYZ@VUQKZao$4qefe+fWVdb?Qm&Sx#Y!z9UG%SmMbC+Y;*`-}-=Jjv(Z@bid92F5myma)wt9RW)))veJ zPS^eU2+uV5*T*a|M-Od8zvhWYjEdA|emZtq|A|0>oT_~V`l+4bp}SNu4QB77Gh9#% z#*%u&f>`xeW`K!8P+wBPc+R0C-z@nq4X8<=R!-a&om?%SiSj+VY2t4I#ojhn_8-DiB!92FBLb$OR3>hueZc8v3O8+uE>_d}K0Z#K4`W z{M1MCw9XM_$e`NKq5(#U8|Oev26A6u!lVD!1q686!@^Eggbdlz#4O@=8~k=jNFD z6L8hsw;6XGg)g?vRZczu06i$C~zsYVV3&r9r3(N~)JEuwhcoUc{om`P)J`pZ(ctdwMB+qA7z z72&tJqUX$^UwKVitFX$Bp$iEl`&&ox-<^4blXcG*lU+`^QH_`+Qni>*?FJ)JB=vbr zk$^p^6hg-Mt899RD@>cn0xk}S$etb>QKfOh>4*9lc0*TmWm)fgT z!j)U7m{bn#!_3)Go(Seb&Bqgy`i{iY&D@ByoccGI(=VBs)b%vTU(ay2&epro&d$v; zsWPv`fq&oH(T_O5;tTV?e<#)d_>uo^Wxm2OKs=oJ&Yb@f@iY8rmFJtk)VG3eFwoT{ z36y)FQ2Qy#6auY?9ls$wP-qfgYb-V;BB3nLtdcwJl3NF!*35#0#nh0-VR1Q{Jl;xYWD5i z9+xSrH11(Zk!Jh&i}@n9@W|LWP3^UDIBQ?-2feDdnF2Ud?zf}fbGuAT+)%K)@7Qs% zcNdk%ys8$+4Hx|iWMNkA&}pT0BD(T}t&H+^Ob$IZgz90V!@VPgZcKqTOcM z9%_?A%O)$LR3N-9Ke@N=25hu6b;-AL(l~jTFp=N~C*Nf}d|4QjSQ*}@_MTx=p3?bf zdFsPwh43Ct8gxG>gU?v#K?_v%wo3^FR;L)~x$#C!2Fs>M9&-e){O*sK`pR%MuaDVz z23~XS_N&`%hJs1gi+)4nB7??`Uz>&C_wl+l#%q>g?BQI#Ote}b{dF4f{7AH|65YWE z_8olybCxu|`;_^l<)L7rxlo1CHqqim=%;ntk|bN(5((1C{MOXGAN50z$Rb4{eHTP4 zpk$0`TuYYF9!fsMR2nV=Iu`U0!~{>hw|F4_qW84@^ z;xB>?D^~i$fX%-#2KV^&8plyXM0N$4R=EsJjWDtn%P2*(-#(j$BG~s zS65d<-pqdLXpAwT2<2c6_j{eo+y^nP$mhYgm!ukK6J6PhyQ9V9l!v2ehJEn~R;JWcXn%3#Tf53!wgM*sF zIRp3^woQ>xRhVLwCvR+_1F0O3oJuluR2j1ELARM@Ya9dM-bQcr^E{{^mW=r3bL(uo z^0G2z^%7m3E`Gf(eih!#2*>O&`{%V*K2mL5s$?+5Fx0LA55=>B-R4fJ-A#n3j#}RC z{(jI7It+`;^7UC#@Jq~thnc&^vkVhiNax14IW;f+`xSr%yZTAM*Qz}?GecePtX})X zq@E{X?d()rb)e(C(-T;&8hw3zYSpx?-R2_DR@A@^F}*}&eZ2?>@+JzO){`!xR9Sl?bA4|_{C@{*Xh_tZFMtCWpx z?e8mESTGWHh1wd+mYito5){BW9)z^a-h;zIM7n;Q#F z)Ft!y;}0|%G`j6IJc=Wulh79^!sCiTZXmr@Nv=o=aQ$%#n1J@6jql&FZn8!dO)7LM zqH*fe*4Acmu)n`gL!8KXf2n*K)6sF4emY>AX%;Q)G}+oFtZ%~0+WArTS$j~_o4vl?K^!c^El$N~#AJ2&Tkw8VsPki^h|#m`bTqGsQjZ*^jBE`yhkPXHTtA*Y^!!O6nH!iP|!4l79tzLUcG zAymM@j}x8s8oaQqKD!jDi@%<<5T?(5%qa7E!j6D{BR+Ydl|gsA@W`b)`ezf_**n}e z-=`J#U0mi}g@ivXAj!4(z9Qq}zBrAFhTbkVs;usMx~kmxhL!@k&K_gMQ=gbWKG?5&zE5Up4&ociNcM3Xv#{ zk@H}18>G5_^-N9{Iy^Ho^YE?ZAT5RdS*cUD5>UI2zqq$d^jnU7fdgnb$*bUE3L0#b z;nlkbfER5igc}X|&ABuzb4Xv|F$CNm$Jyrvoqwq2(P9U`&f@FkENXf!c-0~;c~({# ze44YU;xgMxeY!aiaDnF_>Auox#8V#yaP+X5F{jCCp9Q}%2jdK$cn-tbsmV!Znk1ab zKeVo`VUG3)tDw!6Me;Xqkn4f?sg!A(k5s@I;WBXi6;*@uQb*sTFx-l~hduif8Hp!r z-{64wk~K+3MHRX#=6Wqp-=n(W(NyX4?w!=qv;3$JMq6hQxKm2Z?@u4H9S6lXGi(ne z1CNf*GEF^>2Z@HHQxpj(+XFn&_jfmn8v0Hrp*m_&CF+^TFM7Y5Hgf|I&LSoXa^BwF z!^g!nHIIOOz3p$9@0NNup^xOmuX4dfn>ck)*HUJ%s-!{2){L6HzTDm{KIH!UcaHOT z*d1X>`$$peerNM~^ug=#$5*RgGk#zXcrRWx9HIbf9M?^7I^+j8#UpTKZmqThx#A}& zE9+Ihyug#blqatmkA_<$@M4N|nAdtPmy#9NvYk*mr7u2P-!9!q_qLz>=4Kjf0)omQ zz-p|DIXJm)+7IOpxZeCWjLaSpW~Svi4(?YDK8xR%gi)BK7Is{uoPc1Z_hGp z+l*bsG`WQ7damSoGjDQto3~pdhY$ktRqGp>AP_$C|6YKdgYB!eNW4e)`{4waIkqd}vaMK>s8y`;uR98uuA_mjXejR?1I#j_uFKDWL_wm-{@oGZR$X2uE zNbQ3>mko&N-*9KdHl>KG$g^h>jqv!2(o!XpMhBhR_5#`EVATRc&r^zsZydtbCo6Y< zN}z0{9j0~Jm8MN;YZvdiz_acMW;BNOU{e>Bx?p#GBkHEyWzGb{C=wd5(CSxPRAlpt zm8_}%9pcx6$CTOZ-zHDCZYqC&|0<_V%{7mv4;!TV`T}qNis?;_hYCc$kRa+3^4XjH zzIrm}+R?UemK5!gKT*A}3Hn&-=x@suVWH%V;AfKZJ9)kMRI#c(`n`+m0H z-F$a_2-XI=7QSYCqsF!pu~7bwABorV zYOFeuto^BMq|K#P+MYxYlK8up1pN~PPrg1E@C#V1c6pH_l9XD-m)P!~%JnO)o6iCMy&hO8Jawa72F^U-aYI8q$&u0z&B- zjmFqx97<70ssi6p3FnCg_AUKfuM|@u=W+oqBU|C%;4J~v_j(tbp@Vq`C#MLGV#u{Aa-oa!< zL1b}Cj_jZ94i$Q%%{T%J>&TZ?6awA*DQfCbsFH$$0$FHe_bu}qCPpRDamB}c^@7FD zpYF@rmGg=F5^koH$&5YJ&G`}?)I&L{-V1Ba4i4xF;~Iea6FZaouy&F52X_OBMDqKc zznFV6YWR8_y(_!Vx$9ON*&d`%qWF5VhVc! z(mbsLl#I0y{w8EZNIJ^zQ$r*qwxMa(QjEnYUx$jDMuIG`C&6{`p6uQJY}|SMJL{Q+ z3}c{Ug38P=rtd>pJ_<#QNqUP7$0Y1hVE=#(NqxcoJ!Dae^`)wICQ#8@&(*suns&UM z_e6L$fC6ztiQt>kJP*P_&nSi#vq>L8n8-r;H%;?IT^D|06Ho@f%2;Cq{A|RFD7%yi zhR*~`2B>a`qlA#z1w}8{<6C`sD$}|jF?F%wj~R)jyVe7`3Jn{%)vdJ=Inpp&x5eJT zrD#FEzmln%UHq-7O@*AXkzGKuOSEGF(DDF?n94prhN%e-&qgLs_G*UyzFRw5&cLi& z1@Q+h5itf%acr2?Nc?OlAN#ejv2m}FBeFj%f6X-ay#+`rc|W})GNX*U+&(6ksNnlNR1M05eC!YCwmpLFZ%{I>C&H!x|x0?qoUpy8x^})x=@emp2 zNEL+VmnCyjnd0WsQvW|$(L27awd{1n6K|Rq@||X>6!mGnG`*sO-d@1L1IYXYq~ zUf$j=(^bKD9rO}jRdq6pIoP^Gi`++A)bUi^VFT)6{o<5?w8lJ_P7MupXTYHUXR%vd zm@X_LVvwGhX$WLF*{KXjk|_CAAr0C}$7=`43hwv{nT*zNZ%%Kpc9~qC4!MbiyK#ag zVc|UeiUg=H>hC>0s+?&vw)Yo`In|&mRL&twh2Or>1KDRmWdmisOP9A9XFOpyHBY}_ zb4xRWwAa>>yTu9t$E$Pmazul?j81<4gYM6~;#e)YWlmCz>13C(i^I98m5xeK?1boe z3P7hbkjE);3!8mSP-0%~T6VMHgwfCv$JkUC%8)7cR%0sa6d-evBmba3mVqBcu9exu%>#2v1jGT@fx)&g@oK`z>U~NT1Zr{?mG-uRyy|fGkcA ziKNLYuI7==AO|WK)eKDXVxXUH5q4g|hLK^*k~sj+e2Tubd}^e|HHzivbphmb7i9;F zO*TC&&wLK{j@TA>p;hnosV|7}n6S}#@ld=_WS}$)f?zdT&^d5Z5 zXK5sK@$=L{wLnEA$&eBwU!W}nBc2+coYZ6xelxzN!YZMulkTCRFAt9tM}0_R-7bIJ z&j-*E&WkkxL*LtO&d<*Qga$Q+!}&K#_NW$mY>F#LsMH{uvcI)GiWMBc7#O@lDlqFP zX4tCgz%Vfz7p#m+-%I))X-~S-5rnEkto}N&|7%yhQ(;{iXrKC`)Ia?GW%FX0npS6QdQJcK*?E# F{|9Q84+H=J literal 0 HcmV?d00001 diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 5e2a71e9..04e7d582 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -95,16 +95,22 @@ - Image-only + RGB camera + + + + :/images/webcam.png:/images/webcam.png - - RGB-D camera + + + :/images/kinect_xbox_360.png:/images/kinect_xbox_360.png + Kinect @@ -150,7 +156,20 @@ - + + + + + + + + Stereo camera + + + + :/images/bumblebee2.png:/images/bumblebee2.png + + Bumblebee2 @@ -161,15 +180,12 @@ - - - - - + + - + @@ -946,34 +962,14 @@ true + + + :/images/webcam.png:/images/webcam.png + Usb camera - - - true - - - Images... - - - - - true - - - Video... - - - - - true - - - Database... - - Generate graph local map (*.dot)... @@ -1203,7 +1199,7 @@ - StereoFlyCapture2 + FlyCapture2 @@ -1226,6 +1222,14 @@ Default views + + + true + + + More options... + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 1b1efee4..4957b2d2 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -7,7 +7,7 @@ 0 0 1058 - 858 + 649 @@ -86,7 +86,7 @@ QFrame::Raised - 24 + 3 @@ -1301,8 +1301,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + Hz @@ -1317,177 +1317,1012 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 0.100000000000000 - 30.000000000000000 + 0.000000000000000 - + Input rate (0 means as fast as possible). - + Mirroring mode (flip image horizontally). It has no effect on database source. + + true + - + + + + + + 100 + 0 + + + + + + + + + + + Calibration name. Used to search for calibration files (*.yaml) in "camera_info" folder of the working directory. If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + + + true + + + + + + + 0 0 1 -1 0 0 0 -1 0 + + + + + + + Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33. + + + true + + + + + + + Source type. Select specific driver below. + + + true + + + + + + + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. + + + true + + + + + + + QComboBox::AdjustToContents + + + + RGB-D + + + + + Stereo + + + + + RGB + + + + + Database + + + + + + + + + 0 + 0 + + + + Test + + + + + + + + 0 + 0 + + + + Calibrate + + + + + + + Calibration files are saved in "camera_info" folder of the working directory. + + + true + + + + + + + + + + - - - Image source + + + 0 - - true - - - false - - - - - - - - 0 - + + + + + + RGB-D + + + false + + + false + + - - Usb camera - - - - - Images - - - - - Video file - - - - - - - - Source type. 0-Usb camera (Webcam), 1-Images (directory of images), 2-Video (AVI) - - - true - - - - - - - Image width (set to 0 to use the default size of the source). - - - true - - - - - - - Image height (set to 0 to use the default size of the source). - - - true - - - - - - - 0 - - - 999999999 - - - 160 - - - 0 - - - - - - - 0 - - - 999999999 - - - 120 - - - 0 - - - - - - - - - 0 - - - - - - - Usb device + + + Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). + + + true - - - - - Usb device. - - - - - - - -1 - - - 999999999 - - - 0 - - - - - + + + + + QComboBox::AdjustToContents + + + + OpenNI-PCL + + + + + Freenect + + + + + OpenNI-CV + + + + + OpenNI-CV-ASUS + + + + + OpenNI2 + + + + + Freenect2 + + + + + + + + Driver + + + true + + + + + + + Only RGB images are published. + + + true + + + + + + + + + + false + + + + + + + + + 4 + + + + + + + + 0 + 0 + + + + OpenNI + + + + + + + + + + + + + Path to a *.ONI file. + + + true + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + ... + + + + + + + + + + + + + + + + + + 0 + 0 + + + + OpenNI 2 + + + + + + + + + true + + + + + + + Auto white balance. + + + true + + + + + + + + + + true + + + + + + + Auto exposure. + + + true + + + + + + + 65535 + + + + + + + Exposure. + + + true + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + 1000 + + + 100 + + + + + + + Gain. + + + true + + + + + + + + + + false + + + + + + + Mirroring. + + + true + + + + + + + + + + + + + + Path to a *.ONI file. + + + true + + + + + + + ... + + + + + + + + + + + + + + + 0 + 0 + + + + Freenect2 + + + + + + Format. + + + true + + + + + + + QComboBox::AdjustToContents + + + + RGB+Depth SD + + + + + RGB+Depth HD + + + + + IR+Depth + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + + + + + + + + Stereo + + + false + + + false + + + + + + Grabber for stereo devices (i.e., Bumblebee2). + + + true + + + + + + + + + QComboBox::AdjustToContents + + + + DC1394 + + + + + FlyCapture2 + + + + + Images + + + + + + + + Driver + + + true + + + + + + + + + 2 + + + + + + + + + + 0 + 0 + + + + CameraStereoImages + + + + + + + + + + + + + ... + + + + + + + Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. + + + true + + + + + + + + + + + + + + Path to directory containing stereo images. The images order should be left/right/left/right... and so on. You can also set two directories (separated by ';'), one for left images and one for right images. + + + true + + + + + + + ... + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + + + + + + + + RGB + + + false + + + false + + + + + + + + 0 + + + + Usb camera + + + + + Images + + + + + Video file + + + + + + + + Source type. + + + true + + + + + + + + + 1 + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + Images dataset + + + + + + + + + + + + + 0 + + + 999999999 + + + + + + + false + + + + + + + ... + + + + + + + Refresh the directory files list after each image loaded. + + + + + + + Start position (default 1, 0=start from the last). + + + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + Video (AVI) + + + + + + ... + + + + + + + false + + + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + + + + + + + + Database + + + false + + + false + + + + + + Open database viewer + + + + + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + + + + + + + + + + ... + + + + + + + + + + Start position (index) + + + + + + + 0 + + + 999999999 + + + + + + + Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. + + + true + + + + + + + Ignore goal delay. + + + true + + + + + + + + + + + + + + Use database stamps as input rate. + + + true + + + + + + + + + + + + Qt::Vertical - 0 + 20 0 @@ -1495,619 +2330,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - Images dataset - - - - - - - - - - - - - 0 - - - 999999999 - - - - - - - false - - - - - - - ... - - - - - - - Refresh the directory files list after each image loaded. - - - - - - - Start position (default 1, 0=start from the last). - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - - - - - - - - Video (AVI) - - - - - - ... - - - - - - - false - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - - - - - - - - - - - Database source - - - true - - - false - - - - - - Open database viewer - - - - - - - :/images/mag_glass.png:/images/mag_glass.png - - - - - - - - - - - - - - ... - - - - - - - - - - Start position (index) - - - - - - - 0 - - - 999999999 - - - - - - - Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - - true - - - - - - - Ignore goal delay. - - - true - - - - - - - - - - - - - - Use database stamps as input rate. - - - true - - - - - - - - - - - - - - - - - RGB-D camera - - - true - - - true - - - - - - Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). - - - true - - - - - - - - - QComboBox::AdjustToContents - - - - OpenNI-PCL - - - - - Freenect - - - - - OpenNI-CV - - - - - OpenNI-CV-ASUS - - - - - OpenNI2 - - - - - Freenect2 - - - - - StereoDC1394 - - - - - StereoFlyCapture2 - - - - - StereoImages - - - - - - - - - 0 - 0 - - - - Calibrate - - - - - - - On initialization, calibration files are loaded from the "camera_info" folder in the working directory. - - - true - - - - - - - - 0 - 0 - - - - Test - - - - - - - Driver - - - true - - - - - - - - - OpenNI 2 - - - - - - - - - true - - - - - - - Auto white balance. - - - true - - - - - - - - - - true - - - - - - - Auto exposure. - - - true - - - - - - - 65535 - - - - - - - Exposure. - - - true - - - - - - - 1000 - - - 100 - - - - - - - Gain. - - - true - - - - - - - - - - false - - - - - - - Mirroring. - - - true - - - - - - - - - - Freenect2 - - - - - - Format. - - - true - - - - - - - QComboBox::AdjustToContents - - - - RGB+Depth SD - - - - - RGB+Depth HD - - - - - IR+Depth - - - - - - - - - - - CameraStereoImages - - - - - - - - - - - - - ... - - - - - - - Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. - - - true - - - - - - - - - - - - - - Path to directory containing stereo images. The images order should be left/right/left/right... and so on. You can also set two directories (separated by ';'), one for left images and one for right images. - - - true - - - - - - - ... - - - - - - - - - - QFormLayout::AllNonFixedFieldsGrow - - - - - - - - - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. In case of OpenNI and OpenNI2 drivers, this can be a path to an ONI file. For StereoImages, this is the name used when looking for calibration files. - - - true - - - - - - - 0 0 1 -1 0 0 0 -1 0 - - - - - - - Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33. - - - true - - - - - - - Only RGB images are published. - - - true - - - - - - - - - - false - - - - - - + + + diff --git a/tools/Calibration/main.cpp b/tools/Calibration/main.cpp index b93b1080..f60c3670 100644 --- a/tools/Calibration/main.cpp +++ b/tools/Calibration/main.cpp @@ -25,8 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/CameraThread.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UConversion.h" @@ -130,11 +131,10 @@ int main(int argc, char * argv[]) bool switchImages = false; - rtabmap::Camera * cameraUsb = 0; - rtabmap::CameraRGBD * camera = 0; + rtabmap::Camera * camera = 0; if(driver == -1) { - cameraUsb = new rtabmap::CameraVideo(device); + camera = new rtabmap::CameraVideo(device); } else if(driver == 0) { @@ -211,17 +211,7 @@ int main(int argc, char * argv[]) rtabmap::CameraThread * cameraThread = 0; - if(cameraUsb) - { - if(!cameraUsb->init()) - { - printf("Camera init failed!\n"); - delete cameraUsb; - exit(1); - } - cameraThread = new rtabmap::CameraThread(cameraUsb); - } - else if(camera) + if(camera) { if(!camera->init("")) { diff --git a/tools/Camera/main.cpp b/tools/Camera/main.cpp index d60c0910..a2827b53 100644 --- a/tools/Camera/main.cpp +++ b/tools/Camera/main.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/DBReader.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UFile.h" @@ -40,7 +40,7 @@ void showUsage() "rtabmap-camera [option] \n" " Options:\n" " --device # USB camera device id (default 0).\n" - " --rate # Frame rate (default 30 Hz). 0 means as fast as possible.\n" + " --rate # Frame rate (default 0 Hz). 0 means as fast as possible.\n" " --path "" Path to a directory of images or a video file.\n" " --calibration "" Calibration file (*.yaml).\n\n"); exit(1); @@ -53,7 +53,7 @@ int main(int argc, char * argv[]) int device = 0; std::string path; - float rate = 30.0f; + float rate = 0.0f; std::string calibrationFile; for(int i=1; iinit()) + if(!calibrationFile.empty()) + { + UINFO("Set calibration: %s", calibrationFile.c_str()); + } + if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile))) { delete camera; UERROR("Cannot initialize the camera."); return -1; } - - if(!calibrationFile.empty()) - { - UINFO("Set calibration: %s", calibrationFile.c_str()); - camera->setCalibration(calibrationFile); - } } if(dbReader) @@ -189,7 +187,7 @@ int main(int argc, char * argv[]) } cv::Mat rgb; - rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw(); + rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw(); cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window while(!rgb.empty()) { @@ -199,7 +197,7 @@ int main(int argc, char * argv[]) if(c == 27) break; // if ESC, break and quit - rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw(); + rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw(); } cv::destroyWindow("Video"); if(camera) diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index d7aeb0d9..46a096bc 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" @@ -77,7 +78,7 @@ int main(int argc, char * argv[]) } UINFO("Using driver %d", driver); - rtabmap::CameraRGBD * camera = 0; + rtabmap::Camera * camera = 0; if(driver == 0) { camera = new rtabmap::CameraOpenni(); @@ -156,42 +157,55 @@ int main(int argc, char * argv[]) delete camera; exit(1); } - cv::Mat rgb, depth; - float fx, fy, cx, cy; - double stamp = 0.0; - camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp); - if(rgb.cols != depth.cols || rgb.rows != depth.rows) + rtabmap::SensorData data = camera->takeImage(); + if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", - rgb.cols, rgb.rows, depth.cols, depth.rows); + data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); } - if(!fx || !fy) + if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) { - UWARN("fx and/or fy are not set! The registered cloud cannot be shown."); + UWARN("Camera not calibrated! The registered cloud cannot be shown."); } pcl::visualization::CloudViewer viewer("cloud"); rtabmap::Transform t(1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0); - while(!rgb.empty() && !viewer.wasStopped()) + while(!data.imageRaw().empty() && !viewer.wasStopped()) { - if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1) + cv::Mat rgb = data.imageRaw(); + if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1)) { // depth + cv::Mat depth = data.depthRaw(); if(depth.type() == CV_32FC1) { depth = rtabmap::util3d::cvtDepthFromFloat(depth); } - if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy) + if(rgb.cols == depth.cols && rgb.rows == depth.rows && + data.cameraModels().size() && + data.cameraModels()[0].isValid()) { - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB( + rgb, depth, + data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), + data.cameraModels()[0].fx(), + data.cameraModels()[0].fy()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); viewer.showCloud(cloud, "cloud"); } - else if(!depth.empty() && fx && fy) + else if(!depth.empty() && + data.cameraModels().size() && + data.cameraModels()[0].isValid()) { - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth( + depth, + data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), + data.cameraModels()[0].fx(), + data.cameraModels()[0].fy()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); viewer.showCloud(cloud, "cloud"); } @@ -204,19 +218,25 @@ int main(int argc, char * argv[]) cv::imshow("Video", rgb); // show frame cv::imshow("Depth", tmp); } - else + else if(!data.rightRaw().empty()) { // stereo + cv::Mat right = data.rightRaw(); cv::imshow("Left", rgb); // show frame - cv::imshow("Right", depth); + cv::imshow("Right", right); - if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy) + if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValid()) { - if(depth.channels() == 3) + if(right.channels() == 3) { - cv::cvtColor(depth, depth, CV_BGR2GRAY); + cv::cvtColor(right, right, CV_BGR2GRAY); } - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromStereoImages( + rgb, right, + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); viewer.showCloud(cloud, "cloud"); } @@ -226,9 +246,7 @@ int main(int argc, char * argv[]) if(c == 27) break; // if ESC, break and quit - rgb = cv::Mat(); - depth = cv::Mat(); - camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp); + data = camera->takeImage(); } cv::destroyWindow("Video"); cv::destroyWindow("Depth"); diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index 7bfe2df3..e43115ad 100644 --- a/tools/ConsoleApp/main.cpp +++ b/tools/ConsoleApp/main.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include "rtabmap/core/Rtabmap.h" -#include "rtabmap/core/Camera.h" +#include "rtabmap/core/CameraRGB.h" #include #include #include @@ -53,10 +53,6 @@ void showUsage() " -rateHz #.## Acquisition rate (Hz), for convenience\n" " -repeat # Repeat the process on the data set # times (minimum of 1)\n" " -createGT Generate a ground truth file\n" - " -image_width # Force an image width (Default 0: original size used).\n" - " The height must be also specified if changed.\n" - " -image_height # Force an image height (Default 0: original size used)\n" - " The height must be also specified if changed.\n" " -start_at # When \"path\" is a directory of images, set this parameter\n" " to start processing at image # (default 1).\n" " -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n" @@ -118,8 +114,6 @@ int main(int argc, char * argv[]) int repeat = 0; bool createGT = false; std::string inputDbPath; - int imageWidth = 0; - int imageHeight = 0; int startAt = 1; ParametersMap pm; ULogger::Level logLevel = ULogger::kError; @@ -194,40 +188,6 @@ int main(int argc, char * argv[]) } continue; } - if(strcmp(argv[i], "-image_width") == 0) - { - ++i; - if(i < argc) - { - imageWidth = std::atoi(argv[i]); - if(imageWidth < 0) - { - showUsage(); - } - } - else - { - showUsage(); - } - continue; - } - if(strcmp(argv[i], "-image_height") == 0) - { - ++i; - if(i < argc) - { - imageHeight = std::atoi(argv[i]); - if(imageHeight < 0) - { - showUsage(); - } - } - else - { - showUsage(); - } - continue; - } if(strcmp(argv[i], "-start_at") == 0) { ++i; @@ -328,12 +288,6 @@ int main(int argc, char * argv[]) printf("Cannot create a Ground truth if repeat is on.\n"); showUsage(); } - else if((imageWidth && imageHeight == 0) || - (imageHeight && imageWidth == 0)) - { - printf("If imageWidth is set, imageHeight must be too.\n"); - showUsage(); - } UTimer timer; timer.start(); @@ -342,11 +296,11 @@ int main(int argc, char * argv[]) Camera * camera = 0; if(UDirectory::exists(path)) { - camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight); + camera = new CameraImages(path, startAt, false, 1/rate); } else { - camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight); + camera = new CameraVideo(path, 1/rate); } if(!camera || !camera->init()) @@ -395,7 +349,6 @@ int main(int argc, char * argv[]) printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold()); printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate); printf(" Repeating data set = %s\n", repeat?"true":"false"); - printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight); printf(" Camera starts at image %d (default 1)\n", startAt); if(createGT) { @@ -422,23 +375,23 @@ int main(int argc, char * argv[]) std::list > teleopActions; while(loopDataset <= repeat && g_forever) { - cv::Mat img = camera->takeImage(); + SensorData data = camera->takeImage(); int i=0; double maxIterationTime = 0.0; int maxIterationTimeId = 0; - while(!img.empty() && g_forever) + while(!data.imageRaw().empty() && g_forever) { ++imagesProcessed; iterationTimer.start(); rtabmapTimer.start(); - rtabmap.process(img); + rtabmap.process(data.imageRaw()); double rtabmapTime = rtabmapTimer.elapsed(); loopClosureId = rtabmap.getLoopClosureId(); if(rtabmap.getLoopClosureId()) { ++countLoopDetected; } - img = camera->takeImage(); + data = camera->takeImage(); if(++count % 100 == 0) { printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n", diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index 8cee2a8b..bed3c5c8 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -174,7 +175,7 @@ int main (int argc, char * argv[]) signal(SIGTERM, &sighandler); signal(SIGINT, &sighandler); - rtabmap::CameraRGBD * camera = 0; + rtabmap::Camera * camera = 0; rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0); if(driver == 0) { @@ -263,7 +264,7 @@ int main (int argc, char * argv[]) app->processEvents(); } - if(cam->init()) + if(camera->init()) { cam->start(); diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 5996c758..bb2c6988 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -725,7 +726,7 @@ int main (int argc, char * argv[]) } else { - rtabmap::CameraRGBD * camera = 0; + rtabmap::Camera * camera = 0; rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0); if(driver == 0) { From e5447be23af80552b37d9333bf31a083754e0cc5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 27 Jun 2015 00:08:52 -0400 Subject: [PATCH 22/45] Refactored 3DTo2D and 3DTo3D motion estimations (Memory and OdometryBOW are now using the same methods) --- .../rtabmap/core/util3d_correspondences.h | 2 +- .../rtabmap/core/util3d_motion_estimation.h | 72 ++++ corelib/src/CMakeLists.txt | 1 + corelib/src/Memory.cpp | 327 ++++++------------ corelib/src/OdometryBOW.cpp | 229 +++--------- corelib/src/util3d_correspondences.cpp | 14 +- corelib/src/util3d_features.cpp | 24 +- corelib/src/util3d_motion_estimation.cpp | 235 +++++++++++++ 8 files changed, 492 insertions(+), 412 deletions(-) create mode 100644 corelib/include/rtabmap/core/util3d_motion_estimation.h create mode 100644 corelib/src/util3d_motion_estimation.cpp diff --git a/corelib/include/rtabmap/core/util3d_correspondences.h b/corelib/include/rtabmap/core/util3d_correspondences.h index a76b9285..97ff12fb 100644 --- a/corelib/include/rtabmap/core/util3d_correspondences.h +++ b/corelib/include/rtabmap/core/util3d_correspondences.h @@ -55,7 +55,7 @@ void RTABMAP_EXP findCorrespondences( pcl::PointCloud & inliers1, pcl::PointCloud & inliers2, float maxDepth, - std::set * uniqueCorrespondences = 0); + std::vector * uniqueCorrespondences = 0); // remove depth by z axis void RTABMAP_EXP extractXYZCorrespondences(const std::multimap & words1, diff --git a/corelib/include/rtabmap/core/util3d_motion_estimation.h b/corelib/include/rtabmap/core/util3d_motion_estimation.h new file mode 100644 index 00000000..204bfcb3 --- /dev/null +++ b/corelib/include/rtabmap/core/util3d_motion_estimation.h @@ -0,0 +1,72 @@ +/* +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 UTIL3D_MOTION_ESTIMATION_H_ +#define UTIL3D_MOTION_ESTIMATION_H_ + +#include + +#include +#include +#include +#include + +namespace rtabmap +{ + +namespace util3d +{ + +Transform estimateMotion3DTo2D( + const std::multimap & words3A, + const std::multimap & words2B, + const CameraModel & cameraModel, + int minInliers = 10, + int iterations = 100, + double reprojError = 5., + int flagsPnP = 0, + const Transform & guess = Transform::getIdentity(), + const std::multimap & words3B = std::multimap(), + double * varianceOut = 0, + std::vector * matchesOut = 0, + std::vector * inliersOut = 0); + +Transform estimateMotion3DTo3D( + const std::multimap & words3A, + const std::multimap & words3B, + int minInliers = 10, + double inliersDistance = 0.1, + int iterations = 100, + int refineIterations = 5, + double * varianceOut = 0, + std::vector * matchesOut = 0, + std::vector * inliersOut = 0); + +} // namespace util3d +} // namespace rtabmap + +#endif /* UTIL3D_TRANSFORMS_H_ */ diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 0f431b8e..6af33c18 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -37,6 +37,7 @@ SET(SRC_FILES util3d_surface.cpp util3d_features.cpp util3d_correspondences.cpp + util3d_motion_estimation.cpp SensorData.cpp Graph.cpp diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index f2a0c3ed..6a4cf722 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d_motion_estimation.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/Statistics.h" @@ -2009,186 +2010,46 @@ Transform Memory::computeVisualTransform( const Signature & oldS, const Signature & newS, std::string * rejectedMsg, - int * inliers, + int * inliersOut, double * varianceOut) const { Transform transform; std::string msg; // Guess transform from visual words - if(_bowPnPEstimation) - { - if(_bowEpipolarGeometry) - { - UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); - } + int inliersCount= 0; + double variance = 1.0; - if((!newS.sensorData().rightRaw().empty() || - !newS.sensorData().stereoCameraModel().isValid()) && - (!newS.sensorData().depthRaw().empty() || - newS.sensorData().cameraModels().size() != 1 || + if(_bowEpipolarGeometry && !_bowPnPEstimation) + { + if(!newS.sensorData().stereoCameraModel().isValid() && + (newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid())) { UERROR("Calibrated camera required (multi-cameras not supported)."); } - else + else if((int)oldS.getWords().size() >= _bowMinInliers && + (int)newS.getWords().size() >= _bowMinInliers) { - cv::Mat K; - Transform localTransform; - if(newS.sensorData().cameraModels().size()) - { - K = newS.sensorData().cameraModels()[0].K(); - localTransform = newS.sensorData().cameraModels()[0].localTransform(); - } - else - { - K = newS.sensorData().stereoCameraModel().left().K(); - localTransform = newS.sensorData().stereoCameraModel().left().localTransform(); - } - UASSERT(!K.empty() && !localTransform.isNull()); - // 2D -> 3D - if(!oldS.getWords3().empty() && !newS.getWords().empty()) - { - // find correspondences - std::vector ids = uListToVector(uUniqueKeys(newS.getWords())); - std::vector objectPoints(ids.size()); - std::vector imagePoints(ids.size()); - int oi=0; - std::vector matches(ids.size()); - for(unsigned int i=0; isecond; - if(pcl::isFinite(pt)) - { - objectPoints[oi].x = pt.x; - objectPoints[oi].y = pt.y; - objectPoints[oi].z = pt.z; - imagePoints[oi] = newS.getWords().find(ids[i])->second.pt; - matches[oi++] = ids[i]; - } - } - } + UASSERT(oldS.sensorData().stereoCameraModel().isValid() || (oldS.sensorData().cameraModels().size() == 1 && oldS.sensorData().cameraModels()[0].isValid())); + const CameraModel & cameraModel = oldS.sensorData().stereoCameraModel().isValid()?oldS.sensorData().stereoCameraModel().left():oldS.sensorData().cameraModels()[0]; - objectPoints.resize(oi); - imagePoints.resize(oi); - matches.resize(oi); - - if((int)matches.size() >= _bowMinInliers) - { - //PnPRansac - Transform guess = localTransform.inverse(); - cv::Mat R = (cv::Mat_(3,3) << - (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), - (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), - (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); - cv::Mat rvec(1,3, CV_64FC1); - cv::Rodrigues(R, rvec); - cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); - std::vector inliersV; - cv::solvePnPRansac(objectPoints, - imagePoints, - K, - cv::Mat(), - rvec, - tvec, - true, - _bowIterations, - _bowPnPReprojError, - 0, - inliersV, - _bowPnPFlags); - - if(inliers) - { - *inliers = (int)inliersV.size(); - } - if((int)inliersV.size() >= _bowMinInliers) - { - cv::Rodrigues(rvec, R); - Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), - R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), - R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - - transform = localTransform * pnp; - - UDEBUG("Odom transform = %s", transform.prettyPrint().c_str()); - - // compute variance (like in PCL computeVariance() method of sac_model.h) - if(varianceOut) - { - std::vector errorSqrdDists(inliersV.size()); - oi = 0; - for(unsigned int i=0; i::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]); - if(iter != newS.getWords3().end() && pcl::isFinite(iter->second)) - { - const cv::Point3f & objPt = objectPoints[inliersV[i]]; - pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform); - errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); - } - } - errorSqrdDists.resize(oi); - *varianceOut= 0; - if(errorSqrdDists.size()) - { - std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; - *varianceOut = 2.1981 * median_error_sqr; - } - } - } - else - { - msg = uFormat("PnP not enough inliers (%d[%d] < %d), rejecting the transform...", - (int)inliersV.size(), (int)matches.size(), _bowMinInliers); - UINFO(msg.c_str()); - } - } - else - { - msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers); - UINFO(msg.c_str()); - } - } - else - { - msg = uFormat("Not enough features in the new image (old=%d new=%d min=%d)", - (int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers); - UINFO(msg.c_str()); - } - } - - } - else if(_bowEpipolarGeometry) - { - // we only need the camera transform, send guess words3 for scale estimation - if(oldS.getWords3().size() && oldS.sensorData().cameraModels().size()) - { + // we only need the camera transform, send guess words3 for scale estimation Transform cameraTransform; - double variance = 1; std::multimap inliers3D = util3d::generateWords3DMono( oldS.getWords(), newS.getWords(), - oldS.sensorData().cameraModels()[0], + cameraModel, cameraTransform, - 100, - 4.0f, - 0, // cv::SOLVEPNP_ITERATIVE + _bowIterations, + _bowPnPReprojError, + _bowPnPFlags, // cv::SOLVEPNP_ITERATIVE 1.0f, 0.99f, - oldS.getWords3(), + oldS.getWords3(), // for scale estimation &variance); - if(varianceOut) - { - *varianceOut = variance; - } - if(inliers) - { - *inliers = (int)inliers3D.size(); - } + + inliersCount = (int)inliers3D.size(); if(!cameraTransform.isNull()) { @@ -2197,13 +2058,6 @@ Transform Memory::computeVisualTransform( if(variance <= _bowEpipolarGeometryVar) { transform = cameraTransform.inverse(); - if(_bowForce2D) - { - UDEBUG("Forcing 2D..."); - float x,y,z,r,p,yaw; - transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw); - transform = Transform(x,y,0, 0, 0, yaw); - } } else { @@ -2234,87 +2088,105 @@ Transform Memory::computeVisualTransform( UWARN(msg.c_str()); } } - else + else if(_bowPnPEstimation) { - // 3D -> 3D - if(!oldS.getWords3().empty() && !newS.getWords3().empty()) + if(_bowEpipolarGeometry) { - pcl::PointCloud::Ptr inliersOld(new pcl::PointCloud); - pcl::PointCloud::Ptr inliersNew(new pcl::PointCloud); - util3d::findCorrespondences( - oldS.getWords3(), - newS.getWords3(), - *inliersOld, - *inliersNew, - _bowMaxDepth); + UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); + } - std::list > > pairs2d; - EpipolarGeometry::findPairsUnique(oldS.getWords(), newS.getWords(), pairs2d); - - UDEBUG("3D unique Correspondences = %d (2D unique pairs=%d) words=%d and %d", - (int)inliersOld->size(), (int)pairs2d.size(), (int)oldS.getWords3().size(), (int)newS.getWords3().size()); - - if((int)inliersOld->size() >= _bowMinInliers) + if(!newS.sensorData().stereoCameraModel().isValid() && + (newS.sensorData().cameraModels().size() != 1 || + !newS.sensorData().cameraModels()[0].isValid())) + { + UERROR("Calibrated camera required (multi-cameras not supported)."); + } + else + { + // 3D to 2D + if((int)oldS.getWords3().size() >= _bowMinInliers && + (int)newS.getWords().size() >= _bowMinInliers) { + UASSERT(newS.sensorData().stereoCameraModel().isValid() || (newS.sensorData().cameraModels().size() == 1 && newS.sensorData().cameraModels()[0].isValid())); + const CameraModel & cameraModel = newS.sensorData().stereoCameraModel().isValid()?newS.sensorData().stereoCameraModel().left():newS.sensorData().cameraModels()[0]; - int inliersCount = 0; std::vector inliersV; - Transform t = util3d::transformFromXYZCorrespondences( - inliersOld, - inliersNew, - _bowInlierDistance, + transform = util3d::estimateMotion3DTo2D( + oldS.getWords3(), + newS.getWords(), + cameraModel, + _bowMinInliers, _bowIterations, - true, 3.0, 10, - &inliersV, - varianceOut); + _bowPnPReprojError, + _bowPnPFlags, + Transform::getIdentity(), + newS.getWords3(), + &variance, + 0, + &inliersV); inliersCount = (int)inliersV.size(); - if(!t.isNull() && inliersCount >= _bowMinInliers) + if(transform.isNull()) { - transform = t; - if(_bowForce2D) - { - UDEBUG("Forcing 2D..."); - float x,y,z,r,p,yaw; - transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw); - transform = Transform(x,y,0, 0, 0, yaw); - } - } - else if(inliersCount < _bowMinInliers) - { - msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id()); + msg = uFormat("Not enough inliers %d/%d between %d and %d", + inliersCount, _bowMinInliers, oldS.id(), newS.id()); UINFO(msg.c_str()); } - else if(inliersCount == (int)inliersOld->size()) + else { - msg = uFormat("Rejected identity with full inliers."); - UINFO(msg.c_str()); - } - - if(inliers) - { - *inliers = inliersCount; + transform = transform.inverse(); } } else { - msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id()); + msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)", + (int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers); UINFO(msg.c_str()); } } - else if(!oldS.isBadSignature() && !newS.isBadSignature() && (oldS.getWords3().size()==0 || newS.getWords3().size()==0)) + + } + else + { + // 3D -> 3D + if((int)oldS.getWords3().size() >= _bowMinInliers && + (int)newS.getWords3().size() >= _bowMinInliers) { - msg = uFormat("Words 3D empty?!? olds=%d=%d newS=%d=%d", - oldS.id(), (int)oldS.getWords3().size(), - newS.id(), (int)newS.getWords3().size()); - UWARN(msg.c_str()); + std::vector inliersV; + transform = util3d::estimateMotion3DTo3D( + oldS.getWords3(), + newS.getWords3(), + _bowMinInliers, + _bowInlierDistance, + _bowIterations, + 10, + &variance, + 0, + &inliersV); + inliersCount = (int)inliersV.size(); + if(transform.isNull()) + { + msg = uFormat("Not enough inliers %d/%d between %d and %d", + inliersCount, _bowMinInliers, oldS.id(), newS.id()); + UINFO(msg.c_str()); + } + else + { + transform = transform.inverse(); + } + } + else + { + msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)", + (int)oldS.getWords3().size(), (int)newS.getWords3().size(), _bowMinInliers); + UINFO(msg.c_str()); } } if(!transform.isNull()) { // verify if it is a 180 degree transform, well verify > 90 - float roll,pitch,yaw; - transform.getEulerAngles(roll, pitch, yaw); + float x,y,z, roll,pitch,yaw; + transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if(fabs(roll) > CV_PI/2 || fabs(pitch) > CV_PI/2 || fabs(yaw) > CV_PI/2) @@ -2324,12 +2196,25 @@ Transform Memory::computeVisualTransform( roll, pitch, yaw); UWARN(msg.c_str()); } + else if(_bowForce2D) + { + UDEBUG("Forcing 2D..."); + transform = Transform(x,y,0, 0, 0, yaw); + } } if(rejectedMsg) { *rejectedMsg = msg; } + if(inliersOut) + { + *inliersOut = inliersCount; + } + if(varianceOut) + { + *varianceOut = variance; + } UDEBUG("transform=%s", transform.prettyPrint().c_str()); return transform; } diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 27d856e6..48b65bd0 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_correspondences.h" +#include "rtabmap/core/util3d_motion_estimation.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/utilite/ULogger.h" @@ -208,7 +209,7 @@ Transform OdometryBOW::computeTransform( } double variance = 0; - int inliers = 0; + int inliersCount = 0; int correspondences = 0; int nFeatures = 0; @@ -229,8 +230,11 @@ Transform OdometryBOW::computeTransform( Transform transform; if((int)localMap_.size() >= this->getMinInliers()) { + std::vector matches, inliers; + Transform t; if(this->isPnPEstimationUsed()) { + // 3D to 2D if(data.cameraModels().size() > 1) { UERROR("PnP cannot be used on multi-cameras setup."); @@ -240,117 +244,19 @@ Transform OdometryBOW::computeTransform( UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid())); const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0]; - // find correspondences - std::vector ids = uListToVector(uUniqueKeys(newSignature->getWords())); - std::vector objectPoints(ids.size()); - std::vector imagePoints(ids.size()); - int oi=0; - std::vector matches(ids.size()); - for(unsigned int i=0; isecond; - objectPoints[oi].x = pt.x; - objectPoints[oi].y = pt.y; - objectPoints[oi].z = pt.z; - imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt; - matches[oi++] = ids[i]; - } - } - - objectPoints.resize(oi); - imagePoints.resize(oi); - matches.resize(oi); - - if(this->isInfoDataFilled() && info) - { - info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end()); - } - correspondences = (int)matches.size(); - - if((int)matches.size() >= this->getMinInliers()) - { - //PnPRansac - cv::Mat K = cameraModel.K(); - Transform guess = (this->getPose() * cameraModel.localTransform()).inverse(); - cv::Mat R = (cv::Mat_(3,3) << - (double)guess.r11(), (double)guess.r12(), (double)guess.r13(), - (double)guess.r21(), (double)guess.r22(), (double)guess.r23(), - (double)guess.r31(), (double)guess.r32(), (double)guess.r33()); - cv::Mat rvec(1,3, CV_64FC1); - cv::Rodrigues(R, rvec); - cv::Mat tvec = (cv::Mat_(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z()); - std::vector inliersV; - cv::solvePnPRansac(objectPoints, - imagePoints, - K, - cv::Mat(), - rvec, - tvec, - true, - this->getIterations(), - this->getPnPReprojError(), - 0, - inliersV, - this->getPnPFlags()); - - inliers = (int)inliersV.size(); - if((int)inliersV.size() >= this->getMinInliers()) - { - cv::Rodrigues(rvec, R); - Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), - R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), - R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); - - // make it incremental - transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse(); - - UDEBUG("Odom transform = %s", transform.prettyPrint().c_str()); - - // compute variance (like in PCL computeVariance() method of sac_model.h) - std::vector errorSqrdDists(inliersV.size()); - oi = 0; - for(unsigned int i=0; i::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]); - if(iter != newSignature->getWords3().end() && pcl::isFinite(iter->second)) - { - const cv::Point3f & objPt = objectPoints[inliersV[i]]; - pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform); - errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); - } - } - errorSqrdDists.resize(oi); - if(errorSqrdDists.size()) - { - std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; - variance = 2.1981 * median_error_sqr; - } - else - { - variance = 1; - } - } - else - { - UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers()); - } - - if(this->isInfoDataFilled() && info && inliersV.size()) - { - info->wordInliers.resize(inliersV.size()); - for(unsigned int i=0; iwordInliers[i] = matches[inliersV[i]]; - } - } - } - else - { - UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers()); - } + t = util3d::estimateMotion3DTo2D( + localMap_, + newSignature->getWords(), + cameraModel, + this->getMinInliers(), + this->getIterations(), + this->getPnPReprojError(), + this->getPnPFlags(), + this->getPose(), + newSignature->getWords3(), + &variance, + &matches, + &inliers); } else { @@ -359,76 +265,51 @@ Transform OdometryBOW::computeTransform( } else { + // 3D to 3D if((int)newSignature->getWords3().size() >= this->getMinInliers()) { - pcl::PointCloud::Ptr inliers1(new pcl::PointCloud); // previous - pcl::PointCloud::Ptr inliers2(new pcl::PointCloud); // new - - // No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above. - // Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering - // by depth here is wrong! - std::set uniqueCorrespondences; - util3d::findCorrespondences( + t = util3d::estimateMotion3DTo3D( localMap_, newSignature->getWords3(), - *inliers1, - *inliers2, - 0, - &uniqueCorrespondences); - - UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size()); - - if(this->isInfoDataFilled() && info) - { - info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end()); - } - - correspondences = (int)inliers1->size(); - if((int)inliers1->size() >= this->getMinInliers()) - { - // the transform returned is global odometry pose, not incremental one - std::vector inliersV; - Transform t = util3d::transformFromXYZCorrespondences( - inliers2, - inliers1, - this->getInlierDistance(), - this->getIterations(), - this->getRefineIterations()>0, 3.0, this->getRefineIterations(), - &inliersV, - &variance); - - inliers = (int)inliersV.size(); - if(!t.isNull() && inliers >= this->getMinInliers()) - { - // make it incremental - transform = this->getPose().inverse() * t; - - UDEBUG("Odom transform = %s", transform.prettyPrint().c_str()); - } - else - { - UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences); - } - - if(this->isInfoDataFilled() && info && inliersV.size()) - { - info->wordInliers.resize(inliersV.size()); - for(unsigned int i=0; iwordInliers[i] = info->wordMatches[inliersV[i]]; - } - } - } - else - { - UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers()); - } + this->getMinInliers(), + this->getInlierDistance(), + this->getIterations(), + this->getRefineIterations(), + &variance, + &matches, + &inliers); } else { UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers()); } } + + correspondences = matches.size(); + inliersCount = inliers.size(); + if(this->isInfoDataFilled() && info) + { + info->wordMatches = matches; + info->wordInliers = inliers; + } + + if(!t.isNull()) + { + // make it incremental + transform = this->getPose().inverse() * t; + } + else if(correspondences < this->getMinInliers()) + { + UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers()); + } + else if(inliersCount < this->getMinInliers()) + { + UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers()); + } + else + { + UWARN("Unknown estimation error"); + } } else { @@ -534,7 +415,7 @@ Transform OdometryBOW::computeTransform( if(info) { info->variance = variance; - info->inliers = inliers; + info->inliers = inliersCount; info->matches = correspondences; info->features = nFeatures; info->localMapSize = (int)localMap_.size(); @@ -544,7 +425,7 @@ Transform OdometryBOW::computeTransform( timer.elapsed(), output.isNull()?"true":"false", nFeatures, - inliers, + inliersCount, correspondences, variance, (int)localMap_.size(), diff --git a/corelib/src/util3d_correspondences.cpp b/corelib/src/util3d_correspondences.cpp index a065f975..40e25de0 100644 --- a/corelib/src/util3d_correspondences.cpp +++ b/corelib/src/util3d_correspondences.cpp @@ -355,12 +355,16 @@ void findCorrespondences( pcl::PointCloud & inliers1, pcl::PointCloud & inliers2, float maxDepth, - std::set * uniqueCorrespondences) + std::vector * uniqueCorrespondences) { std::list ids = uUniqueKeys(words1); // Find pairs inliers1.resize(ids.size()); inliers2.resize(ids.size()); + if(uniqueCorrespondences) + { + uniqueCorrespondences->resize(ids.size()); + } int oi=0; for(std::list::iterator iter=ids.begin(); iter!=ids.end(); ++iter) @@ -375,16 +379,20 @@ void findCorrespondences( (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) && (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth))) { - ++oi; if(uniqueCorrespondences) { - uniqueCorrespondences->insert(*iter); + uniqueCorrespondences->at(oi) = *iter; } + ++oi; } } } inliers1.resize(oi); inliers2.resize(oi); + if(uniqueCorrespondences) + { + uniqueCorrespondences->resize(oi); + } } } diff --git a/corelib/src/util3d_features.cpp b/corelib/src/util3d_features.cpp index 045ca975..e6338b64 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -351,18 +351,6 @@ std::multimap generateWords3DMono( } } - if(!useCameraTransformGuess) - { - cv::Mat R, T; - EpipolarGeometry::findRTFromP(P, R, T); - - Transform t(R.at(0,0), R.at(0,1), R.at(0,2), T.at(0), - R.at(1,0), R.at(1,1), R.at(1,2), T.at(1), - R.at(2,0), R.at(2,1), R.at(2,2), T.at(2)); - - cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform(); - } - if(refGuess3D.size()) { // scale estimation @@ -489,7 +477,6 @@ std::multimap generateWords3DMono( else { UWARN("No inliers after PnP!"); - cameraTransform = Transform(); } } } @@ -498,6 +485,17 @@ std::multimap generateWords3DMono( UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess"); } } + else if(!useCameraTransformGuess) + { + cv::Mat R, T; + EpipolarGeometry::findRTFromP(P, R, T); + + Transform t(R.at(0,0), R.at(0,1), R.at(0,2), T.at(0), + R.at(1,0), R.at(1,1), R.at(1,2), T.at(1), + R.at(2,0), R.at(2,1), R.at(2,2), T.at(2)); + + cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform(); + } } } } diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp new file mode 100644 index 00000000..558ae8e3 --- /dev/null +++ b/corelib/src/util3d_motion_estimation.cpp @@ -0,0 +1,235 @@ +/* +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. +*/ + +#include "rtabmap/core/util3d_motion_estimation.h" + +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UMath.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d_registration.h" +#include "rtabmap/core/util3d_correspondences.h" + +namespace rtabmap +{ + +namespace util3d +{ + +Transform estimateMotion3DTo2D( + const std::multimap & words3A, + const std::multimap & words2B, + const CameraModel & cameraModel, + int minInliers, + int iterations, + double reprojError, + int flagsPnP, + const Transform & guess, + const std::multimap & words3B, + double * varianceOut, + std::vector * matchesOut, + std::vector * inliersOut) +{ + + Transform transform; + std::vector matches, inliers; + + if(varianceOut) + { + *varianceOut = 1.0; + } + + // find correspondences + std::vector ids = uListToVector(uUniqueKeys(words2B)); + std::vector objectPoints(ids.size()); + std::vector imagePoints(ids.size()); + int oi=0; + matches.resize(ids.size()); + for(unsigned int i=0; isecond; + objectPoints[oi].x = pt.x; + objectPoints[oi].y = pt.y; + objectPoints[oi].z = pt.z; + imagePoints[oi] = words2B.find(ids[i])->second.pt; + matches[oi++] = ids[i]; + } + } + + objectPoints.resize(oi); + imagePoints.resize(oi); + matches.resize(oi); + + if((int)matches.size() >= minInliers) + { + //PnPRansac + cv::Mat K = cameraModel.K(); + Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse(); + cv::Mat R = (cv::Mat_(3,3) << + (double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(), + (double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(), + (double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33()); + + cv::Mat rvec(1,3, CV_64FC1); + cv::Rodrigues(R, rvec); + cv::Mat tvec = (cv::Mat_(1,3) << + (double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z()); + + cv::solvePnPRansac( + objectPoints, + imagePoints, + K, + cv::Mat(), + rvec, + tvec, + true, + iterations, + reprojError, + 0, + inliers, + flagsPnP); + + if((int)inliers.size() >= minInliers) + { + cv::Rodrigues(rvec, R); + Transform pnp(R.at(0,0), R.at(0,1), R.at(0,2), tvec.at(0), + R.at(1,0), R.at(1,1), R.at(1,2), tvec.at(1), + R.at(2,0), R.at(2,1), R.at(2,2), tvec.at(2)); + + transform = (cameraModel.localTransform() * pnp).inverse(); + + // compute variance (like in PCL computeVariance() method of sac_model.h) + if(varianceOut && words3B.size()) + { + std::vector errorSqrdDists(inliers.size()); + oi = 0; + for(unsigned int i=0; i::const_iterator iter = words3B.find(matches[inliers[i]]); + if(iter != words3B.end() && pcl::isFinite(iter->second)) + { + const cv::Point3f & objPt = objectPoints[inliers[i]]; + pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform); + errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); + } + } + errorSqrdDists.resize(oi); + if(errorSqrdDists.size()) + { + std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; + *varianceOut = 2.1981 * median_error_sqr; + } + } + } + } + + if(matchesOut) + { + *matchesOut = matches; + } + if(inliersOut) + { + inliersOut->resize(inliers.size()); + for(unsigned int i=0; iat(i) = matches[inliers[i]]; + } + } + + return transform; +} + +Transform estimateMotion3DTo3D( + const std::multimap & words3A, + const std::multimap & words3B, + int minInliers, + double inliersDistance, + int iterations, + int refineIterations, + double * varianceOut, + std::vector * matchesOut, + std::vector * inliersOut) +{ + Transform transform; + pcl::PointCloud::Ptr inliers1(new pcl::PointCloud); // previous + pcl::PointCloud::Ptr inliers2(new pcl::PointCloud); // new + + std::vector matches; + util3d::findCorrespondences( + words3A, + words3B, + *inliers1, + *inliers2, + 0, + &matches); + + if(varianceOut) + { + *varianceOut = 1.0; + } + + if((int)inliers1->size() >= minInliers) + { + std::vector inliers; + Transform t = util3d::transformFromXYZCorrespondences( + inliers2, + inliers1, + inliersDistance, + iterations, + refineIterations>0, + 3.0, + refineIterations, + &inliers, + varianceOut); + + if(!t.isNull() && (int)inliers.size() >= minInliers) + { + transform = t; + } + + if(matchesOut) + { + *matchesOut = matches; + } + + if(inliersOut) + { + inliersOut->resize(inliers.size()); + for(unsigned int i=0; iat(i) = matches[inliers[i]]; + } + } + } + return transform; +} + +} + +} From b7faef35f17469c696d97ca7cb47c4289e4785a4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 27 Jun 2015 01:43:29 -0400 Subject: [PATCH 23/45] Refactored motion estimation parameters in the GUI. --- corelib/include/rtabmap/core/Memory.h | 6 +- corelib/include/rtabmap/core/Odometry.h | 4 +- corelib/include/rtabmap/core/Parameters.h | 10 +- corelib/include/rtabmap/core/Rtabmap.h | 1 + corelib/src/Memory.cpp | 34 +- corelib/src/Odometry.cpp | 4 +- corelib/src/OdometryBOW.cpp | 2 +- corelib/src/OdometryOpticalFlow.cpp | 2 +- corelib/src/Rtabmap.cpp | 4 + guilib/src/DatabaseViewer.cpp | 10 +- guilib/src/PreferencesDialog.cpp | 12 +- guilib/src/ui/DatabaseViewer.ui | 99 +- guilib/src/ui/preferencesDialog.ui | 1506 +++++++++++---------- tools/OdometryViewer/main.cpp | 2 +- 14 files changed, 921 insertions(+), 775 deletions(-) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 2d79db19..5bb8eb98 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -178,7 +178,6 @@ public: float getBowInlierDistance() const {return _bowInlierDistance;} int getBowIterations() const {return _bowIterations;} 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, double * variance = 0) const; Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const; @@ -271,11 +270,10 @@ private: int _bowMinInliers; float _bowInlierDistance; int _bowIterations; - float _bowMaxDepth; + int _bowRefineIterations; bool _bowForce2D; - bool _bowEpipolarGeometry; float _bowEpipolarGeometryVar; - bool _bowPnPEstimation; + bool _bowEstimationType; double _bowPnPReprojError; int _bowPnPFlags; float _icpMaxTranslation; diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 10f46f5a..38a11643 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -60,7 +60,7 @@ public: int getRefineIterations() const {return _refineIterations;} float getMaxDepth() const {return _maxDepth;} bool isInfoDataFilled() const {return _fillInfoData;} - bool isPnPEstimationUsed() const {return _pnpEstimation;} + bool getEstimationType() const {return _estimationType;} double getPnPReprojError() const {return _pnpReprojError;} int getPnPFlags() const {return _pnpFlags;} const Transform & previousTransform() const {return previousTransform_;} @@ -85,7 +85,7 @@ private: float _particleNoiseR; float _particleLambdaR; bool _fillInfoData; - bool _pnpEstimation; + int _estimationType; double _pnpReprojError; int _pnpFlags; Transform _pose; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 4a82e332..1fc9cb0c 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -313,6 +313,7 @@ class RTABMAP_EXP Parameters // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); + RTABMAP_PARAM(Odom, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)"); RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits."); RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences."); RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform."); @@ -325,7 +326,6 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); - RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error."); RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory."); @@ -363,14 +363,13 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(LccIcp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m)."); RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad)."); + RTABMAP_PARAM(LccBow, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform."); RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences."); RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words."); - RTABMAP_PARAM(LccBow, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit)."); - RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); - RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false, "Use epipolar geometry to compute the loop closure transform."); + RTABMAP_PARAM(LccBow, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); + RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure."); - RTABMAP_PARAM(LccBow, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error."); RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure."); @@ -378,6 +377,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio."); RTABMAP_PARAM(LccReextract, FeatureType, int, 4, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); RTABMAP_PARAM(LccReextract, MaxWords, int, 600, "0 no limits."); + RTABMAP_PARAM(LccReextract, MaxDepth, float, 0.0, "Max depth of the words (0 means no limit)."); RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation."); RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 8a39d18e..ff89222e 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -193,6 +193,7 @@ private: float _reextractNNDR; int _reextractFeatureType; int _reextractMaxWords; + float _reextractMaxDepth; bool _startNewMapOnLoopClosure; float _goalReachedRadius; // meters bool _planVirtualLinks; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 6a4cf722..732d3fe1 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -101,11 +101,10 @@ Memory::Memory(const ParametersMap & parameters) : _bowMinInliers(Parameters::defaultLccBowMinInliers()), _bowInlierDistance(Parameters::defaultLccBowInlierDistance()), _bowIterations(Parameters::defaultLccBowIterations()), - _bowMaxDepth(Parameters::defaultLccBowMaxDepth()), + _bowRefineIterations(Parameters::defaultLccBowRefineIterations()), _bowForce2D(Parameters::defaultLccBowForce2D()), - _bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()), _bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()), - _bowPnPEstimation(Parameters::defaultLccBowPnPEstimation()), + _bowEstimationType(Parameters::defaultLccBowEstimationType()), _bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()), _bowPnPFlags(Parameters::defaultLccBowPnPFlags()), @@ -441,11 +440,10 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccBowMinInliers(), _bowMinInliers); Parameters::parse(parameters, Parameters::kLccBowInlierDistance(), _bowInlierDistance); Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations); - Parameters::parse(parameters, Parameters::kLccBowMaxDepth(), _bowMaxDepth); + Parameters::parse(parameters, Parameters::kLccBowRefineIterations(), _bowRefineIterations); Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D); - Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry); + Parameters::parse(parameters, Parameters::kLccBowEstimationType(), _bowEstimationType); Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar); - Parameters::parse(parameters, Parameters::kLccBowPnPEstimation(), _bowPnPEstimation); Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError); Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags); Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation); @@ -474,7 +472,6 @@ void Memory::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str()); UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str()); UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str()); - UASSERT_MSG(_bowMaxDepth >= 0.0f, uFormat("value=%f", _bowMaxDepth).c_str()); UASSERT_MSG(_icpDecimation > 0, uFormat("value=%d", _icpDecimation).c_str()); UASSERT_MSG(_icpMaxDepth >= 0.0f, uFormat("value=%f", _icpMaxDepth).c_str()); UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str()); @@ -2020,7 +2017,7 @@ Transform Memory::computeVisualTransform( int inliersCount= 0; double variance = 1.0; - if(_bowEpipolarGeometry && !_bowPnPEstimation) + if(_bowEstimationType == 2) // Epipolar Geometry { if(!newS.sensorData().stereoCameraModel().isValid() && (newS.sensorData().cameraModels().size() != 1 || @@ -2088,13 +2085,8 @@ Transform Memory::computeVisualTransform( UWARN(msg.c_str()); } } - else if(_bowPnPEstimation) + else if(_bowEstimationType == 1) // PnP { - if(_bowEpipolarGeometry) - { - UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); - } - if(!newS.sensorData().stereoCameraModel().isValid() && (newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid())) @@ -2158,7 +2150,7 @@ Transform Memory::computeVisualTransform( _bowMinInliers, _bowInlierDistance, _bowIterations, - 10, + _bowRefineIterations, &variance, 0, &inliersV); @@ -2919,14 +2911,7 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const if(foutSign) { - if(words3D) - { - fprintf(foutSign, "SignatureID WordsID... (Max features depth=%f)\n", _bowMaxDepth); - } - else - { - fprintf(foutSign, "SignatureID WordsID...\n"); - } + fprintf(foutSign, "SignatureID WordsID...\n"); const std::map & signatures = this->getSignatures(); for(std::map::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) { @@ -2941,8 +2926,7 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const { //show only valid point according to current parameters if(pcl::isFinite(jter->second) && - (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0) && - (_bowMaxDepth <= 0 || jter->second.x <= _bowMaxDepth)) + (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0)) { fprintf(foutSign, "%d ", (*jter).first); } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 54db7178..d0954f1c 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -51,7 +51,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _particleNoiseR(Parameters::defaultOdomParticleNoiseR()), _particleLambdaR(Parameters::defaultOdomParticleLambdaR()), _fillInfoData(Parameters::defaultOdomFillInfoData()), - _pnpEstimation(Parameters::defaultOdomPnPEstimation()), + _estimationType(Parameters::defaultOdomEstimationType()), _pnpReprojError(Parameters::defaultOdomPnPReprojError()), _pnpFlags(Parameters::defaultOdomPnPFlags()), _resetCurrentCount(0), @@ -69,7 +69,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D); Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic); Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData); - Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation); + Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType); Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError); Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags); UASSERT(_pnpFlags>=0 && _pnpFlags <=2); diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 48b65bd0..448b9fcf 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -232,7 +232,7 @@ Transform OdometryBOW::computeTransform( { std::vector matches, inliers; Transform t; - if(this->isPnPEstimationUsed()) + if(this->getEstimationType() == 1) // PnP { // 3D to 2D if(data.cameraModels().size() > 1) diff --git a/corelib/src/OdometryOpticalFlow.cpp b/corelib/src/OdometryOpticalFlow.cpp index 5f4f0f02..f3c1eb6a 100644 --- a/corelib/src/OdometryOpticalFlow.cpp +++ b/corelib/src/OdometryOpticalFlow.cpp @@ -234,7 +234,7 @@ Transform OdometryOpticalFlow::computeTransform( if(ki && ki >= this->getMinInliers()) { - if(this->isPnPEstimationUsed()) + if(this->getEstimationType() == 1) // PnP { // find correspondences if(this->isInfoDataFilled() && info) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3e68668d..e8959fa0 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -105,6 +105,7 @@ Rtabmap::Rtabmap() : _reextractNNDR(Parameters::defaultLccReextractNNDR()), _reextractFeatureType(Parameters::defaultLccReextractFeatureType()), _reextractMaxWords(Parameters::defaultLccReextractMaxWords()), + _reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()), _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()), _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()), _planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()), @@ -402,6 +403,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR); Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType); Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords); + Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius); Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks); @@ -1627,6 +1629,7 @@ bool Rtabmap::process( uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR))); uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords))); + uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth))); uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0")); uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0")); uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false")); @@ -1783,6 +1786,7 @@ bool Rtabmap::process( uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR))); uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords))); + uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth))); uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0")); uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0")); uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false")); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index d4aa1a48..b0c570de 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2967,7 +2967,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex()))); parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0")); @@ -2995,9 +2995,9 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); + parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex()))); memory_->parseParameters(parameters); t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } @@ -3086,8 +3086,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked()))); + parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex()))); parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0")); @@ -3122,10 +3121,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value()))); parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value()))); parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value()))); - parameters.insert(ParametersPair(Parameters::kLccBowPnPEstimation(), uBool2Str(ui_->checkBox_pnp->isChecked()))); + parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex()))); memory_->parseParameters(parameters); t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 7ae62dd1..9b874116 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -551,11 +551,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str()); _ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str()); _ui->loopClosure_bowIterations->setObjectName(Parameters::kLccBowIterations().c_str()); - _ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccBowMaxDepth().c_str()); + _ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kLccBowRefineIterations().c_str()); _ui->loopClosure_bowForce2D->setObjectName(Parameters::kLccBowForce2D().c_str()); - _ui->loopClosure_bowEpipolarGeometry->setObjectName(Parameters::kLccBowEpipolarGeometry().c_str()); + _ui->loopClosure_estimationType->setObjectName(Parameters::kLccBowEstimationType().c_str()); + connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int))); + _ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultLccBowEstimationType()); _ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str()); - _ui->loopClosure_pnpEstimation->setObjectName(Parameters::kLccBowPnPEstimation().c_str()); _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str()); _ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str()); @@ -564,6 +565,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->reextract_nndrRatio->setObjectName(Parameters::kLccReextractNNDR().c_str()); _ui->reextract_type->setObjectName(Parameters::kLccReextractFeatureType().c_str()); _ui->reextract_maxFeatures->setObjectName(Parameters::kLccReextractMaxWords().c_str()); + _ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccReextractMaxDepth().c_str()); _ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str()); _ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kLccIcpMaxTranslation().c_str()); @@ -600,7 +602,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str()); _ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str()); _ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str()); - _ui->odom_pnpEstimation->setObjectName(Parameters::kOdomPnPEstimation().c_str()); + _ui->odom_estimationType->setObjectName(Parameters::kOdomEstimationType().c_str()); + connect(_ui->odom_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odomEstimation, SLOT(setCurrentIndex(int))); + _ui->stackedWidget_odomEstimation->setCurrentIndex(Parameters::defaultOdomEstimationType()); _ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str()); _ui->odom_pnpFlags->setObjectName(Parameters::kOdomPnPFlags().c_str()); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 16723161..329e2643 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -50,8 +50,8 @@ 0 0 - 196 - 184 + 166 + 173 @@ -236,8 +236,8 @@ 0 0 - 195 - 184 + 165 + 173 @@ -418,7 +418,7 @@ 0 0 1285 - 22 + 25 @@ -798,15 +798,15 @@ - 5 + 1 0 0 - 312 - 314 + 314 + 303 @@ -1026,7 +1026,7 @@ 0 0 351 - 355 + 347 @@ -1044,7 +1044,7 @@ 1000 - 30 + 100 @@ -1055,29 +1055,6 @@ - - - - m - - - 2 - - - 100.000000000000000 - - - 4.000000000000000 - - - - - - - Max feature depth - - - @@ -1170,15 +1147,22 @@ - PnP (2D->3D estimation) + Motion estimation. - - - - + + + + 3D to 3D + + + + + 3D to 2D (PnP) + + @@ -1285,7 +1269,7 @@ - + Qt::Vertical @@ -1298,6 +1282,29 @@ + + + + Max feature depth + + + + + + + m + + + 2 + + + 100.000000000000000 + + + 4.000000000000000 + + + @@ -1308,8 +1315,8 @@ 0 0 - 330 - 304 + 333 + 306 @@ -1525,8 +1532,8 @@ 0 0 - 248 - 319 + 243 + 284 @@ -1720,7 +1727,7 @@ 0 0 201 - 126 + 117 @@ -1819,8 +1826,8 @@ 0 0 - 283 - 322 + 285 + 309 diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 4957b2d2..54972467 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -7,7 +7,7 @@ 0 0 1058 - 649 + 592 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 20 @@ -5866,9 +5866,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Note that when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word->Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure. You may consider to enable "Re-extract features" option below to increase the number of correspondences. + + + true + + + - + 1 @@ -5881,122 +5891,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - Minimum visual word correspondences to compute geometry transform. - - - - - - - m - - - 0.000000000000000 - - - 5.000000000000000 - - - - - - - Max feature depth. Note that parameter "Visual Word"->"Max words depth" is applied before this. + Minimum visual word correspondences to accept the estimated transformation. true - - - - Use epipolar geometry to compute the loop closure transform. "Maximum distance for visual word correspondences" is not used in this mode. - - - true - - - - - - - PnP: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses "Minimum visual word correspondences" and "Maximum iterations" above. - - - true - - - - + - - - - PnP reprojection error. - - - true - - - - - - - PnP flags. - - - true - - - - - - - - Iterative - - - - - EPNP - - - - - P3P - - - - - - - - pix - - - 1 - - - 0.100000000000000 - - - 1.000000000000000 - - - 8.000000000000000 - - - - + 1 @@ -6012,14 +5924,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - Maximum iterations to compute the transform from visual words. + Maximum RANSAC iterations. - + QComboBox::AdjustToContents @@ -6041,57 +5953,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - - - Epipolar geometry maximum variance to accept the loop closure. - - - true - - - - - - - m - - - 3 - - - 0.000000000000000 - - - 0.001000000000000 - - - 0.020000000000000 - - - - - - - Maximum distance for visual word correspondences. - - - - - - - - - - - + Force 2D transform (3DoF: x,y and yaw). @@ -6101,26 +5963,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 3 - - - 0.001000000000000 - - - 0.010000000000000 - - - 0.020000000000000 - - - - + When enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See "ICP" panel for parameters. @@ -6130,12 +5973,261 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + 3D to 3D + + + + + 3D to 2D (PnP) + + + + + 2D to 2D (Epipolar Geometry) + + + + + + + + Motion estimation approach using 3D and/or 2D visual words correspondences. + + + true + + + + + + + 0 + + + + + 0 + + + 0 + + + + + 3D to 3D + + + + + + m + + + 3 + + + 0.001000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + Maximum distance accepted between visual word correspondences. + + + true + + + + + + + 0 + + + 10000 + + + 1 + + + 10 + + + + + + + Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining. + + + true + + + + + + + + + + + + 0 + + + 0 + + + + + 3D to 2D (PnP) + + + + + + pix + + + 1 + + + 0.100000000000000 + + + 1.000000000000000 + + + 8.000000000000000 + + + + + + + Reprojection error. + + + true + + + + + + + + Iterative + + + + + EPNP + + + + + P3P + + + + + + + + Flags. + + + true + + + + + + + + + + + + 0 + + + 0 + + + + + 2D to 2D (Epipolar Geometry) + + + + + + Experimental! + + + true + + + + + + + + + m + + + 3 + + + 0.000000000000000 + + + 0.001000000000000 + + + 0.020000000000000 + + + + + + + Epipolar geometry maximum variance to accept the loop closure. + + + true + + + + + + + + + + + + - Re-extract features on global loop closure + Re-extract features true @@ -6147,27 +6239,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - By activating the re-extraction of the features, the features from the two images will be re-extracted using the settings below, and matched directly instead of using the big vocabulary. This adds an overhead processing time but it will mostly produce more corresponding words between the images, so better transformation computed. We recommend to use binary features for fast extraction and matching. - - - true - - - - - - - If not activated, when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word->Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure. - - - true - - - - - - - Epipolar geometry is ignored (if set above) by this option. + By activating the re-extraction of the features, the features from the two images will be re-extracted using the settings below, and matched directly instead of using the big vocabulary. This adds an overhead processing time but it will mostly produce more corresponding words between the images, so better transformations computed. We recommend to use binary features for fast extraction and matching. true @@ -6176,7 +6248,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 3 @@ -6211,7 +6283,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 1 @@ -6247,7 +6319,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector. @@ -6257,7 +6329,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + NNDR ratio @@ -6326,6 +6398,29 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + m + + + 0.000000000000000 + + + 5.000000000000000 + + + + + + + Max feature depth. + + + true + + + @@ -6987,7 +7082,68 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + + + + + + + Particle filtering to smooth the odometry trajectory. See "Particle Filter" panel for the related parameters. + + + true + + + + + + + + + + + + + + + + + + + + + Fill info with data (inliers/outliers features to be shown in Odometry view). + + + true + + + + + + + Data buffer size (0 means inf). + + + true + + + + + + + Odometry strategy. More info corresponding panels. + + + true + + + + Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. @@ -6997,6 +7153,30 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + + + + + + + + 999999 + + + + + + + If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). + + + true + + + @@ -7019,34 +7199,14 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - Feature detector. In BOW/Mono modes, the related descriptor is also used. In Optical flow mode, only the keypoint detector is used. - - - true + + + + 999999 - - - - Odometry strategy: - - - false - - - - - - - - - - - + Force 2D transform (3DoF: x,y and yaw). @@ -7056,350 +7216,264 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - Fill info with data (inliers/outliers features to be shown in Odometry view). - - - true - - - - + Test selected odometry - - - - 2-Optical flow estimate the location of 2D features from last frame to new frame, then computes RANSAC transformation with corresponding 3D features. - - - true - - - - - - - 1-BOW matches features extracted from both frames using nearest neighbor with descriptors, then computes RANSAC transformation estimation with corresponding 3D features. - - - true - - - - - - - Data buffer size (0 means inf). - - - true - - - - - - - 999999 - - - - - - - 3-Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. - - - true - - - - - - - - - - - - - - QComboBox::AdjustToContents - - - - SURF - - - - - SIFT - - - - - ORB - - - - - FAST+FREAK - - - - - FAST+BRIEF - - - - - GFTT+FREAK - - - - - GFTT+BRIEF - - - - - BRISK - - - - - - - - Particle filtering to smooth the odometry trajectory. See "Particle Filter" panel for the related parameters. - - - true - - - - - - - - - - - - - - 999999 - - - - - - - If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). - - - true - - - - - - - - - - + + + + + + + true + + + - Transformation estimation (RANSAC) + Motion estimation - - - - - 8 - - - 1000 - - - 10 - - + + + + + + + + 3D to 3D + + + + + 3D to 2D (PnP) + + + + + + + + Motion estimation approach using 3D and/or 2D visual words correspondences. + + + true + + + + + + + 8 + + + 1000 + + + 10 + + + + + + + Minimum feature correspondences to accept the estimated transformation. + + + true + + + + + + + 1 + + + 10000 + + + 1 + + + 100 + + + + + + + Maximum iterations to compute the transform from 3D features. + + + true + + + + - - - - Minimum feature correspondences to compute geometry transform. - - - true - - - - - - - m - - - 3 - - - 0.001000000000000 - - - 0.010000000000000 - - - 0.005000000000000 - - - - - - - Maximum distance for 3D feature correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost). - - - true - - - - - - - 1 - - - 10000 - - - 1 - - - 100 - - - - - - - Maximum iterations to compute the transform from 3D features. - - - true - - - - - - + + + 0 - - 10000 - - - 1 - - - 10 - - - - - - - Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining. - - - true - - - - - - - PnP reprojection error. - - - true - - - - - - - pix - - - 1 - - - 0.100000000000000 - - - 1.000000000000000 - - - 8.000000000000000 - - - - - - - - - - - - - - PnP: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses "Minimum feature correspondences" and "Maximum iterations" above. - - - true - - - - - - - PnP flags. - - - true - - - - - - - - Iterative - - - - - EPNP - - - - - P3P - - + + + + 0 + + + 0 + + + + + 3D to 3D + + + + + + m + + + 3 + + + 0.001000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + Maximum distance for 3D feature correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost). + + + true + + + + + + + 0 + + + 10000 + + + 1 + + + 10 + + + + + + + Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining. + + + true + + + + + + + + + + + + 0 + + + 0 + + + + + 3D to 2D (PnP) + + + + + + pix + + + 1 + + + 0.100000000000000 + + + 1.000000000000000 + + + 8.000000000000000 + + + + + + + Reprojection error. + + + true + + + + + + + + Iterative + + + + + EPNP + + + + + P3P + + + + + + + + Flags. + + + true + + + + + + + + @@ -7408,17 +7482,17 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - Features filtering + Features - + 999999 - + ROI ratios [left, right, top, bottom] between 0 and 1. @@ -7428,7 +7502,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 0.0 0.0 0.0 0.0 @@ -7438,7 +7512,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Max features extracted from the images (0 means inf). @@ -7448,7 +7522,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Maximum feature depth. @@ -7458,7 +7532,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + m @@ -7480,6 +7554,63 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + Feature detector. In BOW/Mono modes, the related descriptor is also used. In Optical flow mode, only the keypoint detector is used. + + + true + + + + + + + QComboBox::AdjustToContents + + + + SURF + + + + + SIFT + + + + + ORB + + + + + FAST+FREAK + + + + + FAST+BRIEF + + + + + GFTT+FREAK + + + + + GFTT+BRIEF + + + + + BRISK + + + + @@ -7606,131 +7737,145 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + BOW - - - - - 0 - - - 999999 - - - 1 - - - 0 - - - - - + + + - Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving. + Features extracted from frames are matached a using nearest neighbor approach. It maintains a local map of features to match to. true - - - - QComboBox::AdjustToContents - - - - FLANN Linear - + + + + + + 0 + + + 999999 + + + 1 + + + 0 + + - - - FLANN KdTree - + + + + Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving. + + + true + + - - - FLANN LSH - + + + + QComboBox::AdjustToContents + + + + FLANN Linear + + + + + FLANN KdTree + + + + + FLANN LSH + + + + + Brute Force + + + + + Brute Force GPU + + + - - - Brute Force - + + + + Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector. + + + true + + - - - Brute Force GPU - + + + + 1 + + + 0.100000000000000 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.700000000000000 + + - - - - - - Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector. - - - true - - - - - - - 1 - - - 0.100000000000000 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.700000000000000 - - - - - - - NNDR ratio + + + + NNDR ratio (A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor) Lower the ratio -> higher the precision. 0 means disabled, matching the nearest. - - - true - - - - - - - - - - Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated. - - - true - - - - - - - ... - - + + + true + + + + + + + + + + ... + + + + + + + Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated. + + + true + + + + @@ -7761,12 +7906,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - The process is as follow: - - Features from the last frame are estimated in the new frame using an optical flow approach (see cv::calcOpticalFlowPyrLK()). - - 3D features from the new frame are extracted from the estimated positions. - - Using RANSAC, a transformation is estimated between corresponding 3D features. - - New features are extracted from the new frame to be used for the next time. - - Optionally, the 2D position of the features can be refined for sub pixel precision (see cv::cornerSubPix()). + Features from the last frame are estimated in the new frame using an optical flow approach (see cv::calcOpticalFlowPyrLK()). true @@ -7915,6 +8055,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare Mono + + + + Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed. + + + true + + + diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index bb2c6988..1381d764 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -122,7 +122,7 @@ int main (int argc, char * argv[]) float sec = 0.0f; bool gpu = false; int localHistory = rtabmap::Parameters::defaultOdomBowLocalHistorySize(); - bool p2p = rtabmap::Parameters::defaultOdomPnPEstimation(); + bool p2p = false; for(int i=1; i Date: Sun, 28 Jun 2015 12:51:40 -0400 Subject: [PATCH 24/45] fixed camera flickers when moving the camera over Z-axis --- guilib/include/rtabmap/gui/CloudViewer.h | 4 +- guilib/src/CloudViewer.cpp | 48 +++++++++++++++++++----- 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 7922e088..34379fc9 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -201,13 +201,13 @@ protected: virtual void keyPressEvent(QKeyEvent * event); virtual void mousePressEvent(QMouseEvent * event); virtual void mouseMoveEvent(QMouseEvent * event); + virtual void wheelEvent(QWheelEvent * event); virtual void contextMenuEvent(QContextMenuEvent * event); virtual void handleAction(QAction * event); QMenu * menu() {return _menu;} private: void createMenu(); - void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void); void addGrid(); void removeGrid(); @@ -230,6 +230,8 @@ private: unsigned int _maxTrajectorySize; unsigned int _gridCellCount; float _gridCellSize; + cv::Vec3d _lastCameraOrientation; + cv::Vec3d _lastCameraPose; QMap _addedClouds; // include cloud, scan, meshes Transform _lastPose; std::list _gridLines; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 09734748..82aa4072 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -47,15 +48,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -void CloudViewer::mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void) -{ - if (event.getButton () == pcl::visualization::MouseEvent::LeftButton || - event.getButton () == pcl::visualization::MouseEvent::MiddleButton) - { - this->update(); // this will apply frustum - } -} - CloudViewer::CloudViewer(QWidget *parent) : QVTKWidget(parent), _visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)), @@ -75,6 +67,8 @@ CloudViewer::CloudViewer(QWidget *parent) : _maxTrajectorySize(100), _gridCellCount(50), _gridCellSize(1), + _lastCameraOrientation(0,0,0), + _lastCameraPose(0,0,0), _workingDirectory("."), _defaultBgColor(Qt::black), _currentBgColor(Qt::black) @@ -88,7 +82,6 @@ CloudViewer::CloudViewer(QWidget *parent) : //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle()); - _visualizer->registerMouseCallback (&CloudViewer::mouseEventOccurred, *this, (void*)_visualizer); _visualizer->setCameraPosition( -1, 0, 0, 0, 0, 0, @@ -681,6 +674,7 @@ void CloudViewer::setCameraPosition( float focalX, float focalY, float focalZ, float upX, float upY, float upZ) { + _lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0); _visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ); } @@ -891,6 +885,7 @@ void CloudViewer::setCameraFree() void CloudViewer::setCameraLockZ(bool enabled) { + _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0); _aLockViewZ->setChecked(enabled); } @@ -1168,12 +1163,31 @@ void CloudViewer::mousePressEvent(QMouseEvent * event) void CloudViewer::mouseMoveEvent(QMouseEvent * event) { QVTKWidget::mouseMoveEvent(event); + // camera view up z locked? if(_aLockViewZ->isChecked()) { std::vector cameras; _visualizer->getCameras(cameras); + cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal)); + double norm = cv::norm(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal)); + + if( _lastCameraOrientation!=cv::Vec3d(0,0,0) && + _lastCameraPose!=cv::Vec3d(0,0,0) && + ( (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) && + uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1]) ) || + (norm && fabs(cameras.front().pos[2]-cameras.front().focal[2])/norm > 0.9999))) + { + cameras.front().pos[0] = _lastCameraPose[0]; + cameras.front().pos[1] = _lastCameraPose[1]; + cameras.front().pos[2] = _lastCameraPose[2]; + } + else if(newCameraOrientation != cv::Vec3d(0,0,0)) + { + _lastCameraOrientation = newCameraOrientation; + _lastCameraPose = cv::Vec3d(cameras.front().pos); + } cameras.front().view[0] = 0; cameras.front().view[1] = 0; cameras.front().view[2] = 1; @@ -1184,6 +1198,19 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event) cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]); } + + emit configChanged(); +} + +void CloudViewer::wheelEvent(QWheelEvent * event) +{ + QVTKWidget::wheelEvent(event); + if(_aLockViewZ->isChecked()) + { + std::vector cameras; + _visualizer->getCameras(cameras); + _lastCameraPose = cv::Vec3d(cameras.front().pos); + } emit configChanged(); } @@ -1214,6 +1241,7 @@ void CloudViewer::handleAction(QAction * a) } else if(a == _aResetCamera) { + _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0); if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull()) { // reset relative to last current pose From 4f96fd3530c861ddd15422f00d7368864bb69b52 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 28 Jun 2015 19:22:18 -0400 Subject: [PATCH 25/45] Version 0.10.1: user_data is now a cv::Mat to avoid a deep copy when SensorData is copied --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/DBDriver.h | 9 +- corelib/include/rtabmap/core/Memory.h | 3 +- corelib/include/rtabmap/core/Rtabmap.h | 2 +- corelib/include/rtabmap/core/RtabmapThread.h | 2 +- corelib/include/rtabmap/core/SensorData.h | 32 +- corelib/include/rtabmap/core/Signature.h | 5 - corelib/include/rtabmap/core/UserDataEvent.h | 6 +- corelib/src/DBDriver.cpp | 16 +- corelib/src/DBDriverSqlite3.cpp | 670 ++++++------------- corelib/src/DBDriverSqlite3.h | 5 +- corelib/src/DBReader.cpp | 21 +- corelib/src/Memory.cpp | 69 +- corelib/src/Rtabmap.cpp | 19 +- corelib/src/RtabmapThread.cpp | 6 +- corelib/src/SensorData.cpp | 179 ++++- corelib/src/Signature.cpp | 14 - corelib/src/resources/DatabaseSchema.sql.in | 2 +- examples/WifiMapping/MapBuilderWifi.h | 63 +- examples/WifiMapping/WifiThread.h | 8 +- examples/WifiMapping/main.cpp | 7 +- guilib/src/DatabaseViewer.cpp | 16 +- 22 files changed, 516 insertions(+), 640 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a3ee7721..f68fe0d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 10) -SET(RTABMAP_PATCH_VERSION 0) +SET(RTABMAP_PATCH_VERSION 1) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 23b255d6..988d630c 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -95,9 +95,9 @@ public: void loadWords(const std::set & wordIds, std::list & vws); // Specific queries... - void loadNodeData(std::list & signatures, bool loadMetricData) const; + void loadNodeData(std::list & signatures) const; void getNodeData(int signatureId, SensorData & data) const; - bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const; + bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) 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; @@ -134,9 +134,8 @@ private: virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) 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, SensorData & data) const = 0; - virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const = 0; + virtual void loadNodeDataQuery(std::list & signatures) const = 0; + virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const = 0; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const = 0; virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0; diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 5bb8eb98..be98c74f 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -127,7 +127,7 @@ public: int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const; bool labelSignature(int id, const std::string & label); std::map getAllLabels() const; - bool setUserData(int id, const std::vector & data); + bool setUserData(int id, const cv::Mat & data); int getDatabaseMemoryUsed() const; // in bytes double getDbSavingTime() const; Transform getOdomPose(int signatureId, bool lookInDatabase = false) const; @@ -137,7 +137,6 @@ public: int & weight, std::string & label, double & stamp, - std::vector & userData, bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; SensorData getNodeData(int nodeId, bool uncompressedData = false); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index ff89222e..98798eed 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -106,7 +106,7 @@ public: int triggerNewMap(); bool labelLocation(int id, const std::string & label); - bool setUserData(int id, const std::vector & data); + bool setUserData(int id, const cv::Mat & data); void generateDOTGraph(const std::string & path, int id=0, int margin=5); void generateTOROGraph(const std::string & path, bool optimized, bool global); void exportPoses(const std::string & path, bool optimized, bool global); diff --git a/corelib/include/rtabmap/core/RtabmapThread.h b/corelib/include/rtabmap/core/RtabmapThread.h index cc364091..3c84c187 100644 --- a/corelib/include/rtabmap/core/RtabmapThread.h +++ b/corelib/include/rtabmap/core/RtabmapThread.h @@ -120,7 +120,7 @@ private: double _rotVariance; double _transVariance; - std::vector _userData; + cv::Mat _userData; UMutex _userDataMutex; }; diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index 0d3bd123..e1fa028a 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -52,7 +52,7 @@ public: const cv::Mat & image, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // Mono constructor SensorData( @@ -60,7 +60,7 @@ public: const CameraModel & cameraModel, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // RGB-D constructor SensorData( @@ -69,7 +69,7 @@ public: const CameraModel & cameraModel, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // RGB-D constructor + 2d laser scan SensorData( @@ -80,7 +80,7 @@ public: const CameraModel & cameraModel, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // Multi-cameras RGB-D constructor SensorData( @@ -89,7 +89,7 @@ public: const std::vector & cameraModels, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // Multi-cameras RGB-D constructor + 2d laser scan SensorData( @@ -100,7 +100,7 @@ public: const std::vector & cameraModels, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // Stereo constructor SensorData( @@ -109,7 +109,7 @@ public: const StereoCameraModel & cameraModel, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); // Stereo constructor + 2d laser scan SensorData( @@ -120,7 +120,7 @@ public: const StereoCameraModel & cameraModel, int id = 0, double stamp = 0.0, - const std::vector & userData = std::vector()); + const cv::Mat & userData = cv::Mat()); virtual ~SensorData() {} @@ -136,7 +136,8 @@ public: _laserScanCompressed.empty() && _cameraModels.size() == 0 && !_stereoCameraModel.isValid() && - _userData.size() == 0 && + !_userDataRaw.empty() && + !_userDataCompressed.empty() && _keypoints.size() == 0 && _descriptors.empty()); } @@ -166,14 +167,16 @@ public: cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();} void uncompressData(); - void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw); - void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const; + void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0); + void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const; const std::vector & cameraModels() const {return _cameraModels;} const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;} - void setUserData(const std::vector & data) {_userData = data;} - const std::vector & userData() const {return _userData;} + void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw + void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too. + const cv::Mat & userDataRaw() const {return _userDataRaw;} + const cv::Mat & userDataCompressed() const {return _userDataCompressed;} void setFeatures(const std::vector & keypoints, const cv::Mat & descriptors) { @@ -200,7 +203,8 @@ private: StereoCameraModel _stereoCameraModel; // user data - std::vector _userData; + cv::Mat _userDataCompressed; // compressed data + cv::Mat _userDataRaw; // features std::vector _keypoints; diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index e7fa84ad..a9b6357d 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -58,7 +58,6 @@ public: double stamp = 0.0, const std::string & label = std::string(), const Transform & pose = Transform(), - const std::vector & userData = std::vector(), const SensorData & sensorData = SensorData()); virtual ~Signature(); @@ -77,9 +76,6 @@ public: void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;} const std::string & getLabel() const {return _label;} - void setUserData(const std::vector & data); - const std::vector & getUserData() const {return _userData;} - double getStamp() const {return _stamp;} void addLinks(const std::list & links); @@ -130,7 +126,6 @@ private: std::map _links; // id, transform int _weight; std::string _label; - std::vector _userData; bool _saved; // If it's saved to bd bool _modified; bool _linksModified; // Optimization when updating signatures in database diff --git a/corelib/include/rtabmap/core/UserDataEvent.h b/corelib/include/rtabmap/core/UserDataEvent.h index beb08882..a62502b2 100644 --- a/corelib/include/rtabmap/core/UserDataEvent.h +++ b/corelib/include/rtabmap/core/UserDataEvent.h @@ -40,17 +40,17 @@ namespace rtabmap class UserDataEvent : public UEvent { public: - UserDataEvent(const std::vector & data) : + UserDataEvent(const cv::Mat & data) : UEvent(0), data_(data) {} ~UserDataEvent() {} virtual std::string getClassName() const {return "UserDataEvent";} - const std::vector & data() const {return data_;} + const cv::Mat & data() const {return data_;} private: - std::vector data_; + cv::Mat data_; }; } diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index b4f6dba5..dba269a5 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -390,7 +390,7 @@ void DBDriver::loadWords(const std::set & wordIds, std::list } } -void DBDriver::loadNodeData(std::list & signatures, bool loadMetricData) const +void DBDriver::loadNodeData(std::list & signatures) const { // Don't look in the trash, we assume that if we want to load // data of a signature, it is not in thrash! Print an error if so. @@ -406,7 +406,7 @@ void DBDriver::loadNodeData(std::list & signatures, bool loadMetric _trashesMutex.unlock(); _dbSafeAccessMutex.lock(); - this->loadNodeDataQuery(signatures, loadMetricData); + this->loadNodeDataQuery(signatures); _dbSafeAccessMutex.unlock(); } @@ -431,7 +431,11 @@ void DBDriver::getNodeData( if(!found) { _dbSafeAccessMutex.lock(); - this->getNodeDataQuery(signatureId, data); + std::list signatures; + Signature tmp(signatureId); + signatures.push_back(&tmp); + loadNodeDataQuery(signatures); + data = signatures.front()->sensorData(); _dbSafeAccessMutex.unlock(); } } @@ -441,8 +445,7 @@ bool DBDriver::getNodeInfo(int signatureId, int & mapId, int & weight, std::string & label, - double & stamp, - std::vector & userData) const + double & stamp) const { bool found = false; // look in the trash @@ -454,7 +457,6 @@ bool DBDriver::getNodeInfo(int signatureId, weight = _trashSignatures.at(signatureId)->getWeight(); label = _trashSignatures.at(signatureId)->getLabel(); stamp = _trashSignatures.at(signatureId)->getStamp(); - userData = _trashSignatures.at(signatureId)->getUserData(); found = true; } _trashesMutex.unlock(); @@ -462,7 +464,7 @@ bool DBDriver::getNodeInfo(int signatureId, if(!found) { _dbSafeAccessMutex.lock(); - found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData); + found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp); _dbSafeAccessMutex.unlock(); } return found; diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index a2ea085d..86f56f84 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "VisualWord.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/util3d.h" +#include "rtabmap/core/Compression.h" #include "DatabaseSchema_sql.h" #include @@ -445,9 +446,9 @@ long DBDriverSqlite3::getMemoryUsedQuery() const } } -void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, bool loadMetricData) const +void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) const { - UDEBUG("load data (metric=%s) for %d signatures", loadMetricData?"true":"false", (int)signatures.size()); + UDEBUG("load data for %d signatures", (int)signatures.size()); if(_ppDb) { UTimer timer; @@ -456,62 +457,65 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo sqlite3_stmt * ppStmt = 0; std::stringstream query; - if(loadMetricData) + if(uStrNumCmp(_version, "0.10.1") >= 0) { - if(uStrNumCmp(_version, "0.10.0") >= 0) - { - query << "SELECT image, depth, calibration, scan_max_pts, scan " - << "FROM Data " - << "WHERE id = ?" - <<";"; - } - else if(uStrNumCmp(_version, "0.8.11") >= 0) - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = ?" - <<";"; - } - else if(uStrNumCmp(_version, "0.7.0") >= 0) - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = ?" - <<";"; - } - else - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = ?" - <<";"; - } + query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data " + << "FROM Data " + << "WHERE id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.10.0") >= 0) + { + query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data " + << "FROM Data " + << "INNER JOIN Node " + << "ON Data.id = Node.id " + << "WHERE Data.id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.8.11") >= 0) + { + query << "SELECT Image.data, " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data " + << "FROM Image " + << "INNER JOIN Node " + << "on Image.id = Node.id " + << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data + << "ON Image.id = Depth.id " + << "WHERE Image.id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.8.8") >= 0) + { + query << "SELECT Image.data, " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data " + << "FROM Image " + << "INNER JOIN Node " + << "on Image.id = Node.id " + << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data + << "ON Image.id = Depth.id " + << "WHERE Image.id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.7.0") >= 0) + { + query << "SELECT Image.data, " + "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d " + << "FROM Image " + << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data + << "ON Image.id = Depth.id " + << "WHERE Image.id = ?" + <<";"; } else { - if(uStrNumCmp(_version, "0.10.0") >= 0) - { - query << "SELECT image " - << "FROM Data " - << "WHERE id = ?" - <<";"; - } - else - { - query << "SELECT data " - << "FROM Image " - << "WHERE id = ?" - <<";"; - } + query << "SELECT Image.data, " + "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d " + << "FROM Image " + << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data + << "ON Image.id = Depth.id " + << "WHERE Image.id = ?" + <<";"; } rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); @@ -542,6 +546,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo StereoCameraModel stereoModel; Transform localTransform = Transform::getIdentity(); cv::Mat scanCompressed; + cv::Mat userDataCompressed; data = sqlite3_column_blob(ppStmt, index); dataSize = sqlite3_column_bytes(ppStmt, index++); @@ -552,133 +557,152 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); } - if(loadMetricData) + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + + //Create the depth image + if(dataSize>4 && data) + { + depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); + } + + if(uStrNumCmp(_version, "0.10.0") < 0) + { + data = sqlite3_column_blob(ppStmt, index); // local transform + dataSize = sqlite3_column_bytes(ppStmt, index++); + if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) + { + memcpy(localTransform.data(), data, dataSize); + } + } + + // calibration + if(uStrNumCmp(_version, "0.10.0") >= 0) { data = sqlite3_column_blob(ppStmt, index); dataSize = sqlite3_column_bytes(ppStmt, index++); - - //Create the depth image - if(dataSize>4 && data) + // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(dataSize > 0 && data) { - depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); - } - - if(uStrNumCmp(_version, "0.10.0") < 0) - { - data = sqlite3_column_blob(ppStmt, index); // local transform - dataSize = sqlite3_column_bytes(ppStmt, index++); - if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) + float * dataFloat = (float*)data; + if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) { - memcpy(localTransform.data(), data, dataSize); - } - } - - // calibration - if(uStrNumCmp(_version, "0.10.0") >= 0) - { - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); - // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras - // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float - if(dataSize > 0 && data) - { - float * dataFloat = (float*)data; - if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) + int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); + UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); + int max = cameraCount*(4+localTransform.size()); + for(int i=0; i= 0) - { - double fx = sqlite3_column_double(ppStmt, index++); - double fyOrBaseline = sqlite3_column_double(ppStmt, index++); - double cx = sqlite3_column_double(ppStmt, index++); - double cy = sqlite3_column_double(ppStmt, index++); - if(fyOrBaseline < 1.0) + else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float)) { - //it is a baseline - stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); + UDEBUG("Loading calibration of a stereo camera"); + memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float)); + stereoModel = StereoCameraModel( + dataFloat[0], // fx + dataFloat[1], // fy + dataFloat[2], // cx + dataFloat[3], // cy + dataFloat[4], // baseline + localTransform); } else { - models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); + UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize); } } + + } + else if(uStrNumCmp(_version, "0.7.0") >= 0) + { + double fx = sqlite3_column_double(ppStmt, index++); + double fyOrBaseline = sqlite3_column_double(ppStmt, index++); + double cx = sqlite3_column_double(ppStmt, index++); + double cy = sqlite3_column_double(ppStmt, index++); + if(fyOrBaseline < 1.0) + { + //it is a baseline + stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); + } else { - float depthConstant = sqlite3_column_double(ppStmt, index++); - float fx = 1.0f/depthConstant; - float fy = 1.0f/depthConstant; - float cx = 0.0f; - float cy = 0.0f; - models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); + models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); } + } + else + { + float depthConstant = sqlite3_column_double(ppStmt, index++); + float fx = 1.0f/depthConstant; + float fy = 1.0f/depthConstant; + float cx = 0.0f; + float cy = 0.0f; + models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); + } - int laserScanMaxPts = 0; - if(uStrNumCmp(_version, "0.8.11") >= 0) - { - laserScanMaxPts = sqlite3_column_int(ppStmt, index++); - } + int laserScanMaxPts = 0; + if(uStrNumCmp(_version, "0.8.11") >= 0) + { + laserScanMaxPts = sqlite3_column_int(ppStmt, index++); + } + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + //Create the laserScan + if(dataSize>4 && data) + { + scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d + } + + if(uStrNumCmp(_version, "0.8.8") >= 0) + { data = sqlite3_column_blob(ppStmt, index); dataSize = sqlite3_column_bytes(ppStmt, index++); - //Create the laserScan + //Create the userData if(dataSize>4 && data) { - scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d - } - - if(models.size()) - { - (*iter)->sensorData() = SensorData( - scanCompressed, - laserScanMaxPts, - imageCompressed, - depthOrRightCompressed, - models, - (*iter)->id()); - } - else - { - (*iter)->sensorData() = SensorData( - scanCompressed, - laserScanMaxPts, - imageCompressed, - depthOrRightCompressed, - stereoModel, - (*iter)->id()); + if(uStrNumCmp(_version, "0.10.1") >= 0) + { + userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData + } + else + { + // compress data (set uncompressed data to signed to make difference with compressed type) + userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data)); + } } + } + if(models.size()) + { + (*iter)->sensorData() = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + models, + (*iter)->id(), + 0, + userDataCompressed); + } + else + { + (*iter)->sensorData() = SensorData( + scanCompressed, + laserScanMaxPts, + imageCompressed, + depthOrRightCompressed, + stereoModel, + (*iter)->id(), + 0, + userDataCompressed); } rc = sqlite3_step(ppStmt); // next result... @@ -697,232 +721,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures, boo } } -void DBDriverSqlite3::getNodeDataQuery( - int signatureId, - SensorData & sensorData) const -{ - if(_ppDb) - { - UTimer timer; - timer.start(); - int rc = SQLITE_OK; - sqlite3_stmt * ppStmt = 0; - std::stringstream query; - - if(uStrNumCmp(_version, "0.10.0") >= 0) - { - query << "SELECT image, depth, calibration, scan_max_pts, scan " - << "FROM Data " - << "WHERE id = " << signatureId - <<";"; - } - else if(uStrNumCmp(_version, "0.8.11") >= 0) - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = " << signatureId - <<";"; - } - else if(uStrNumCmp(_version, "0.7.0") >= 0) - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = " << signatureId - <<";"; - } - else - { - query << "SELECT Image.data, " - "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d " - << "FROM Image " - << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data - << "ON Image.id = Depth.id " - << "WHERE Image.id = " << signatureId - <<";"; - } - - 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()); - - const void * data = 0; - int dataSize = 0; - int index = 0; - - cv::Mat imageCompressed; - cv::Mat depthOrRightCompressed; - std::vector models; - StereoCameraModel stereoModel; - Transform localTransform = Transform::getIdentity(); - int laserScanMaxPts; - cv::Mat scanCompressed; - - ULOGGER_DEBUG("Loading data for %d...", signatureId); - - // Process the result if one - rc = sqlite3_step(ppStmt); - if(rc == SQLITE_ROW) - { - index = 0; - - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); - - //Create the image - if(dataSize>4 && data) - { - imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); - } - - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); - - //Create the depth image - if(dataSize>4 && data) - { - depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); - } - - if(uStrNumCmp(_version, "0.10.0") < 0) - { - data = sqlite3_column_blob(ppStmt, index); // local transform - dataSize = sqlite3_column_bytes(ppStmt, index++); - if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data) - { - memcpy(localTransform.data(), data, dataSize); - } - } - - // calibration - if(uStrNumCmp(_version, "0.10.0") >= 0) - { - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); - // multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras - // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float - if(dataSize > 0 && data) - { - float * dataFloat = (float*)data; - if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) - { - int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); - UDEBUG("Loading calibration for %d cameras", cameraCount); - int max = cameraCount*(4+localTransform.size()); - for(int i=0; i= 0) - { - double fx = sqlite3_column_double(ppStmt, index++); - double fyOrBaseline = sqlite3_column_double(ppStmt, index++); - double cx = sqlite3_column_double(ppStmt, index++); - double cy = sqlite3_column_double(ppStmt, index++); - if(fyOrBaseline < 1.0) - { - //it is a baseline - stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); - } - else - { - models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); - } - } - else - { - float depthConstant = sqlite3_column_double(ppStmt, index++); - float fx = 1.0f/depthConstant; - float fy = 1.0f/depthConstant; - float cx = 0.0f; - float cy = 0.0f; - models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); - } - - laserScanMaxPts = 0; - if(uStrNumCmp(_version, "0.8.11") >= 0) - { - laserScanMaxPts = sqlite3_column_int(ppStmt, index++); - } - - data = sqlite3_column_blob(ppStmt, index); // depth2d - dataSize = sqlite3_column_bytes(ppStmt, index++); - //Create the depth2d - if(dataSize>4 && data) - { - scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); - } - - if(models.size()) - { - sensorData = SensorData( - scanCompressed, - laserScanMaxPts, - imageCompressed, - depthOrRightCompressed, - models, - signatureId); - } - else - { - sensorData = SensorData( - scanCompressed, - laserScanMaxPts, - imageCompressed, - depthOrRightCompressed, - stereoModel, - signatureId); - } - - rc = sqlite3_step(ppStmt); // next result... - } - 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()); - ULOGGER_DEBUG("Time=%fs", timer.ticks()); - } -} - bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, - double & stamp, - std::vector & userData) const + double & stamp) const { bool found = false; if(_ppDb && signatureId) @@ -931,15 +735,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, sqlite3_stmt * ppStmt = 0; std::stringstream query; - // Prepare the query... Get the map from signature and visual words - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - query << "SELECT pose, map_id, weight, label, stamp, user_data " - "FROM Node " - "WHERE id = " << signatureId << - ";"; - } - else if(uStrNumCmp(_version, "0.8.5") >= 0) + if(uStrNumCmp(_version, "0.8.5") >= 0) { query << "SELECT pose, map_id, weight, label, stamp " "FROM Node " @@ -986,18 +782,6 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, stamp = sqlite3_column_double(ppStmt, index++); // stamp } - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data - - if(dataSize && data) - { - userData.resize(dataSize); - memcpy(userData.data(), data, dataSize); - } - } - rc = sqlite3_step(ppStmt); // next result... } UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); @@ -1347,13 +1131,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< unsigned int loaded = 0; // Load nodes information - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - query << "SELECT id, map_id, weight, pose, stamp, label, user_data " - << "FROM Node " - << "WHERE id=?;"; - } - else if(uStrNumCmp(_version, "0.8.5") >= 0) + if(uStrNumCmp(_version, "0.8.5") >= 0) { query << "SELECT id, map_id, weight, pose, stamp, label " << "FROM Node " @@ -1384,7 +1162,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< const void * data = 0; int dataSize = 0; std::string label; - std::vector userData; // Process the result if one rc = sqlite3_step(ppStmt); @@ -1412,18 +1189,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< } } - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - data = sqlite3_column_blob(ppStmt, index); - dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data - - if(dataSize && data) - { - userData.resize(dataSize); - memcpy(userData.data(), data, dataSize); - } - } - rc = sqlite3_step(ppStmt); } UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); @@ -1438,8 +1203,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< weight, stamp, label, - pose, - userData); + pose); s->setSaved(true); nodes.push_back(s); ++loaded; @@ -1996,18 +1760,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd Signature * s = 0; std::string query; - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - if(updateTimestamp) - { - query = "UPDATE Node SET weight=?, label=?, user_data=?, time_enter = DATETIME('NOW') WHERE id=?;"; - } - else - { - query = "UPDATE Node SET weight=?, label=?, user_data=? WHERE id=?;"; - } - } - else if(uStrNumCmp(_version, "0.8.5") >= 0) + if(uStrNumCmp(_version, "0.8.5") >= 0) { if(updateTimestamp) { @@ -2055,20 +1808,6 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd } } - if(uStrNumCmp(_version, "0.8.8") >= 0) - { - if(s->getUserData().empty()) - { - rc = sqlite3_bind_null(ppStmt, index++); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - } - else - { - rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - } - } - rc = sqlite3_bind_int(ppStmt, index++, s->id()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); @@ -2392,7 +2131,11 @@ void DBDriverSqlite3::saveQuery(const std::list & words) const std::string DBDriverSqlite3::queryStepNode() const { - if(uStrNumCmp(_version, "0.8.8") >= 0) + if(uStrNumCmp(_version, "0.10.1") >= 0) + { + return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);"; + } + else if(uStrNumCmp(_version, "0.8.8") >= 0) { return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);"; } @@ -2438,16 +2181,20 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const } } - if(uStrNumCmp(_version, "0.8.8") >= 0) + if(uStrNumCmp(_version, "0.10.1") >= 0) { - if(s->getUserData().empty()) + // ignore user_data + } + else if(uStrNumCmp(_version, "0.8.8") >= 0) + { + if(s->sensorData().userDataCompressed().empty()) { rc = sqlite3_bind_null(ppStmt, index++); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } else { - rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC); + rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } } @@ -2613,7 +2360,14 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor std::string DBDriverSqlite3::queryStepSensorData() const { UASSERT(uStrNumCmp(_version, "0.10.0") >= 0); - return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);"; + if(uStrNumCmp(_version, "0.10.1") >= 0) + { + return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);"; + } + else + { + return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);"; + } } void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const @@ -2712,6 +2466,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, } UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + if(uStrNumCmp(_version, "0.10.1") >= 0) + { + // user_data + if(!sensorData.userDataCompressed().empty()) + { + rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC); + } + else + { + rc = sqlite3_bind_zeroblob(ppStmt, index++, 4); + } + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + } + //step rc=sqlite3_step(ppStmt); UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index 2cb99426..d5811731 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -70,9 +70,8 @@ private: virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) 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, SensorData & data) const; - virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector & userData) const; + virtual void loadNodeDataQuery(std::list & signatures) const; + virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren) const; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const; virtual void getLastIdQuery(const std::string & tableName, int & id) const; diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 48f96eeb..e28b7839 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -156,17 +156,20 @@ void DBReader::mainLoop() int goalId = 0; double previousStamp = odom.data().stamp(); odom.data().setStamp(UTimer::now()); - if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0) + if(odom.data().userDataRaw().type() == CV_8SC1 && + odom.data().userDataRaw().cols >= 7 && // including null str ending + odom.data().userDataRaw().rows == 1 && + memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event - std::string goalStr = uBytes2Str(odom.data().userData()); + std::string goalStr = (const char *)odom.data().userDataRaw().data; if(!goalStr.empty()) { std::list strs = uSplit(goalStr, ':'); if(strs.size() == 2) { goalId = atoi(strs.rbegin()->c_str()); - odom.data().setUserData(std::vector()); + odom.data().setUserData(cv::Mat()); } } } @@ -196,8 +199,7 @@ void DBReader::mainLoop() double stamp; int mapId; Transform localTransform, pose; - std::vector userData; - _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData); + _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp); if(previousStamp && stamp && stamp > previousStamp) { double delay = stamp - previousStamp; @@ -252,7 +254,6 @@ OdometryEvent DBReader::getNextData() if(!this->isKilled() && _currentId != _ids.end()) { int mapId; - std::vector userData; SensorData data; _dbDriver->getNodeData(*_currentId, data); @@ -261,7 +262,7 @@ OdometryEvent DBReader::getNextData() int weight; std::string label; double stamp; - _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData); + _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp); cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); if(!_odometryIgnored) @@ -338,11 +339,11 @@ OdometryEvent DBReader::getNextData() data.uncompressData(); data.setId(seq); data.setStamp(stamp); - data.setUserData(userData); - UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d", + UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d", data.laserScanRaw().empty()?0:1, data.imageRaw().empty()?0:1, - data.depthOrRightRaw().empty()?0:1); + data.depthOrRightRaw().empty()?0:1, + data.userDataRaw().empty()?0:1); odom = OdometryEvent(data, pose, infMatrix.inv()); } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 732d3fe1..89af3562 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1875,30 +1875,17 @@ std::map Memory::getAllLabels() const return labels; } -bool Memory::setUserData(int id, const std::vector & data) +bool Memory::setUserData(int id, const cv::Mat & data) { Signature * s = this->_getSignature(id); if(s) { - s->setUserData(data); + s->sensorData().setUserData(data); return true; } - else if(_dbDriver) - { - std::list ids; - ids.push_back(id); - std::list signatures; - _dbDriver->loadSignatures(ids,signatures); - if(signatures.size()) - { - signatures.front()->setUserData(data); - _dbDriver->asyncSave(signatures.front()); // move it again to trash - return true; - } - } else { - UERROR("Node %d not found, failed to set user data (size=%d)!", id, data.size()); + UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total()); } return false; } @@ -2257,7 +2244,7 @@ Transform Memory::computeIcpTransform( } if(depthToLoad.size()) { - _dbDriver->loadNodeData(depthToLoad, true); + _dbDriver->loadNodeData(depthToLoad); } } @@ -2614,7 +2601,7 @@ Transform Memory::computeScanMatchingTransform( } if(depthToLoad.size() && _dbDriver) { - _dbDriver->loadNodeData(depthToLoad, true); + _dbDriver->loadNodeData(depthToLoad); } std::string msg; @@ -3203,8 +3190,7 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const int mapId, weight; std::string label; double stamp; - std::vector userData; - getNodeInfo(signatureId, pose, mapId, weight, label, stamp, userData, lookInDatabase); + getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase); return pose; } @@ -3214,7 +3200,6 @@ bool Memory::getNodeInfo(int signatureId, int & weight, std::string & label, double & stamp, - std::vector & userData, bool lookInDatabase) const { const Signature * s = this->getSignature(signatureId); @@ -3225,12 +3210,11 @@ bool Memory::getNodeInfo(int signatureId, weight = s->getWeight(); label = s->getLabel(); stamp = s->getStamp(); - userData = s->getUserData(); return true; } else if(lookInDatabase && _dbDriver) { - return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, userData); + return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp); } return false; } @@ -3272,7 +3256,7 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData) { std::list signatures; signatures.push_back(s); - _dbDriver->loadNodeData(signatures, true); + _dbDriver->loadNodeData(signatures); if(uncompressedData) { s->sensorData().uncompressData(); @@ -3309,7 +3293,7 @@ SensorData Memory::getSignatureDataConst(int locationId) const std::list signatures; Signature tmp = *s; signatures.push_back(&tmp); - _dbDriver->loadNodeData(signatures, true); + _dbDriver->loadNodeData(signatures); r = tmp.sensorData(); } else @@ -3324,7 +3308,7 @@ SensorData Memory::getSignatureDataConst(int locationId) const Signature * sTmp = signatures.front(); if(sTmp->sensorData().imageCompressed().empty()) { - _dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull()); + _dbDriver->loadNodeData(signatures); } r = sTmp->sensorData(); if(loadedFromTrash.size()) @@ -4156,12 +4140,15 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p rtabmap::CompressionThread ctImage(image, std::string(".jpg")); rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png")); rtabmap::CompressionThread ctDepth2d(laserScan); + rtabmap::CompressionThread ctUserData(data.userDataRaw()); ctImage.start(); ctDepth.start(); ctDepth2d.start(); + ctUserData.start(); ctImage.join(); ctDepth.join(); ctDepth2d.join(); + ctUserData.join(); s = new Signature(id, _idMapCount, @@ -4169,7 +4156,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p data.stamp(), "", pose, - data.userData(), stereoCameraModel.isValid()? SensorData( ctDepth2d.getCompressedData(), @@ -4177,39 +4163,53 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p ctImage.getCompressedData(), ctDepth.getCompressedData(), stereoCameraModel, - id): + id, + 0, + ctUserData.getCompressedData()): SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), ctImage.getCompressedData(), ctDepth.getCompressedData(), cameraModels, - id)); + id, + 0, + ctUserData.getCompressedData())); } else { + rtabmap::CompressionThread ctDepth2d(laserScan); + rtabmap::CompressionThread ctUserData(data.userDataRaw()); + ctDepth2d.start(); + ctUserData.start(); + ctDepth2d.join(); + ctUserData.join(); + s = new Signature(id, _idMapCount, (data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1 data.stamp(), "", pose, - data.userData(), stereoCameraModel.isValid()? SensorData( - rtabmap::compressData2(laserScan), + ctDepth2d.getCompressedData(), data.laserScanMaxPts(), cv::Mat(), cv::Mat(), stereoCameraModel, - id): + id, + 0, + ctUserData.getCompressedData()): SensorData( - rtabmap::compressData2(laserScan), + ctDepth2d.getCompressedData(), data.laserScanMaxPts(), cv::Mat(), cv::Mat(), cameraModels, - id)); + id, + 0, + ctUserData.getCompressedData())); } s->setWords(words); s->setWords3(words3D); @@ -4218,6 +4218,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p s->sensorData().setImageRaw(image); s->sensorData().setDepthOrRightRaw(depthOrRightImage); s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts()); + s->sensorData().setUserDataRaw(data.userDataRaw()); } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index e8959fa0..d6a0b3c5 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -666,7 +666,7 @@ bool Rtabmap::labelLocation(int id, const std::string & label) return false; } -bool Rtabmap::setUserData(int id, const std::vector & data) +bool Rtabmap::setUserData(int id, const cv::Mat & data) { if(_memory) { @@ -2298,15 +2298,14 @@ bool Rtabmap::process( std::string label; double stamp = 0; std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false); + _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false); signatures.insert(std::make_pair(iter->first, Signature(iter->first, mapId, weight, stamp, label, - odomPose, - userData))); + odomPose))); } statistics_.setPoses(poses); statistics_.setConstraints(constraints); @@ -2878,8 +2877,7 @@ void Rtabmap::get3DMap( int mapId = -1; std::string label; double stamp = 0; - std::vector userData; - _memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true); + _memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, true); SensorData data = _memory->getNodeData(*iter); data.setId(*iter); signatures.insert(std::make_pair(*iter, @@ -2889,7 +2887,6 @@ void Rtabmap::get3DMap( stamp, label, odomPose, - userData, data))); } } @@ -2940,16 +2937,14 @@ void Rtabmap::getGraph( int mapId = -1; std::string label; double stamp = 0; - std::vector userData; - _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global); + _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, global); signatures->insert(std::make_pair(iter->first, Signature(iter->first, mapId, weight, stamp, label, - odomPose, - userData))); + odomPose))); } } } @@ -3058,7 +3053,7 @@ bool Rtabmap::computePath(int targetNode, bool global) { // set goal to latest signature std::string goalStr = uFormat("GOAL:%d", targetNode); - setUserData(0, uStr2Bytes(goalStr)); + setUserData(0, cv::Mat(1, goalStr.size()+1, CV_8SC1, (void *)goalStr.c_str()).clone()); } updateGoalIndex(); } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 7b57adb5..79e6f951 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -188,7 +188,7 @@ void RtabmapThread::mainLoop() _stateMutex.unlock(); int id = 0; - std::vector userData; + cv::Mat userData; switch(state) { case kStateDetecting: @@ -269,7 +269,7 @@ void RtabmapThread::mainLoop() _userDataMutex.lock(); { userData = _userData; - _userData.clear(); + _userData = cv::Mat(); } _userDataMutex.unlock(); _rtabmap->setUserData(0, userData); @@ -596,7 +596,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) cv::Mat(), odomEvent.data().id(), odomEvent.data().stamp(), - odomEvent.data().userData()); + odomEvent.data().userDataRaw()); _dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance)); } else diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index b378de28..d31b43fe 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -47,11 +47,10 @@ SensorData::SensorData( const cv::Mat & image, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), - _laserScanMaxPts(0), - _userData(userData) + _laserScanMaxPts(0) { if(image.rows == 1) { @@ -64,6 +63,15 @@ SensorData::SensorData( image.type() == CV_8UC3); // RGB _imageRaw = image; } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // Mono constructor @@ -72,12 +80,11 @@ SensorData::SensorData( const CameraModel & cameraModel, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(0), - _cameraModels(std::vector(1, cameraModel)), - _userData(userData) + _cameraModels(std::vector(1, cameraModel)) { if(image.rows == 1) { @@ -90,6 +97,15 @@ SensorData::SensorData( image.type() == CV_8UC3); // RGB _imageRaw = image; } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // RGB-D constructor @@ -99,12 +115,11 @@ SensorData::SensorData( const CameraModel & cameraModel, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(0), - _cameraModels(std::vector(1, cameraModel)), - _userData(userData) + _cameraModels(std::vector(1, cameraModel)) { if(rgb.rows == 1) { @@ -129,6 +144,15 @@ SensorData::SensorData( depth.type() == CV_16UC1); // Depth in millimetre _depthOrRightRaw = depth; } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // RGB-D constructor + 2d laser scan @@ -140,12 +164,11 @@ SensorData::SensorData( const CameraModel & cameraModel, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), - _cameraModels(std::vector(1, cameraModel)), - _userData(userData) + _cameraModels(std::vector(1, cameraModel)) { if(rgb.rows == 1) { @@ -179,6 +202,15 @@ SensorData::SensorData( UASSERT(laserScan.type() == CV_8UC1); // Bytes _laserScanCompressed = laserScan; } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // Multi-cameras RGB-D constructor @@ -188,12 +220,11 @@ SensorData::SensorData( const std::vector & cameraModels, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(0), - _cameraModels(cameraModels), - _userData(userData) + _cameraModels(cameraModels) { if(rgb.rows == 1) { @@ -221,6 +252,15 @@ SensorData::SensorData( { UASSERT(cameraModels[i].isValid()); } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // Multi-cameras RGB-D constructor + 2d laser scan @@ -232,12 +272,11 @@ SensorData::SensorData( const std::vector & cameraModels, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), - _cameraModels(cameraModels), - _userData(userData) + _cameraModels(cameraModels) { if(rgb.rows == 1) { @@ -276,6 +315,15 @@ SensorData::SensorData( { UASSERT(cameraModels[i].isValid()); } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } } // Stereo constructor @@ -285,12 +333,11 @@ SensorData::SensorData( const StereoCameraModel & cameraModel, int id, double stamp, - const std::vector & userData): + const cv::Mat & userData): _id(id), _stamp(stamp), _laserScanMaxPts(0), - _stereoCameraModel(cameraModel), - _userData(userData) + _stereoCameraModel(cameraModel) { if(left.rows == 1) { @@ -314,6 +361,15 @@ SensorData::SensorData( _depthOrRightRaw = right; } + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } + } // Stereo constructor + 2d laser scan @@ -325,12 +381,11 @@ SensorData::SensorData( const StereoCameraModel & cameraModel, int id, double stamp, - const std::vector & userData) : + const cv::Mat & userData) : _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), - _stereoCameraModel(cameraModel), - _userData(userData) + _stereoCameraModel(cameraModel) { if(left.rows == 1) { @@ -363,18 +418,60 @@ SensorData::SensorData( UASSERT(laserScan.type() == CV_8UC1); // Bytes _laserScanCompressed = laserScan; } + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } +} + +void SensorData::setUserDataRaw(const cv::Mat & userDataRaw) +{ + if(!_userDataRaw.empty()) + { + UWARN("Writing new user data over existing user data. This may result in data loss."); + } + _userDataRaw = userDataRaw; +} + +void SensorData::setUserData(const cv::Mat & userData) +{ + if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty())) + { + UWARN("Writing new user data over existing user data. This may result in data loss."); + } + _userDataRaw = cv::Mat(); + _userDataCompressed = cv::Mat(); + + if(!userData.empty()) + { + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + _userDataCompressed = compressData2(userData); + } + } } void SensorData::uncompressData() { uncompressData(_imageCompressed.empty()?0:&_imageRaw, _depthOrRightCompressed.empty()?0:&_depthOrRightRaw, - _laserScanCompressed.empty()?0:&_laserScanRaw); + _laserScanCompressed.empty()?0:&_laserScanRaw, + _userDataCompressed.empty()?0:&_userDataRaw); } -void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) +void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) { - uncompressDataConst(imageRaw, depthRaw, laserScanRaw); + uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw); if(imageRaw && !imageRaw->empty() && _imageRaw.empty()) { _imageRaw = *imageRaw; @@ -387,9 +484,13 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat { _laserScanRaw = *laserScanRaw; } + if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty()) + { + _userDataRaw = *userDataRaw; + } } -void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const +void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const { if(imageRaw) { @@ -403,13 +504,19 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv: { *laserScanRaw = _laserScanRaw; } + if(userDataRaw) + { + *userDataRaw = _userDataRaw; + } if( (imageRaw && imageRaw->empty()) || (depthRaw && depthRaw->empty()) || - (laserScanRaw && laserScanRaw->empty())) + (laserScanRaw && laserScanRaw->empty()) || + (userDataRaw && userDataRaw->empty())) { rtabmap::CompressionThread ctImage(_imageCompressed, true); rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true); rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false); + rtabmap::CompressionThread ctUserData(_userDataCompressed, false); if(imageRaw && imageRaw->empty()) { ctImage.start(); @@ -422,9 +529,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv: { ctLaserScan.start(); } + if(userDataRaw && userDataRaw->empty()) + { + ctUserData.start(); + } ctImage.join(); ctDepth.join(); ctLaserScan.join(); + ctUserData.join(); if(imageRaw && imageRaw->empty()) { *imageRaw = ctImage.getUncompressedData(); @@ -450,6 +562,15 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv: UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id()); } } + if(userDataRaw && userDataRaw->empty()) + { + *userDataRaw = ctUserData.getUncompressedData(); + + if(userDataRaw->empty()) + { + UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id()); + } + } } } diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index fa9a2689..bd5909df 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -54,14 +54,12 @@ Signature::Signature( double stamp, const std::string & label, const Transform & pose, - const std::vector & userData, const SensorData & sensorData): _id(id), _mapId(mapId), _stamp(stamp), _weight(weight), _label(label), - _userData(userData), _saved(false), _modified(true), _linksModified(true), @@ -81,18 +79,6 @@ Signature::~Signature() //UDEBUG("id=%d", _id); } -void Signature::setUserData(const std::vector & data) -{ - if(!_userData.empty() && !data.empty()) - { - UWARN("Node %d: Current user data (%d bytes) overwritten by new data (%d bytes)", - _id, (int)_userData.size(), (int)data.size()); - } - - _modified = true; - _userData = data; -} - void Signature::addLinks(const std::list & links) { for(std::list::const_iterator iter = links.begin(); iter!=links.end(); ++iter) diff --git a/corelib/src/resources/DatabaseSchema.sql.in b/corelib/src/resources/DatabaseSchema.sql.in index e9a5f27e..47051b2c 100644 --- a/corelib/src/resources/DatabaseSchema.sql.in +++ b/corelib/src/resources/DatabaseSchema.sql.in @@ -20,7 +20,6 @@ CREATE TABLE Node ( stamp FLOAT, pose BLOB, label TEXT, - user_data BLOB, time_enter DATE, PRIMARY KEY (id) ); @@ -32,6 +31,7 @@ CREATE TABLE Data ( calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform scan BLOB, -- compressed data (Laser scan) scan_max_pts INTEGER, -- Laser scan max points + user_data BLOB, -- compressed data (User data) time_enter DATE, PRIMARY KEY (id) ); diff --git a/examples/WifiMapping/MapBuilderWifi.h b/examples/WifiMapping/MapBuilderWifi.h index 1328fa10..216d15bb 100644 --- a/examples/WifiMapping/MapBuilderWifi.h +++ b/examples/WifiMapping/MapBuilderWifi.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MAPBUILDERWIFI_H_ #include "../RGBDMapping/MapBuilder.h" +#include "rtabmap/core/UserDataEvent.h" using namespace rtabmap; @@ -64,6 +65,28 @@ public: this->unregisterFromEventsManager(); } +protected: + virtual void handleEvent(UEvent * event) + { + if(event->getClassName().compare("UserDataEvent") == 0) + { + UserDataEvent * rtabmapEvent = (UserDataEvent *)event; + // convert userData to wifi levels + if(!rtabmapEvent->data().empty()) + { + UASSERT(rtabmapEvent->data().type() == CV_64FC1 && + rtabmapEvent->data().cols == 2 && + rtabmapEvent->data().rows == 1); + + // format [int level, double stamp] + int level = rtabmapEvent->data().at(0); + double stamp = rtabmapEvent->data().at(1); + wifiLevels_.insert(std::make_pair(stamp, level)); + } + } + MapBuilder::handleEvent(event); + } + protected slots: virtual void processStatistics(const rtabmap::Statistics & stats) { @@ -76,7 +99,6 @@ protected slots: // Add WIFI symbols //============================ std::map nodeStamps; // - std::map > wifiLevels; for(std::map::const_iterator iter=stats.getSignatures().begin(); iter!=stats.getSignatures().end(); @@ -84,34 +106,22 @@ protected slots: { // Sort stamps by stamps->id nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first)); - - // convert userData to wifi levels - if(iter->second.getUserData().size()) - { - UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double)); - - // format [int level, double stamp] - int level; - double stamp; - memcpy(&level, iter->second.getUserData().data(), sizeof(int)); - memcpy(&stamp, iter->second.getUserData().data()+sizeof(int), sizeof(double)); - - wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp))); - } } - for(std::map >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter) + int id = 0; + for(std::map::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id) { // The Wifi value may be taken between two nodes, interpolate its position. - double stampWifi = iter->second.second; + double stampWifi = iter->first; std::map::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin()) { --previousNode; } - std::map::iterator nextNode = nodeStamps.upper_bound(iter->second.second); // upper bound of the stamp + std::map::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp - if(previousNode != nodeStamps.end() && nextNode != nodeStamps.end() && + if(previousNode != nodeStamps.end() && + nextNode != nodeStamps.end() && previousNode->second != nextNode->second && uContains(poses, previousNode->second) && uContains(poses, nextNode->second)) { @@ -130,18 +140,18 @@ protected slots: Transform wifiPose = (poseA*v).translation(); // rip off the rotation - std::string cloudName = uFormat("level%d", iter->first); + std::string cloudName = uFormat("level%d", id); if(clouds.contains(cloudName)) { if(!cloudViewer_->updateCloudPose(cloudName, wifiPose)) { - UERROR("Updating pose cloud %d failed!", iter->first); + UERROR("Updating pose cloud %d failed!", id); } } else { // Make a line with points - int quality = dBm2Quality(iter->second.first)/10; + int quality = dBm2Quality(iter->second)/10; pcl::PointCloud::Ptr cloud(new pcl::PointCloud); for(int i=0; i<10; ++i) { @@ -167,7 +177,7 @@ protected slots: //UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size()); if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, wifiPose, Qt::yellow)) { - UERROR("Adding cloud %d to viewer failed!", iter->first); + UERROR("Adding cloud %d to viewer failed!", id); } else { @@ -175,10 +185,6 @@ protected slots: } } } - else - { - UWARN("Bounds not found!"); - } } //============================ @@ -186,6 +192,9 @@ protected slots: //============================ MapBuilder::processStatistics(stats); } + +private: + std::map wifiLevels_; }; diff --git a/examples/WifiMapping/WifiThread.h b/examples/WifiMapping/WifiThread.h index 4913f4aa..8c288831 100644 --- a/examples/WifiMapping/WifiThread.h +++ b/examples/WifiMapping/WifiThread.h @@ -207,10 +207,10 @@ private: { double stamp = UTimer::now(); - // Create user data [level, stamp] with the value (int = 4 bytes) and a timestamp (double = 8 bytes) - std::vector data(sizeof(int) + sizeof(double)); - memcpy(data.data(), &dBm, sizeof(int)); - memcpy(data.data()+sizeof(int), &stamp, sizeof(double)); + // Create user data [level, stamp] with the value and a timestamp + cv::Mat data(1, 2, CV_64FC1); + data.at(0) = double(dBm); + data.at(1) = stamp; this->post(new UserDataEvent(data)); //UWARN("posting level %d dBm", dBm); } diff --git a/examples/WifiMapping/main.cpp b/examples/WifiMapping/main.cpp index 05ad15c6..f0581a82 100644 --- a/examples/WifiMapping/main.cpp +++ b/examples/WifiMapping/main.cpp @@ -56,7 +56,7 @@ int main(int argc, char * argv[]) ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); - std::string interfaceName = "eth0"; + std::string interfaceName = "wlan0"; int driver = 0; bool mirroring = false; @@ -155,8 +155,9 @@ int main(int argc, char * argv[]) if(!camera->init()) { - UERROR("Camera init failed!"); - //exit(1); + UERROR("Camera init failed! Try another camera driver."); + showUsage(); + exit(1); } CameraThread cameraThread(camera); if(mirroring) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index b0c570de..6556f70a 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -767,8 +767,7 @@ void DatabaseViewer::exportDatabase() int mapId = -1; std::string label; double stamp = 0; - std::vector userData; - if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true)) + if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, true)) { if(frameRate == 0 || previousStamp == 0 || @@ -832,7 +831,7 @@ void DatabaseViewer::exportDatabase() data.cameraModels(), data.id(), data.stamp(), - dialog.isUserDataExported()?data.userData():std::vector()); + dialog.isUserDataExported()?data.userDataRaw():cv::Mat()); } else { @@ -844,7 +843,7 @@ void DatabaseViewer::exportDatabase() data.stereoCameraModel(), data.id(), data.stamp(), - dialog.isUserDataExported()?data.userData():std::vector()); + dialog.isUserDataExported()?data.userDataRaw():cv::Mat()); } recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance); @@ -966,9 +965,8 @@ void DatabaseViewer::updateIds() int w; std::string l; double s; - std::vector d; int mapId; - memory_->getNodeInfo(ids_[i], p, mapId, w, l, s, d, true); + memory_->getNodeInfo(ids_[i], p, mapId, w, l, s, true); mapIds_.insert(std::make_pair(ids_[i], mapId)); } @@ -1235,8 +1233,7 @@ void DatabaseViewer::view3DMap() Transform odomPose; std::string label; double stamp; - std::vector userData; - if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true)) + if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true)) { color = (Qt::GlobalColor)(mapId % 12 + 7 ); } @@ -1592,8 +1589,7 @@ void DatabaseViewer::update(int value, int w; std::string l; double s; - std::vector d; - memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true); + memory_->getNodeInfo(id, odomPose, mapId, w, l, s, true); weight->setNum(w); label->setText(l.c_str()); From 52696496613deccde58db48ed98e415d15d6343c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 28 Jun 2015 20:16:38 -0400 Subject: [PATCH 26/45] Updated camera view rotation limit when approaching z axis --- guilib/src/CloudViewer.cpp | 54 ++++++++++++++++++++++++++++++++++---- 1 file changed, 49 insertions(+), 5 deletions(-) diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 82aa4072..6b9c6c3c 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -44,10 +44,54 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include namespace rtabmap { +class MyInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle +{ +public: + virtual void Rotate() + { + if (this->CurrentRenderer == NULL) + { + return; + } + + vtkRenderWindowInteractor *rwi = this->Interactor; + + int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0]; + int dy = rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1]; + + int *size = this->CurrentRenderer->GetRenderWindow()->GetSize(); + + double delta_elevation = -20.0 / size[1]; + double delta_azimuth = -20.0 / size[0]; + + double rxf = dx * delta_azimuth * this->MotionFactor; + double ryf = dy * delta_elevation * this->MotionFactor; + + vtkCamera *camera = this->CurrentRenderer->GetActiveCamera(); + camera->Azimuth(rxf); + camera->Elevation(ryf); + camera->OrthogonalizeViewUp(); + + if (this->AutoAdjustCameraClippingRange) + { + this->CurrentRenderer->ResetCameraClippingRange(); + } + + if (rwi->GetLightFollowCamera()) + { + this->CurrentRenderer->UpdateLightsGeometryToFollowCamera(); + } + + //rwi->Render(); + } +}; + + CloudViewer::CloudViewer(QWidget *parent) : QVTKWidget(parent), _visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)), @@ -80,7 +124,8 @@ CloudViewer::CloudViewer(QWidget *parent) : // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as // the "Invalid drawable" warning when the view is not visible. //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); - this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle()); + vtkSmartPointer interactor(new MyInteractorStyle()); + this->GetInteractor()->SetInteractorStyle (interactor); _visualizer->setCameraPosition( -1, 0, 0, @@ -1171,13 +1216,11 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event) _visualizer->getCameras(cameras); cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal)); - double norm = cv::norm(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal)); if( _lastCameraOrientation!=cv::Vec3d(0,0,0) && _lastCameraPose!=cv::Vec3d(0,0,0) && - ( (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) && - uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1]) ) || - (norm && fabs(cameras.front().pos[2]-cameras.front().focal[2])/norm > 0.9999))) + (uSign(_lastCameraOrientation[0]) != uSign(newCameraOrientation[0]) && + uSign(_lastCameraOrientation[1]) != uSign(newCameraOrientation[1]))) { cameras.front().pos[0] = _lastCameraPose[0]; cameras.front().pos[1] = _lastCameraPose[1]; @@ -1198,6 +1241,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event) cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]); } + this->update(); emit configChanged(); } From fce1816c21f58f7c6ab8c81996d2c14439b74abd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 29 Jun 2015 00:19:45 -0400 Subject: [PATCH 27/45] added subtractFiltering() method to filter point clouds by subtracting the previous cloud --- corelib/include/rtabmap/core/Graph.h | 8 ++ .../include/rtabmap/core/util3d_filtering.h | 27 ++++ corelib/src/Graph.cpp | 55 +++++++ corelib/src/util3d_filtering.cpp | 76 ++++++++++ guilib/include/rtabmap/gui/MainWindow.h | 2 +- .../include/rtabmap/gui/PreferencesDialog.h | 2 + guilib/src/CloudViewer.cpp | 7 +- guilib/src/MainWindow.cpp | 66 ++++++--- guilib/src/PreferencesDialog.cpp | 26 +++- guilib/src/ui/preferencesDialog.ui | 136 +++++++++++++----- 10 files changed, 341 insertions(+), 64 deletions(-) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 648283ae..dda25071 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -156,6 +156,14 @@ std::multimap::iterator RTABMAP_EXP findLink( std::multimap & links, int from, int to); +std::multimap::const_iterator RTABMAP_EXP findLink( + const std::multimap & links, + int from, + int to); +std::multimap::const_iterator RTABMAP_EXP findLink( + const std::multimap & links, + int from, + int to); /** * Get only the the most recent or older poses in the defined radius. diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index 97ab658c..e9c2ac4e 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -115,6 +115,33 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering( float radiusSearch, int minNeighborsInRadius); +/** + * For convenience. + */ +pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::PointCloud::Ptr & substractCloud, + float radiusSearch, + int minNeighborsInRadius = 0); + +/** + * Subtract a cloud from another one using radius filtering. + * @param cloud the input cloud. + * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked. + * @param cloud the input cloud to subtract. + * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked. + * @param radiusSearch the radius in meter. + * @return the indices of the points satisfying the parameters. + */ +pcl::IndicesPtr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const pcl::PointCloud::Ptr & substractCloud, + const pcl::IndicesPtr & substractIndices, + float radiusSearch, + int minNeighborsInRadius = 0); + + /** * For convenience. */ diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 9a463cb7..34391c39 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -947,6 +947,61 @@ std::multimap::iterator findLink( } return links.end(); } +std::multimap::const_iterator findLink( + const std::multimap & links, + int from, + int to) +{ + std::multimap::const_iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second.to() == to) + { + return iter; + } + ++iter; + } + + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second.to() == from) + { + return iter; + } + ++iter; + } + return links.end(); +} + +std::multimap::const_iterator findLink( + const std::multimap & links, + int from, + int to) +{ + std::multimap::const_iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second == to) + { + return iter; + } + ++iter; + } + + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second == from) + { + return iter; + } + ++iter; + } + return links.end(); +} std::map radiusPosesFiltering( const std::map & poses, diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index f12ac7ab..61f5a35a 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -281,6 +281,82 @@ pcl::IndicesPtr radiusFiltering( } } +pcl::PointCloud::Ptr subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::PointCloud::Ptr & substractCloud, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::IndicesPtr indices(new std::vector); + pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius); + pcl::PointCloud::Ptr out(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *indicesOut, *out); + return out; +} + + +pcl::IndicesPtr subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const pcl::PointCloud::Ptr & substractCloud, + const pcl::IndicesPtr & substractIndices, + float radiusSearch, + int minNeighborsInRadius) +{ + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree(false)); + + if(indices->size()) + { + pcl::IndicesPtr output(new std::vector(indices->size())); + int oi = 0; // output iterator + if(substractIndices->size()) + { + tree->setInputCloud(substractCloud, substractIndices); + } + else + { + tree->setInputCloud(substractCloud); + } + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector kDistances; + int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances); + if(k <= minNeighborsInRadius) + { + output->at(oi++) = indices->at(i); + } + } + output->resize(oi); + return output; + } + else + { + pcl::IndicesPtr output(new std::vector(cloud->size())); + int oi = 0; // output iterator + if(substractIndices->size()) + { + tree->setInputCloud(substractCloud, substractIndices); + } + else + { + tree->setInputCloud(substractCloud); + } + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector kDistances; + int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances); + if(k <= minNeighborsInRadius) + { + output->at(oi++) = i; + } + } + output->resize(oi); + return output; + } +} + pcl::IndicesPtr normalFiltering( const pcl::PointCloud::Ptr & cloud, diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 28886777..9b636168 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -206,7 +206,7 @@ signals: private: void update3DMapVisibility(bool cloudsShown, bool scansShown); void updateMapCloud(const std::map & poses, const Transform & pose, const std::multimap & constraints, const std::map & mapIds, bool verboseProgress = false); - void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId); + void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId); void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId); void drawKeypoints(const std::multimap & refWords, const std::multimap & loopWords); void setupMainLayout(bool vertical); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 83cd5d3e..70f29a88 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -157,8 +157,10 @@ public: double getMeshSmoothingRadius() const; bool isCloudFiltering() const; + bool isSubtractFiltering() const; double getCloudFilteringRadius() const; double getCloudFilteringAngle() const; + int getSubstractFilteringMinPts() const; bool getGridMapShown() const; double getGridMapResolution() const; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 6b9c6c3c..5427e099 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -94,7 +94,6 @@ public: CloudViewer::CloudViewer(QWidget *parent) : QVTKWidget(parent), - _visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)), _aLockCamera(0), _aFollowCamera(0), _aResetCamera(0), @@ -119,13 +118,15 @@ CloudViewer::CloudViewer(QWidget *parent) : { this->setMinimumSize(200, 200); + int argc = 0; + _visualizer = new pcl::visualization::PCLVisualizer(argc, 0, "PCLVisualizer", vtkSmartPointer(new MyInteractorStyle()), false); + this->SetRenderWindow(_visualizer->getRenderWindow()); // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as // the "Invalid drawable" warning when the view is not visible. //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); - vtkSmartPointer interactor(new MyInteractorStyle()); - this->GetInteractor()->SetInteractorStyle (interactor); + this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle()); _visualizer->setCameraPosition( -1, 0, 0, diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 3fc5def1..cbbb5875 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1645,6 +1645,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int _preferencesDialog->getCloudDecimation(0), _preferencesDialog->getCloudMaxDepth(0), _preferencesDialog->getCloudVoxelSize(0)); + _createdClouds.insert(std::make_pair(nodeId, cloud)); if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud()) { @@ -1654,7 +1655,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int int minClusterSize = 20; cv::Mat ground, obstacles; pcl::PointCloud::Ptr voxelizedCloud = cloud; - if(voxelizedCloud->size()) + if(voxelizedCloud->size() && cellSize > _preferencesDialog->getCloudVoxelSize(0)) { voxelizedCloud = util3d::voxelize(cloud, cellSize); } @@ -1671,19 +1672,60 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks()); } + pcl::PointCloud::Ptr cloudFiltered = cloud; + if(_preferencesDialog->isSubtractFiltering() && + _preferencesDialog->getCloudVoxelSize(0) > 0.0 && + cloud->size() && + _createdClouds.size() && + _currentPosesMap.size() && + _currentLinksMap.size()) + { + // find link to previous neighbor + std::map::const_iterator previousIter = _currentPosesMap.find(nodeId); + Link link; + if(previousIter != _currentPosesMap.begin()) + { + --previousIter; + std::multimap::const_iterator linkIter = graph::findLink(_currentLinksMap, nodeId, previousIter->first); + if(linkIter != _currentLinksMap.end()) + { + link = linkIter->second; + if(link.from() != nodeId) + { + link = link.inverse(); + } + } + } + if(link.isValid()) + { + std::map::Ptr>::iterator iter = _createdClouds.find(link.to()); + if(iter!=_createdClouds.end() && iter->second->size()) + { + pcl::PointCloud::Ptr previousCloud = util3d::transformPointCloud(iter->second, link.transform()); + cloudFiltered = util3d::subtractFiltering( + cloud, + previousCloud, + _preferencesDialog->getCloudVoxelSize(0), + _preferencesDialog->getSubstractFilteringMinPts()); + UDEBUG("Filtering %d from %d -> %d", (int)previousCloud->size(), (int)cloud->size(), (int)cloudFiltered->size()); + + } + } + } + if(_preferencesDialog->isCloudMeshing()) { pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); - if(cloud->size()) + if(cloudFiltered->size()) { pcl::PointCloud::Ptr cloudWithNormals; if(_preferencesDialog->getMeshSmoothing()) { - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius()); + cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius()); } else { - cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch()); + cloudWithNormals = util3d::computeNormals(cloudFiltered, _preferencesDialog->getMeshNormalKSearch()); } mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius()); } @@ -1696,10 +1738,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int { UERROR("Adding mesh cloud %d to viewer failed!", nodeId); } - else - { - _createdClouds.insert(std::make_pair(nodeId, tmp)); - } } } else @@ -1707,23 +1745,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int if(_preferencesDialog->getMeshSmoothing()) { pcl::PointCloud::Ptr cloudWithNormals; - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius()); - cloud->clear(); - pcl::copyPointCloud(*cloudWithNormals, *cloud); + cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius()); + cloudFiltered.reset(new pcl::PointCloud); + pcl::copyPointCloud(*cloudWithNormals, *cloudFiltered); } QColor color = Qt::gray; if(mapId >= 0) { color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); } - if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color)) + if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloudFiltered, pose, color)) { UERROR("Adding cloud %d to viewer failed!", nodeId); } - else - { - _createdClouds.insert(std::make_pair(nodeId, cloud)); - } } } else if(iter->getWords3().size()) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 9b874116..7bdaf0cb 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -295,9 +295,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_mls, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); - connect(_ui->groupBox_poseFiltering, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_nodeFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_subtractFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_cloudFilterRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_cloudFilterAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->spinBox_substractFilteringMinPts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -999,9 +1001,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_mls->setChecked(false); _ui->doubleSpinBox_mlsRadius->setValue(0.04); - _ui->groupBox_poseFiltering->setChecked(true); + _ui->checkBox_nodeFiltering->setChecked(true); + _ui->checkBox_subtractFiltering->setChecked(false); _ui->doubleSpinBox_cloudFilterRadius->setValue(0.1); _ui->doubleSpinBox_cloudFilterAngle->setValue(30); + _ui->spinBox_substractFilteringMinPts->setValue(0); _ui->checkBox_map_shown->setChecked(false); _ui->doubleSpinBox_map_resolution->setValue(0.05); @@ -1261,9 +1265,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->checkBox_mls->setChecked(settings.value("meshSmoothing", _ui->checkBox_mls->isChecked()).toBool()); _ui->doubleSpinBox_mlsRadius->setValue(settings.value("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value()).toDouble()); - _ui->groupBox_poseFiltering->setChecked(settings.value("cloudFiltering", _ui->groupBox_poseFiltering->isChecked()).toBool()); + _ui->checkBox_nodeFiltering->setChecked(settings.value("cloudFiltering", _ui->checkBox_nodeFiltering->isChecked()).toBool()); + _ui->checkBox_subtractFiltering->setChecked(settings.value("subtractFiltering", _ui->checkBox_subtractFiltering->isChecked()).toBool()); _ui->doubleSpinBox_cloudFilterRadius->setValue(settings.value("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value()).toDouble()); _ui->doubleSpinBox_cloudFilterAngle->setValue(settings.value("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value()).toDouble()); + _ui->spinBox_substractFilteringMinPts->setValue(settings.value("cloudFilteringAngleMinPts", _ui->spinBox_substractFilteringMinPts->value()).toDouble()); _ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool()); _ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble()); @@ -1551,9 +1557,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("meshSmoothing", _ui->checkBox_mls->isChecked()); settings.setValue("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value()); - settings.setValue("cloudFiltering", _ui->groupBox_poseFiltering->isChecked()); + settings.setValue("cloudFiltering", _ui->checkBox_nodeFiltering->isChecked()); + settings.setValue("subtractFiltering", _ui->checkBox_subtractFiltering->isChecked()); settings.setValue("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value()); settings.setValue("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value()); + settings.setValue("subtractFilteringMinPts", _ui->spinBox_substractFilteringMinPts->value()); settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked()); settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()); @@ -3069,7 +3077,11 @@ double PreferencesDialog::getMeshSmoothingRadius() const } bool PreferencesDialog::isCloudFiltering() const { - return _ui->groupBox_poseFiltering->isChecked(); + return _ui->checkBox_nodeFiltering->isChecked(); +} +bool PreferencesDialog::isSubtractFiltering() const +{ + return _ui->checkBox_subtractFiltering->isChecked(); } double PreferencesDialog::getCloudFilteringRadius() const { @@ -3079,6 +3091,10 @@ double PreferencesDialog::getCloudFilteringAngle() const { return _ui->doubleSpinBox_cloudFilterAngle->value(); } +int PreferencesDialog::getSubstractFilteringMinPts() const +{ + return _ui->spinBox_substractFilteringMinPts->value(); +} bool PreferencesDialog::getGridMapShown() const { return _ui->checkBox_map_shown->isChecked(); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 54972467..f16553f4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -86,7 +86,7 @@ QFrame::Raised - 20 + 1 @@ -346,21 +346,21 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Cloud filtering - true + false - true + false - + - For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept. + For visualization purpose, superposed clouds can be filtered by one or both approaches below. true @@ -368,50 +368,108 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - - m - - - 0.010000000000000 - - - 0.100000000000000 - - - + - + - Radius. + Node filtering. By comparing poses in the same area, only one cloud in a fixed radius and angle is shown. - - - - - - degrees - - - 0 - - - 180.000000000000000 - - - 30.000000000000000 + + true - + + + + + m + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + Radius. + + + + + + + degrees + + + 0 + + + 180.000000000000000 + + + 30.000000000000000 + + + + + + + Angle. + + + + + + + - Angle. + + + + true + + + + Cloud subtraction filtering. When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using "Node filtering" at the same time may generate large "holes" in the map (so better to use without "Node filtering"). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds. + + + true + + + + + + + + + + + + + + + + Minimum number of previous cloud's points in the fixed radius in order to substract the point in the new cloud (radius is the voxel size). Increasing this value reduces the black contours between clouds. + + + true + + + + + + + + From dc48b4d4f46e7a7843aa8196bf9145e7aa1fbf98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Mon, 6 Jul 2015 17:25:38 -0400 Subject: [PATCH 28/45] some fixes for CameraFlyCapture2 driver on Windows, fixed OdometryMono with stereo cameras --- corelib/include/rtabmap/core/Odometry.h | 10 ++- corelib/src/CameraRGB.cpp | 2 +- corelib/src/CameraStereo.cpp | 8 +- corelib/src/DBDriver.cpp | 2 +- corelib/src/Memory.cpp | 2 +- corelib/src/Odometry.cpp | 5 -- corelib/src/OdometryMono.cpp | 113 +++++++++++++++++------- guilib/src/CloudViewer.cpp | 3 + guilib/src/PreferencesDialog.cpp | 28 ++++-- guilib/src/ui/mainWindow.ui | 3 + guilib/src/ui/preferencesDialog.ui | 6 +- 11 files changed, 126 insertions(+), 56 deletions(-) diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 38a11643..c1fde748 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -60,7 +60,7 @@ public: int getRefineIterations() const {return _refineIterations;} float getMaxDepth() const {return _maxDepth;} bool isInfoDataFilled() const {return _fillInfoData;} - bool getEstimationType() const {return _estimationType;} + int getEstimationType() const {return _estimationType;} double getPnPReprojError() const {return _pnpReprojError;} int getPnPFlags() const {return _pnpFlags;} const Transform & previousTransform() const {return previousTransform_;} @@ -179,6 +179,12 @@ private: double flowEps_; int flowMaxLevel_; + int stereoWinSize_; + int stereoIterations_; + double stereoEps_; + int stereoMaxLevel_; + float stereoMaxSlope_; + Memory * memory_; int localHistoryMaxSize_; float initMinFlow_; @@ -187,7 +193,7 @@ private: float fundMatrixReprojError_; float fundMatrixConfidence_; - cv::Mat refDepth_; + cv::Mat refDepthOrRight_; std::map cornersMap_; std::multimap localMap_; std::map > keyFrameWords3D_; diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index aa5664cb..31c88fd0 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -258,7 +258,7 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string } else { - uint32_t guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID); + unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID); if(guid != 0 && guid != 0xffffffff) { _guid = uFormat("%08x", guid); diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index 00e43296..5e717120 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -605,8 +605,6 @@ SensorData CameraStereoFlyCapture2::captureImage() FlyCapture2::Image grabbedImage; if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK) { - stamp = UTimer::now(); - // right and left image extracted from grabbed image ImageContainer imageCont; @@ -701,10 +699,10 @@ SensorData CameraStereoFlyCapture2::captureImage() triclopsGetBaseline(triclopsCtx_, &baseline); StereoCameraModel model( - fx fx, - cx - cy + fx, + cx, + cy, baseline, this->getLocalTransform()); data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now()); diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index dba269a5..5fac2edd 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -571,7 +571,7 @@ void DBDriver::getAllLinks(std::multimap & links, bool ignoreNullLink for(std::map::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter) { links.erase(iter->first); - for(std::multimap::const_iterator jter=iter->second->getLinks().begin(); + for(std::map::const_iterator jter=iter->second->getLinks().begin(); jter!=iter->second->getLinks().end(); ++jter) { diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 89af3562..deefd3e7 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -907,7 +907,7 @@ std::multimap Memory::getAllLinks(bool lookInDatabase, bool ignoreNul for(std::map::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) { links.erase(iter->first); - for(std::multimap::const_iterator jter=iter->second->getLinks().begin(); + for(std::map::const_iterator jter=iter->second->getLinks().begin(); jter!=iter->second->getLinks().end(); ++jter) { diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index d0954f1c..51ae1bf9 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -162,11 +162,6 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) } UASSERT(!data.imageRaw().empty()); - if(dynamic_cast(this) == 0 && dynamic_cast(this) == 0) - { - UERROR("Depth or stereo images required with the odometry selected!"); - return Transform(); - } if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) diff --git a/corelib/src/OdometryMono.cpp b/corelib/src/OdometryMono.cpp index 42951ab0..36e8612f 100644 --- a/corelib/src/OdometryMono.cpp +++ b/corelib/src/OdometryMono.cpp @@ -50,6 +50,11 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) : flowIterations_(Parameters::defaultOdomFlowIterations()), flowEps_(Parameters::defaultOdomFlowEps()), flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()), + stereoWinSize_(Parameters::defaultStereoWinSize()), + stereoIterations_(Parameters::defaultStereoIterations()), + stereoEps_(Parameters::defaultStereoEps()), + stereoMaxLevel_(Parameters::defaultStereoMaxLevel()), + stereoMaxSlope_(Parameters::defaultStereoMaxSlope()), localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()), initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()), initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()), @@ -64,6 +69,12 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_); Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_); + Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_); + Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_); + Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_); + Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_); + Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_); + Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_); Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_); Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_); @@ -139,7 +150,7 @@ void OdometryMono::reset(const Transform & initialPose) Odometry::reset(initialPose); memory_->init("", false, ParametersMap()); localMap_.clear(); - refDepth_ = cv::Mat(); + refDepthOrRight_ = cv::Mat(); cornersMap_.clear(); keyFrameWords3D_.clear(); keyFramePoses_.clear(); @@ -607,7 +618,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * cv::RANSAC, fundMatrixReprojError_, fundMatrixConfidence_); - std::cout << "F=" << F << std::endl; + //std::cout << "F=" << F << std::endl; if(!F.empty()) { @@ -693,7 +704,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * P0.at(2,2) = 1; UDEBUG("Computing P...done!"); - std::cout << "P=" << P << std::endl; + //std::cout << "P=" << P << std::endl; cv::Mat R, T; EpipolarGeometry::findRTFromP(P, R, T); @@ -712,6 +723,41 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * oi = 0; UASSERT(newCorners.size() == cloud->size()); + + pcl::PointCloud::Ptr newCorners3D(new pcl::PointCloud); + + if(refDepthOrRight_.type() == CV_8UC1) + { + newCorners3D = util3d::generateKeypoints3DStereo( + refCorners, + refS->sensorData().imageRaw(), + refDepthOrRight_, + cameraModel.fx(), + data.stereoCameraModel().baseline(), + cameraModel.cx(), + cameraModel.cy(), + Transform::getIdentity(), + stereoWinSize_, + stereoMaxLevel_, + stereoIterations_, + stereoEps_, + stereoMaxSlope_ ); + } + else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1) + { + std::vector tmpKpts; + cv::KeyPoint::convert(refCorners, tmpKpts); + CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy()); + newCorners3D = util3d::generateKeypoints3DDepth( + tmpKpts, + refDepthOrRight_, + m); + } + else if(!refDepthOrRight_.empty()) + { + UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type()); + } + for(unsigned int i=0; isize(); ++i) { if(cloud->at(i).z>0) @@ -719,17 +765,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * imagePoints[oi] = newCorners[i]; tmpCornersId[oi] = cornerIds[i]; (*inliersRef)[oi] = cloud->at(i); - if(!refDepth_.empty()) + if(!newCorners3D->empty()) { - (*inliersRefGuess)[oi] = util3d::projectDepthTo3D( - refDepth_, - refCorners[i].x, - refCorners[i].y, - cameraModel.cx(), - cameraModel.cy(), - cameraModel.fx(), - cameraModel.fy(), - true); + (*inliersRefGuess)[oi] = newCorners3D->at(i); } ++oi; } @@ -745,7 +783,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * //estimate scale float scale = 1; std::multimap scales; // - if(!refDepth_.empty()) // scale known + if(!newCorners3D->empty()) // scale known { UASSERT(inliersRefGuess->size() == inliersRef->size()); for(unsigned int i=0; isize(); ++i) @@ -754,6 +792,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * { float s = inliersRefGuess->at(i).z/inliersRef->at(i).z; std::vector errorSqrdDists(inliersRef->size()); + oi = 0; for(unsigned int j=0; jsize(); ++j) { if(cloud->at(j).z>0) @@ -763,30 +802,40 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * refPt.y *= s; refPt.z *= s; const pcl::PointXYZ & guess = inliersRefGuess->at(j); - errorSqrdDists[j] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z); + errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z); } } - std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); - double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; - float variance = 2.1981 * median_error_sqr; - //UDEBUG("scale %d = %f variance = %f", i, s, variance); - if(variance > 0) + errorSqrdDists.resize(oi); + if(errorSqrdDists.size() > 2) { - scales.insert(std::make_pair(variance, s)); + std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); + double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1]; + float variance = 2.1981 * median_error_sqr; + //UDEBUG("scale %d = %f variance = %f", i, s, variance); + if(variance > 0) + { + scales.insert(std::make_pair(variance, s)); + } } } } - UASSERT(scales.size()); - - scale = scales.begin()->second; - UDEBUG("scale used = %f (variance=%f)", scale, scales.begin()->first); - - maxVariance_ = 0.01; - UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first); - if(scales.begin()->first > 0.01) + if(scales.size() == 0) { - UWARN("Too high variance %f (should be < 0.01)"); - reject = true; // 20 cm for good initialization + UWARN("No scales found!?"); + reject = true; + } + else + { + scale = scales.begin()->second; + UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size()); + + maxVariance_ = 0.01; + UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first); + if(scales.begin()->first > 0.01) + { + UWARN("Too high variance %f (should be < 0.01)", scales.begin()->first); + reject = true; // 20 cm for good initialization + } } } @@ -905,7 +954,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * { cornersMap_.insert(std::make_pair(iter->first, iter->second.pt)); } - refDepth_ = data.depthOrRightRaw().clone(); + refDepthOrRight_ = data.depthOrRightRaw().clone(); keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity())); } else diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 5427e099..ad2e4634 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -132,7 +132,10 @@ CloudViewer::CloudViewer(QWidget *parent) : -1, 0, 0, 0, 0, 0, 0, 0, 1); +#ifndef _WIN32 + // Crash on startup on Windows (vtk issue) _visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0); +#endif //setup menu/actions createMenu(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 7bdaf0cb..0b4b8736 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -194,11 +194,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : } if(!CameraStereoDC1394::available()) { - _ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1); + _ui->comboBox_cameraStereo->setItemData(0, 0, Qt::UserRole - 1); } if(!CameraStereoFlyCapture2::available()) { - _ui->comboBox_cameraRGBD->setItemData(7, 0, Qt::UserRole - 1); + _ui->comboBox_cameraStereo->setItemData(1, 0, Qt::UserRole - 1); } _ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable()); _ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable()); @@ -350,6 +350,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex()); connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_rgbd, SLOT(setCurrentIndex(int))); connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + _ui->stackedWidget_stereo->setCurrentIndex(_ui->comboBox_cameraStereo->currentIndex()); + connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereo, SLOT(setCurrentIndex(int))); + connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_autoWhiteBalance, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -1040,21 +1043,34 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_checkBox_useDbStamps->setChecked(false); #ifdef _WIN32 - _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2 + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2 #else if(CameraFreenect::available()) { - _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcFreenect-kSrcOpenNI_PCL); // freenect + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcFreenect-kSrcRGBD); // freenect } else if(CameraOpenNI2::available()) { - _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2 + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2 } else { - _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI_PCL-kSrcOpenNI_PCL); // openni-pcl + _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI_PCL-kSrcRGBD); // openni-pcl } #endif + if(CameraStereoDC1394::available()) + { + _ui->comboBox_cameraStereo->setCurrentIndex(kSrcDC1394-kSrcStereo); // dc1394 + } + else if(CameraStereoFlyCapture2::available()) + { + _ui->comboBox_cameraStereo->setCurrentIndex(kSrcFlyCapture2-kSrcStereo); // flycapture + } + else + { + _ui->comboBox_cameraStereo->setCurrentIndex(kSrcStereoImages-kSrcStereo); // stereo images + } + _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); _ui->openni2_autoExposure->setChecked(true); diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 04e7d582..80a26a84 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -1198,6 +1198,9 @@ + + true + FlyCapture2 diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f16553f4..57cdef27 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,8 +64,8 @@ 0 0 - 760 - 1570 + 759 + 887 @@ -439,7 +439,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - Cloud subtraction filtering. When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using "Node filtering" at the same time may generate large "holes" in the map (so better to use without "Node filtering"). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds. + <html><head/><body><p>Cloud subtraction filtering. <span style=" font-weight:600;">Note that Map's &quot;3D cloud voxel size&quot; parameter below should be set</span>. <br/>When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using &quot;Node filtering&quot; at the same time may generate large &quot;holes&quot; in the map (so better to use without &quot;Node filtering&quot;). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds.</p></body></html> true From decbba9f16a0554e48c373aee8e4a439dd9337b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Mon, 6 Jul 2015 17:46:05 -0400 Subject: [PATCH 29/45] GUI: default use stamps from database --- guilib/src/MainWindow.cpp | 6 ++++-- guilib/src/PreferencesDialog.cpp | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index cbbb5875..b81db148 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2702,7 +2702,8 @@ void MainWindow::startDetection() float inputRate = _preferencesDialog->getGeneralInputRate(); float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate())); int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize())); - if((detectionRate!=0.0f && detectionRate < inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) + if(((detectionRate!=0.0f && detectionRate < inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) && + (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed())) { int button = QMessageBox::question(this, tr("Incompatible frame rates!"), @@ -2717,7 +2718,8 @@ void MainWindow::startDetection() return; } } - if(bufferingSize != 0) + if(bufferingSize != 0 && + (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed())) { int button = QMessageBox::question(this, tr("Some images may be skipped!"), diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 0b4b8736..91b4300c 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1040,7 +1040,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_checkBox_ignoreOdometry->setChecked(false); _ui->source_checkBox_ignoreGoalDelay->setChecked(false); _ui->source_spinBox_databaseStartPos->setValue(0); - _ui->source_checkBox_useDbStamps->setChecked(false); + _ui->source_checkBox_useDbStamps->setChecked(true); #ifdef _WIN32 _ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2 @@ -2147,6 +2147,7 @@ void PreferencesDialog::selectSourceDatabase() _ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes); _ui->source_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";")); _ui->source_spinBox_databaseStartPos->setValue(0); + _ui->source_checkBox_useDbStamps->setChecked(true); } } From 28c9ada06ecc4f26467f05372e8c26b091898a07 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 7 Jul 2015 11:49:41 -0400 Subject: [PATCH 30/45] Changed all remaining IplImage to cv::Mat --- tools/ConsoleApp/main.cpp | 3 +- tools/ImagesJoiner/main.cpp | 56 +++++++++++++------------------------ 2 files changed, 20 insertions(+), 39 deletions(-) diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index e43115ad..a8b834e3 100644 --- a/tools/ConsoleApp/main.cpp +++ b/tools/ConsoleApp/main.cpp @@ -474,8 +474,7 @@ int main(int argc, char * argv[]) // Generate the ground truth file printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, groundTruthMat.rows); - IplImage img = groundTruthMat; - cvSaveImage(GENERATED_GT_NAME, &img); + cv::imwrite(GENERATED_GT_NAME, groundTruthMat); printf(" Creating ground truth file = %fs\n", timer.ticks()); } diff --git a/tools/ImagesJoiner/main.cpp b/tools/ImagesJoiner/main.cpp index 68e6aa76..74ca1733 100644 --- a/tools/ImagesJoiner/main.cpp +++ b/tools/ImagesJoiner/main.cpp @@ -94,60 +94,42 @@ int main(int argc, char * argv[]) std::string targetFilePath = targetDirectory+UDirectory::separator()+uNumber2Str(i++)+"."+ext; - IplImage * imageA = cvLoadImage(fileNameA.c_str(), CV_LOAD_IMAGE_COLOR); - IplImage * imageB = cvLoadImage(fileNameB.c_str(), CV_LOAD_IMAGE_COLOR); + cv::Mat imageA = cv::imread(fileNameA.c_str()); + cv::Mat imageB = cv::imread(fileNameB.c_str()); fileNameA.clear(); fileNameB.clear(); - if(imageA && imageB) + if(!imageA.empty() && !imageB.empty()) { - CvSize sizeA = cvGetSize(imageA); - CvSize sizeB = cvGetSize(imageB); - CvSize targetSize = cvSize(0,0); + cv::Size sizeA = imageA.size(); + cv::Size sizeB = imageB.size(); + cv::Size targetSize(0,0); targetSize.width = sizeA.width + sizeB.width; targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height; - IplImage* targetImage = cvCreateImage(targetSize, imageA->depth, imageA->nChannels); - if(targetImage) + cv::Mat targetImage(targetSize, imageA.type()); + + cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height )); + imageA.copyTo(roiA); + cv::Mat roiB( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) ); + imageB.copyTo(roiB); + + if(!cv::imwrite(targetFilePath.c_str(), targetImage)) { - cvSetImageROI( targetImage, cvRect( 0, 0, sizeA.width, sizeA.height ) ); - cvCopy( imageA, targetImage ); - cvSetImageROI( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) ); - cvCopy( imageB, targetImage ); - cvResetImageROI( targetImage ); - - if(!cvSaveImage(targetFilePath.c_str(), targetImage)) - { - printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str()); - } - else - { - printf("Saved \"%s\" \n", targetFilePath.c_str()); - } - - cvReleaseImage(&targetImage); - - fileNameA = dir.getNextFilePath(); - fileNameB = dir.getNextFilePath(); + printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str()); } else { - printf("Error : can't allocated the target image with size (%d,%d)\n", targetSize.width, targetSize.height); + printf("Saved \"%s\" \n", targetFilePath.c_str()); } + + fileNameA = dir.getNextFilePath(); + fileNameB = dir.getNextFilePath(); } else { printf("Error: loading images failed!\n"); } - - if(imageA) - { - cvReleaseImage(&imageA); - } - if(imageB) - { - cvReleaseImage(&imageB); - } } printf("%d files processed\n", i-1); From bf4715b73cb2dbc95496086c49af8810d62cfa13 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 8 Jul 2015 15:44:49 -0400 Subject: [PATCH 31/45] fixed fatal error (fx not defined) on stereo camera calibration --- corelib/src/CameraStereo.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index 5e717120..ef208066 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -424,13 +424,17 @@ SensorData CameraStereoDC1394::captureImage() // Rectification left = stereoModel_.left().rectifyImage(left); right = stereoModel_.right().rectifyImage(right); - StereoCameraModel model( - stereoModel_.left().fx(), //fx - stereoModel_.left().fy(), //fy - stereoModel_.left().cx(), //cx - stereoModel_.left().cy(), //cy - stereoModel_.baseline(), - this->getLocalTransform()); + StereoCameraModel model; + if(stereoModel_.isValid()) + { + model = StereoCameraModel( + stereoModel_.left().fx(), //fx + stereoModel_.left().fy(), //fy + stereoModel_.left().cx(), //cx + stereoModel_.left().cy(), //cy + stereoModel_.baseline(), + this->getLocalTransform()); + } data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now()); } } From 3b226a0d925196d0485d4a17ead3090f1a64a92d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 9 Jul 2015 10:01:41 -0400 Subject: [PATCH 32/45] fixed fatal error (fx==0) on kinect v2 calibration, added tx,ty,tz to camera local transform in Preferences --- corelib/src/CameraRGBD.cpp | 16 ++++++++++------ guilib/src/CalibrationDialog.cpp | 26 +++++++++++++++++++++----- guilib/src/PreferencesDialog.cpp | 12 +++++++++--- guilib/src/ui/preferencesDialog.ui | 8 ++++---- 4 files changed, 44 insertions(+), 18 deletions(-) diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index f0f3b5e1..493629dc 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -1459,12 +1459,16 @@ SensorData CameraFreenect2::captureImage() } } - CameraModel model( - fx, //fx - fy, //fy - cx, //cx - cy, // cy - this->getLocalTransform()); + CameraModel model; + if(fx && fy) + { + model=CameraModel( + fx, //fx + fy, //fy + cx, //cx + cy, // cy + this->getLocalTransform()); + } data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp); listener_->release(frames); diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 1ef53713..0c15ce2a 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -199,7 +199,11 @@ void CalibrationDialog::setSquareSize(double size) void CalibrationDialog::closeEvent(QCloseEvent* event) { - if(!savedCalibration_ && models_[0].isValid() && (!stereo_ || stereoModel_.isValid())) + if(!savedCalibration_ && models_[0].isValid() && + (!stereo_ || + (stereoModel_.left().isValid() && + stereoModel_.right().isValid()&& + (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))) { QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"), tr("The camera is calibrated but you didn't " @@ -287,6 +291,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & std::vector > pointBuf(2); + bool depthDetected = false; for(int id=0; id<(stereo_?2:1); ++id) { cv::Mat viewGray; @@ -294,6 +299,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & { if(images[id].type() == CV_16UC1) { + depthDetected = true; //assume IR image: convert to gray scaled const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id])); viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1); @@ -488,6 +494,8 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & } } } + ui_->label_baseline->setVisible(!depthDetected); + ui_->label_baseline_name->setVisible(!depthDetected); if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0]))) { @@ -516,7 +524,10 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & images[1] = models_[1].rectifyImage(images[1]); } } - else if(ui_->radioButton_stereoRectified->isChecked() && stereoModel_.isValid()) + else if(ui_->radioButton_stereoRectified->isChecked() && + (stereoModel_.left().isValid() && + stereoModel_.right().isValid()&& + (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))) { images[0] = stereoModel_.left().rectifyImage(images[0]); images[1] = stereoModel_.right().rectifyImage(images[1]); @@ -833,7 +844,10 @@ void CalibrationDialog::calibrate() //ui_->label_error_stereo->setNum(totalAvgErr); } - if(stereo_ && stereoModel_.isValid()) + if(stereo_ && + stereoModel_.left().isValid() && + stereoModel_.right().isValid()&& + (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)) { ui_->radioButton_rectified->setEnabled(true); ui_->radioButton_stereoRectified->setEnabled(true); @@ -878,7 +892,9 @@ bool CalibrationDialog::save() } else { - UASSERT(stereoModel_.isValid()); + UASSERT(stereoModel_.left().isValid() && + stereoModel_.right().isValid()&& + (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)); QString cameraName = stereoModel_.name().c_str(); QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml"); QString name = QFileInfo(filePath).baseName(); @@ -889,7 +905,7 @@ bool CalibrationDialog::save() std::string leftPath = base+"_left.yaml"; std::string rightPath = base+"_right.yaml"; std::string posePath = base+"_pose.yaml"; - if(stereoModel_.save(dir.toStdString(), name.toStdString())) + if(stereoModel_.save(dir.toStdString(), name.toStdString(), false)) { QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\"."). arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str())); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 91b4300c..839032f2 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -3221,7 +3221,7 @@ Transform PreferencesDialog::getSourceLocalTransform() const QString str = _ui->lineEdit_sourceLocalTransform->text(); str.replace("PI_2", QString::number(3.141592/2.0)); QStringList list = str.split(' '); - if(list.size() == 6 || list.size() == 9) + if(list.size() == 6 || list.size() == 9 || list.size() == 12) { std::vector numbers(list.size()); bool ok = false; @@ -3237,16 +3237,22 @@ Transform PreferencesDialog::getSourceLocalTransform() const } if(ok) { - if(list.size() == 6) + if(numbers.size() == 6) { t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]); } - else // 9 + else if(numbers.size() == 9) { t = Transform(numbers[0], numbers[1], numbers[2], 0, numbers[3], numbers[4], numbers[5], 0, numbers[6], numbers[7], numbers[8], 0); } + else if(numbers.size() == 12) + { + t = Transform(numbers[0], numbers[1], numbers[2], numbers[9], + numbers[3], numbers[4], numbers[5], numbers[10], + numbers[6], numbers[7], numbers[8], numbers[11]); + } } } else diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 57cdef27..d3106a96 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,8 +64,8 @@ 0 0 - 759 - 887 + 760 + 1570 @@ -86,7 +86,7 @@ QFrame::Raised - 1 + 3 @@ -1436,7 +1436,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 values): r11 r12 r13 r21 r22 r23 r31 r32 r33. + Local transform from /base_link to /camera_link. Format (6 values): x y z roll pitch yaw. Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz]. true From bf295c42745062662618a52d64f1b59bf11f79ea Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 9 Jul 2015 10:55:54 -0400 Subject: [PATCH 33/45] added error message when freenect2 is not linked on the right libusb (causing a deadlock when killing the camera) --- corelib/include/rtabmap/core/CameraThread.h | 1 + corelib/src/CameraRGBD.cpp | 11 ++++----- corelib/src/CameraThread.cpp | 26 +++++++++++++++++++++ 3 files changed, 32 insertions(+), 6 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 2768d2f2..2662606a 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -62,6 +62,7 @@ public: private: virtual void mainLoop(); + virtual void mainLoopKill(); private: Camera * _camera; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 493629dc..1f346a70 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #endif #ifdef WITH_DC1394 @@ -1249,16 +1250,14 @@ SensorData CameraFreenect2::captureImage() { libfreenect2::FrameMap frames; #ifndef LIBFREENECT2_THREADING_STDLIB - UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of " - "libfreenect2, this can be done by setting LD_LIBRARY_PATH to " - "\"libfreenect2/depends/libusb/lib\""); + UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. " + "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\""); listener_->waitForNewFrame(frames); #else if(!listener_->waitForNewFrame(frames, 1000)) { - UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of " - "libfreenect2, this can be done by setting LD_LIBRARY_PATH to " - "\"libfreenect2/depends/libusb/lib\""); + UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. " + "Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\""); } else #endif diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index cdb1aa62..7610fb0d 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Camera.h" #include "rtabmap/core/CameraEvent.h" +#include "rtabmap/core/CameraRGBD.h" #include #include @@ -106,4 +107,29 @@ void CameraThread::mainLoop() } } +void CameraThread::mainLoopKill() +{ + if(dynamic_cast(_camera) != 0) + { + int i=20; + while(i-->0) + { + uSleep(100); + if(!this->isKilled()) + { + break; + } + } + if(this->isKilled()) + { + //still in killed state, maybe a deadlock + UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked " + "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. " + "Note that rtabmap should link on libusb of libfreenect2. " + "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\""); + } + + } +} + } // namespace rtabmap From 2d7be6be4830b6c1974f261065f8982448983b81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Sat, 11 Jul 2015 12:42:46 -0400 Subject: [PATCH 34/45] modified how correspondences ratio is computed (icp 2D and 3D), also fixed build with OpenCV3+Cuda --- corelib/include/rtabmap/core/Memory.h | 2 +- .../rtabmap/core/util3d_registration.h | 28 +- corelib/src/CameraRGB.cpp | 2 +- corelib/src/Memory.cpp | 264 +++++++++++------- corelib/src/OdometryICP.cpp | 36 ++- corelib/src/Rtabmap.cpp | 6 +- corelib/src/VWDictionary.cpp | 13 +- corelib/src/util3d_registration.cpp | 258 ++++++----------- guilib/src/DatabaseViewer.cpp | 66 +++-- guilib/src/MainWindow.cpp | 48 ++-- tools/DataRecorder/CMakeLists.txt | 2 + tools/OdometryViewer/CMakeLists.txt | 2 + 12 files changed, 388 insertions(+), 339 deletions(-) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index be98c74f..f5b334c3 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -272,7 +272,7 @@ private: int _bowRefineIterations; bool _bowForce2D; float _bowEpipolarGeometryVar; - bool _bowEstimationType; + int _bowEstimationType; double _bowPnPReprojError; int _bowPnPFlags; float _icpMaxTranslation; diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index c180e3bf..6538ddfb 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -56,32 +56,42 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences( std::vector * inliers = 0, double * variance = 0); +void RTABMAP_EXP computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut); +void RTABMAP_EXP computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut); + Transform RTABMAP_EXP icp( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); Transform RTABMAP_EXP icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); Transform RTABMAP_EXP icp2D( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); pcl::PointCloud::Ptr RTABMAP_EXP getICPReadyCloud( const cv::Mat & depth, diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 31c88fd0..55bdd762 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -115,7 +115,7 @@ unsigned int CameraImages::imagesCount() const { if(_dir) { - return _dir->getFileNames().size(); + return (unsigned int)_dir->getFileNames().size(); } return 0; } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index deefd3e7..9e743578 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2348,28 +2348,44 @@ Transform Memory::computeIcpTransform( if(newCloud->size() && oldCloud->size()) { - icpT = util3d::icpPointToPlane(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + icpT = util3d::icpPointToPlane( + newCloud, oldCloud, _icpMaxCorrespondenceDistance, _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + oldCloud, + _icpMaxCorrespondenceDistance, + variance, + correspondences); } } else { - icpT = util3d::icp(newCloudXYZ, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + icpT = util3d::icp( + newCloudXYZ, oldCloudXYZ, _icpMaxCorrespondenceDistance, _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + oldCloudXYZ, + _icpMaxCorrespondenceDistance, + variance, + correspondences); } // verify if there are enough correspondences - correspondencesRatio = float(correspondences)/float(newS.sensorData().depthOrRightRaw().total()); + correspondencesRatio = float(correspondences)/float(newCloudXYZ->size()>oldCloudXYZ->size()?newCloudXYZ->size():oldCloudXYZ->size()); UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", hasConverged?"true":"false", @@ -2456,10 +2472,12 @@ Transform Memory::computeIcpTransform( pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.sensorData().laserScanRaw(), guess); //voxelize + pcl::PointCloud::Ptr oldCloudVoxelized = oldCloud; + pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(_icp2VoxelSize > _laserScanVoxelSize) { - oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize); - newCloud = util3d::voxelize(newCloud, _icp2VoxelSize); + oldCloudVoxelized = util3d::voxelize(oldCloud, _icp2VoxelSize); + newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } if(newCloud->size() && oldCloud->size()) @@ -2469,33 +2487,14 @@ Transform Memory::computeIcpTransform( float correspondencesRatio = -1.0f; int correspondences = 0; double variance = 1; - icpT = util3d::icp2D(newCloud, - oldCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud()); + icpT = util3d::icp2D( + newCloudVoxelized, + oldCloudVoxelized, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - &hasConverged, - &variance, - &correspondences); - - // verify if there are enough correspondences - - if(newS.sensorData().laserScanMaxPts()) - { - correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); - } - else - { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", - newS.id()); - } - - UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", - newS.id(), oldS.id(), - hasConverged?"true":"false", - variance, - correspondences, - (int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()), - correspondencesRatio*100.0f); + hasConverged, + *newCloudRegistered); //pcl::io::savePCDFile("oldCloud.pcd", *oldCloud); //pcl::io::savePCDFile("newCloud.pcd", *newCloud); @@ -2507,22 +2506,8 @@ Transform Memory::computeIcpTransform( // UWARN("saved newCloudFinal.pcd"); //} - if(varianceOut) - { - *varianceOut = variance; - } - if(correspondencesOut) - { - *correspondencesOut = correspondences; - } - if(correspondencesRatioOut) - { - *correspondencesRatioOut = correspondencesRatio; - } - if(!icpT.isNull() && - hasConverged && - correspondencesRatio >= _icp2CorrespondenceRatio) + hasConverged) { float ix,iy,iz, iroll,ipitch,iyaw; icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); @@ -2530,8 +2515,8 @@ Transform Memory::computeIcpTransform( (fabs(ix) > _icpMaxTranslation || fabs(iy) > _icpMaxTranslation || fabs(iz) > _icpMaxTranslation)) - || - (_icpMaxRotation>0.0f && + || + (_icpMaxRotation>0.0f && (fabs(iroll) > _icpMaxRotation || fabs(ipitch) > _icpMaxRotation || fabs(iyaw) > _icpMaxRotation))) @@ -2541,14 +2526,72 @@ Transform Memory::computeIcpTransform( } else { - transform = icpT * guess; - transform = transform.inverse(); + if(_icp2VoxelSize <= _laserScanVoxelSize) + { + newCloud = util3d::transformPointCloud(newCloud, icpT); + } + else + { + newCloud = newCloudRegistered; + } + + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + util3d::computeVarianceAndCorrespondences( + newCloud, + oldCloud, + _icpMaxCorrespondenceDistance, + variance, + correspondences); + + // verify if there are enough correspondences + if(newS.sensorData().laserScanMaxPts()) + { + correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); + } + else + { + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + newS.id()); + } + + UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", + newS.id(), oldS.id(), + hasConverged?"true":"false", + variance, + correspondences, + (int)(newS.sensorData().laserScanMaxPts()), + correspondencesRatio*100.0f); + + if(varianceOut) + { + *varianceOut = variance; + } + if(correspondencesOut) + { + *correspondencesOut = correspondences; + } + if(correspondencesRatioOut) + { + *correspondencesRatioOut = correspondencesRatio; + } + + if(correspondencesRatio < _icp2CorrespondenceRatio) + { + msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)", + correspondences, correspondencesRatio, _icp2CorrespondenceRatio); + UINFO(msg.c_str()); + } + else + { + transform = icpT * guess; + transform = transform.inverse(); + } } } else { - msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)", - hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio); + msg = uFormat("Cannot compute transform (converged=%s var=%f)", + hasConverged?"true":"false", variance); UINFO(msg.c_str()); } } @@ -2638,49 +2681,28 @@ Transform Memory::computeScanMatchingTransform( newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId)); //voxelize + pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize) { - newCloud = util3d::voxelize(newCloud, _icp2VoxelSize); + newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } Transform transform; - if(assembledOldClouds->size() && newCloud->size()) + if(assembledOldClouds->size() && newCloudVoxelized->size()) { int correspondences = 0; bool hasConverged = false; - Transform icpT = util3d::icp2D(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform icpT = util3d::icp2D( + newCloudVoxelized, assembledOldClouds, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - &hasConverged, - variance, - &correspondences); + hasConverged, + *newCloudRegistered); UDEBUG("icpT=%s", icpT.prettyPrint().c_str()); - // verify if there enough correspondences - float correspondencesRatio = 0.0f; - if(newS->sensorData().laserScanMaxPts()) - { - correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); - } - else - { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", - newS->id()); - } - - UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio*100.0f); - - if(inliers) - { - *inliers = correspondences; - } - //pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true); //pcl::io::savePCDFile("new.pcd", *newCloud, true); //UWARN("local scan matching old.pcd, new.pcd saved!"); @@ -2691,19 +2713,71 @@ Transform Memory::computeScanMatchingTransform( // UWARN("local scan matching newFinal.pcd saved!"); //} - if(!icpT.isNull() && hasConverged && - correspondencesRatio >= _icp2CorrespondenceRatio) + if(!icpT.isNull() && hasConverged) { - transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); + if(_icp2VoxelSize <= _laserScanVoxelSize) + { + newCloud = util3d::transformPointCloud(newCloud, icpT); + } + else + { + newCloud = newCloudRegistered; + } + + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + double v = 1; + util3d::computeVarianceAndCorrespondences( + newCloud, + assembledOldClouds, + _icpMaxCorrespondenceDistance, + v, + correspondences); + if(variance) + { + *variance = v; + } + + // verify if there enough correspondences + float correspondencesRatio = 0.0f; + if(newS->sensorData().laserScanMaxPts()) + { + correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); + } + else + { + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + newS->id()); + } + + UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", + variance?*variance:-1, + correspondences, + (int)newCloud->size(), + correspondencesRatio*100.0f); + + if(inliers) + { + *inliers = correspondences; + } + + if(correspondencesRatio >= _icp2CorrespondenceRatio) + { + transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); + } + else + { + msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)", + variance?*variance:-1, + correspondences, + (int)newCloud->size(), + correspondencesRatio); + UINFO(msg.c_str()); + } } else { - msg = uFormat("Constraints failed... hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", - hasConverged?"true":"false", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio); + msg = uFormat("Constraints failed... hasConverged=%s", + hasConverged?"true":"false"); UINFO(msg.c_str()); } } diff --git a/corelib/src/OdometryICP.cpp b/corelib/src/OdometryICP.cpp index 2ca731a7..90af92df 100644 --- a/corelib/src/OdometryICP.cpp +++ b/corelib/src/OdometryICP.cpp @@ -112,14 +112,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints) { - int correspondences = 0; - Transform transform = util3d::icpPointToPlane(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform transform = util3d::icpPointToPlane( + newCloud, _previousCloudNormal, _maxCorrespondenceDistance, _maxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + int correspondences = 0; + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + _previousCloudNormal, + _maxCorrespondenceDistance, + variance, + correspondences); // verify if there are enough correspondences float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size()); @@ -147,14 +155,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * //point to point if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints) { - int correspondences = 0; - Transform transform = util3d::icp(newCloudXYZ, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform transform = util3d::icp( + newCloudXYZ, _previousCloud, _maxCorrespondenceDistance, _maxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + int correspondences = 0; + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + _previousCloud, + _maxCorrespondenceDistance, + variance, + correspondences); // verify if there are enough correspondences float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size()); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index d6a0b3c5..3cae0da2 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1553,7 +1553,7 @@ bool Rtabmap::process( iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved; ++iter) { - std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false); + std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false); for(std::map::reverse_iterator jter=ids.rbegin(); jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; ++jter) @@ -2311,7 +2311,7 @@ bool Rtabmap::process( statistics_.setConstraints(constraints); statistics_.setSignatures(signatures); statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size()); - localGraphSize = poses.size(); + localGraphSize = (int)poses.size(); } //Start trashing @@ -3053,7 +3053,7 @@ bool Rtabmap::computePath(int targetNode, bool global) { // set goal to latest signature std::string goalStr = uFormat("GOAL:%d", targetNode); - setUserData(0, cv::Mat(1, goalStr.size()+1, CV_8SC1, (void *)goalStr.c_str()).clone()); + setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); } updateGoalIndex(); } diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index a8242105..fd9fee37 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -506,15 +506,16 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, #ifdef HAVE_OPENCV_CUDAFEATURES2D cv::cuda::GpuMat newDescriptorsGpu(descriptors); cv::cuda::GpuMat lastDescriptorsGpu(_dataTree); + cv::Ptr gpuMatcher; if(type==CV_8U) { - cv::cuda::BruteForceMatcher_GPU gpuMatcher; - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); + gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } else { - cv::cuda::BruteForceMatcher_GPU > gpuMatcher; - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2); + gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } #endif #endif @@ -742,12 +743,12 @@ std::vector VWDictionary::findNN(const std::list & vws) const if(type==CV_8U) { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); - gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } else { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2); - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } #endif #endif diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index eefe9d5b..d3247b90 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -222,14 +222,79 @@ Transform transformFromXYZCorrespondences( return Transform(); } +void computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut) +{ + variance = 1; + correspondencesOut = 0; + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloudA); + est->setInputSource(cloudB); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 1]; + variance = (2.1981 * median_error_sqr); + } + + correspondencesOut = (int)correspondences.size(); +} + +void computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut) +{ + variance = 1; + correspondencesOut = 0; + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloudA); + est->setInputSource(cloudB); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 1]; + variance = (2.1981 * median_error_sqr); + } + + correspondencesOut = (int)correspondences.size(); +} + // return transform from source to target (All points must be finite!!!) Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -247,63 +312,8 @@ Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || 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(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } @@ -313,9 +323,8 @@ Transform icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -337,63 +346,8 @@ Transform icpPointToPlane( //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || 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(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } @@ -402,9 +356,8 @@ Transform icp2D(const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -426,63 +379,8 @@ Transform icp2D(const pcl::PointCloud::ConstPtr & cloud_source, //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || 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(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 6556f70a..1fa32b63 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2764,8 +2764,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update pcl::PointCloud::Ptr cloudA(new pcl::PointCloud); pcl::PointCloud::Ptr cloudB(new pcl::PointCloud); - pcl::PointCloud::Ptr scanA(new pcl::PointCloud); - pcl::PointCloud::Ptr scanB(new pcl::PointCloud); + pcl::PointCloud::Ptr scanAVoxelized(new pcl::PointCloud); + pcl::PointCloud::Ptr scanBVoxelized(new pcl::PointCloud); float correspondenceRatio = 0.0f; if(ui_->checkBox_icp_2d->isChecked()) { @@ -2776,6 +2776,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(!oldLaserScan.empty() && !newLaserScan.empty()) { // 2D + pcl::PointCloud::Ptr scanA(new pcl::PointCloud); + pcl::PointCloud::Ptr scanB(new pcl::PointCloud); scanA = util3d::cvMat2Cloud(oldLaserScan); scanB = util3d::cvMat2Cloud(newLaserScan, t); @@ -2785,21 +2787,40 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value()); scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value()); } + else + { + scanAVoxelized = scanA; + scanBVoxelized = scanB; + } if(scanB->size() && scanA->size()) { - transform = util3d::icp2D(scanB, + pcl::PointCloud::Ptr scanBRegistered(new pcl::PointCloud); + transform = util3d::icp2D( + scanB, scanA, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *scanBRegistered); if(!transform.isNull()) { if(dataTo.laserScanMaxPts()) { + pcl::PointCloud::Ptr scanBTransformed = scanBRegistered; + if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f) + { + scanBTransformed = util3d::transformPointCloud(scanB, transform); + } + + util3d::computeVarianceAndCorrespondences( + scanBTransformed, + scanA, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); + correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts()); } else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()) @@ -2844,25 +2865,38 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update UWARN("removed nan normals..."); } - transform = util3d::icpPointToPlane(cloudBNormals, + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); + transform = util3d::icpPointToPlane( + cloudBNormals, cloudANormals, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudANormals, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); } else { + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icp(cloudB, cloudA, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudA, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); } - correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total()); + correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size()); } else { @@ -2913,8 +2947,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(ui_->dockWidget_constraints->isVisible()) { cloudB = util3d::transformPointCloud(cloudB, transform); - scanB = util3d::transformPointCloud(scanB, transform); - this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB); + scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform); + this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index b81db148..07668bea 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3505,22 +3505,22 @@ void MainWindow::postProcessing() } _initProgressDialog->appendText(tr("Refining links...")); - int decimation=8; - float maxDepth=2.0f; - float voxelSize=0.01f; - int samples = 0; - float maxCorrespondences = 0.05f; - float correspondenceRatio = 0.7f; - float icpIterations = 30; + int decimation=Parameters::defaultLccIcp3Decimation(); + float maxDepth=Parameters::defaultLccIcp3MaxDepth(); + float voxelSize=Parameters::defaultLccIcp3VoxelSize(); + int samples = Parameters::defaultLccIcp3Samples(); + float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance(); + float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio(); + float icpIterations = Parameters::defaultLccIcp3Iterations(); 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::kLccIcp3CorrespondenceRatio(), correspondenceRatio); - Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences); + Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations); - bool pointToPlane = false; - int pointToPlaneNormalNeighbors = 20; + bool pointToPlane = Parameters::defaultLccIcp3PointToPlane(); + int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors(); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors); @@ -3605,24 +3605,36 @@ void MainWindow::postProcessing() UWARN("removed nan normals..."); } + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icpPointToPlane(cloudBNormals, cloudANormals, - maxCorrespondences, + maxCorrespondenceDistance, icpIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudANormals, + maxCorrespondenceDistance, + variance, + correspondences); } else { UDEBUG(""); + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icp(cloudB, cloudA, - maxCorrespondences, + maxCorrespondenceDistance, icpIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudA, + maxCorrespondenceDistance, + variance, + correspondences); } float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size()); diff --git a/tools/DataRecorder/CMakeLists.txt b/tools/DataRecorder/CMakeLists.txt index 2f85242f..d7361db8 100644 --- a/tools/DataRecorder/CMakeLists.txt +++ b/tools/DataRecorder/CMakeLists.txt @@ -8,6 +8,7 @@ SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/guilib/include ${CMAKE_CURRENT_SOURCE_DIR} + ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -16,6 +17,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4") ENDIF() SET(LIBRARIES + ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} ) diff --git a/tools/OdometryViewer/CMakeLists.txt b/tools/OdometryViewer/CMakeLists.txt index dfa5f316..3ec03e4a 100644 --- a/tools/OdometryViewer/CMakeLists.txt +++ b/tools/OdometryViewer/CMakeLists.txt @@ -4,6 +4,7 @@ SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/guilib/include ${CMAKE_CURRENT_SOURCE_DIR} + ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -12,6 +13,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4") ENDIF() SET(LIBRARIES + ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} ) From 82ef6231c44bcee81c27ed1c7a97409cd47edfd8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 15 Jul 2015 17:43:08 -0400 Subject: [PATCH 35/45] fixed deleted nodes in localization to be not saved in database --- corelib/src/Memory.cpp | 40 ++++++++++++++++++++++------------------ 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 9e743578..27677be1 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -336,7 +336,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter Memory::~Memory() { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing)); - UDEBUG(""); + if(!_memoryChanged && !_linksChanged) { if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database."))); @@ -1776,7 +1776,8 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) && _dbDriver && - s->id()>0) + s->id()>0 && + (_incrementalMemory || s->isSaved())) { _dbDriver->asyncSave(s); } @@ -2816,26 +2817,29 @@ bool Memory::addLink(const Link & link) toS->addLink(Link(link.to(), link.from(), link.type(), link.transform().inverse(), link.infMatrix())); fromS->addLink(link); - if(link.type()!=Link::kVirtualClosure) + if(_incrementalMemory) { - _linksChanged = true; - } - - if(_incrementalMemory && link.type() == Link::kGlobalClosure) - { - _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id(); - - // update weights only if the memory is incremental - UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0); - if(fromS->id() > toS->id()) + if(link.type()!=Link::kVirtualClosure) { - fromS->setWeight(fromS->getWeight() + toS->getWeight()); - toS->setWeight(0); + _linksChanged = true; } - else + + if(link.type() == Link::kGlobalClosure) { - toS->setWeight(toS->getWeight() + fromS->getWeight()); - fromS->setWeight(0); + _lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id(); + + // update weights only if the memory is incremental + UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0); + if(fromS->id() > toS->id()) + { + fromS->setWeight(fromS->getWeight() + toS->getWeight()); + toS->setWeight(0); + } + else + { + toS->setWeight(toS->getWeight() + fromS->getWeight()); + fromS->setWeight(0); + } } } return true; From 185bc12cae7e83e9df8e9db8dd55ddd9f22e91f7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 15 Jul 2015 18:15:27 -0400 Subject: [PATCH 36/45] sending words too when getting map --- corelib/include/rtabmap/core/Memory.h | 4 +++ corelib/src/Memory.cpp | 36 +++++++++++++++++++++++++++ corelib/src/Rtabmap.cpp | 5 ++++ 3 files changed, 45 insertions(+) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index f5b334c3..15fb1d83 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include #include +#include namespace rtabmap { @@ -140,6 +141,9 @@ public: bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; SensorData getNodeData(int nodeId, bool uncompressedData = false); + void getNodeWords(int nodeId, + std::multimap & words, + std::multimap & words3); SensorData getSignatureDataConst(int locationId) const; std::set getAllSignatureIds() const; bool memoryChanged() const {return _memoryChanged;} diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 27677be1..bcf981ef 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -3354,6 +3354,42 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData) return r; } +void Memory::getNodeWords(int nodeId, + std::multimap & words, + std::multimap & words3) +{ + UDEBUG("nodeId=%d", nodeId); + Signature * s = this->_getSignature(nodeId); + if(s) + { + words = s->getWords(); + words3 = s->getWords3(); + } + else if(_dbDriver) + { + // load from database + std::list signatures; + std::list ids; + ids.push_back(nodeId); + std::set loadedFromTrash; + _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash); + if(signatures.size()) + { + words = signatures.front()->getWords(); + words3 = signatures.front()->getWords3(); + if(loadedFromTrash.size()) + { + //put back + _dbDriver->asyncSave(signatures.front()); + } + else + { + delete signatures.front(); + } + } + } +} + SensorData Memory::getSignatureDataConst(int locationId) const { UDEBUG(""); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 3cae0da2..e2344d56 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2880,6 +2880,9 @@ void Rtabmap::get3DMap( _memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, true); SensorData data = _memory->getNodeData(*iter); data.setId(*iter); + std::multimap words; + std::multimap words3; + _memory->getNodeWords(*iter, words, words3); signatures.insert(std::make_pair(*iter, Signature(*iter, mapId, @@ -2888,6 +2891,8 @@ void Rtabmap::get3DMap( label, odomPose, data))); + signatures.at(*iter).setWords(words); + signatures.at(*iter).setWords3(words3); } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1)) From 6872b165502a948537ecb17ad28f0e9353167d2a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 16 Jul 2015 11:52:09 -0400 Subject: [PATCH 37/45] rgbd_camera: Added option to save stereo images to directory or side-by-side avi file --- tools/CameraRGBD/main.cpp | 256 ++++++++++++++++++++++++++++++-------- 1 file changed, 204 insertions(+), 52 deletions(-) diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 46a096bc..759b25d4 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -31,6 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UMath.h" +#include "rtabmap/utilite/UFile.h" +#include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UConversion.h" #include #include #include @@ -39,7 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. void showUsage() { printf("\nUsage:\n" - "rtabmap-rgbd_camera driver\n" + "rtabmap-rgbd_camera [options] driver\n" " driver Driver number to use: 0=OpenNI-PCL (Kinect)\n" " 1=OpenNI2 (Kinect and Xtion PRO Live)\n" " 2=Freenect (Kinect)\n" @@ -47,10 +50,21 @@ void showUsage() " 4=OpenNI-CV-ASUS (Xtion PRO Live)\n" " 5=Freenect2 (Kinect v2)\n" " 6=DC1394 (Bumblebee2)\n" - " 7=FlyCapture2 (Bumblebee2)\n"); + " 7=FlyCapture2 (Bumblebee2)\n" + " Options:\n" + " -rate #.# Input rate Hz (default 0=inf)\n" + " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n"); exit(1); } +// catch ctrl-c +bool running = true; +void sighandler(int sig) +{ + printf("\nSignal %d caught...\n", sig); + running = false; +} + int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); @@ -59,74 +73,120 @@ int main(int argc, char * argv[]) //ULogger::setPrintWhere(false); int driver = 0; + std::string stereoSavePath; + float rate = 0.0f; if(argc < 2) { showUsage(); } else { - if(strcmp(argv[argc-1], "--help") == 0) + for(int i=1; i 7) - { - UERROR("driver should be between 0 and 6."); - showUsage(); + if(strcmp(argv[i], "-rate") == 0) + { + ++i; + if(i < argc) + { + rate = uStr2Float(argv[i]); + if(rate < 0.0f) + { + showUsage(); + } + } + else + { + showUsage(); + } + continue; + } + if(strcmp(argv[i], "-save_stereo") == 0) + { + ++i; + if(i < argc) + { + stereoSavePath = argv[i]; + } + else + { + showUsage(); + } + continue; + } + if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0) + { + showUsage(); + } + + // last + driver = atoi(argv[i]); + if(driver < 0 || driver > 7) + { + UERROR("driver should be between 0 and 6."); + showUsage(); + } } } UINFO("Using driver %d", driver); rtabmap::Camera * camera = 0; - if(driver == 0) + if(driver < 6) { - camera = new rtabmap::CameraOpenni(); - } - else if(driver == 1) - { - if(!rtabmap::CameraOpenNI2::available()) + if(!stereoSavePath.empty()) { - UERROR("Not built with OpenNI2 support..."); - exit(-1); + UWARN("-save_stereo option cannot be used with RGB-D drivers."); + stereoSavePath.clear(); } - camera = new rtabmap::CameraOpenNI2(); - } - else if(driver == 2) - { - if(!rtabmap::CameraFreenect::available()) + + if(driver == 0) { - UERROR("Not built with Freenect support..."); - exit(-1); + camera = new rtabmap::CameraOpenni(); } - camera = new rtabmap::CameraFreenect(); - } - else if(driver == 3) - { - if(!rtabmap::CameraOpenNICV::available()) + else if(driver == 1) { - UERROR("Not built with OpenNI from OpenCV support..."); - exit(-1); + if(!rtabmap::CameraOpenNI2::available()) + { + UERROR("Not built with OpenNI2 support..."); + exit(-1); + } + camera = new rtabmap::CameraOpenNI2(); } - camera = new rtabmap::CameraOpenNICV(false); - } - else if(driver == 4) - { - if(!rtabmap::CameraOpenNICV::available()) + else if(driver == 2) { - UERROR("Not built with OpenNI from OpenCV support..."); - exit(-1); + if(!rtabmap::CameraFreenect::available()) + { + UERROR("Not built with Freenect support..."); + exit(-1); + } + camera = new rtabmap::CameraFreenect(); } - camera = new rtabmap::CameraOpenNICV(true); - } - else if(driver == 5) - { - if(!rtabmap::CameraFreenect2::available()) + else if(driver == 3) { - UERROR("Not built with Freenect2 support..."); - exit(-1); + if(!rtabmap::CameraOpenNICV::available()) + { + UERROR("Not built with OpenNI from OpenCV support..."); + exit(-1); + } + camera = new rtabmap::CameraOpenNICV(false); + } + else if(driver == 4) + { + if(!rtabmap::CameraOpenNICV::available()) + { + UERROR("Not built with OpenNI from OpenCV support..."); + exit(-1); + } + camera = new rtabmap::CameraOpenNICV(true); + } + else if(driver == 5) + { + if(!rtabmap::CameraFreenect2::available()) + { + UERROR("Not built with Freenect2 support..."); + exit(-1); + } + camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD); } - camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD); } else if(driver == 6) { @@ -157,21 +217,69 @@ int main(int argc, char * argv[]) delete camera; exit(1); } + rtabmap::SensorData data = camera->takeImage(); if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows) { UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.", data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); } + pcl::visualization::CloudViewer * viewer = 0; if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid())) { UWARN("Camera not calibrated! The registered cloud cannot be shown."); } - pcl::visualization::CloudViewer viewer("cloud"); + else + { + viewer = new pcl::visualization::CloudViewer("cloud"); + } rtabmap::Transform t(1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0); - while(!data.imageRaw().empty() && !viewer.wasStopped()) + + cv::VideoWriter videoWriter; + UDirectory dir; + if(!stereoSavePath.empty() && + !data.imageRaw().empty() && + !data.rightRaw().empty()) + { + if(UFile::getExtension(stereoSavePath).compare("avi") == 0) + { + if(data.imageRaw().size() == data.rightRaw().size()) + { + if(rate <= 0) + { + UERROR("You should set the input rate when saving stereo images to a video file."); + showUsage(); + } + cv::Size targetSize = data.imageRaw().size(); + targetSize.width *= 2; + videoWriter.open(stereoSavePath, CV_FOURCC('M', 'J', 'P', 'G'), rate, targetSize, data.imageRaw().channels() == 3); + } + else + { + UERROR("Images not the same size, cannot save stereo images to the video file."); + } + } + else if(UDirectory::exists(stereoSavePath)) + { + UDirectory::makeDir(stereoSavePath+"/"+"left"); + UDirectory::makeDir(stereoSavePath+"/"+"right"); + } + else + { + UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str()); + stereoSavePath.clear(); + } + } + + // to catch the ctrl-c + signal(SIGABRT, &sighandler); + signal(SIGTERM, &sighandler); + signal(SIGINT, &sighandler); + + int id=1; + while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running) { cv::Mat rgb = data.imageRaw(); if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1)) @@ -194,7 +302,8 @@ int main(int argc, char * argv[]) data.cameraModels()[0].fx(), data.cameraModels()[0].fy()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); - viewer.showCloud(cloud, "cloud"); + if(viewer) + viewer->showCloud(cloud, "cloud"); } else if(!depth.empty() && data.cameraModels().size() && @@ -207,7 +316,7 @@ int main(int argc, char * argv[]) data.cameraModels()[0].fx(), data.cameraModels()[0].fy()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); - viewer.showCloud(cloud, "cloud"); + viewer->showCloud(cloud, "cloud"); } cv::Mat tmp; @@ -238,7 +347,8 @@ int main(int argc, char * argv[]) data.stereoCameraModel().left().fx(), data.stereoCameraModel().baseline()); cloud = rtabmap::util3d::transformPointCloud(cloud, t); - viewer.showCloud(cloud, "cloud"); + if(viewer) + viewer->showCloud(cloud, "cloud"); } } @@ -246,8 +356,50 @@ int main(int argc, char * argv[]) if(c == 27) break; // if ESC, break and quit + if(videoWriter.isOpened()) + { + cv::Mat left = data.imageRaw(); + cv::Mat right = data.rightRaw(); + if(left.size() == right.size()) + { + cv::Size targetSize = left.size(); + targetSize.width *= 2; + cv::Mat targetImage(targetSize, left.type()); + if(right.type() != left.type()) + { + cv::Mat tmp; + cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY); + right = tmp; + } + UASSERT(left.type() == right.type()); + + cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height )); + left.copyTo(roiA); + cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) ); + right.copyTo(roiB); + + videoWriter.write(targetImage); + printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str()); + } + else + { + UERROR("Left and right images are not the same size!?"); + } + } + else if(!stereoSavePath.empty()) + { + cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw()); + cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw()); + printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str()); + } + ++id; data = camera->takeImage(); } + printf("Closing...\n"); + if(viewer) + { + delete viewer; + } cv::destroyWindow("Video"); cv::destroyWindow("Depth"); delete camera; From 8754da742014bc733c4c9a1436ce29d32e984cc1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 16 Jul 2015 14:44:25 -0400 Subject: [PATCH 38/45] Added StereoVideo source input (side-by-side video) --- corelib/include/rtabmap/core/CameraModel.h | 3 + corelib/include/rtabmap/core/CameraRGB.h | 7 + corelib/include/rtabmap/core/CameraStereo.h | 36 ++++ corelib/src/CameraRGB.cpp | 72 +++++-- corelib/src/CameraStereo.cpp | 162 +++++++++++++-- .../include/rtabmap/gui/PreferencesDialog.h | 6 + guilib/src/PreferencesDialog.cpp | 68 +++++++ guilib/src/ui/preferencesDialog.ui | 189 +++++++++++++++--- 8 files changed, 485 insertions(+), 58 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 1f1c8da1..8837fc57 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -80,6 +80,7 @@ public: const cv::Mat & R() const {return R_;} //rectification matrix const cv::Mat & P() const {return P_;} //projection matrix + void setLocalTransform(const Transform & transform) {localTransform_ = transform;} const Transform & localTransform() const {return localTransform_;} const cv::Size & imageSize() const {return imageSize_;} @@ -157,6 +158,8 @@ public: void scale(double scale); + void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);} + const Transform & localTransform() const {return left_.localTransform();} Transform stereoTransform() const; const CameraModel & left() const {return left_;} diff --git a/corelib/include/rtabmap/core/CameraRGB.h b/corelib/include/rtabmap/core/CameraRGB.h index e1bb899a..889feb30 100644 --- a/corelib/include/rtabmap/core/CameraRGB.h +++ b/corelib/include/rtabmap/core/CameraRGB.h @@ -52,6 +52,7 @@ public: CameraImages(const std::string & path, int startAt = 1, bool refreshDir = false, + bool rectifyImages = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraImages(); @@ -71,9 +72,13 @@ private: // If the list of files in the directory is refreshed // on each call of takeImage() bool _refreshDir; + bool _rectifyImages; int _count; UDirectory * _dir; std::string _lastFileName; + + std::string _cameraName; + CameraModel _model; }; @@ -93,6 +98,7 @@ public: float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); CameraVideo(const std::string & filePath, + bool rectifyImages = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraVideo(); @@ -109,6 +115,7 @@ protected: private: // File type std::string _filePath; + bool _rectifyImages; cv::VideoCapture _capture; Source _src; diff --git a/corelib/include/rtabmap/core/CameraStereo.h b/corelib/include/rtabmap/core/CameraStereo.h index ef897f98..3136aeb5 100644 --- a/corelib/include/rtabmap/core/CameraStereo.h +++ b/corelib/include/rtabmap/core/CameraStereo.h @@ -107,6 +107,7 @@ public: CameraStereoImages( const std::string & path, const std::string & timestampsPath = "", // "times.txt" + bool rectifyImages = false, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraStereoImages(); @@ -122,9 +123,44 @@ private: CameraImages * camera_; CameraImages * camera2_; std::string timestampsPath_; + bool rectifyImages_; std::list stamps_; StereoCameraModel stereoModel_; std::string cameraName_; }; + +///////////////////////// +// CameraStereoVideo +///////////////////////// +class CameraImages; +class RTABMAP_EXP CameraStereoVideo : + public Camera +{ +public: + static bool available(); + +public: + CameraStereoVideo( + const std::string & path, + bool rectifyImages = false, + float imageRate=0.0f, + const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoVideo(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(); + +private: + cv::VideoCapture capture_; + std::string path_; + bool rectifyImages_; + StereoCameraModel stereoModel_; + std::string cameraName_; +}; + } // namespace rtabmap diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 55bdd762..be306ea2 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -50,12 +50,14 @@ namespace rtabmap CameraImages::CameraImages(const std::string & path, int startAt, bool refreshDir, + bool rectifyImages, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform), _path(path), _startAt(startAt), _refreshDir(refreshDir), + _rectifyImages(rectifyImages), _count(0), _dir(0) { @@ -72,6 +74,8 @@ CameraImages::~CameraImages(void) bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName) { + _cameraName = cameraName; + UDEBUG(""); if(_dir) { @@ -98,17 +102,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string { UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount()); } + + // look for calibration files + if(!calibrationFolder.empty() && !cameraName.empty()) + { + if(!_model.load(calibrationFolder + "/" + cameraName + ".yaml")) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f", + _model.fx(), + _model.fy(), + _model.cx(), + _model.cy()); + } + } + + _model.setLocalTransform(this->getLocalTransform()); + if(_rectifyImages && !_model.isValid()) + { + UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); + return false; + } + return _dir->isValid(); } bool CameraImages::isCalibrated() const { - return false; + return _model.isValid(); } std::string CameraImages::getSerial() const { - return ""; + return _cameraName; } unsigned int CameraImages::imagesCount() const @@ -189,13 +219,18 @@ SensorData CameraImages::captureImage() } } } + + if(!img.empty() && _model.isValid() && _rectifyImages) + { + img = _model.rectifyImage(img); + } } else { UWARN("Directory is not set, camera must be initialized."); } - return SensorData(img); + return SensorData(img, _model, this->getNextSeqID(), UTimer::now()); } @@ -203,21 +238,26 @@ SensorData CameraImages::captureImage() ///////////////////////// // CameraVideo ///////////////////////// -CameraVideo::CameraVideo(int usbDevice, - float imageRate, - const Transform & localTransform) : +CameraVideo::CameraVideo( + int usbDevice, + float imageRate, + const Transform & localTransform) : Camera(imageRate, localTransform), + _rectifyImages(false), _src(kUsbDevice), _usbDevice(usbDevice) { } -CameraVideo::CameraVideo(const std::string & filePath, - float imageRate, - const Transform & localTransform) : +CameraVideo::CameraVideo( + const std::string & filePath, + bool rectifyImages, + float imageRate, + const Transform & localTransform) : Camera(imageRate, localTransform), _filePath(filePath), + _rectifyImages(rectifyImages), _src(kVideoFile), _usbDevice(0) { @@ -274,13 +314,19 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string } else { - UINFO("Camera parameters: fx=%f cx=%f cy=%f cy=%f", + UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f", _model.fx(), + _model.fy(), _model.cx(), - _model.cy(), _model.cy()); } } + _model.setLocalTransform(this->getLocalTransform()); + if(_rectifyImages && !_model.isValid()) + { + UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); + return false; + } } return true; } @@ -302,7 +348,7 @@ SensorData CameraVideo::captureImage() { if(_capture.read(img)) { - if(_model.isValid()) + if(_model.isValid() && (_src != kVideoFile || _rectifyImages)) { img = _model.rectifyImage(img); } @@ -322,7 +368,7 @@ SensorData CameraVideo::captureImage() ULOGGER_WARN("The camera must be initialized before requesting an image."); } - return SensorData(img); + return SensorData(img, _model, this->getNextSeqID(), UTimer::now()); } } // namespace rtabmap diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index ef208066..bbffbc1e 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -733,12 +733,14 @@ bool CameraStereoImages::available() CameraStereoImages::CameraStereoImages( const std::string & path, const std::string & timestampsPath, + bool rectifyImages, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform), camera_(0), camera2_(0), - timestampsPath_(timestampsPath) + timestampsPath_(timestampsPath), + rectifyImages_(rectifyImages) { std::vector paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';')); if(paths.size() >= 1) @@ -771,10 +773,9 @@ CameraStereoImages::~CameraStereoImages() bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName) { // look for calibration files - cameraName_.clear(); + cameraName_ = cameraName; if(!calibrationFolder.empty() && !cameraName.empty()) { - cameraName_ = cameraName; if(!stereoModel_.load(calibrationFolder, cameraName)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", @@ -789,6 +790,13 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std:: stereoModel_.baseline()); } } + stereoModel_.setLocalTransform(this->getLocalTransform()); + if(rectifyImages_ && !stereoModel_.isValid()) + { + UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid."); + return false; + } + bool success = false; if(camera_ == 0) { @@ -893,20 +901,148 @@ SensorData CameraStereoImages::captureImage() if(!right.imageRaw().empty()) { // Rectification - //left = stereoModel_.left().rectifyImage(left); - //right = stereoModel_.right().rectifyImage(right); - StereoCameraModel model( - stereoModel_.left().fx(), //fx - stereoModel_.left().fy(), //fy - stereoModel_.left().cx(), //cx - stereoModel_.left().cy(), //cy - stereoModel_.baseline(), - this->getLocalTransform()); - data = SensorData(left.imageRaw(), right.imageRaw(), model, this->getNextSeqID(), stamp); + cv::Mat leftImage = left.imageRaw(); + cv::Mat rightImage = right.imageRaw(); + if(rightImage.type() != CV_8UC1) + { + cv::Mat tmp; + cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); + rightImage = tmp; + } + if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid()) + { + leftImage = stereoModel_.left().rectifyImage(leftImage); + rightImage = stereoModel_.right().rectifyImage(rightImage); + } + data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp); } } } return data; } +// +// CameraStereoVideo +// +bool CameraStereoVideo::available() +{ + return true; +} + +CameraStereoVideo::CameraStereoVideo( + const std::string & path, + bool rectifyImages, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + path_(path), + rectifyImages_(rectifyImages) +{ +} + +CameraStereoVideo::~CameraStereoVideo() +{ + capture_.release(); +} + +bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName) +{ + if(capture_.isOpened()) + { + capture_.release(); + } + ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str()); + capture_.open(path_.c_str()); + + if(!capture_.isOpened()) + { + ULOGGER_ERROR("Camera: Failed to create a capture object!"); + capture_.release(); + return false; + } + else + { + // look for calibration files + cameraName_ = cameraName; + if(!calibrationFolder.empty() && !cameraName.empty()) + { + if(!stereoModel_.load(calibrationFolder, cameraName)) + { + UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", + cameraName.c_str(), calibrationFolder.c_str()); + } + else + { + UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", + stereoModel_.left().fx(), + stereoModel_.left().cx(), + stereoModel_.left().cy(), + stereoModel_.baseline()); + } + } + stereoModel_.setLocalTransform(this->getLocalTransform()); + if(rectifyImages_ && !stereoModel_.isValid()) + { + UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid."); + return false; + } + } + return true; +} + +bool CameraStereoVideo::isCalibrated() const +{ + return stereoModel_.isValid(); +} + +std::string CameraStereoVideo::getSerial() const +{ + return cameraName_; +} + +SensorData CameraStereoVideo::captureImage() +{ + SensorData data; + + cv::Mat img; + if(capture_.isOpened()) + { + if(capture_.read(img)) + { + // Rectification + cv::Mat leftImage(img, cv::Rect( 0, 0, img.size().width/2, img.size().height )); + cv::Mat rightImage(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height )); + bool rightCvt = false; + if(rightImage.type() != CV_8UC1) + { + cv::Mat tmp; + cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); + rightImage = tmp; + rightCvt = true; + } + if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid()) + { + leftImage = stereoModel_.left().rectifyImage(leftImage); + rightImage = stereoModel_.right().rectifyImage(rightImage); + } + else + { + leftImage = leftImage.clone(); + if(!rightCvt) + { + rightImage = rightImage.clone(); + } + } + data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now()); + } + } + else + { + ULOGGER_WARN("The camera must be initialized before requesting an image."); + } + + return data; +} + + } // namespace rtabmap diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 70f29a88..c3148394 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -93,6 +93,7 @@ public: kSrcDC1394 = 100, kSrcFlyCapture2 = 101, kSrcStereoImages = 102, + kSrcStereoVideo = 103, kSrcRGB = 200, kSrcUsbDevice = 200, @@ -184,7 +185,9 @@ public: int getSourceImagesSuffixIndex() const; //Images group int getSourceImagesStartPos() const; //Images group bool getSourceImagesRefreshDir() const; //Images group + bool getSourceImagesRectify() const; //Images group QString getSourceVideoPath() const; //Video group + bool getSourceVideoRectify() const; //Video group QString getSourceDatabasePath() const; //Database group bool getSourceDatabaseOdometryIgnored() const; //Database group bool getSourceDatabaseGoalDelayIgnored() const; //Database group @@ -196,6 +199,8 @@ public: int getSourceOpenni2Gain() const; //Openni group bool getSourceOpenni2Mirroring() const; //Openni group int getSourceFreenect2Format() const; //Openni group + bool getSourceStereoImagesRectify() const; + bool getSourceStereoVideoRectify() const; bool isSourceRGBDColorOnly() const; Transform getSourceLocalTransform() const; //Openni group Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null @@ -261,6 +266,7 @@ private slots: void selectSourceStereoImagesPath(); void selectSourceImagesPath(); void selectSourceVideoPath(); + void selectSourceStereoVideoPath(); void selectSourceOniPath(); void selectSourceOni2Path(); void updateSourceGrpVisibility(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 839032f2..f162e667 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -333,9 +333,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_images_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_rgbImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //video group connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath())); connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_rgbVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //database group connect(_ui->source_database_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceDatabase())); connect(_ui->toolButton_dbViewer, SIGNAL(clicked()), this, SLOT(openDatabaseViewer())); @@ -363,6 +365,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_cameraStereoImages_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPath())); connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath())); + connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); @@ -1036,6 +1042,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice); _ui->source_images_spinBox_startPos->setValue(1); _ui->source_images_refreshDir->setChecked(false); + _ui->checkBox_rgbImages_rectify->setChecked(false); + _ui->checkBox_rgbVideo_rectify->setChecked(false); _ui->source_checkBox_ignoreOdometry->setChecked(false); _ui->source_checkBox_ignoreGoalDelay->setChecked(false); @@ -1084,6 +1092,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394); _ui->lineEdit_cameraStereoImages_timestamps->setText(""); _ui->lineEdit_cameraStereoImages_path->setText(""); + _ui->checkBox_stereoImages_rectify->setChecked(false); + _ui->lineEdit_cameraStereoVideo_path->setText(""); + _ui->checkBox_stereoVideo_rectify->setChecked(false); } else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName()) { @@ -1348,16 +1359,24 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("StereoImages"); _ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString()); _ui->lineEdit_cameraStereoImages_path->setText(settings.value("path", _ui->lineEdit_cameraStereoImages_path->text()).toString()); + _ui->checkBox_stereoImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoImages_rectify->isChecked()).toBool()); settings.endGroup(); // StereoImages + settings.beginGroup("StereoVideo"); + _ui->lineEdit_cameraStereoVideo_path->setText(settings.value("path", _ui->lineEdit_cameraStereoVideo_path->text()).toString()); + _ui->checkBox_stereoVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoVideo_rectify->isChecked()).toBool()); + settings.endGroup(); // StereoVideo + settings.beginGroup("Images"); _ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString()); _ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt()); _ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool()); + _ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool()); settings.endGroup(); // images settings.beginGroup("Video"); _ui->source_video_lineEdit_path->setText(settings.value("path", _ui->source_video_lineEdit_path->text()).toString()); + _ui->checkBox_rgbVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbVideo_rectify->isChecked()).toBool()); settings.endGroup(); // video settings.beginGroup("Database"); @@ -1639,16 +1658,24 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.beginGroup("StereoImages"); settings.setValue("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()); settings.setValue("path", _ui->lineEdit_cameraStereoImages_path->text()); + settings.setValue("rectify", _ui->checkBox_stereoImages_rectify->isChecked()); settings.endGroup(); // StereoImages + settings.beginGroup("StereoVideo"); + settings.setValue("path", _ui->lineEdit_cameraStereoVideo_path->text()); + settings.setValue("rectify", _ui->checkBox_stereoVideo_rectify->isChecked()); + settings.endGroup(); // StereoVideo + settings.beginGroup("Images"); settings.setValue("path", _ui->source_images_lineEdit_path->text()); settings.setValue("startPos", _ui->source_images_spinBox_startPos->value()); settings.setValue("refreshDir", _ui->source_images_refreshDir->isChecked()); + settings.setValue("rectify", _ui->checkBox_rgbImages_rectify->isChecked()); settings.endGroup(); // images settings.beginGroup("Video"); settings.setValue("path", _ui->source_video_lineEdit_path->text()); + settings.setValue("rectify", _ui->checkBox_rgbVideo_rectify->isChecked()); settings.endGroup(); // video settings.beginGroup("Database"); @@ -2224,6 +2251,20 @@ void PreferencesDialog::selectSourceVideoPath() } } +void PreferencesDialog::selectSourceStereoVideoPath() +{ + QString dir = _ui->lineEdit_cameraStereoVideo_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_cameraStereoVideo_path->text(), tr("Videos (*.avi *.mpg *.mp4)")); + if(!path.isEmpty()) + { + _ui->lineEdit_cameraStereoVideo_path->setText(path); + } +} + void PreferencesDialog::selectSourceOniPath() { QString dir = _ui->lineEdit_openniOniPath->text(); @@ -3274,10 +3315,18 @@ bool PreferencesDialog::getSourceImagesRefreshDir() const { return _ui->source_images_refreshDir->isChecked(); } +bool PreferencesDialog::getSourceImagesRectify() const +{ + return _ui->checkBox_rgbImages_rectify->isChecked(); +} QString PreferencesDialog::getSourceVideoPath() const { return _ui->source_video_lineEdit_path->text(); } +bool PreferencesDialog::getSourceVideoRectify() const +{ + return _ui->checkBox_rgbVideo_rectify->isChecked(); +} QString PreferencesDialog::getSourceDatabasePath() const { return _ui->source_database_lineEdit_path->text(); @@ -3323,6 +3372,14 @@ int PreferencesDialog::getSourceFreenect2Format() const { return _ui->comboBox_freenect2Format->currentIndex(); } +bool PreferencesDialog::getSourceStereoImagesRectify() const +{ + return _ui->checkBox_stereoImages_rectify->isChecked(); +} +bool PreferencesDialog::getSourceStereoVideoRectify() const +{ + return _ui->checkBox_stereoVideo_rectify->isChecked(); +} bool PreferencesDialog::isSourceRGBDColorOnly() const { @@ -3438,6 +3495,15 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) camera = new CameraStereoImages( _ui->lineEdit_cameraStereoImages_path->text().append(QDir::separator()).toStdString(), _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(), + _ui->checkBox_stereoImages_rectify->isChecked(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } + else if(driver == kSrcStereoVideo) + { + camera = new CameraStereoVideo( + _ui->lineEdit_cameraStereoVideo_path->text().toStdString(), + _ui->checkBox_stereoVideo_rectify->isChecked(), this->getGeneralInputRate(), this->getSourceLocalTransform()); } @@ -3452,6 +3518,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) { camera = new CameraVideo( this->getSourceVideoPath().toStdString(), + this->getSourceVideoRectify(), this->getGeneralInputRate(), this->getSourceLocalTransform()); } @@ -3461,6 +3528,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) this->getSourceImagesPath().toStdString(), this->getSourceImagesStartPos(), this->getSourceImagesRefreshDir(), + this->getSourceVideoRectify(), this->getGeneralInputRate(), this->getSourceLocalTransform()); } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d3106a96..e27acfa6 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 - 760 - 1570 + -290 + 755 + 1591 @@ -1538,7 +1538,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 0 + 2 @@ -1948,7 +1948,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -1969,6 +1969,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Images + + + Video (Side-by-Side) + + @@ -1986,9 +1991,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 2 + 3 - + @@ -2001,7 +2006,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - CameraStereoImages + Stereo Images @@ -2018,6 +2023,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + + + + + ... + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -2028,13 +2060,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - @@ -2045,15 +2070,44 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - ... + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + Stereo side-by-side video (*.avi) + + - + Qt::Vertical @@ -2065,6 +2119,37 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + ... + + + + + + + + + + + + + + + + + + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + @@ -2157,6 +2242,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Images dataset + + + + false + + + + + + + Refresh the directory files list after each image loaded. + + + true + + + + + + + Start position (default 1, 0=start from the last). + + + true + + + @@ -2174,13 +2286,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - false - - - @@ -2188,17 +2293,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - Refresh the directory files list after each image loaded. + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true - - + + - Start position (default 1, 0=start from the last). + @@ -2242,6 +2350,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + + + + + + + + From d80c730d3b28c57f28ce2cb193ee23f5e0b4518b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 16 Jul 2015 20:59:33 -0400 Subject: [PATCH 39/45] rgbd_camera: added option to choose codec (FourCC) when recording stereo images to video --- tools/CameraRGBD/main.cpp | 37 +++++++++++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 759b25d4..d82fdc6e 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -53,7 +53,10 @@ void showUsage() " 7=FlyCapture2 (Bumblebee2)\n" " Options:\n" " -rate #.# Input rate Hz (default 0=inf)\n" - " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n"); + " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n" + " -fourcc \"XXXX\" Four characters FourCC code (default is \"MJPG\") used\n" + " when saving stereo images to a video file.\n" + " See http://www.fourcc.org/codecs.php for more codes.\n"); exit(1); } @@ -75,6 +78,7 @@ int main(int argc, char * argv[]) int driver = 0; std::string stereoSavePath; float rate = 0.0f; + std::string fourcc = "MJPG"; if(argc < 2) { showUsage(); @@ -113,10 +117,33 @@ int main(int argc, char * argv[]) } continue; } + if(strcmp(argv[i], "-fourcc") == 0) + { + ++i; + if(i < argc) + { + fourcc = argv[i]; + if(fourcc.size() != 4) + { + UERROR("fourcc should be 4 characters."); + showUsage(); + } + } + else + { + showUsage(); + } + continue; + } if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0) { showUsage(); } + else if(i< argc-1) + { + printf("Unrecognized option \"%s\"", argv[i]); + showUsage(); + } // last driver = atoi(argv[i]); @@ -254,7 +281,13 @@ int main(int argc, char * argv[]) } cv::Size targetSize = data.imageRaw().size(); targetSize.width *= 2; - videoWriter.open(stereoSavePath, CV_FOURCC('M', 'J', 'P', 'G'), rate, targetSize, data.imageRaw().channels() == 3); + UASSERT(fourcc.size() == 4); + videoWriter.open( + stereoSavePath, + CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)), + rate, + targetSize, + data.imageRaw().channels() == 3); } else { From 80ab6a670edeb0c0f240e741eff56cbae00c7a98 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 17 Jul 2015 13:32:18 -0400 Subject: [PATCH 40/45] Preferences: all label texts are selectable --- guilib/src/ui/preferencesDialog.ui | 856 ++++++++++++++++++++++++++++- 1 file changed, 851 insertions(+), 5 deletions(-) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index e27acfa6..3af818a8 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -290 + 0 755 1591 @@ -116,6 +116,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -133,6 +136,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -143,6 +149,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -181,6 +190,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -201,6 +213,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -211,6 +226,9 @@ false + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -231,6 +249,9 @@ true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -271,6 +292,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -365,6 +389,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -377,6 +404,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -399,6 +429,9 @@ Show a yellow background when the number of odometry inliers goes under this thr Radius. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -422,6 +455,9 @@ Show a yellow background when the number of odometry inliers goes under this thr Angle. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -444,6 +480,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -463,6 +502,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -495,6 +537,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -530,6 +575,9 @@ Show a yellow background when the number of odometry inliers goes under this thr Show in 3D map view. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -553,6 +601,9 @@ Show a yellow background when the number of odometry inliers goes under this thr Resolution (cell size). + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -560,6 +611,9 @@ Show a yellow background when the number of odometry inliers goes under this thr Opacity. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -580,6 +634,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -590,6 +647,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -644,6 +704,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -700,6 +763,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -748,6 +814,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -822,6 +891,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -870,6 +942,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -900,6 +975,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -920,6 +998,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -975,6 +1056,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -995,6 +1079,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1005,6 +1092,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1015,6 +1105,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1063,6 +1156,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1073,6 +1169,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1093,6 +1192,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1110,6 +1212,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1189,6 +1294,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1229,6 +1337,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1274,6 +1385,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1294,6 +1408,9 @@ Show a yellow background when the number of odometry inliers goes under this thr true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1330,6 +1447,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1359,7 +1479,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -1384,6 +1504,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Input rate (0 means as fast as possible). + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1394,6 +1517,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1424,6 +1550,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1441,6 +1570,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1451,6 +1583,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1461,6 +1596,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1524,6 +1662,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1538,7 +1679,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 2 + 0 @@ -1669,6 +1810,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1874,6 +2018,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -1991,7 +2138,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 3 + 2 @@ -2058,6 +2205,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2068,6 +2218,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2078,6 +2231,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2148,6 +2304,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2208,6 +2367,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2257,6 +2419,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2267,6 +2432,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2301,6 +2469,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2358,6 +2529,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2442,6 +2616,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Start position (index) + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2462,6 +2639,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2472,6 +2652,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2489,6 +2672,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2576,6 +2762,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2597,6 +2786,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2617,6 +2809,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2627,6 +2822,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2647,6 +2845,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2676,6 +2877,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2699,6 +2903,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2719,6 +2926,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2742,6 +2952,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2791,6 +3004,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2811,6 +3027,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2831,6 +3050,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2841,6 +3063,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2864,6 +3089,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2911,6 +3139,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2931,6 +3162,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2954,6 +3188,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -2977,6 +3214,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3006,6 +3246,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Publish signature data. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3023,6 +3266,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Publish loop closure hypotheses (pdf). + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3040,6 +3286,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Publish loop closure likelihood. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3072,6 +3321,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3092,6 +3344,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3107,6 +3362,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3156,6 +3414,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Prediction probabilities for each loop closure event: + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3166,6 +3427,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3244,6 +3508,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3254,6 +3521,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3264,6 +3534,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3356,6 +3629,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3366,6 +3642,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag false + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3376,6 +3655,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3399,6 +3681,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3419,6 +3704,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3439,6 +3727,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3459,6 +3750,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3479,6 +3773,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3489,6 +3786,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3509,6 +3809,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3559,6 +3862,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3579,6 +3885,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3589,6 +3898,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3645,6 +3957,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3679,6 +3994,9 @@ see Sqlite3 doc 'PRAGMA cache_size'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3719,6 +4037,9 @@ see Sqlite3 doc 'PRAGMA journal_mode'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3729,6 +4050,9 @@ see Sqlite3 doc 'PRAGMA journal_mode'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3740,6 +4064,9 @@ see Sqlite3 doc 'PRAGMA synchronous'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3751,6 +4078,9 @@ see Sqlite3 doc 'PRAGMA temp_store'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3802,6 +4132,9 @@ see Sqlite3 doc 'PRAGMA temp_store'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3894,6 +4227,9 @@ see Sqlite3 doc 'PRAGMA temp_store'. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3932,6 +4268,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3961,6 +4300,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -3996,6 +4338,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4013,6 +4358,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4033,6 +4381,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4053,6 +4404,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4073,6 +4427,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4093,6 +4450,9 @@ generate the number of words requested. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4120,6 +4480,9 @@ Lower the ratio -> higher the precision. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4152,6 +4515,9 @@ Lower the ratio -> higher the precision. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4169,6 +4535,9 @@ Lower the ratio -> higher the precision. Nearest neighbor strategy. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4212,6 +4581,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4222,6 +4594,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4252,6 +4627,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4271,6 +4649,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4299,6 +4680,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4325,6 +4709,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4354,6 +4741,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4402,6 +4792,9 @@ When set to false, no new words are added to dictionary, so no more updates are Octave layers. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4412,6 +4805,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4425,6 +4821,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4438,6 +4837,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4468,6 +4870,9 @@ When set to false, no new words are added to dictionary, so no more updates are U-SURF used. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4515,6 +4920,9 @@ When set to false, no new words are added to dictionary, so no more updates are Octaves. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4522,6 +4930,9 @@ When set to false, no new words are added to dictionary, so no more updates are Hessian threshold. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4558,6 +4969,9 @@ When set to false, no new words are added to dictionary, so no more updates are Sigma. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4584,6 +4998,9 @@ When set to false, no new words are added to dictionary, so no more updates are Contrast threshold. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4591,6 +5008,9 @@ When set to false, no new words are added to dictionary, so no more updates are Edge threshold. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4621,6 +5041,9 @@ When set to false, no new words are added to dictionary, so no more updates are nOctaveLayers. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4628,6 +5051,9 @@ When set to false, no new words are added to dictionary, so no more updates are nFeatures. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4685,6 +5111,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4705,6 +5134,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4725,6 +5157,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4748,6 +5183,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4799,6 +5237,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4845,6 +5286,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4862,6 +5306,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4879,6 +5326,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4896,6 +5346,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4913,6 +5366,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4926,6 +5382,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4943,6 +5402,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -4960,6 +5422,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5006,6 +5471,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5026,6 +5494,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5043,6 +5514,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5060,6 +5534,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5132,6 +5609,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5155,6 +5635,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5172,6 +5655,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5182,6 +5668,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5192,6 +5681,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5228,6 +5720,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5251,6 +5746,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5271,6 +5769,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5339,6 +5840,9 @@ When set to false, no new words are added to dictionary, so no more updates are Hypotheses verification. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5370,6 +5874,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5396,6 +5903,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5422,6 +5932,9 @@ When set to false, no new words are added to dictionary, so no more updates are true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5474,6 +5987,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5486,6 +6002,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5515,6 +6034,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5541,6 +6063,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5570,6 +6095,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5587,6 +6115,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5597,6 +6128,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5620,6 +6154,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5630,6 +6167,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5709,6 +6249,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5733,6 +6276,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5740,6 +6286,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Graph optimization algorithm. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5750,6 +6299,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5760,6 +6312,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5767,6 +6322,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Iterations. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5774,6 +6332,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Stop optimizing when the error improvement is less than this value. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5803,6 +6364,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5825,6 +6389,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5847,6 +6414,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5885,6 +6455,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5902,6 +6475,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5912,6 +6488,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5922,6 +6501,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5950,6 +6532,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5972,6 +6557,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5989,6 +6577,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -5999,6 +6590,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6047,6 +6641,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6057,6 +6654,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6082,6 +6682,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6112,6 +6715,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Maximum RANSAC iterations. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6144,6 +6750,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6154,6 +6763,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6183,6 +6795,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6190,7 +6805,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 0 + 1 @@ -6233,6 +6848,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6259,6 +6877,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6307,6 +6928,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6336,6 +6960,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6365,6 +6992,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6396,6 +7026,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6427,6 +7060,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6500,6 +7136,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6510,6 +7149,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6522,6 +7164,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6579,6 +7224,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare Feature detector + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6602,6 +7250,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6640,6 +7291,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare Iterative closest point (ICP) parameters. + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6671,6 +7325,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6681,6 +7338,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6731,6 +7391,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6783,6 +7446,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6806,6 +7472,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6835,6 +7504,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6845,6 +7517,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6855,6 +7530,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6881,6 +7559,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6898,6 +7579,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6921,6 +7605,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6956,6 +7643,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -6979,6 +7669,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7005,6 +7698,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7037,6 +7733,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7076,6 +7775,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7108,6 +7810,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7134,6 +7839,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7163,6 +7871,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7173,6 +7884,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7223,6 +7937,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7261,6 +7978,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7280,6 +8000,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7304,6 +8027,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7314,6 +8040,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7324,6 +8053,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7334,6 +8066,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7358,6 +8093,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7397,6 +8135,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7448,6 +8189,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7471,6 +8215,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7497,6 +8244,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7547,6 +8297,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7573,6 +8326,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7683,6 +8439,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7703,6 +8462,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7713,6 +8475,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7745,6 +8510,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7811,6 +8579,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7839,6 +8610,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7865,6 +8639,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7894,6 +8671,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7935,6 +8715,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -7963,6 +8746,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8005,6 +8791,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8036,6 +8825,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8056,6 +8848,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8094,6 +8889,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8126,6 +8924,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8152,6 +8953,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8162,6 +8966,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8188,6 +8995,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8246,6 +9056,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8256,6 +9069,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8287,6 +9103,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8313,6 +9132,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8323,6 +9145,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8349,6 +9174,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8406,6 +9234,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8484,6 +9315,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8494,6 +9328,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8504,6 +9341,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8514,6 +9354,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + @@ -8546,6 +9389,9 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + From fb68b3f67d26b400763f1f849f303c1e955fe90a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 17 Jul 2015 16:24:02 -0400 Subject: [PATCH 41/45] fixed build on linux --- tools/CameraRGBD/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index d82fdc6e..24b2048a 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include void showUsage() { From 6bbde728403cdb5344a170ba97e8c23a2803efa2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Jul 2015 16:02:11 -0400 Subject: [PATCH 42/45] Source images: fixed starting position to 1 (not 0) when selecting a folder, removed all asserts on valid caemra model in SensorData --- corelib/src/SensorData.cpp | 9 --------- guilib/src/PreferencesDialog.cpp | 2 +- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index d31b43fe..6c01cbbe 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -248,10 +248,6 @@ SensorData::SensorData( depth.type() == CV_16UC1); // Depth in millimetre _depthOrRightRaw = depth; } - for(unsigned int i=0; isource_images_lineEdit_path->setText(path); - _ui->source_images_spinBox_startPos->setValue(0); + _ui->source_images_spinBox_startPos->setValue(1); } } From 8c7f6ced6f26f2c0e5f2a7569e802b726619fe87 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Jul 2015 17:26:41 -0400 Subject: [PATCH 43/45] removed asserts on CameraModel constructor when fx != 0 (but added the check in isValid() method) --- corelib/include/rtabmap/core/CameraModel.h | 4 +++- corelib/src/CameraModel.cpp | 4 ++-- tools/ConsoleApp/main.cpp | 14 +++++++------- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 8837fc57..e9669f33 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -65,7 +65,9 @@ public: bool isValid() const {return !K_.empty() && !D_.empty() && !R_.empty() && - !P_.empty();} + !P_.empty() && + fx()>0.0 && + fy()>0.0;} const std::string & name() const {return name_;} diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 8063ef67..d4915976 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -81,8 +81,8 @@ CameraModel::CameraModel( P_(cv::Mat::eye(3, 4, CV_64FC1)), localTransform_(localTransform) { - UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str()); - UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str()); + UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str()); + UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str()); UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str()); UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str()); P_.at(0,0) = fx; diff --git a/tools/ConsoleApp/main.cpp b/tools/ConsoleApp/main.cpp index a8b834e3..fb063015 100644 --- a/tools/ConsoleApp/main.cpp +++ b/tools/ConsoleApp/main.cpp @@ -289,6 +289,12 @@ int main(int argc, char * argv[]) showUsage(); } + ULogger::setType(ULogger::kTypeConsole); + //ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false); + //ULogger::setBuffered(true); + ULogger::setLevel(logLevel); + ULogger::setExitLevel(exitLevel); + UTimer timer; timer.start(); std::queue iterationMeanTime; @@ -296,7 +302,7 @@ int main(int argc, char * argv[]) Camera * camera = 0; if(UDirectory::exists(path)) { - camera = new CameraImages(path, startAt, false, 1/rate); + camera = new CameraImages(path, startAt, false, false, 1/rate); } else { @@ -311,12 +317,6 @@ int main(int argc, char * argv[]) std::map groundTruth; - ULogger::setType(ULogger::kTypeConsole); - //ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false); - //ULogger::setBuffered(true); - ULogger::setLevel(logLevel); - ULogger::setExitLevel(exitLevel); - // Create tasks Rtabmap rtabmap; if(inputDbPath.empty()) From d312652cc4f7d08183b0c2d6a3c237b1d9133446 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Jul 2015 14:11:09 -0400 Subject: [PATCH 44/45] MainWindow: minor fixes --- guilib/src/MainWindow.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 07668bea..2d1d4c12 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1010,9 +1010,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { refMapId = stat.getSignatures().at(stat.refImageId()).mapId(); } - if(uContains(stat.getSignatures(), stat.loopClosureId())) + if(_cachedSignatures.contains(stat.loopClosureId())) { - loopMapId = stat.getSignatures().at(stat.loopClosureId()).mapId(); + loopMapId = _cachedSignatures.value(stat.loopClosureId()).mapId(); } _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId)); @@ -2702,12 +2702,12 @@ void MainWindow::startDetection() float inputRate = _preferencesDialog->getGeneralInputRate(); float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate())); int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize())); - if(((detectionRate!=0.0f && detectionRate < inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) && + if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) && (_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed())) { int button = QMessageBox::question(this, tr("Incompatible frame rates!"), - tr("\"Source/Input rate\" (%1 Hz) is higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the " + tr("\"Source/Input rate\" (%1 Hz) is equal to/higher than \"RTAB-Map/Detection rate\" (%2 Hz). As the " "source input is a directory of images/video/database, some images may be " "skipped by the detector. You may want to increase the \"RTAB-Map/Detection rate\" over " "the \"Source/Input rate\" to guaranty that all images are processed. Would you want to " @@ -5199,8 +5199,12 @@ void MainWindow::changeState(MainWindow::State newState) _ui->menuSelect_source->menuAction()->setVisible(!monitoring); _ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring); _ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring); - _ui->toolBar->setVisible(!monitoring); - _ui->toolBar->toggleViewAction()->setVisible(!monitoring); + bool wasMonitoring = _state==kMonitoring || _state == kMonitoringPaused; + if(wasMonitoring != monitoring) + { + _ui->toolBar->setVisible(!monitoring); + _ui->toolBar->toggleViewAction()->setVisible(!monitoring); + } QList actions = _ui->menuTools->actions(); for(int i=0; i Date: Sun, 19 Jul 2015 14:26:10 -0400 Subject: [PATCH 45/45] MainWindow: minor fixes --- guilib/src/MainWindow.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2d1d4c12..49130431 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1010,9 +1010,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { refMapId = stat.getSignatures().at(stat.refImageId()).mapId(); } - if(_cachedSignatures.contains(stat.loopClosureId())) + int highestHypothesisId = static_cast(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f)); + int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId; + if(_cachedSignatures.contains(loopId)) { - loopMapId = _cachedSignatures.value(stat.loopClosureId()).mapId(); + loopMapId = _cachedSignatures.value(loopId).mapId(); } _ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId)); @@ -1026,7 +1028,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } UDEBUG(""); - int highestHypothesisId = static_cast(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f)); bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f); // update cache