0.18: Camera calibration and LaserScan Info refactoring (#324)

* Saving full camera calibration in database, added angle min/max/inc to LaserScan.

* Updated laserscan info save/load in db

* Database: added Tag table, added env_sensors field to Node

* fixed serialization/deserialization of stereo camera model

* fixed multi-calibration db saving

* fixed rebase errors

* Tango: Added saving environmental sensors option

* Memory: Save env sensors

* Tango: fixed env sensor ids

* DBViewer: show env sensors values

* DBViewer: added calibration details on tooltip

* increased package version to 0.18.0

* Fixed LaserScan copies when angleIncrement is valid

* fixed build error without OctoMap dependency
This commit is contained in:
matlabbe
2018-10-23 14:35:14 -04:00
committed by GitHub
parent 8701ae6de0
commit 8e99291e13
51 changed files with 2063 additions and 491 deletions

View File

@@ -115,6 +115,9 @@ public:
bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory) const;
std::vector<unsigned char> serialize() const;
unsigned int deserialize(const std::vector<unsigned char>& data);
unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
CameraModel scaled(double scale) const;
CameraModel roi(const cv::Rect & roi) const;

View File

@@ -165,8 +165,9 @@ 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, LaserScan & info) 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;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void loadTags(int signatureId, std::map<int, TransformStamped> & tags) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
@@ -259,11 +260,12 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const = 0;
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, LaserScan & 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, GPS & 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, EnvSensors & sensors) 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;

View File

@@ -131,11 +131,12 @@ protected:
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
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;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -151,18 +152,17 @@ private:
std::string queryStepSensorData() const;
std::string queryStepLinkUpdate() const;
std::string queryStepLink() const;
std::string queryStepTag() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepOccupancyGridUpdate() const;
void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
void stepImage(
sqlite3_stmt * ppStmt,
int id,
const cv::Mat & imageBytes) const;
void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepTag(sqlite3_stmt * ppStmt, int nodeId, int tagId, const TransformStamped & pose) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -175,6 +175,7 @@ private:
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
void loadTagsQuery(std::list<Signature *> & signatures) const;
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
protected:

View File

@@ -0,0 +1,85 @@
/*
Copyright (c) 2010-2018, 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 CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
namespace rtabmap {
class EnvSensor
{
public:
enum Type {
// built-in types
kUndefined = 0,
kWifiSignalStrength, // dBm
kAmbientTemperature, // Celcius
kAmbientAirPressure, // hPa
kAmbientLight, // lx
kAmbientRelativeHumidity, // %
// user types
kCustomSensor1 = 100,
kCustomSensor2,
kCustomSensor3,
kCustomSensor4,
kCustomSensor5,
kCustomSensor6,
kCustomSensor7,
kCustomSensor8,
kCustomSensor9
};
public:
EnvSensor() :
type_(kUndefined),
value_(0.0),
stamp_(0.0)
{}
EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
type_(type),
value_(value),
stamp_(stamp)
{}
virtual ~EnvSensor() {}
const Type & type() const {return type_;}
const double & value() const {return value_;}
const double & stamp() const {return stamp_;}
private:
Type type_;
double value_;
double stamp_;
};
typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

View File

@@ -0,0 +1,78 @@
/*
Copyright (c) 2010-2018, 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 CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
#include <rtabmap/core/GeodeticCoords.h>
namespace rtabmap {
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 /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

View File

@@ -81,47 +81,6 @@ private:
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_ */

View File

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <list>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <rtabmap/core/GPS.h>
namespace rtabmap {
class Memory;

View File

@@ -54,16 +54,44 @@ public:
static bool isScanHasNormals(const Format & format);
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints = 0,
int maxRange = 0,
const Transform & localTransform = Transform::getIdentity());
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleInc,
const Transform & localTransform = Transform::getIdentity());
public:
LaserScan();
LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
LaserScan(const cv::Mat & data,
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform = Transform::getIdentity());
LaserScan(const cv::Mat & data,
Format format,
float minRange,
float maxRange,
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform = Transform::getIdentity());
const cv::Mat & data() const {return data_;}
int maxPoints() const {return maxPoints_;}
float maxRange() const {return maxRange_;}
Format format() const {return format_;}
int maxPoints() const {return maxPoints_;}
float rangeMin() const {return rangeMin_;}
float rangeMax() const {return rangeMax_;}
float angleMin() const {return angleMin_;}
float angleMax() const {return angleMax_;}
float angleIncrement() const {return angleIncrement_;}
Transform localTransform() const {return localTransform_;}
bool isEmpty() const {return data_.empty();}
@@ -74,7 +102,7 @@ public:
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
LaserScan clone() const;
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
@@ -84,9 +112,13 @@ public:
private:
cv::Mat data_;
int maxPoints_;
float maxRange_;
Format format_;
int maxPoints_;
float rangeMin_;
float rangeMax_;
float angleMin_;
float angleMax_;
float angleIncrement_;
Transform localTransform_;
};

View File

@@ -186,6 +186,7 @@ public:
Transform & groundTruth,
std::vector<float> & velocity,
GPS & gps,
EnvSensors & sensors,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int nodeId, bool uncompressedData = false) const;

View File

@@ -33,11 +33,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/LaserScan.h>
#include <rtabmap/core/IMU.h>
#include <rtabmap/core/GPS.h>
#include <rtabmap/core/EnvSensor.h>
namespace rtabmap
{
@@ -235,18 +236,16 @@ public:
const Transform & globalPose() const {return globalPose_;}
const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
void setGPS(const GPS & gps)
{
gps_ = gps;
}
void setGPS(const GPS & gps) {gps_ = gps;}
const GPS & gps() const {return gps_;}
void setIMU(const IMU & imu)
{
imu_ = imu;
}
void setIMU(const IMU & imu) {imu_ = imu; }
const IMU & imu() const {return imu_;}
void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;}
void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
const EnvSensors & envSensors() const {return _envSensors;}
long getMemoryUsed() const; // Return memory usage in Bytes
void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
@@ -281,6 +280,12 @@ private:
float _cellSize;
cv::Point3f _viewPoint;
// environmental sensors
EnvSensors _envSensors;
// tags
std::map<int, TransformStamped> _tags;
// features
std::vector<cv::KeyPoint> _keypoints;
std::vector<cv::Point3f> _keypoints3D;

View File

@@ -90,6 +90,10 @@ public:
void removeLink(int idTo);
void removeVirtualLinks();
void setTags(const std::map<int, TransformStamped> & tags) {_tags = tags;}
void addTag(int id, const TransformStamped & pose) {_tags.insert(std::make_pair(id, pose));}
const std::map<int, TransformStamped> & getTags() const {return _tags;}
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -141,6 +145,7 @@ private:
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
std::map<int, TransformStamped> _tags;
int _weight;
std::string _label;
bool _saved; // If it's saved to bd

View File

@@ -97,6 +97,9 @@ public:
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
bool saveStereoTransform(const std::string & directory) const;
std::vector<unsigned char> serialize() const;
unsigned int deserialize(const std::vector<unsigned char>& data);
unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}

View File

@@ -155,6 +155,21 @@ private:
RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
class TransformStamped
{
public:
TransformStamped(const Transform & transform, const double & stamp) :
transform_(transform),
stamp_(stamp)
{}
const Transform & transform() const {return transform_;}
const double & stamp() const {return stamp_;}
private:
Transform transform_;
double stamp_;
};
}
#endif /* TRANSFORM_H_ */