i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added GPS class for convenience, database viewer can view GPS values and export to KML format

Browse Source
This commit is contained in:
matlabbe
2017-09-26 14:13:06 -04:00
parent 8759fda632
commit 9691a4f361
35 changed files with 1150 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -159,7 +159,7 @@ public:
void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -253,7 +253,7 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
corelib/include/rtabmap/core/GeodeticCoords.h
+60
View File
@@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
* https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
*/
/* +---------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| http://www.mrpt.org/ |
| |
| Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
| See: http://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See details in http://www.mrpt.org/License |
+---------------------------------------------------------------------------+ */
#ifndef GEODETICCOORDS_H_
#define GEODETICCOORDS_H_
@@ -52,12 +66,58 @@ public:
cv::Point3d toGeocentric_WGS84() const;
cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
void fromGeocentric_WGS84(const cv::Point3d& geocentric);
void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
private:
double latitude_; // deg
double longitude_; // deg
double altitude_; // m
};
class GPS
{
public:
GPS():
stamp_(0.0),
longitude_(0.0),
latitude_(0.0),
altitude_(0.0),
error_(0.0),
bearing_(0.0)
{}
GPS(const double & stamp,
const double & longitude,
const double & latitude,
const double & altitude,
const double & error,
const double & bearing):
stamp_(stamp),
longitude_(longitude),
latitude_(latitude),
altitude_(altitude),
error_(error),
bearing_(bearing)
{}
const double & stamp() const {return stamp_;}
const double & longitude() const {return longitude_;}
const double & latitude() const {return latitude_;}
const double & altitude() const {return altitude_;}
const double & error() const {return error_;}
const double & bearing() const {return bearing_;}
GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
private:
double stamp_; // in sec
double longitude_; // DD
double latitude_; // DD
double altitude_; // m
double error_; // m
double bearing_; // deg (North 0->360 clockwise)
};
}
#endif /* GEODETICCOORDS_H_ */
corelib/include/rtabmap/core/Graph.h
+6
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include <list>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/GeodeticCoords.h>
namespace rtabmap {
class Memory;
@@ -58,6 +59,11 @@ bool RTABMAP_EXP importPoses(
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
std::map<int, double> * stamps = 0); // optional for format 1
bool RTABMAP_EXP exportGPS(
const std::string & filePath,
const std::map<int, GPS> & gpsValues,
unsigned int rgba = 0xFFFFFFFF);
/**
* Compute translation and rotation errors for KITTI datasets.
* See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
corelib/include/rtabmap/core/Memory.h
+2 -1
View File
@@ -177,7 +177,7 @@ public:
double & stamp,
Transform & groundTruth,
std::vector<float> & velocity,
std::vector<double> & gps,
GPS & gps,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -301,6 +301,7 @@ private:
bool _memoryChanged; // False by default, become true only when Memory::update() is called.
bool _linksChanged; // False by default, become true when links are modified.
int _signaturesAdded;
GPS _gpsOrigin;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
corelib/include/rtabmap/core/OdometryORBSLAM2.h
-1
View File
@@ -53,7 +53,6 @@ private:
private:
#ifdef RTABMAP_ORB_SLAM2
ORBSLAM2System * orbslam2_;
ORB_SLAM2::System * system_;
bool firstFrame_;
#endif
Transform originLocalTransform_;
corelib/include/rtabmap/core/Optimizer.h
+5 -1
View File
@@ -75,6 +75,7 @@ public:
bool isCovarianceIgnored() const {return covarianceIgnored_;}
double epsilon() const {return epsilon_;}
bool isRobust() const {return robust_;}
bool priorsIgnored() const {return priorsIgnored_;}
// setters
void setIterations(int iterations) {iterations_ = iterations;}
@@ -82,6 +83,7 @@ public:
void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
void setEpsilon(double epsilon) {epsilon_ = epsilon;}
void setRobust(bool enabled) {robust_ = enabled;}
void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
virtual void parseParameters(const ParametersMap & parameters);
@@ -128,7 +130,8 @@ protected:
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust());
bool robust = Parameters::defaultOptimizerRobust(),
bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored());
Optimizer(const ParametersMap & parameters);
private:
@@ -137,6 +140,7 @@ private:
bool covarianceIgnored_;
double epsilon_;
bool robust_;
bool priorsIgnored_;
};
} /* namespace rtabmap */
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -362,6 +362,7 @@ class RTABMAP_EXP Parameters
#endif
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod 3=Eigen");
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
corelib/include/rtabmap/core/SensorData.h
+5 -10
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/LaserScanInfo.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -225,17 +226,11 @@ public:
const Transform & globalPose() const {return globalPose_;}
const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
void setGPS(double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing)
void setGPS(const GPS & gps)
{
gps_ = std::vector<double>(6,0.0);
gps_[0]=stamp;
gps_[1]=longitude;
gps_[2]=latitude;
gps_[3]=altitude;
gps_[4]=accuracy;
gps_[5]=bearing;
gps_ = gps;
}
const std::vector<double> & gps() const {return gps_;}
const GPS & gps() const {return gps_;}
long getMemoryUsed() const; // Return memory usage in Bytes
@@ -278,7 +273,7 @@ private:
Transform globalPose_;
cv::Mat globalPoseCovariance_; // 6x6 double
std::vector<double> gps_;
GPS gps_;
};
}