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

Moved some util3d methods to Transform.h, Graph.h and Compression.h

Browse Source 
Added computePath() method implementating A star on graph
GUI: SetWindowModified() and user should explicitly save the GUI config to keep them
Calibration: added mirror checkbox, added device id argument
This commit is contained in:
Mathieu Labbe
2015-01-23 11:17:42 -05:00
parent fdf7f69783
commit 7a6bf630ca
34 changed files with 1817 additions and 1282 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
corelib/include/rtabmap/core/Compression.h Normal file
View File

@@ -0,0 +1,87 @@
/*
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 COMPRESSION_H_
#define COMPRESSION_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/utilite/UThread.h>
#include <opencv2/opencv.hpp>
namespace rtabmap {
/**
* Compress image or data
*
* Example compression:
* cv::Mat image;// an image
* CompressionThread ct(image);
* ct.start();
* ct.join();
* std::vector<unsigned char> bytes = ct.getCompressedData();
*
* Example uncompression
* std::vector<unsigned char> bytes;// a compressed image
* CompressionThread ct(bytes);
* ct.start();
* ct.join();
* cv::Mat image = ct.getUncompressedData();
*/
class RTABMAP_EXP CompressionThread : public UThread
{
public:
// format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const cv::Mat & bytes, bool isImage);
const cv::Mat & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;}
protected:
virtual void mainLoop();
private:
cv::Mat compressedData_;
cv::Mat uncompressedData_;
std::string format_;
bool image_;
bool compressMode_;
};
std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
} /* namespace rtabmap */
#endif /* COMPRESSION_H_ */

96
corelib/include/rtabmap/core/Graph.h Normal file
View File

@@ -0,0 +1,96 @@
/*
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 GRAPH_H_
#define GRAPH_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <map>
#include <list>
#include <rtabmap/core/Link.h>
namespace rtabmap {
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
// <int, depth> depth=0 means infinite depth
std::map<int, int> RTABMAP_EXP generateDepthGraph(
const std::multimap<int, Link> & links,
int fromId,
int depth = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
bool RTABMAP_EXP saveTOROGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,
float angle,
bool keepLatest = true);
std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
const std::map<int, Transform> & poses,
float radius,
float angle);
std::vector<int> RTABMAP_EXP computePath(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, int> & links,
int from,
int to);
} /* namespace rtabmap */
#endif /* GRAPH_H_ */

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

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace rtabmap {
@@ -89,6 +91,8 @@ public:
void getTranslation(float & x, float & y, float & z) const;
float getNorm() const;
float getNormSquared() const;
float getDistance(const Transform & t) const;
float getDistanceSquared(const Transform & t) const;
std::string prettyPrint() const;
Transform operator*(const Transform & t) const;
@@ -96,10 +100,17 @@ public:
bool operator==(const Transform & t) const;
bool operator!=(const Transform & t) const;
static Transform getIdentity()
{
return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
}
Eigen::Matrix4f toEigen4f() const;
Eigen::Matrix4d toEigen4d() const;
Eigen::Affine3f toEigen3f() const;
Eigen::Affine3d toEigen3d() const;
public:
static Transform getIdentity();
static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
static Transform fromEigen3f(const Eigen::Affine3f & matrix);
static Transform fromEigen3d(const Eigen::Affine3d & matrix);
private:
std::vector<float> data_;

4
corelib/include/rtabmap/core/impl/util3d.hpp
View File

@@ -127,7 +127,7 @@ typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
PointCloudPtr output(new PointCloud);
pcl::transformPointCloud<PointT>(*cloud, *output, transformToEigen4f(transform));
pcl::transformPointCloud<PointT>(*cloud, *output, transform.toEigen4f());
return output;
}
@@ -136,7 +136,7 @@ PointT transformPoint(
const PointT & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transformToEigen3f(transform));
return pcl::transformPoint(pt, transform.toEigen3f());
}
template<typename PointT>

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

@@ -49,41 +49,6 @@ namespace rtabmap
namespace util3d
{
/**
* Compress image or data
*
* Example compression:
* cv::Mat image;// an image
* CompressionThread ct(image);
* ct.start();
* ct.join();
* std::vector<unsigned char> bytes = ct.getCompressedData();
*
* Example uncompression
* std::vector<unsigned char> bytes;// a compressed image
* CompressionThread ct(bytes);
* ct.start();
* ct.join();
* cv::Mat image = ct.getUncompressedData();
*/
class RTABMAP_EXP CompressionThread : public UThread
{
public:
// format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const cv::Mat & bytes, bool isImage);
const cv::Mat & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;}
protected:
virtual void mainLoop();
private:
cv::Mat compressedData_;
cv::Mat uncompressedData_;
std::string format_;
bool image_;
bool compressMode_;
};
cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
cv::Mat RTABMAP_EXP depthFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
@@ -235,19 +200,6 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
// remove depth by z axis
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
@@ -374,61 +326,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
int samples,
const Transform & transform = Transform::getIdentity());
inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
{
Eigen::Matrix4f m;
m << transform[0], transform[1], transform[2], transform[3],
transform[4], transform[5], transform[6], transform[7],
transform[8], transform[9], transform[10], transform[11],
0,0,0,1;
return m;
}
inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
{
Eigen::Matrix4d m;
m << transform[0], transform[1], transform[2], transform[3],
transform[4], transform[5], transform[6], transform[7],
transform[8], transform[9], transform[10], transform[11],
0,0,0,1;
return m;
}
inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
{
return Eigen::Affine3f(transformToEigen4f(transform));
}
inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
{
return Eigen::Affine3d(transformToEigen4d(transform));
}
inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
@@ -449,56 +346,6 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
float gp3MaximumAngle = 2*M_PI/3,
bool gp3NormalConsistency = false);
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
// <int, depth> depth=0 means infinite depth
std::map<int, int> RTABMAP_EXP generateDepthGraph(
const std::multimap<int, Link> & links,
int fromId,
int depth = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
bool RTABMAP_EXP saveTOROGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,
float angle,
bool keepLatest = true);
std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
const std::map<int, Transform> & poses,
float radius,
float angle);
bool RTABMAP_EXP occupancy2DFromCloud3D(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
cv::Mat & ground,