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

merged multicamera branch into devel branch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-06-16 17:41:51 -04:00
parent 6a7a9fb9b0 039ae45d57
commit 50eeda5b95
61 changed files with 3383 additions and 2918 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*-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_;}

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();

79
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;
}
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
};
}

19
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(
@@ -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<int, float> getNeighborsIdRadius(
int signatureId,
@@ -131,8 +137,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;}
@@ -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<int> * 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

57
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,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<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;
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<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(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<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;
};

19
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 = "");
@@ -117,21 +120,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 posesConstraintsOnly = false);
bool global,
std::map<int, Signature> * 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

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

@@ -150,19 +150,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() {}
@@ -170,10 +162,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");}
@@ -181,10 +169,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>
@@ -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> _state;
std::stack<ParametersMap> _stateParam;
std::list<SensorData> _dataBuffer;
std::list<OdometryEvent> _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<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;}
const 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;
};
}

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

@@ -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<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3,
int mapId = -1,
int weight = 0,
double stamp = 0.0,
const std::string & label = std::string(),
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 +111,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 +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<int, cv::KeyPoint> _words; // word <id, keypoint>
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
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

21
corelib/include/rtabmap/core/Statistics.h
View File

@@ -136,11 +136,7 @@ public:
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
void setSignature(const Signature & s) {_signature = s;}
void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -159,11 +155,7 @@ public:
int loopClosureId() const {return _loopClosureId;}
int localLoopClosureId() const {return _localLoopClosureId;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, std::string> & getLabels() const {return _labels;}
const std::map<int, double> & getStamps() const {return _stamps;}
const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
const Signature & getSignature() const {return _signature;}
const std::map<int, Signature> & getSignatures() const {return _signatures;}
const std::map<int, Transform> & poses() const {return _poses;}
const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -185,14 +177,7 @@ private:
int _loopClosureId;
int _localLoopClosureId;
// extended data start here...
std::map<int, int> _mapIds;
std::map<int, std::string> _labels;
std::map<int, double> _stamps;
std::map<int, std::vector<unsigned char> > _userDatas;
// Signature data
Signature _signature;
std::map<int, Signature> _signatures;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;

51
corelib/include/rtabmap/core/Transform.h
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
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<float> data_;
cv::Mat data_;
};
RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);

33
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,38 @@ 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::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,

57
corelib/include/rtabmap/core/util3d_conversions.h
View File

@@ -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 <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/Transform.h>
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<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_CONVERSIONS_H_ */

28
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,20 +47,17 @@ 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,
@@ -69,7 +67,7 @@ pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::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<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,