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

Added CalibrationDialog in Preferences->source for convenience. The database is also changed to handle new FX, FY, CX and CY intrinsic parameters instead of only depthConstant parameter. Added version of the database in a new table Admin. Updated to version 0.7.0.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1613 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-26 04:36:00 +00:00
parent 1557b3d44d
commit f090c81aaf
44 changed files with 1389 additions and 715 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/CameraEvent.h
View File

@@ -53,14 +53,14 @@ public:
UEvent(kCodeNoMoreImages)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, float depthConstant, const Transform & localTransform, int seq=0) :
CameraEvent(const cv::Mat & image, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
UEvent(kCodeImageDepth),
_image(image, depth, depthConstant, Transform(), localTransform, seq)
_image(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float depthConstant, const Transform & localTransform, int seq=0) :
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) :
UEvent(kCodeImageDepth),
_image(image, depth, depth2d, depthConstant, Transform(), localTransform, seq)
_image(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
{
}

55
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -63,18 +63,24 @@ class RTABMAP_EXP CameraRGBD
{
public:
virtual ~CameraRGBD();
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
virtual bool init() = 0;
//getters
float getImageRate() const {return _imageRate;}
const Transform & getLocalTransform() const {return _localTransform;}
float getFocalLength() const {return _focalLength;}
float getFx() const {return _fx;}
float getFy() const {return _fy;}
float getCx() const {return _cx;}
float getCy() const {return _cy;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void setFocalLength(float focalLength) {_focalLength = focalLength;}
void setFx(float fx) {_fx = fx;}
void setFy(float fy) {_fy = fy;}
void setCx(float cx) {_cx = cx;}
void setCy(float cy) {_cy = cy;}
protected:
/**
@@ -84,15 +90,21 @@ protected:
*/
CameraRGBD(float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float focalLength = 0.0f);
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;
private:
float _imageRate;
Transform _localTransform;
UTimer * _frameRateTimer;
float _focalLength;
float _fx;
float _fy;
float _cx;
float _cy;
};
/////////////////////////
@@ -109,7 +121,10 @@ public:
CameraOpenni(const std::string & deviceId="",
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float focalLength = 0.0f);
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraOpenni();
void image_cb (
@@ -120,7 +135,7 @@ public:
bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
private:
pcl::Grabber* interface_;
@@ -147,13 +162,16 @@ public:
CameraOpenNICV(bool asus = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float focalLength = 0.0f);
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraOpenNICV();
virtual bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
private:
bool _asus;
@@ -174,19 +192,23 @@ public:
public:
CameraOpenNI2(float imageRate = 0,
const Transform & localTransform = Transform::getIdentity(),
float focalLength = 0.0f);
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraOpenNI2();
virtual bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
private:
openni::Device * _device;
openni::VideoStream * _color;
openni::VideoStream * _depth;
float _depthFocal;
float _depthFx;
float _depthFy;
};
@@ -206,13 +228,16 @@ public:
CameraFreenect(int deviceId= 0,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float focalLength = 0.0f);
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraFreenect();
bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
private:
int deviceId_;

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

@@ -88,7 +88,7 @@ public:
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image) const;
void getPose(int signatureId, Transform & pose, int & mapId) const;
void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
@@ -127,7 +127,7 @@ private:
virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;

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

@@ -33,7 +33,7 @@ public:
bool init(int startIndex=0);
void setFrameRate(float frameRate);
void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & depthConstant, Transform & localTransform, Transform & pose);
void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, Transform & pose);
protected:
virtual void mainLoopBegin();

10
corelib/include/rtabmap/core/Features2d.h
View File

@@ -43,13 +43,19 @@ namespace rtabmap {
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
float maxDepth);
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
float maxDepth);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);

37
corelib/include/rtabmap/core/Image.h
View File

@@ -33,7 +33,10 @@ public:
_descriptors(descriptors),
_featureType(featureType),
_keypoints(keypoints),
_depthConstant(0.0f),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f),
_localTransform(Transform::getIdentity())
{
}
@@ -41,7 +44,10 @@ public:
// Metric constructor
Image(const cv::Mat & image,
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
@@ -49,7 +55,10 @@ public:
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depthConstant(depthConstant),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
@@ -59,7 +68,10 @@ public:
Image(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depth2d,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
@@ -68,7 +80,10 @@ public:
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depth2d(depth2d),
_depthConstant(depthConstant),
_fx(fx),
_fy(fy),
_cx(cx),
_cy(cy),
_pose(pose),
_localTransform(localTransform)
{
@@ -85,11 +100,14 @@ public:
void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}
const cv::Mat & depth() const {return _depth;}
const cv::Mat & depth2d() const {return _depth2d;}
float depthConstant() const {return _depthConstant;}
float depthFx() const {return _fx;}
float depthFy() const {return _fy;}
float depthCx() const {return _cx;}
float depthCy() const {return _cy;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
@@ -103,7 +121,10 @@ private:
// Metric stuff
cv::Mat _depth;
cv::Mat _depth2d;
float _depthConstant;
float _fx;
float _fy;
float _cx;
float _cy;
Transform _pose;
Transform _localTransform;
};

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

@@ -109,7 +109,10 @@ public:
int locationId, std::vector<unsigned char> & rgb,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & depthConstant,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform) const;
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
@@ -141,7 +144,10 @@ public:
void extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors);

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

@@ -100,7 +100,10 @@ public:
void get3DMap(std::map<int, std::vector<unsigned char> > & images,
std::map<int, std::vector<unsigned char> > & depths,
std::map<int, std::vector<unsigned char> > & depths2d,
std::map<int, float> & depthConstants,
std::map<int, float> & depthFxs,
std::map<int, float> & depthFys,
std::map<int, float> & depthCxs,
std::map<int, float> & depthCys,
std::map<int, Transform> & localTransforms,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,

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

@@ -147,7 +147,10 @@ public:
const std::map<int, std::vector<unsigned char> > & images,
const std::map<int, std::vector<unsigned char> > & depths,
const std::map<int, std::vector<unsigned char> > & depths2d,
const std::map<int, float> & depthConstants,
const std::map<int, float> & depthFxs,
const std::map<int, float> & depthFys,
const std::map<int, float> & depthCxs,
const std::map<int, float> & depthCys,
const std::map<int, Transform> & localTransforms,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints) :
@@ -155,7 +158,10 @@ public:
_images(images),
_depths(depths),
_depths2d(depths2d),
_depthConstants(depthConstants),
_depthFxs(depthFxs),
_depthFys(depthFys),
_depthCxs(depthCxs),
_depthCys(depthCys),
_localTransforms(localTransforms),
_poses(poses),
_constraints(constraints)
@@ -166,7 +172,10 @@ public:
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
const std::map<int, float> & getDepthFys() const {return _depthFys;}
const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
const std::map<int, float> & getDepthCys() const {return _depthCys;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Transform> & getPoses() const {return _poses;}
const std::multimap<int, Link> & getConstraints() const {return _constraints;}
@@ -177,7 +186,10 @@ private:
std::map<int, std::vector<unsigned char> > _images;
std::map<int, std::vector<unsigned char> > _depths;
std::map<int, std::vector<unsigned char> > _depths2d;
std::map<int, float> _depthConstants;
std::map<int, float> _depthFxs;
std::map<int, float> _depthFys;
std::map<int, float> _depthCxs;
std::map<int, float> _depthCys;
std::map<int, Transform> _localTransforms;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;

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

@@ -50,7 +50,10 @@ public:
const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(),
const std::vector<unsigned char> & image = std::vector<unsigned char>(),
const std::vector<unsigned char> & depth = std::vector<unsigned char>(),
float depthConstant = 0.0f,
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const Transform & localTransform =Transform::getIdentity());
virtual ~Signature();
@@ -106,14 +109,17 @@ public:
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepth(const std::vector<unsigned char> & depth, float depthConstant);
void setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy);
void setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;}
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 std::vector<unsigned char> & getDepth() const {return _depth;}
const std::vector<unsigned char> & getDepth2D() const {return _depth2D;}
float getDepthConstant() const {return _depthConstant;}
float getDepthFx() const {return _fx;}
float getDepthFy() const {return _fy;}
float getDepthCx() const {return _cx;}
float getDepthCy() const {return _cy;}
const Transform & getPose() const {return _pose;}
const Transform & getLocalTransform() const {return _localTransform;}
@@ -138,7 +144,10 @@ private:
std::vector<unsigned char> _depth; // compressed image CV_16UC1
std::vector<unsigned char> _depth2D; // compressed data CV_32FC2
float _depthConstant;
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>

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

@@ -114,7 +114,10 @@ public:
void setImages(const std::map<int, std::vector<unsigned char> > & images) {_images = images;}
void setDepths(const std::map<int, std::vector<unsigned char> > & depths) {_depths = depths;}
void setDepth2ds(const std::map<int, std::vector<unsigned char> > & depth2ds) {_depth2ds = depth2ds;}
void setDepthConstants(const std::map<int, float> & depthConstants) {_depthConstants = depthConstants;}
void setDepthFxs(const std::map<int, float> & fxs) {_depthFxs = fxs;}
void setDepthFys(const std::map<int, float> & fys) {_depthFys = fys;}
void setDepthCxs(const std::map<int, float> & cxs) {_depthCxs = cxs;}
void setDepthCys(const std::map<int, float> & cys) {_depthCys = cys;}
void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
@@ -139,7 +142,10 @@ public:
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepth2ds() const {return _depth2ds;}
const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
const std::map<int, float> & getDepthFys() const {return _depthFys;}
const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
const std::map<int, float> & getDepthCys() const {return _depthCys;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Transform> & poses() const {return _poses;}
@@ -170,7 +176,10 @@ private:
// Metric data
std::map<int, std::vector<unsigned char> > _depths;
std::map<int, std::vector<unsigned char> > _depth2ds;
std::map<int, float> _depthConstants;
std::map<int, float> _depthFxs;
std::map<int, float> _depthFys;
std::map<int, float> _depthCxs;
std::map<int, float> _depthCys;
std::map<int, Transform> _localTransforms;
std::map<int, Transform> _poses;

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

@@ -84,18 +84,16 @@ cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
const Transform & transform);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
pcl::PointXYZ RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
int x, int y,
float depthConstant);
pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
int x, int y,
float cx, float cy,
@@ -150,21 +148,12 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float depthConstant,
int decimation);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
float depthConstant,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
@@ -290,7 +279,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float depthConstant,
float fx,
float fy,
float cx,
float cy,
int decimation,
double maxDepth,
float voxel,