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 11a872db..5c536b36 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*-fx) + { + } virtual ~StereoCameraModel() {} - bool isValid() const {return left_.isValid() && right_.isValid();} + 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, bool ignoreStereoTransform = true); @@ -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 458c6292..59816069 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; } Link merge(const Link & link) const @@ -82,19 +124,19 @@ public: UASSERT(type_ == link.type()); UASSERT(!transform_.isNull()); UASSERT(!link.transform().isNull()); - UASSERT(rotVariance_ > 0 && link.rotVariance() > 0 && transVariance_ > 0 && link.transVariance() > 0); + UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); + UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1); 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())); + infMatrix_ + link.infMatrix()); } Link inverse() const { - return Link(to_, from_, type_, transform_.inverse(), rotVariance_, transVariance_); + return Link(to_, from_, type_, transform_.inverse(), infMatrix_); } private: @@ -102,8 +144,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 679ec6f7..554d7310 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( @@ -91,7 +97,7 @@ public: int maxCheckedInDatabase = -1, bool incrementMarginOnLoop = false, bool ignoreLoopIds = false, - bool ignoreBadSignatures = false, + bool ignoreIntermediateNodes = false, double * dbAccessTime = 0) const; std::map getNeighborsIdRadius( int signatureId, @@ -131,8 +137,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;} @@ -185,7 +191,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); @@ -203,6 +209,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..365cb350 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,69 @@ 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)) + { + } 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(generateCovarianceMatrix(rotVariance, transVariance)), + _info(info) + { + } virtual ~OdometryEvent() {} virtual std::string getClassName() const {return "OdometryEvent";} - bool isValid() const {return !_data.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 fd7c99fe..730cfafc 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 = ""); @@ -117,21 +120,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 posesConstraintsOnly = false); + bool global, + std::map * signatures = 0); void clearPath(); bool computePath(int targetNode, bool global); bool computePath(const Transform & targetPose, bool global); @@ -166,7 +161,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/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index a49081c7..865a2c6d 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -150,19 +150,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() {} @@ -170,10 +162,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");} @@ -181,10 +169,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 93c22965..5ce197c1 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 @@ -93,8 +94,8 @@ private: virtual void mainLoop(); virtual void mainLoopKill(); void process(); - void addData(const SensorData & data); - bool getData(SensorData & data); + void addData(const OdometryEvent & odomEvent); + bool getData(OdometryEvent & data); void pushNewState(State newState, const ParametersMap & parameters = ParametersMap()); void publishMap(bool optimized, bool full) const; void publishGraph(bool optimized, bool full) const; @@ -104,7 +105,7 @@ private: std::stack _state; std::stack _stateParam; - std::list _dataBuffer; + std::list _dataBuffer; UMutex _dataMutex; USemaphore _dataAdded; unsigned int _dataBufferMaxSize; @@ -115,8 +116,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..48a1085d 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;} + const 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..e7fa84ad 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -53,23 +53,13 @@ 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 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 +111,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 +139,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 // in base_link frame (localTransform applied)) 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/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/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5164b158..4de46ae3 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,38 @@ 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::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/include/rtabmap/core/util3d_features.h b/corelib/include/rtabmap/core/util3d_features.h index 1970d2d0..e75a2e27 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,20 +47,17 @@ 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, @@ -69,7 +67,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( float baseline, float cx, float cy, - const Transform & transform = Transform::getIdentity(), + Transform localTransform = Transform::getIdentity(), int flowWinSize = 9, int flowMaxLevel = 4, int flowIterations = 20, @@ -83,7 +81,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP generateKeypoints3DStereo( float baseline, float cx, float cy, - const Transform & transform = Transform::getIdentity(), + Transform localTransform = Transform::getIdentity(), int flowWinSize = 9, int flowMaxLevel = 4, int flowIterations = 20, @@ -93,11 +91,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/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/CameraModel.cpp b/corelib/src/CameraModel.cpp index 702dac75..65a60e97 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(); @@ -176,6 +214,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()) @@ -364,7 +418,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 95549d59..5ed3cb80 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -109,13 +109,13 @@ 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; double stamp = UTimer::now(); if(_cameraRGBD) { - _cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy, stamp); + _cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp); } else { @@ -125,8 +125,19 @@ void CameraThread::mainLoop() if(!rgb.empty()) { if(_cameraRGBD) - { - SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, stamp); + { + SensorData data; + if(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 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..2dbd4d07 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... @@ -1216,8 +1349,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< weight, stamp, label, - std::multimap(), - std::multimap(), pose, userData); s->setSaved(true); @@ -1900,7 +2031,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 +2139,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 +2179,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 +2373,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 +2413,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 +2427,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 +2442,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 +2455,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 +2497,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 +2509,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 +2521,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 +2648,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 +2665,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 dc316cad..e1478778 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.data().id()) { 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) @@ -242,31 +242,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; @@ -274,8 +269,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 @@ -285,7 +279,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); } @@ -339,33 +333,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()); } } } @@ -373,7 +350,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 7f1ffa1b..058e1a2c 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -263,20 +263,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; @@ -304,18 +307,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; @@ -491,7 +483,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, @@ -500,12 +492,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); @@ -689,15 +696,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(); @@ -716,18 +723,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 2d89e97a..aaa4e488 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -41,13 +41,13 @@ 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" #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" @@ -544,7 +544,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; @@ -564,7 +575,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..."); @@ -576,7 +587,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; @@ -699,7 +710,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 @@ -715,14 +726,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()); } @@ -872,7 +884,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 + bool ignoreIntermediateNodes, // default false double * dbAccessTime ) const { @@ -911,7 +923,7 @@ std::map Memory::getNeighborsId(int signatureId, const std::map * links = &tmpLinks; if(s) { - if(!ignoreBadSignatures || !s->isBadSignature()) + if(!ignoreIntermediateNodes || s->getWeight() != -1) { ids.insert(std::pair(*jter, m)); } @@ -943,7 +955,7 @@ std::map Memory::getNeighborsId(int signatureId, UASSERT(iter->second.type() != Link::kUndef); if(iter->second.type() == Link::kNeighbor) { - if(ignoreBadSignatures && s->isBadSignature()) + if(ignoreIntermediateNodes && s->getWeight()==-1) { // stay on the same margin if(currentMargin.insert(iter->first).second) @@ -1963,141 +1975,147 @@ Transform Memory::computeVisualTransform( UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); } - // 2D -> 3D - if(!oldS.getWords3().empty() && !newS.getWords().empty()) + if(!newS.sensorData().rightRaw().empty() && !newS.sensorData().stereoCameraModel().isValid()) { - // 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; i 3D + if(!oldS.getWords3().empty() && !newS.getWords().empty()) { - if(oldS.getWords3().count(ids[i]) == 1) + // 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)) + if(oldS.getWords3().count(ids[i]) == 1) { - 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]; + pcl::PointXYZ pt = oldS.getWords3().find(ids[i])->second; + 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); + 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) + if((int)matches.size() >= _bowMinInliers) { - *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)); + //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); - 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) + if(inliers) { - std::vector errorSqrdDists(inliersV.size()); - oi = 0; - for(unsigned int i=0; i= _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::multimap::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]); - if(iter != newS.getWords3().end() && pcl::isFinite(iter->second)) + std::vector errorSqrdDists(inliersV.size()); + oi = 0; + for(unsigned int i=0; isecond, transform); - errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); + std::multimap::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; } } - 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("PnP not enough inliers (%d[%d] < %d), rejecting the transform...", - (int)inliersV.size(), (int)matches.size(), _bowMinInliers); + msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers); UINFO(msg.c_str()); } } else { - msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers); + 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 - { - 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()) + 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, @@ -2148,11 +2166,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 { @@ -2275,12 +2298,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()); @@ -2289,11 +2312,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); } @@ -2311,14 +2334,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); @@ -2365,140 +2388,130 @@ 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 { - 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()); } } @@ -2515,11 +2528,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) @@ -2545,9 +2558,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 { @@ -2626,7 +2639,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()); } } @@ -2658,7 +2673,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); } @@ -2675,10 +2690,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 @@ -2698,7 +2713,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 @@ -2724,9 +2739,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 { @@ -2785,71 +2800,60 @@ 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()) + // update weights only if the memory is incremental + UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0); + 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; @@ -2880,6 +2884,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(""); @@ -3063,7 +3093,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) { Signature * s = this->_getSignature(*iter); UASSERT(s!=0); - if(!s->isBadSignature() && s->id() != signature->id()) + if(s->getWeight() >= 0 && s->id() != signature->id()) { sB = s; break; @@ -3196,7 +3226,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); @@ -3211,7 +3241,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()); @@ -3285,23 +3315,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) { @@ -3311,65 +3347,33 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData) std::list signatures; signatures.push_back(s); _dbDriver->loadNodeData(signatures, true); - r = *s; + if(uncompressedData) + { + s->sensorData().uncompressData(); + } + r = s->sensorData(); } else { - std::list ids; - ids.push_back(locationId); - std::list signatures; - std::set loadedFromTrash; - _dbDriver->loadSignatures(ids, signatures, &loadedFromTrash); - if(signatures.size()) + _dbDriver->getNodeData(nodeId, r); + 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; - } + r.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()); - } - else - { - r.uncompressData(); - } - } - 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) { @@ -3377,9 +3381,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 { @@ -3391,11 +3396,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 @@ -3682,28 +3687,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()); @@ -3731,22 +3722,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); @@ -3802,22 +3803,22 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) pcl::PointCloud::Ptr keypoints3D(new pcl::PointCloud); if(data.keypoints().size() == 0) { - if(_feature2D->getMaxFeatures() >= 0 && !data.image().empty()) + if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty()) { // 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; @@ -3865,7 +3866,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) //generate a disparity map disparity = util2d::disparityFromStereoImages( imageMono, - data.rightImage(), + data.depthOrRightRaw(), leftCorners, _stereoFlowWinSize, _stereoFlowMaxLevel, @@ -3879,7 +3880,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()); } @@ -3894,14 +3895,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; @@ -3943,7 +3947,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()); } @@ -3957,7 +3961,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); @@ -4006,7 +4013,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) descriptors = cv::Mat(); } } - else if(data.image().empty()) + else if(data.imageRaw().empty()) { UDEBUG("Empty image, cannot extract features..."); } @@ -4021,25 +4028,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, @@ -4053,16 +4060,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) @@ -4071,7 +4081,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); @@ -4137,21 +4150,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 @@ -4176,29 +4188,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)); @@ -4228,49 +4239,48 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats) s = new Signature(id, _idMapCount, - 0, + (data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1 data.stamp(), "", - 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 { s = new Signature(id, _idMapCount, - 0, + (data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1 data.stamp(), "", - words, - words3D, - data.pose(), - data.userData(), - rtabmap::compressData2(laserScan), - cv::Mat(), - cv::Mat(), - fx, - fyOrBaseline, - cx, - cy, - data.localTransform(), - data.laserScanMaxPts()); + pose, + data.userData(), + SensorData( + rtabmap::compressData2(laserScan), + data.laserScanMaxPts(), + cv::Mat(), cv::Mat(), CameraModel(), id)); } + s->setWords(words); + s->setWords3(words3D); 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()); } @@ -4507,13 +4517,12 @@ void Memory::getMetricConstraints( uContains(poses, jter->first) && graph::findLink(links, *iter, jter->first) == links.end()) { - // 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()) + while(s && s->getWeight() == -1) { // skip to next neighbor, well we assume that bad signatures // are only linked by max 2 neighbor links. diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index d4f6c95b..434a67f3 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -161,16 +161,16 @@ 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.stereoCameraModel().isValid() && + (data.cameraModels().size() == 0 || !data.cameraModels()[0].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 Transform(); } diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 9b5a0674..7235f08e 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 cf4a3f1c..5f4f0f02 100644 --- a/corelib/src/OdometryOpticalFlow.cpp +++ b/corelib/src/OdometryOpticalFlow.cpp @@ -124,6 +124,17 @@ Transform OdometryOpticalFlow::computeTransform( { UTimer timer; Transform output; + if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid()) + { + UERROR("Calibrated stereo camera required"); + return output; + } + if(!data.depthRaw().empty() && + (data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid())) + { + UERROR("Calibrated camera required (multi-cameras not supported)."); + return output; + } double variance = 0; int inliers = 0; @@ -136,20 +147,20 @@ Transform OdometryOpticalFlow::computeTransform( 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(); } std::vector newCorners; UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d", - (int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightImage().empty()?0:1); + (int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1); if(!refFrame_.empty() && - !data.depthOrRightImage().empty() && + ((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) && refCorners_.size() && refCorners3D_->size()) { @@ -158,11 +169,9 @@ Transform OdometryOpticalFlow::computeTransform( // 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 K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K(); + Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform(); + Transform guess = (this->previousTransform() * 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(), @@ -263,7 +272,7 @@ Transform OdometryOpticalFlow::computeTransform( if((int)inliersV.size() >= this->getMinInliers()) { // make it incremental - output = (data.localTransform() * pnp).inverse(); + output = (localTransform * pnp).inverse(); variance = 1; // FIXME, is there a way to compute a variance from the PNP approach? } else @@ -294,17 +303,17 @@ Transform OdometryOpticalFlow::computeTransform( info->newCorners.resize(newCornersKept.size()); } int oi = 0; - if(!data.rightImage().empty()) + if(!data.rightRaw().empty()) { // stereo pcl::PointCloud::Ptr newCorners3D = util3d::generateKeypoints3DStereo( newCornersKept, newLeftFrame, - data.rightImage(), - data.fx(), - data.baseline(), - data.cx(), - data.cy(), + data.rightRaw(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline(), + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), Transform::getIdentity(), stereoWinSize_, stereoMaxLevel_, @@ -319,7 +328,7 @@ Transform OdometryOpticalFlow::computeTransform( { //Add 3D correspondences! correspondencesRef->at(oi) = refCorners3DKept->at(i); - correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), data.localTransform()); + correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), localTransform); if(this->isInfoDataFilled() && info) { info->refCorners[oi] = refCornersKept[i]; @@ -334,17 +343,18 @@ Transform OdometryOpticalFlow::computeTransform( //depth for(unsigned int i=0; igetMaxDepth() == 0.0f || pt.z < this->getMaxDepth())) { //Add 3D correspondences! correspondencesRef->at(oi) = refCorners3DKept->at(i); - correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform()); + correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform); + if(this->isInfoDataFilled() && info) { info->refCorners[oi] = refCornersKept[i]; @@ -444,17 +454,17 @@ Transform OdometryOpticalFlow::computeTransform( newCorners3D->resize(newCorners.size()); std::vector newCornersFiltered(newCorners.size()); int oi=0; - if(!data.rightImage().empty()) + if(!data.rightRaw().empty()) { /// stereo pcl::PointCloud::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo( newCorners, newLeftFrame, - data.rightImage(), - data.fx(), - data.baseline(), - data.cx(), - data.cy(), + data.rightRaw(), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline(), + data.stereoCameraModel().left().cx(), + data.stereoCameraModel().left().cy(), Transform::getIdentity(), stereoWinSize_, stereoMaxLevel_, @@ -467,7 +477,7 @@ Transform OdometryOpticalFlow::computeTransform( if(pcl::isFinite(refCorners3DTmp->at(i)) && (this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth())) { - newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.localTransform()); + newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform()); newCornersFiltered[oi] = newCorners[i]; ++oi; } @@ -478,15 +488,22 @@ Transform OdometryOpticalFlow::computeTransform( // depth for(unsigned int i=0; igetMaxDepth() == 0.0f || pt.z < this->getMaxDepth())) { - newCorners3D->at(oi) = util3d::transformPoint(pt, data.localTransform()); + newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform()); newCornersFiltered[oi] = newCorners[i]; ++oi; } diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 4bccf25c..f74dd9be 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 66a1e643..3fff7b98 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 @@ -372,7 +372,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); @@ -792,7 +792,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(""); @@ -863,7 +866,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()); @@ -877,7 +880,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); @@ -885,7 +888,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) @@ -895,7 +898,7 @@ bool Rtabmap::process(const SensorData & data) _newMapOdomChangeDistance, mapId, lastPose.prettyPrint().c_str(), - data.pose().prettyPrint().c_str()); + odomPose.prettyPrint().c_str()); } } } @@ -908,16 +911,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; } @@ -929,6 +930,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); @@ -980,7 +982,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"); @@ -1023,13 +1025,13 @@ bool Rtabmap::process(const SensorData & data) Link tmp = signature->getLinks().begin()->second.inverse(); - // if the previous signature is a bad signature, remove it from the local graph + // if the previous node is an intermediate node, remove it from the local graph if(_constraints.size() && _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to()) { const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to()); UASSERT(s!=0); - if(s->isBadSignature()) + if(s->getWeight() == -1) { tmp = _constraints.rbegin()->second.merge(tmp); _optimizedPoses.erase(s->id()); @@ -1070,7 +1072,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", @@ -1470,17 +1472,21 @@ bool Rtabmap::process(const SensorData & data) { 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)) @@ -1638,16 +1644,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()); @@ -1683,7 +1686,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) @@ -1797,16 +1800,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()); @@ -1838,7 +1838,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) { @@ -1856,7 +1856,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 @@ -1927,7 +1927,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; @@ -1967,6 +1967,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(); @@ -2015,7 +2016,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 } } } @@ -2085,44 +2086,6 @@ bool Rtabmap::process(const SensorData & data) 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); @@ -2146,11 +2109,6 @@ bool Rtabmap::process(const SensorData & data) //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 @@ -2178,6 +2136,12 @@ bool Rtabmap::process(const SensorData & data) ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); } + Signature lastSignatureData(signature->id()); + if(_publishLastSignatureData) + { + 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(); @@ -2206,11 +2170,13 @@ bool Rtabmap::process(const SensorData & data) _memory->deleteLocation(signature->id()); } - timeMemoryCleanup = timer.ticks(); - ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); - // Pass this point signature should not be used, since it could have been transferred... signature = 0; + + timeMemoryCleanup = timer.ticks(); + ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); + + //============================================================ // TRANSFER @@ -2275,6 +2241,7 @@ bool Rtabmap::process(const SensorData & data) //============================================================== // Finalize statistics and log files //============================================================== + int localGraphSize = 0; if(_publishStats) { statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000); @@ -2293,36 +2260,49 @@ bool Rtabmap::process(const SensorData & data) // 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()); + localGraphSize = poses.size(); } //Start trashing @@ -2359,7 +2339,7 @@ bool Rtabmap::process(const SensorData & data) timeLocalTimeDetection, timeLocalSpaceDetection, timeMapOptimization); - 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(), @@ -2374,9 +2354,11 @@ bool Rtabmap::process(const SensorData & data) lcHypothesisReactivated, refUniqueWordsCount, retrievalId, - 0.0f, + 0, rehearsalMaxId, - rehearsalMaxId>0?1:0); + rehearsalMaxId>0?1:0, + localGraphSize, + data.id()); if(_statisticLogsBufferedInRAM) { _bufferedLogsF.push_back(logF); @@ -2403,7 +2385,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 @@ -2838,13 +2820,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 { @@ -2870,22 +2849,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 @@ -2900,11 +2863,24 @@ 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, + odomPose, + userData, + data))); } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1)) @@ -2920,13 +2896,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 posesConstraintsOnly) + bool global, + std::map * signatures) { if(_memory && _memory->getLastWorkingSignature()) { @@ -2948,8 +2920,8 @@ void Rtabmap::getGraph( std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - - if(!posesConstraintsOnly) + + if(signatures) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { @@ -2958,12 +2930,16 @@ void Rtabmap::getGraph( int mapId = -1; std::string label; double stamp = 0; - std::vector userData; + std::vector userData; _memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global); - 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)); + signatures->insert(std::make_pair(iter->first, + Signature(iter->first, + mapId, + weight, + stamp, + label, + odomPose, + userData))); } } } @@ -3115,12 +3091,8 @@ bool Rtabmap::computePath(int targetNode, bool global) UTimer totalTimer; 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, true); + 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)) @@ -3153,8 +3125,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global) std::map mapIds; std::map stamps; std::map labels; - std::map > userDatas; - this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global, true); + std::map > userDatas; + 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); @@ -3306,7 +3278,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 80fe155a..b16b1ecb 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -130,20 +130,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::publishGraph(bool optimized, bool full) const @@ -158,20 +151,14 @@ void RtabmapThread::publishGraph(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)); } @@ -314,16 +301,16 @@ 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) { UDEBUG("OdometryEvent"); OdometryEvent * e = (OdometryEvent*)event; - if(e->isValid()) + if(!e->pose().isNull()) { - this->addData(e->data()); + this->addData(*e); } else { @@ -522,12 +509,12 @@ void RtabmapThread::handleEvent(UEvent* event) //============================================================ void RtabmapThread::process() { - SensorData data; + OdometryEvent data; if(_state.empty() && getData(data)) { 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()); @@ -542,16 +529,10 @@ void RtabmapThread::process() } } -void RtabmapThread::addData(const SensorData & sensorData) +void RtabmapThread::addData(const OdometryEvent & odomEvent) { if(!_paused) { - if(!sensorData.isValid()) - { - ULOGGER_ERROR("data not valid !?"); - return; - } - bool ignoreFrame = false; if(_rate>0.0f) { @@ -559,9 +540,8 @@ void RtabmapThread::addData(const SensorData & sensorData) { ignoreFrame = true; } - } - 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); @@ -578,48 +558,45 @@ 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(); { + if(_rotVariance <= 0) + { + _rotVariance = 1.0; + } + if(_transVariance <= 0) + { + _transVariance = 1.0; + } 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); + odomEvent.data().id(), + odomEvent.data().stamp(), + odomEvent.data().userData()); + _dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance)); } else { - _dataBuffer.push_back(sensorData); + _dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance)); } - if(_rotVariance <= 0) - { - _rotVariance = 1.0f; - } - if(_transVariance <= 0) - { - _transVariance = 1.0f; - } - _dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance); + UDEBUG("Added data %d", odomEvent.data().id()); + _rotVariance = 0; _transVariance = 0; while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize) @@ -638,7 +615,7 @@ void RtabmapThread::addData(const SensorData & sensorData) } } -bool RtabmapThread::getData(SensorData & image) +bool RtabmapThread::getData(OdometryEvent & data) { ULOGGER_DEBUG(""); @@ -651,7 +628,7 @@ bool RtabmapThread::getData(SensorData & image) { if(!_dataBuffer.empty()) { - image = _dataBuffer.front(); + data = _dataBuffer.front(); _dataBuffer.pop_front(); dataFilled = true; } diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 947b173a..b378de28 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -27,138 +27,430 @@ 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.type() == CV_32FC2) + { + _laserScanRaw = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = 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.type() == CV_32FC2) + { + _laserScanRaw = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = 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.type() == CV_32FC2) + { + _laserScanRaw = laserScan; + } + else if(!laserScan.empty()) + { + UASSERT(laserScan.type() == CV_8UC1); // Bytes + _laserScanCompressed = laserScan; + } +} + +void SensorData::uncompressData() +{ + 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) +{ + 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(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()); + } + } + } } } // namespace rtabmap diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index f6424e69..fa9a2689 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -39,17 +39,11 @@ namespace rtabmap Signature::Signature() : _id(0), // invalid id _mapId(-1), - _stamp(0.0), - _weight(-1), + _weight(0), _saved(false), _modified(true), _linksModified(true), - _enabled(false), - _fx(0.0f), - _fy(0.0f), - _cx(0.0f), - _cy(0.0f), - _laserScanMaxPts(0) + _enabled(false) { } @@ -59,19 +53,9 @@ Signature::Signature( int weight, double stamp, const std::string & label, - const std::multimap & words, - 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), @@ -81,20 +65,15 @@ Signature::Signature( _saved(false), _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) + _sensorData(sensorData) { + if(_sensorData.id() == 0) + { + _sensorData.setId(id); + } + UASSERT(_sensorData.id() == _id); } Signature::~Signature() @@ -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/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/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 39de9a91..d479aeaa 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -27,10 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include #include +#include #include namespace rtabmap @@ -494,6 +496,383 @@ 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(); + } + cloud = 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; +} + +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 b6e623a4..beea66fb 100644 --- a/corelib/src/util3d_features.cpp +++ b/corelib/src/util3d_features.cpp @@ -44,36 +44,49 @@ 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(pcl::isFinite(pt) && + !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 +97,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 +108,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; } @@ -120,7 +132,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( float baseline, float cx, float cy, - const Transform & transform, + Transform localTransform, int flowWinSize, int flowMaxLevel, int flowIterations, @@ -137,7 +149,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( baseline, cx, cy, - transform, + localTransform, flowWinSize, flowMaxLevel, flowIterations, @@ -153,7 +165,7 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( float baseline, float cx, float cy, - const Transform & transform, + Transform localTransform, int flowWinSize, int flowMaxLevel, int flowIterations, @@ -163,6 +175,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(fx > 0.0f && baseline > 0.0f); // Find features in the new left image std::vector status; @@ -198,14 +211,18 @@ pcl::PointCloud::Ptr generateKeypoints3DStereo( pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D( leftCorners[i], disparity, - cx, cy, fx, baseline); + cx, + cy, + fx, + baseline); if(pcl::isFinite(tmpPt)) { pt = tmpPt; - if(!transform.isNull() && !transform.isIdentity()) + if(!localTransform.isNull() && + !localTransform.isIdentity()) { - pt = util3d::transformPoint(pt, transform); + pt = util3d::transformPoint(pt, localTransform); } } } @@ -223,11 +240,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, @@ -237,6 +250,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) @@ -290,10 +304,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; @@ -313,7 +324,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(), @@ -336,7 +347,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()))); } } @@ -349,7 +360,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()) @@ -441,7 +452,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(), @@ -473,7 +484,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/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/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index 00b4d08d..6166fe18 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(uContains(stats.getSignatures(), iter->first)) { - Signature s = stats.getSignature(); - s.uncompressData(); // make sure data is uncompressed + Signature s = stats.getSignatures().at(iter->first); + 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/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/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 f8da2747..0509d474 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 @@ -125,7 +125,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 789d88bc..bcd93ae9 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 @@ -163,7 +163,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); @@ -196,7 +196,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); @@ -229,19 +229,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 d39f9f2d..976ea83c 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 4215fcf1..c4dec3b7 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" @@ -631,23 +630,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())); } } @@ -660,8 +657,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event) iter->second.from(), iter->second.to(), iter->second.transform(), - iter->second.rotVariance(), - iter->second.transVariance()); + iter->second.infMatrix()); } } @@ -760,6 +756,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) @@ -805,31 +804,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(); @@ -1115,8 +1130,8 @@ void DatabaseViewer::view3DMap() QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); if(ok) { - int decimation = item.toInt(); - double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok); + int decimation = item.toInt(); + 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()); @@ -1154,60 +1169,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(); } @@ -1239,8 +1229,8 @@ void DatabaseViewer::generate3DMap() QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok); if(ok) { - int decimation = item.toInt(); - double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok); + int decimation = item.toInt(); + 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_); @@ -1264,48 +1254,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(); } @@ -1552,19 +1518,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; @@ -1574,16 +1542,16 @@ 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); } @@ -1594,32 +1562,21 @@ void DatabaseViewer::update(int value, } // 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(); } @@ -1744,29 +1701,34 @@ void DatabaseViewer::update(int value, view->setSceneRect(rect); } } - + void DatabaseViewer::updateStereo() { if(ui_->horizontalSlider_A->maximum()) { int id = ids_.at(ui_->horizontalSlider_A->value()); - Signature data = memory_->getSignatureData(id, true); + SensorData data = memory_->getNodeData(id, true); updateStereo(&data); } } -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; @@ -1808,7 +1770,7 @@ void DatabaseViewer::updateStereo(const Signature * data) std::vector rightCorners; cv::calcOpticalFlowPyrLK( leftMono, - data->getDepthRaw(), + data->depthOrRightRaw(), leftCorners, rightCorners, status, @@ -1840,13 +1802,16 @@ 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 = pcl::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform().toEigen3f()); status[i] = 100; //blue - ++inliers; + ++inliers; cloud->at(oi++) = pt; } } @@ -1909,8 +1874,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() && @@ -2148,15 +2115,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) @@ -2164,51 +2131,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()) { @@ -2216,6 +2141,7 @@ void DatabaseViewer::updateConstraintView( } if(cloudTo->size()) { + cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan); } } @@ -2300,8 +2226,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()) { @@ -2413,51 +2339,30 @@ 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; 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))); @@ -2468,8 +2373,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; @@ -2804,9 +2709,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); @@ -2816,8 +2721,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()) { @@ -2844,9 +2749,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()) { @@ -2859,112 +2764,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!"); } } @@ -3068,8 +2921,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) { @@ -3188,8 +3041,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) { @@ -3207,8 +3060,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/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 64494aa8..b46eb2ef 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -417,7 +417,8 @@ void GraphViewer::updateGraph(const std::map & poses, if(wasEmpty) { - this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio); + QRectF rect = this->scene()->itemsBoundingRect(); + this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); } } diff --git a/guilib/src/LoopClosureViewer.cpp b/guilib/src/LoopClosureViewer.cpp index 1c671bd8..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" @@ -106,74 +105,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 +123,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 14595e06..9025b9f6 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" @@ -438,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(notifyNoMoreImages())); @@ -672,12 +670,13 @@ 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); } else { // we receive too many odometry events! just send without data - emit odometryReceived(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->info()); + OdometryEvent tmp(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->pose(), odomEvent->covariance(), odomEvent->info()); + emit odometryReceived(tmp); } } } @@ -701,14 +700,14 @@ 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; // Process Data - if(data.isValid()) + if(!odom.data().imageRaw().empty()) { - Transform pose = data.pose(); + Transform pose = odom.pose(); bool lost = false; bool lostStateChanged = false; @@ -722,11 +721,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); @@ -750,84 +749,96 @@ 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)) { - UERROR("Adding scanOdom to viewer failed!"); + pcl::PointCloud::Ptr cloud; + cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw()); + 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)); } - _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(!odom.pose().isNull()) + { + // update camera position + _ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); + } + _ui->widget_cloudViewer->update(); + 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); } } @@ -840,7 +851,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); } @@ -853,55 +864,55 @@ 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(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) + if(odom.info().type == 1 && odom.info().refCorners.size()) { - //draw lines - UASSERT(info.refCorners.size() == info.newCorners.size()); - std::set inliers(info.cornerInliers.begin(), info.cornerInliers.end()); - for(unsigned int i=0; iimageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown()) { - if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) + //draw lines + UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size()); + std::set inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end()); + for(unsigned int i=0; iimageView_odometry->setFeatureColor(i, Qt::green); // inliers - } - if(_ui->imageView_odometry->isLinesShown()) - { - _ui->imageView_odometry->addLine( - 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); + if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) + { + _ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers + } + if(_ui->imageView_odometry->isLinesShown()) + { + _ui->imageView_odometry->addLine( + odom.info().refCorners[i].x, + odom.info().refCorners[i].y, + odom.info().newCorners[i].x, + odom.info().newCorners[i].y, + inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); + } } } } } - 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(); @@ -914,74 +925,74 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap } //Process info - 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.timeEstimation > 0) + if(odom.info().timeEstimation > 0) { - _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)data.id(), (float)info.timeEstimation*1000.0f); + _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f); } - if(info.timeParticleFiltering > 0) + if(odom.info().timeParticleFiltering > 0) { - _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)data.id(), (float)info.timeParticleFiltering*1000.0f); + _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*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; - if(!info.transform.isNull()) + float x=0.0f,y,z, roll,pitch,yaw; + if(!odom.info().transform.isNull()) { - 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); + odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + _ui->statsToolBox->updateStat("Odometry/Tx/m", (float)odom.data().id(), x); + _ui->statsToolBox->updateStat("Odometry/Ty/m", (float)odom.data().id(), y); + _ui->statsToolBox->updateStat("Odometry/Tz/m", (float)odom.data().id(), z); + _ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)odom.data().id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI); } - if(!info.transformFiltered.isNull()) + if(!odom.info().transformFiltered.isNull()) { - 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); + odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + _ui->statsToolBox->updateStat("Odometry/Fx/m", (float)odom.data().id(), x); + _ui->statsToolBox->updateStat("Odometry/Fy/m", (float)odom.data().id(), y); + _ui->statsToolBox->updateStat("Odometry/Fz/m", (float)odom.data().id(), z); + _ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)odom.data().id(), roll*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI); + _ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI); } - if(info.interval > 0) + if(odom.info().interval > 0) { - _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("Odometry/Interval/ms", (float)odom.data().id(), odom.info().interval*1000.f); + _ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)odom.data().id(), x/odom.info().interval*3.6f); } - if(info.distanceTravelled > 0) + if(odom.info().distanceTravelled > 0) { - _ui->statsToolBox->updateStat("Odometry/Distance/m", (float)data.id(), info.distanceTravelled); + _ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled); } - _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; } @@ -994,8 +1005,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)); @@ -1012,12 +1030,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f); // update cache - Signature signature = stat.getSignature(); - signature.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); + } // For intermediate empty nodes, keep latest image shown - if(!signature.getImageRaw().empty() || signature.getWords().size()) + if(!signature.sensorData().imageRaw().empty() || signature.getWords().size()) { _ui->imageView_source->clear(); _ui->imageView_loopClosure->clear(); @@ -1098,7 +1120,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(); } } @@ -1108,10 +1130,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(); @@ -1198,10 +1220,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; @@ -1213,7 +1240,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(); @@ -1294,7 +1321,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 @@ -1304,7 +1331,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); @@ -1391,7 +1418,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)); } @@ -1427,7 +1454,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)); } @@ -1605,25 +1632,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()) { @@ -1758,10 +1779,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); @@ -1977,10 +1998,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); @@ -2000,7 +2023,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 @@ -3288,15 +3311,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; @@ -3305,7 +3328,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; @@ -3354,6 +3377,7 @@ void MainWindow::postProcessing() int loopClosuresAdded = 0; if(detectMoreLoopClosures) { + UDEBUG(""); Memory memory(parameters); if(reextractFeatures) { @@ -3426,13 +3450,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) { @@ -3502,6 +3528,7 @@ void MainWindow::postProcessing() if(refineNeighborLinks || refineLoopClosureLinks) { + UDEBUG(""); if(refineLoopClosureLinks) { _initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded); @@ -3556,83 +3583,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()); } @@ -4945,70 +4985,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, @@ -5031,23 +5007,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()) { @@ -5135,22 +5110,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 c74b81b5..4af699b0 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -61,8 +61,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f validDecimationValue_(1) { - qRegisterMetaType("rtabmap::SensorData"); - qRegisterMetaType("rtabmap::OdometryInfo"); + qRegisterMetaType("rtabmap::OdometryEvent"); imageView_->setImageDepthShown(false); imageView_->setMinimumSize(320, 240); @@ -147,15 +146,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; @@ -164,11 +163,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); @@ -181,16 +180,18 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap cloudView_->setBackgroundColor(Qt::black); } - timeLabel_->setText(QString("%1 s").arg(info.timeEstimation)); + timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation)); - 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); - - if(!data.depth().empty()) + UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality); + + if(!odom.data().depthRaw().empty()) { - 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(); } @@ -199,54 +200,28 @@ 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_); } } else - { - validDecimationValue_ = decimationSpin_->value(); + { + validDecimationValue_ = decimationSpin_->value(); } - // 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(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()); - } + pcl::PointCloud::Ptr cloud; + 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")) { @@ -259,10 +234,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 { @@ -271,18 +246,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; @@ -291,17 +266,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(); @@ -313,7 +288,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); } @@ -326,55 +301,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)); } } @@ -395,8 +370,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 0b7b9eaa..b52225a0 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/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 80914435..d7aeb0d9 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" 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