mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 09:07:47 +08:00
Merge branch 'master' of github.com:introlab/rtabmap into gtest
This commit is contained in:
matlabbe
@@ -38,7 +38,7 @@ class IMUFilter;
|
||||
|
||||
/**
|
||||
* Class Camera
|
||||
*
|
||||
*
|
||||
*/
|
||||
class RTABMAP_CORE_EXPORT Camera : public SensorCapture
|
||||
{
|
||||
@@ -48,7 +48,7 @@ public:
|
||||
SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
|
||||
float getImageRate() const {return getFrameRate();}
|
||||
void setImageRate(float imageRate) {setFrameRate(imageRate);}
|
||||
void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
|
||||
void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0, bool baseFrameConversion = false); // Take ownership of filter
|
||||
bool isInterIMUPublishing() const {return publishInterIMU_;}
|
||||
|
||||
bool initFromFile(const std::string & calibrationPath);
|
||||
@@ -73,6 +73,7 @@ private:
|
||||
private:
|
||||
IMUFilter * imuFilter_;
|
||||
bool publishInterIMU_;
|
||||
bool imuBaseFrameConversion_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -38,3 +38,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/core/camera/CameraRGBDImages.h>
|
||||
#include <rtabmap/core/camera/CameraK4A.h>
|
||||
#include <rtabmap/core/camera/CameraSeerSense.h>
|
||||
#include <rtabmap/core/camera/CameraOrbbecSDK.h>
|
||||
|
||||
@@ -129,6 +129,7 @@ public:
|
||||
std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
|
||||
#endif
|
||||
cv::Mat * textures = 0) const;
|
||||
void saveFlannIndex(const std::vector<unsigned char> & indexData) const;
|
||||
|
||||
public:
|
||||
// Mutex-protected methods of abstract versions below
|
||||
@@ -161,19 +162,20 @@ public:
|
||||
void executeNoResult(const std::string & sql) const;
|
||||
|
||||
// Load objects
|
||||
void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
|
||||
void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
|
||||
void load(VWDictionary & dictionary, bool lastStateOnly = true) const;
|
||||
void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
|
||||
Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
|
||||
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
|
||||
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage
|
||||
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
|
||||
|
||||
// Specific queries...
|
||||
void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
|
||||
void loadNodeData(Signature & signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
|
||||
void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
|
||||
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, std::vector<StereoCameraModel> & stereoModels) 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, EnvSensors & sensors) const;
|
||||
void getLocalFeatures(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
|
||||
void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
|
||||
void getWeight(int signatureId, int & weight) const;
|
||||
void getLastNodeIds(std::set<int> & ids) const;
|
||||
@@ -274,11 +276,12 @@ protected:
|
||||
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
|
||||
#endif
|
||||
cv::Mat * textures) const = 0;
|
||||
virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const = 0;
|
||||
|
||||
// Load objects
|
||||
virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
|
||||
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
|
||||
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
|
||||
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0;
|
||||
virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
|
||||
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
|
||||
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
|
||||
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
|
||||
|
||||
@@ -286,6 +289,7 @@ protected:
|
||||
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) 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, EnvSensors & sensors) const = 0;
|
||||
virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const = 0;
|
||||
virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
|
||||
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
|
||||
virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
|
||||
|
||||
@@ -135,10 +135,12 @@ protected:
|
||||
#endif
|
||||
cv::Mat * textures) const;
|
||||
|
||||
virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
|
||||
|
||||
// Load objects
|
||||
virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
|
||||
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
|
||||
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
|
||||
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const;
|
||||
virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
|
||||
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
|
||||
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
|
||||
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
|
||||
|
||||
@@ -146,6 +148,7 @@ protected:
|
||||
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) 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, EnvSensors & sensors) const;
|
||||
virtual void getLocalFeaturesQuery(int signatureId, std::multimap<int, int> & words, std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & points, cv::Mat & descriptors) const;
|
||||
virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
|
||||
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
|
||||
virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
|
||||
@@ -190,6 +193,8 @@ private:
|
||||
const cv::Point3f & viewpoint) const;
|
||||
|
||||
private:
|
||||
void loadWordsQuery(std::list<Signature *> & signatures) const;
|
||||
void loadWordIdsQuery(std::list<Signature *> & signatures) const;
|
||||
void loadLinksQuery(std::list<Signature *> & signatures) const;
|
||||
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
|
||||
|
||||
|
||||
@@ -104,6 +104,7 @@ namespace rtabmap {
|
||||
|
||||
class ORBextractor;
|
||||
class SPDetector;
|
||||
class SPDetectorRpautrat;
|
||||
|
||||
class Stereo;
|
||||
#if CV_MAJOR_VERSION < 3
|
||||
@@ -129,7 +130,8 @@ public:
|
||||
kFeatureSurfFreak=12, //new 0.20.4
|
||||
kFeatureGfttDaisy=13, //new 0.20.6
|
||||
kFeatureSurfDaisy=14, //new 0.20.6
|
||||
kFeaturePyDetector=15}; //new 0.20.8
|
||||
kFeaturePyDetector=15, //new 0.20.8
|
||||
kFeatureSuperPointRpautrat=16}; // new 0.23.3
|
||||
|
||||
static std::string typeName(Type type)
|
||||
{
|
||||
@@ -164,6 +166,8 @@ public:
|
||||
return "GFTT+Daisy";
|
||||
case kFeatureSurfDaisy:
|
||||
return "SURF+Daisy";
|
||||
case kFeatureSuperPointRpautrat:
|
||||
return "SUPERPOINT-RPAUTRAT";
|
||||
default:
|
||||
return "Unknown";
|
||||
}
|
||||
@@ -626,6 +630,31 @@ private:
|
||||
bool cuda_;
|
||||
};
|
||||
|
||||
//SuperPointRpautrat
|
||||
class RTABMAP_CORE_EXPORT SuperPointRpautrat : public Feature2D
|
||||
{
|
||||
public:
|
||||
SuperPointRpautrat(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SuperPointRpautrat();
|
||||
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual Feature2D::Type getType() const { return kFeatureSuperPointRpautrat; }
|
||||
|
||||
private:
|
||||
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
|
||||
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
cv::Ptr<SPDetectorRpautrat> superPoint_;
|
||||
|
||||
std::string superpointWeightsPath_;
|
||||
std::string superpointModelPath_;
|
||||
std::string outputDir_;
|
||||
float threshold_;
|
||||
bool nms_;
|
||||
int minDistance_;
|
||||
bool cuda_;
|
||||
};
|
||||
|
||||
//GFTT_DAISY
|
||||
class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
|
||||
{
|
||||
|
||||
@@ -37,37 +37,48 @@ namespace rtabmap {
|
||||
class RTABMAP_CORE_EXPORT FlannIndex
|
||||
{
|
||||
public:
|
||||
// A forward of the internal enum, indexes should match. See src/rtflann/defines.h
|
||||
enum flann_algorithm_t
|
||||
{
|
||||
FLANN_INDEX_LINEAR = 0,
|
||||
FLANN_INDEX_KDTREE = 1,
|
||||
FLANN_INDEX_KDTREE_SINGLE = 4,
|
||||
FLANN_INDEX_LSH = 6,
|
||||
};
|
||||
|
||||
FlannIndex();
|
||||
virtual ~FlannIndex();
|
||||
|
||||
void release();
|
||||
std::vector<unsigned char> serializeIndex(bool computeChecksum = true) const;
|
||||
|
||||
size_t indexedFeatures() const;
|
||||
|
||||
// return Bytes
|
||||
size_t memoryUsed() const;
|
||||
|
||||
// Note that useDistanceL1 doesn't have any effect if LSH is used
|
||||
void buildLinearIndex(
|
||||
void buildIndex(
|
||||
flann_algorithm_t algorithm,
|
||||
const cv::Mat & features,
|
||||
bool useDistanceL1 = false,
|
||||
float rebalancingFactor = 2.0f);
|
||||
void buildKDTreeIndex(
|
||||
const cv::Mat & features,
|
||||
int trees = 4,
|
||||
bool useDistanceL1 = false,
|
||||
float rebalancingFactor = 2.0f);
|
||||
void buildKDTreeSingleIndex(
|
||||
const cv::Mat & features,
|
||||
int leafMaxSize = 10,
|
||||
bool reorder = true,
|
||||
bool useDistanceL1 = false,
|
||||
float rebalancingFactor = 2.0f);
|
||||
void buildLSHIndex(
|
||||
const cv::Mat & features,
|
||||
unsigned int table_number = 12,
|
||||
unsigned int key_size = 20,
|
||||
unsigned int multi_probe_level = 2,
|
||||
float rebalancingFactor = 2.0f);
|
||||
// Return false if the indexData doesn't correspond to expected features used and parameters.
|
||||
bool loadIndex(
|
||||
const std::vector<unsigned char> & indexData,
|
||||
flann_algorithm_t algorithm,
|
||||
const cv::Mat & features,
|
||||
bool useDistanceL1 = false,
|
||||
float rebalancingFactor = 2.0f,
|
||||
std::string * errorMsg = NULL);
|
||||
bool loadIndex(
|
||||
const unsigned char * indexData,
|
||||
size_t indexDataSize,
|
||||
flann_algorithm_t algorithm,
|
||||
const cv::Mat & features,
|
||||
bool useDistanceL1 = false,
|
||||
float rebalancingFactor = 2.0f,
|
||||
std::string * errorMsg = NULL);
|
||||
|
||||
bool isBuilt();
|
||||
|
||||
@@ -104,9 +115,9 @@ private:
|
||||
unsigned int nextIndex_;
|
||||
int featuresType_;
|
||||
int featuresDim_;
|
||||
bool isLSH_;
|
||||
bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
|
||||
float rebalancingFactor_;
|
||||
flann_algorithm_t algorithm_;
|
||||
|
||||
// keep feature in memory until the tree is rebuilt
|
||||
// (in case the word is deleted when removed from the VWDictionary)
|
||||
|
||||
@@ -53,6 +53,7 @@ public:
|
||||
public:
|
||||
virtual ~GlobalMap();
|
||||
|
||||
bool fullUpdateNeeded(const std::map<int, Transform> & poses) const;
|
||||
bool update(const std::map<int, Transform> & poses); // return true if map has changed
|
||||
|
||||
virtual void clear();
|
||||
|
||||
@@ -56,7 +56,7 @@ bool RTABMAP_CORE_EXPORT exportPoses(
|
||||
|
||||
bool RTABMAP_CORE_EXPORT importPoses(
|
||||
const std::string & filePath,
|
||||
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
|
||||
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
|
||||
std::map<int, double> * stamps = 0); // optional for format 1 and 9
|
||||
|
||||
@@ -264,6 +264,7 @@ private:
|
||||
void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
|
||||
void clear();
|
||||
void loadDataFromDb(bool postInitClosingEvents);
|
||||
void saveFlannIndex(bool postInitClosingEvents);
|
||||
void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
|
||||
|
||||
void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
|
||||
@@ -299,6 +300,7 @@ private:
|
||||
float _similarityThreshold;
|
||||
bool _binDataKept;
|
||||
bool _rawDescriptorsKept;
|
||||
bool _loadVisualLocalFeaturesOnInit;
|
||||
bool _saveDepth16Format;
|
||||
bool _notLinkedNodesKeptInDb;
|
||||
bool _saveIntermediateNodeData;
|
||||
@@ -306,6 +308,7 @@ private:
|
||||
std::string _depthCompressionFormat;
|
||||
bool _incrementalMemory;
|
||||
bool _localizationDataSaved;
|
||||
bool _flannIndexSaved;
|
||||
bool _reduceGraph;
|
||||
int _maxStMemSize;
|
||||
float _recentWmRatio;
|
||||
@@ -353,6 +356,7 @@ private:
|
||||
bool _linksChanged; // False by default, become true when links are modified.
|
||||
int _signaturesAdded;
|
||||
bool _allNodesInWM;
|
||||
bool _receivingOdometryFeatures;
|
||||
GPS _gpsOrigin;
|
||||
std::vector<CameraModel> _rectCameraModels;
|
||||
std::vector<StereoCameraModel> _rectStereoCameraModels;
|
||||
|
||||
@@ -53,10 +53,11 @@ public:
|
||||
kTypeOkvis = 6,
|
||||
kTypeLOAM = 7,
|
||||
kTypeMSCKF = 8,
|
||||
kTypeVINS = 9,
|
||||
kTypeVINSFusion = 9,
|
||||
kTypeOpenVINS = 10,
|
||||
kTypeFLOAM = 11,
|
||||
kTypeOpen3D = 12
|
||||
kTypeOpen3D = 12,
|
||||
kTypeCuVSLAM = 13
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#define ODOMETRYTHREAD_H_
|
||||
|
||||
#include <rtabmap/core/rtabmap_core_export.h>
|
||||
#include <rtabmap/core/SensorEvent.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <rtabmap/utilite/UEventsHandler.h>
|
||||
@@ -55,18 +56,19 @@ private:
|
||||
// MAIN LOOP
|
||||
//============================================================
|
||||
virtual void mainLoop();
|
||||
void addData(const SensorData & data);
|
||||
bool getData(SensorData & data);
|
||||
void addData(const SensorEvent & data);
|
||||
bool getData(SensorEvent & data);
|
||||
|
||||
private:
|
||||
USemaphore _dataAdded;
|
||||
UMutex _dataMutex;
|
||||
std::list<SensorData> _dataBuffer;
|
||||
std::list<SensorEvent> _dataBuffer;
|
||||
std::list<SensorData> _imuBuffer;
|
||||
Odometry * _odometry;
|
||||
unsigned int _dataBufferMaxSize;
|
||||
bool _resetOdometry;
|
||||
Transform _resetPose;
|
||||
Transform _previousGuessPose;
|
||||
double _oldestAsyncImuStamp;
|
||||
double _newestAsyncImuStamp;
|
||||
};
|
||||
|
||||
@@ -204,6 +204,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM.");
|
||||
RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db.");
|
||||
RTABMAP_PARAM(Mem, RawDescriptorsKept, bool, true, "Raw descriptors kept in memory.");
|
||||
RTABMAP_PARAM(Mem, LoadVisualLocalFeaturesOnInit, bool, true, "Load all local visual features (keypoints, descriptors and 3D points) in RAM when loading an existing database. This can add significant time to initialize the memory but the features will be already loaded before computing loop closure transforms. If false, the features are loaded on-demand from the database when a loop closure transformation should be estimated.");
|
||||
RTABMAP_PARAM(Mem, MapLabelsAdded, bool, true, "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
|
||||
RTABMAP_PARAM(Mem, SaveDepth16Format, bool, false, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
|
||||
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
|
||||
@@ -212,8 +213,8 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM_STR(Mem, DepthCompressionFormat, ".rvl", "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
|
||||
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
|
||||
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
|
||||
RTABMAP_PARAM(Mem, LocalizationDataSaved, bool, false, uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
|
||||
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
|
||||
RTABMAP_PARAM(Mem, LocalizationDataSaved, bool, false, uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
|
||||
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
|
||||
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
|
||||
RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
|
||||
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
|
||||
@@ -251,15 +252,17 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
|
||||
#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
|
||||
// OpenCV>2 without xFeatures2D module doesn't have BRIEF
|
||||
RTABMAP_PARAM(Kp, DetectorStrategy, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
|
||||
RTABMAP_PARAM(Kp, DetectorStrategy, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
|
||||
#else
|
||||
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
|
||||
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
|
||||
#endif
|
||||
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood.");
|
||||
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
|
||||
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
|
||||
RTABMAP_PARAM_STR(Kp, DictionaryPath, "", "Path of the pre-computed dictionary");
|
||||
RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true, "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
|
||||
RTABMAP_PARAM(Kp, FlannIndexSaved, bool, false, uFormat("Save FLANN index during localization session (when %s=false). The FLANN index will be saved to database after the first time localization mode is used, then on next sessions, the index is reloaded from the database instead of being rebuilt again. This can save significant loading time when the visual word dictionary is big (>1M words). Note that if the dictionary is modified (parameters or data), the index will be rebuilt and saved again on the next session.", kMemIncrementalMemory().c_str()).c_str());
|
||||
RTABMAP_PARAM(Kp, SerializeWithChecksum, bool, true, "On serialization of the FLANN index, compute checksum of the data used by the FLANN index. This adds a slight overhead on serialization/deserialization to make sure that the dictionary data correspond to same data used when the index was built.");
|
||||
RTABMAP_PARAM(Kp, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
|
||||
RTABMAP_PARAM(Kp, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
|
||||
RTABMAP_PARAM(Kp, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
|
||||
@@ -343,6 +346,13 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(SuperPoint, NMSRadius, int, 4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
|
||||
RTABMAP_PARAM(SuperPoint, Cuda, bool, true, "Use Cuda device for Torch, otherwise CPU device is used by default.");
|
||||
|
||||
RTABMAP_PARAM_STR(SuperPointRpautrat, WeightsPath, "", "[Required] SuperPoint weights file (*.pth).");
|
||||
RTABMAP_PARAM_STR(SuperPointRpautrat, ModelPath, "", "[Required] SuperPoint python model file (superpoint_pytorch.py).");
|
||||
RTABMAP_PARAM(SuperPointRpautrat, Threshold, float, 0.005, "Detector response threshold to accept keypoint.");
|
||||
RTABMAP_PARAM(SuperPointRpautrat, NMS, bool, true, "If true, non-maximum suppression is applied to detected keypoints.");
|
||||
RTABMAP_PARAM(SuperPointRpautrat, NMSRadius, int, 4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointRpautratNMS().c_str()));
|
||||
RTABMAP_PARAM(SuperPointRpautrat, Cuda, bool, true, "Use Cuda device for Torch, otherwise CPU device is used by default.");
|
||||
|
||||
RTABMAP_PARAM_STR(PyDetector, Path, "", "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
|
||||
RTABMAP_PARAM(PyDetector, Cuda, bool, true, "Use cuda.");
|
||||
|
||||
@@ -359,14 +369,14 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
|
||||
// RGB-D SLAM
|
||||
RTABMAP_PARAM(RGBD, Enabled, bool, true, "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
|
||||
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
|
||||
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, uFormat("Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
|
||||
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, uFormat("Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated. To update the map when not moving, both %s and %s should be set to 0.", Parameters::kRGBDLinearUpdate().c_str(), Parameters::kRGBDAngularUpdate().c_str()));
|
||||
RTABMAP_PARAM(RGBD, LinearSpeedUpdate, float, 0.0, "Maximum linear speed (m/s) to update the map (0 means not limit).");
|
||||
RTABMAP_PARAM(RGBD, AngularSpeedUpdate, float, 0.0, "Maximum angular speed (rad/s) to update the map (0 means not limit).");
|
||||
RTABMAP_PARAM(RGBD, AggressiveLoopThr, float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
|
||||
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
|
||||
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
|
||||
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
|
||||
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
|
||||
RTABMAP_PARAM(RGBD, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
|
||||
RTABMAP_PARAM(RGBD, ForceOdom3DoF, bool, true, uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
|
||||
RTABMAP_PARAM(RGBD, StartAtOrigin, bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
|
||||
@@ -428,7 +438,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
#endif
|
||||
#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, Robust, bool, false, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
|
||||
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(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM)
|
||||
@@ -453,8 +463,8 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
|
||||
|
||||
// Odometry
|
||||
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
|
||||
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
|
||||
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
|
||||
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
|
||||
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
|
||||
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
|
||||
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
|
||||
@@ -548,7 +558,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket.");
|
||||
RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket.");
|
||||
|
||||
// Odometry ORB_SLAM2
|
||||
// Odometry ORB_SLAM
|
||||
RTABMAP_PARAM_STR(OdomORBSLAM, VocPath, "", "Path to ORB vocabulary (*.txt).");
|
||||
RTABMAP_PARAM(OdomORBSLAM, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
|
||||
RTABMAP_PARAM(OdomORBSLAM, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
|
||||
@@ -603,8 +613,8 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(OdomMSCKF, InitCovExTrans, double, 0.000025, "");
|
||||
RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize, int, 20, "");
|
||||
|
||||
// Odometry VINS
|
||||
RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS config file.");
|
||||
// Odometry VINS-Fusion
|
||||
RTABMAP_PARAM_STR(OdomVINSFusion, ConfigPath, "", "Path of VINS-Fusion config file.");
|
||||
|
||||
// Odometry OpenVINS
|
||||
RTABMAP_PARAM(OdomOpenVINS, UseStereo, bool, true, "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
|
||||
@@ -672,6 +682,9 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth.");
|
||||
RTABMAP_PARAM(OdomOpen3D, Method, int, 0, "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
|
||||
|
||||
// Odometry cuVSLAM
|
||||
RTABMAP_PARAM(OdomCuVSLAM, MulticamMode, int, 0, "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
|
||||
|
||||
// Common registration parameters
|
||||
RTABMAP_PARAM(Reg, RepeatOnce, bool, true, "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
|
||||
RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
|
||||
@@ -701,9 +714,9 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform.");
|
||||
#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
|
||||
// OpenCV>2 without xFeatures2D module doesn't have BRIEF
|
||||
RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
|
||||
RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
|
||||
#else
|
||||
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
|
||||
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector 16=SuperPoint-Rpautrat");
|
||||
#endif
|
||||
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
|
||||
RTABMAP_PARAM(Vis, SSC, bool, false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
#ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
|
||||
#define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
|
||||
|
||||
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
|
||||
|
||||
#include <string>
|
||||
#include <rtabmap/utilite/UMutex.h>
|
||||
@@ -23,7 +24,7 @@ namespace rtabmap {
|
||||
* Create a single PythonInterface on main thread at
|
||||
* global scope before any Python classes.
|
||||
*/
|
||||
class PythonInterface
|
||||
class RTABMAP_CORE_EXPORT PythonInterface
|
||||
{
|
||||
public:
|
||||
PythonInterface();
|
||||
@@ -34,7 +35,7 @@ private:
|
||||
pybind11::gil_scoped_release* release_;
|
||||
};
|
||||
|
||||
std::string getPythonTraceback();
|
||||
std::string RTABMAP_CORE_EXPORT getPythonTraceback();
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -107,7 +107,11 @@ public:
|
||||
bool isIncrementalFlann() const {return _incrementalFlann;}
|
||||
void setIncrementalDictionary();
|
||||
void setFixedDictionary(const std::string & dictionaryPath);
|
||||
bool isModified() const;
|
||||
|
||||
std::vector<unsigned char> serializeIndex() const;
|
||||
void deserializeIndex(const std::vector<unsigned char> & data);
|
||||
void deserializeIndex(const unsigned char * data, size_t size);
|
||||
void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
|
||||
|
||||
void clear(bool printWarningsIfNotEmpty = true);
|
||||
@@ -137,10 +141,12 @@ private:
|
||||
std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
|
||||
std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
|
||||
bool _newWordsComparedTogether;
|
||||
bool _serializeWithChecksum;
|
||||
int _lastWordId;
|
||||
bool useDistanceL1_;
|
||||
FlannIndex * _flannIndex;
|
||||
cv::Mat _dataTree;
|
||||
bool _modified;
|
||||
NNStrategy _strategy;
|
||||
std::map<int ,int> _mapIndexId;
|
||||
std::map<int ,int> _mapIdIndex;
|
||||
|
||||
@@ -94,14 +94,14 @@ public:
|
||||
_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
|
||||
}
|
||||
|
||||
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
|
||||
// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
|
||||
void setOdometryPath(const std::string & filePath, int format = 0)
|
||||
{
|
||||
_odometryPath = filePath;
|
||||
_odometryFormat = format;
|
||||
}
|
||||
|
||||
// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
|
||||
// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
|
||||
void setGroundTruthPath(const std::string & filePath, int format = 0)
|
||||
{
|
||||
_groundTruthPath = filePath;
|
||||
|
||||
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
Copyright (c) 2010-2025, 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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rtabmap/core/CameraModel.h"
|
||||
#include "rtabmap/core/Camera.h"
|
||||
#include "rtabmap/core/Version.h"
|
||||
|
||||
#ifdef RTABMAP_ORBBEC_SDK
|
||||
namespace ob
|
||||
{
|
||||
class Pipeline;
|
||||
class Align;
|
||||
}
|
||||
#endif
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class RTABMAP_CORE_EXPORT CameraOrbbecSDK :
|
||||
public Camera
|
||||
{
|
||||
public:
|
||||
static bool available();
|
||||
|
||||
public:
|
||||
// deviceId can be either an index (e.g., "0"), an UID (e.g, "2-1-2" or "gmsl-1") or a serial ("AAA6454S")
|
||||
CameraOrbbecSDK(
|
||||
std::string deviceId = "",
|
||||
unsigned int colorWidth = 800,
|
||||
unsigned int colorHeight = 600,
|
||||
unsigned int depthWidth = 800,
|
||||
unsigned int depthHeight = 600,
|
||||
float imageRate = 0.0f,
|
||||
const Transform & localTransform = Transform::getIdentity());
|
||||
virtual ~CameraOrbbecSDK();
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
|
||||
virtual bool isCalibrated() const;
|
||||
virtual std::string getSerial() const;
|
||||
|
||||
void close();
|
||||
|
||||
// Should be set before initializing
|
||||
void enableColorRectification(bool enabled);
|
||||
void enableImu(bool enabled);
|
||||
|
||||
void enableDepthMM(bool enabled);
|
||||
|
||||
protected:
|
||||
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
|
||||
|
||||
private:
|
||||
|
||||
#ifdef RTABMAP_ORBBEC_SDK
|
||||
std::string deviceId_;
|
||||
unsigned int colorWidth_;
|
||||
unsigned int colorHeight_;
|
||||
unsigned int depthWidth_;
|
||||
unsigned int depthHeight_;
|
||||
ob::Pipeline * pipeline_;
|
||||
ob::Pipeline * imuPipeline_;
|
||||
ob::Align * alignFilter_;
|
||||
CameraModel model_;
|
||||
Transform imuLocalTransform_;
|
||||
bool imuLocalTransformInitialized_;
|
||||
uint64_t lastAccStamp_;
|
||||
uint64_t lastImageStamp_;
|
||||
bool globalTimestampAvailable_;
|
||||
bool rectifyColor_;
|
||||
bool convertDepthToMM_;
|
||||
bool imuPublished_;
|
||||
std::map<double, cv::Vec6f> imuBuffer_;
|
||||
UMutex imuMutex_;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace rtabmap
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
Copyright (c) 2025 Felix Toft
|
||||
Copyright (c) 2010-2016, 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 ODOMETRYCUVSLAM_H_
|
||||
#define ODOMETRYCUVSLAM_H_
|
||||
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
#include <memory>
|
||||
#include <deque>
|
||||
#include <array>
|
||||
|
||||
#ifdef RTABMAP_CUVSLAM
|
||||
#include <cuvslam.h>
|
||||
#include <ground_constraint.h>
|
||||
#include <cuda_runtime.h>
|
||||
#endif
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class RTABMAP_CORE_EXPORT OdometryCuVSLAM : public Odometry
|
||||
{
|
||||
public:
|
||||
OdometryCuVSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
|
||||
virtual ~OdometryCuVSLAM();
|
||||
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
virtual Odometry::Type getType() {return Odometry::kTypeCuVSLAM;}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
|
||||
virtual void cleanupCuVSLAMResources();
|
||||
|
||||
private:
|
||||
#ifdef RTABMAP_CUVSLAM
|
||||
CUVSLAM_TrackerHandle cuvslam_handle_;
|
||||
CUVSLAM_GroundConstraintHandle ground_constraint_handle_;
|
||||
|
||||
std::vector<CUVSLAM_Camera> cuvslam_cameras_;
|
||||
std::vector<std::array<float, 12>> intrinsics_;
|
||||
|
||||
// State tracking
|
||||
bool initialized_;
|
||||
bool lost_;
|
||||
bool tracking_;
|
||||
bool planar_constraints_;
|
||||
int multicam_mode_;
|
||||
Transform previous_pose_;
|
||||
double last_timestamp_;
|
||||
|
||||
// Configuration Thresholds
|
||||
double velocity_ratio_threshold_high_ = 1.5; // The maximum velocity ratio of guess / estimated velocity needed to detect lost state.
|
||||
double velocity_ratio_threshold_low_ = 0.5; // The minimum velocity ratio of guess / estimated velocity needed to detect lost state.
|
||||
double velocity_difference_threshold_ = 0.1; // The maximum velocity difference between the guess and the estimated velocity needed to detect lost state.
|
||||
double zero_estimated_velocity_threshold_ = 0.00001; // The minimum cuVSLAM estimated velocity needed to detect lost state.
|
||||
double min_landmarks_threshold_ = 30; // The minimum number of landmarks needed to start tracking after an initialization.
|
||||
|
||||
// Forward cuVLSAM covariance directly to RTAB-Map.
|
||||
// When true this disables covariance based lost detection.
|
||||
bool use_raw_covariance_ = false;
|
||||
|
||||
//visualization
|
||||
std::vector<CUVSLAM_Observation> observations_;
|
||||
std::vector<CUVSLAM_Landmark> landmarks_;
|
||||
|
||||
// GPU memory management
|
||||
std::vector<uint8_t *> gpu_left_image_data_; // pointers to all gpu images
|
||||
std::vector<uint8_t *> gpu_right_image_data_;
|
||||
std::vector<size_t> gpu_left_image_sizes_; // size of one image
|
||||
std::vector<size_t> gpu_right_image_sizes_;
|
||||
cudaStream_t cuda_stream_;
|
||||
#endif
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* ODOMETRYCUVSLAM_H_ */
|
||||
@@ -25,39 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef ODOMETRYVINS_H_
|
||||
#define ODOMETRYVINS_H_
|
||||
#pragma once
|
||||
#pragma message("Warning: OdometryVINS.h is deprecated. Please use OdometryVINSFusion.h instead.")
|
||||
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class VinsEstimator;
|
||||
|
||||
class RTABMAP_CORE_EXPORT OdometryVINS : public Odometry
|
||||
{
|
||||
public:
|
||||
OdometryVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
|
||||
virtual ~OdometryVINS();
|
||||
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
|
||||
virtual bool canProcessRawImages() const {return true;}
|
||||
virtual bool canProcessAsyncIMU() const {return true;}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
|
||||
|
||||
private:
|
||||
#ifdef RTABMAP_VINS
|
||||
VinsEstimator * vinsEstimator_;
|
||||
bool initGravity_;
|
||||
Transform previousPose_;
|
||||
Transform previousLocalTransform_;
|
||||
IMU lastImu_;
|
||||
#endif
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* ODOMETRYVINS_H_ */
|
||||
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, 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 ODOMETRYVINSFUSION_H_
|
||||
#define ODOMETRYVINSFUSION_H_
|
||||
|
||||
#include <rtabmap/core/Odometry.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class VinsFusionEstimator;
|
||||
|
||||
class RTABMAP_CORE_EXPORT OdometryVINSFusion : public Odometry
|
||||
{
|
||||
public:
|
||||
OdometryVINSFusion(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
|
||||
virtual ~OdometryVINSFusion();
|
||||
|
||||
virtual void reset(const Transform & initialPose = Transform::getIdentity());
|
||||
virtual Odometry::Type getType() {return Odometry::kTypeVINSFusion;}
|
||||
virtual bool canProcessRawImages() const {return true;}
|
||||
virtual bool canProcessAsyncIMU() const {return true;}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
|
||||
|
||||
private:
|
||||
#ifdef RTABMAP_VINS_FUSION
|
||||
VinsFusionEstimator * vinsEstimator_;
|
||||
bool initGravity_;
|
||||
Transform previousPose_;
|
||||
Transform previousLocalTransform_;
|
||||
IMU lastImu_;
|
||||
double lastImuStamp_;
|
||||
#endif
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* ODOMETRYVINSFUSION_H_ */
|
||||
Reference in New Issue
Block a user