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

Added multi-camera feature

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:46:48 -04:00
parent e6923daf1c
commit c6d0d47b1c
51 changed files with 2833 additions and 2297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -43,16 +43,29 @@ public:
// D is the distortion coefficients 1x5 CV_64FC1
// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
CameraModel(const std::string & name, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P);
CameraModel(
const std::string & name,
const cv::Size & imageSize,
const cv::Mat & K,
const cv::Mat & D,
const cv::Mat & R,
const cv::Mat & P,
const Transform & localTransform = Transform::getIdentity());
// minimal
CameraModel(
double fx,
double fy,
double cx,
double cy,
const Transform & localTransform = Transform::getIdentity(),
double Tx = 0.0f);
virtual ~CameraModel() {}
bool isValid() const {return !K_.empty() &&
!D_.empty() &&
!R_.empty() &&
!P_.empty() &&
imageSize_.height &&
imageSize_.width &&
!name_.empty();}
!P_.empty();}
const std::string & name() const {return name_;}
@@ -67,6 +80,8 @@ public:
const cv::Mat & R() const {return R_;} //rectification matrix
const cv::Mat & P() const {return P_;} //projection matrix
const Transform & localTransform() const {return localTransform_;}
const cv::Size & imageSize() const {return imageSize_;}
int imageWidth() const {return imageSize_.width;}
int imageWeight() const {return imageSize_.height;}
@@ -74,6 +89,8 @@ public:
bool load(const std::string & filePath);
bool save(const std::string & filePath);
void scale(double scale);
// For depth images, your should use cv::INTER_NEAREST
cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
cv::Mat rectifyDepth(const cv::Mat & raw) const;
@@ -87,20 +104,23 @@ private:
cv::Mat P_;
cv::Mat mapX_;
cv::Mat mapY_;
Transform localTransform_;
};
class RTABMAP_EXP StereoCameraModel
{
public:
StereoCameraModel() {}
StereoCameraModel(const std::string & name,
StereoCameraModel(
const std::string & name,
const cv::Size & imageSize1,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) :
left_(name+"_left", imageSize1, K1, D1, R1, P1),
right_(name+"_right", imageSize2, K2, D2, R2, P2),
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
@@ -108,9 +128,21 @@ public:
F_(F)
{
}
//minimal
StereoCameraModel(
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-right_.fx())
{
}
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
const std::string & name() const {return name_;}
bool load(const std::string & directory, const std::string & cameraName);
@@ -123,7 +155,9 @@ public:
const cv::Mat & E() const {return E_;} //extrinsic essential matrix
const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
Transform transform() const;
void scale(double scale);
Transform stereoTransform() const;
const CameraModel & left() const {return left_;}
const CameraModel & right() const {return right_;}

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

@@ -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 <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
@@ -95,8 +96,7 @@ public:
// Specific queries...
void loadNodeData(std::list<Signature *> & 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<unsigned char> & userData) const;
void loadLinks(int signatureId, std::map<int, Link> & 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<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 & 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<unsigned char> & userData) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

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

@@ -34,7 +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 <rtabmap/core/OdometryEvent.h>
#include <opencv2/core/core.hpp>
@@ -59,7 +59,7 @@ public:
bool init(int startIndex=0);
void setFrameRate(float frameRate);
SensorData getNextData();
OdometryEvent getNextData();
protected:
virtual void mainLoopBegin();

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

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/core/core.hpp>
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<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
UASSERT(min > 0.0);
return 1.0/min;
}
double transVariance() const
{
double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(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<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(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<double>(0,0) = 1.0/transVariance;
infMatrix_.at<double>(1,1) = 1.0/transVariance;
infMatrix_.at<double>(2,2) = 1.0/transVariance;
infMatrix_.at<double>(3,3) = 1.0/rotVariance;
infMatrix_.at<double>(4,4) = 1.0/rotVariance;
infMatrix_.at<double>(5,5) = 1.0/rotVariance;
}
private:
@@ -81,8 +123,7 @@ private:
int to_;
Transform transform_;
Type type_;
float rotVariance_;
float transVariance_;
cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
};
}

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

@@ -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<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
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<int, int> getNeighborsId(
@@ -130,8 +136,8 @@ public:
std::vector<unsigned char> & 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<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
@@ -184,7 +190,7 @@ public:
private:
void preUpdate();
void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
void clear();
void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
@@ -202,6 +208,7 @@ private:
void copyData(const Signature * from, Signature * to);
Signature * createSignature(
const SensorData & data,
const Transform & pose,
Statistics * stats = 0);
//keypoint stuff

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

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define ODOMETRYEVENT_H_
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/OdometryInfo.h"
@@ -37,20 +39,64 @@ namespace rtabmap {
class OdometryEvent : public UEvent
{
public:
OdometryEvent() :
_covariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
OdometryEvent(
const SensorData & data, const OdometryInfo & info = OdometryInfo()) :
const SensorData & data,
const Transform & pose,
const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
const OdometryInfo & info = OdometryInfo()) :
_data(data),
_pose(pose),
_info(info)
{}
{
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
_covariance = covariance;
}
OdometryEvent(
const SensorData & data,
const Transform & pose,
double rotVariance = 1.0,
double transVariance = 1.0,
const OdometryInfo & info = OdometryInfo()) :
_data(data),
_pose(pose),
_covariance(cv::Mat::eye(6,6,CV_64FC1)),
_info(info)
{
UASSERT(uIsFinite(rotVariance) && rotVariance>0);
UASSERT(uIsFinite(transVariance) && transVariance>0);
_covariance.at<double>(0,0) = transVariance;
_covariance.at<double>(1,1) = transVariance;
_covariance.at<double>(2,2) = transVariance;
_covariance.at<double>(3,3) = rotVariance;
_covariance.at<double>(4,4) = rotVariance;
_covariance.at<double>(5,5) = rotVariance;
}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
bool isValid() const {return !_data.pose().isNull();}
bool isValid() const {return !_pose.isNull();}
SensorData & data() {return _data;}
const SensorData & data() const {return _data;}
const Transform & pose() const {return _pose;}
const cv::Mat & covariance() const {return _covariance;}
const OdometryInfo & info() const {return _info;}
double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}
private:
SensorData _data;
Transform _pose;
cv::Mat _covariance;
OdometryInfo _info;
};

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

@@ -66,7 +66,10 @@ public:
virtual ~Rtabmap();
bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
bool process(const SensorData & data); // for convenience
bool process(
const SensorData & data,
const Transform & odomPose,
const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
void init(const ParametersMap & parameters, const std::string & databasePath = "");
void init(const std::string & configFile = "", const std::string & databasePath = "");
@@ -115,20 +118,13 @@ public:
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global) const;
void getGraph(std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
bool optimized,
bool global);
bool global,
std::map<int, Signature> * signatures = 0);
void clearPath();
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, bool global);

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

@@ -148,19 +148,11 @@ public:
RtabmapEvent3DMap(
const std::map<int, Signature> & signatures,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds,
const std::map<int, double> & stamps,
const std::map<int, std::string> & labels,
const std::map<int, std::vector<unsigned char> > & userDatas) :
const std::multimap<int, Link> & constraints) :
UEvent(0),
_signatures(signatures),
_poses(poses),
_constraints(constraints),
_mapIds(mapIds),
_stamps(stamps),
_labels(labels),
_userDatas(userDatas)
_constraints(constraints)
{}
virtual ~RtabmapEvent3DMap() {}
@@ -168,10 +160,6 @@ public:
const std::map<int, Signature> & getSignatures() const {return _signatures;}
const std::map<int, Transform> & getPoses() const {return _poses;}
const std::multimap<int, Link> & getConstraints() const {return _constraints;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, double> & getStamps() const {return _stamps;}
const std::map<int, std::string> & getLabels() const {return _labels;}
const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
@@ -179,10 +167,6 @@ private:
std::map<int, Signature> _signatures;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;
std::map<int, int> _mapIds;
std::map<int, double> _stamps;
std::map<int, std::string> _labels;
std::map<int, std::vector<unsigned char> > _userDatas;
};
class RtabmapGlobalPathEvent : public UEvent

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

@@ -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 <stack>
@@ -90,8 +91,8 @@ private:
virtual void mainLoop();
virtual void mainLoopKill();
void process();
void addData(const SensorData & data);
void getData(SensorData & data);
void addData(const OdometryEvent & odomEvent);
void getData(OdometryEvent & data);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
void publishMap(bool optimized, bool full) const;
@@ -102,7 +103,7 @@ private:
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
std::list<SensorData> _dataBuffer;
std::list<OdometryEvent> _dataBuffer;
UMutex _dataMutex;
USemaphore _dataAdded;
int _dataBufferMaxSize;
@@ -112,8 +113,8 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
float _rotVariance;
float _transVariance;
double _rotVariance;
double _transVariance;
std::vector<unsigned char> _userData;
UMutex _userDataMutex;

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

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -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<unsigned char> & userData = std::vector<unsigned char>());
// 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<unsigned char> & userData = std::vector<unsigned char>());
// Appearance-only constructor
SensorData(
const cv::Mat & image,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// 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<unsigned char> & userData = std::vector<unsigned char>());
// 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<unsigned char> & userData = std::vector<unsigned char>());
// 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<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// Multi-cameras RGB-D constructor
SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// 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<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// Stereo constructor
SensorData(
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// 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<unsigned char> & userData = std::vector<unsigned char>());
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<CameraModel> & cameraModels() const {return _cameraModels;}
StereoCameraModel stereoCameraModel() const {return _stereoCameraModel;}
void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
const std::vector<unsigned char> & userData() const {return _userData;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
@@ -116,33 +180,28 @@ public:
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
const cv::Mat & descriptors() const {return _descriptors;}
void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
const std::vector<unsigned char> & 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<CameraModel> _cameraModels;
StereoCameraModel _stereoCameraModel;
// user data
std::vector<unsigned char> _userData;
// features
std::vector<cv::KeyPoint> _keypoints;
cv::Mat _descriptors;
// user data
std::vector<unsigned char> _userData;
};
}

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

@@ -61,15 +61,7 @@ public:
const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
const cv::Mat & laserScan = cv::Mat(),
const cv::Mat & image = cv::Mat(),
const cv::Mat & depth = cv::Mat(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const Transform & localTransform =Transform::getIdentity(),
int laserScanMaxPts = 0);
const SensorData & sensorData = SensorData());
virtual ~Signature();
/**
@@ -121,41 +113,17 @@ public:
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & 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<int, pcl::PointXYZ> & 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<int, pcl::PointXYZ> & 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<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const Transform & getPose() const {return _pose;}
cv::Mat getPoseCovariance() const;
SensorData & sensorData() {return _sensorData;}
const SensorData & sensorData() const {return _sensorData;}
private:
int _id;
@@ -173,24 +141,13 @@ private:
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
std::map<int, int> _wordsChanged; // <oldId, newId>
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<int, pcl::PointXYZ> _words3; // word <id, keypoint>
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

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

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
#include <opencv2/core/core.hpp>
#include <list>
@@ -103,6 +104,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
float fx, float baseline,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float voxelSize = 0.0f,
int samples = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
const SensorData & sensorData,
int decimation = 1,
float maxDepth = 0.0f,
float voxelSize = 0.0f,
int samples = 0);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,

30
corelib/include/rtabmap/core/util3d_features.h
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <list>
namespace rtabmap
@@ -46,30 +47,23 @@ namespace util3d
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
const Transform & transform);
const CameraModel & cameraModel);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform);
const StereoCameraModel & stereoCameraMode);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform = Transform::getIdentity(),
const StereoCameraModel & stereoCameraMode,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
@@ -78,11 +72,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,
const std::multimap<int, cv::KeyPoint> & previousKpts,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
const CameraModel & cameraModel,
Transform & cameraTransform,
int pnpIterations = 100,
float pnpReprojError = 8.0f,