Updated version to 0.8.0

Libraries are installed in lib directly with symbolic links, not in lib/rtabmap-0.8. Removed the need of RPATH in cmake.
Saving variance of each link in database (new field Link.variance). The variance is used to generate the constraint information matrices for TORO optimization.
ICP: computing variance instead of fitness.
ICP3: added correspondences ratio parameter
Added OdometryInfo class
Refactoring: renamed depth2d stuff to laserScan. rtabmap::Memory and rtabmap::Signature classes (no more distinct neighbor, loop closure or child loop closure links, only links with different types)

CMakeLists.txt

@@ -15,8 +15,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 7)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_MINOR_VERSION 8)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

bin/.gitignore

@@ -19,3 +19,9 @@
 /librtabmap_cored.so
 /librtabmap_guid.so
 /librtabmap_utilited.so
+/librtabmap_core.so.0.8
+/librtabmap_core.so.0.8.0
+/librtabmap_gui.so.0.8
+/librtabmap_gui.so.0.8.0
+/librtabmap_utilite.so.0.8
+/librtabmap_utilite.so.0.8.0

corelib/include/rtabmap/core/CameraEvent.h

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

corelib/include/rtabmap/core/DBDriver.h

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

corelib/include/rtabmap/core/DBReader.h

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

corelib/include/rtabmap/core/Link.h

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

corelib/include/rtabmap/core/Memory.h

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

corelib/include/rtabmap/core/Odometry.h

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

corelib/include/rtabmap/core/OdometryEvent.h

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

corelib/include/rtabmap/core/OdometryInfo.h

@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the Universite de Sherbrooke nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYINFO_H_
+#define ODOMETRYINFO_H_
+
+namespace rtabmap {
+
+class OdometryInfo
+{
+public:
+ OdometryInfo() :
+ lost(true),
+ matches(-1),
+ inliers(-1),
+ variance(-1),
+ features(-1),
+ localMapSize(-1),
+ time(0.0f)
+ {}
+ bool lost;
+ int matches;
+ int inliers;
+ float variance;
+ int features;
+ int localMapSize;
+ float time;
+};
+
+}
+
+#endif /* ODOMETRYINFO_H_ */