From 59046f1de279427f32343ccda2a5f70a915d926e Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 7 Aug 2023 22:33:23 +0800 Subject: [PATCH 01/13] fix ros gtsam 4.2a9 build (#1108) --- corelib/src/optimizer/OptimizerGTSAM.cpp | 8 ++++ corelib/src/optimizer/gtsam/GravityFactor.h | 44 +++++++++++++++---- corelib/src/optimizer/gtsam/XYFactor.h | 7 ++- corelib/src/optimizer/gtsam/XYZFactor.h | 14 +++++- .../optimizer/vertigo/gtsam/DerivedValue.h | 7 ++- .../vertigo/gtsam/betweenFactorMaxMix.h | 4 +- .../vertigo/gtsam/betweenFactorSwitchable.h | 12 +++++ .../vertigo/gtsam/switchVariableLinear.h | 17 ++++++- .../vertigo/gtsam/switchVariableSigmoid.h | 15 ++++++- 9 files changed, 111 insertions(+), 17 deletions(-) diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index ff256493f0..ac2afff0a4 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -527,7 +527,11 @@ std::map OptimizerGTSAM::optimize( { float x,y,z,roll,pitch,yaw; std::map tmpPoses; +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#endif { if(iter->value.dim() > 1) { @@ -630,7 +634,11 @@ std::map OptimizerGTSAM::optimize( optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); float x,y,z,roll,pitch,yaw; +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) +#endif { if(iter->value.dim() > 1) { diff --git a/corelib/src/optimizer/gtsam/GravityFactor.h b/corelib/src/optimizer/gtsam/GravityFactor.h index f292072f13..02c7a08ffc 100644 --- a/corelib/src/optimizer/gtsam/GravityFactor.h +++ b/corelib/src/optimizer/gtsam/GravityFactor.h @@ -63,15 +63,21 @@ class GravityFactor { /** vector of errors */ Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = boost::none) const; +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalJacobian<2,3> H = {}) const; +#else + OptionalJacobian<2,3> H = boost::none) const; +#endif /** Serialization function */ +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); } +#endif }; /** @@ -85,8 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /// Typedef to this class typedef Rot3GravityFactor This; @@ -111,10 +120,13 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - +#endif /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -124,7 +136,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /** vector of errors */ virtual Vector evaluateError(const Rot3& nRb, // - boost::optional H = boost::none) const { +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return attitudeError(nRb, H); } Unit3 nZ() const { @@ -135,7 +151,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -145,6 +161,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -163,8 +180,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /// Typedef to this class typedef Pose3GravityFactor This; @@ -189,7 +209,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( +#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -202,7 +226,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /** vector of errors */ virtual Vector evaluateError(const Pose3& nTb, // +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else boost::optional H = boost::none) const { +#endif Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; @@ -219,7 +247,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -229,7 +257,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } - +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/corelib/src/optimizer/gtsam/XYFactor.h b/corelib/src/optimizer/gtsam/XYFactor.h index d13a3bc8a0..8a7c65585c 100644 --- a/corelib/src/optimizer/gtsam/XYFactor.h +++ b/corelib/src/optimizer/gtsam/XYFactor.h @@ -41,7 +41,12 @@ class XYFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose2 // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const VALUE& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const VALUE& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif // note that use boost optional like a pointer // only calculate jacobian matrix when non-null pointer exists diff --git a/corelib/src/optimizer/gtsam/XYZFactor.h b/corelib/src/optimizer/gtsam/XYZFactor.h index d4ed63c01a..42e55d0b83 100644 --- a/corelib/src/optimizer/gtsam/XYZFactor.h +++ b/corelib/src/optimizer/gtsam/XYZFactor.h @@ -41,14 +41,24 @@ class XYZFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Pose3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif if(H) { p.translation(H); } return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } - gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Point3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h index 66cf6954e9..ecf5fcad25 100644 --- a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h +++ b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h @@ -72,10 +72,15 @@ class DerivedValue : public gtsam::Value { /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ +#if GTSAM_VERSION_NUMERIC >= 40300 + virtual std::shared_ptr clone() const { + return std::make_shared(static_cast(*this)); + } + #else virtual boost::shared_ptr clone() const { return boost::make_shared(static_cast(*this)); } - +#endif /// equals implementing generic Value interface virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const { // Cast the base class Value pointer to a derived class pointer diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h index 0ee996f64f..40538fc0f0 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h @@ -12,7 +12,7 @@ #include #include -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 namespace gtsam { gtsam::Matrix inverse(const gtsam::Matrix & matrix) { @@ -49,7 +49,7 @@ namespace vertigo { double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); double l1 = nu1 * exp(-0.5*m1); -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 double m2 = nullHypothesisModel->squaredMahalanobisDistance(error); #else double m2 = nullHypothesisModel->distance(error); diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index ab5a443840..1bfe91ea4f 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h @@ -30,9 +30,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const +#endif { // calculate error @@ -64,9 +70,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const +#endif { // calculate error diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index 2260ea6077..e95e0b568b 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableLinear between(const SwitchVariableLinear& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableLinear(l2.value() - value()); @@ -116,11 +121,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 674e897ac7..79e1fca99d 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableSigmoid(l2.value() - value()); @@ -117,11 +122,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; From f7e4b38dd712c7d4a8d9fb86928497602f8b3dd6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Sep 2023 01:19:59 -0700 Subject: [PATCH 02/13] Fixed superglue deadlock on standalone (#896). Fixed some elemSize opencv asserts in debug build (876) --- CMakeLists.txt | 1 + .../include/rtabmap/core/PythonInterface.h | 26 +++---- corelib/src/CMakeLists.txt | 1 + corelib/src/DBDriverSqlite3.cpp | 6 +- corelib/src/SensorData.cpp | 30 ++++---- corelib/src/Signature.cpp | 2 +- corelib/src/python/PyDetector.cpp | 26 +++---- corelib/src/python/PyDetector.h | 2 +- corelib/src/python/PyMatcher.cpp | 24 +++--- corelib/src/python/PyMatcher.h | 2 +- corelib/src/python/PythonInterface.cpp | 77 +++---------------- 11 files changed, 68 insertions(+), 129 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c4fa00992..295d106a3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -389,6 +389,7 @@ IF(WITH_PYTHON) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy) IF(Python3_FOUND) MESSAGE(STATUS "Found Python3") + FIND_PACKAGE(pybind11 REQUIRED) ENDIF(Python3_FOUND) ENDIF(WITH_PYTHON) diff --git a/corelib/include/rtabmap/core/PythonInterface.h b/corelib/include/rtabmap/core/PythonInterface.h index 2216cb5ffa..f2f3a6a272 100644 --- a/corelib/include/rtabmap/core/PythonInterface.h +++ b/corelib/include/rtabmap/core/PythonInterface.h @@ -11,31 +11,31 @@ #include #include -#include + +namespace pybind11 { +class scoped_interpreter; +class gil_scoped_release; +} namespace rtabmap { +/** + * Create a single PythonInterface on main thread at + * global scope before any Python classes. + */ class PythonInterface { public: PythonInterface(); virtual ~PythonInterface(); -protected: - std::string getTraceback(); // should be called between lock() and unlock() - void lock(); - void unlock(); - private: - static UMutex mutex_; - static int refCount_; - -protected: - static PyThreadState * mainThreadState_; - static unsigned long mainThreadID_; - PyThreadState * threadState_; + pybind11::scoped_interpreter* guard_; + pybind11::gil_scoped_release* release_; }; +std::string getPythonTraceback(); + } #endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */ diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 7e6cd99687..25ef96a08d 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -209,6 +209,7 @@ IF(WITH_PYTHON AND Python3_FOUND) ${LIBRARIES} Python3::Python Python3::NumPy + pybind11::embed ) SET(SRC_FILES ${SRC_FILES} diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 453c52201e..280c228acd 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) { _memoryUsedEstimate += (*i)->getMemoryUsed(); // raw data are not kept in database - _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); stepNode(ppStmt, *i); } diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 0c0fddbbc9..147e854d69 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector & keypoints, const unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes { return sizeof(SensorData) + - _imageCompressed.total()*_imageCompressed.elemSize() + - _imageRaw.total()*_imageRaw.elemSize() + - _depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() + - _depthOrRightRaw.total()*_depthOrRightRaw.elemSize() + - _userDataCompressed.total()*_userDataCompressed.elemSize() + - _userDataRaw.total()*_userDataRaw.elemSize() + - _laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() + - _laserScanRaw.data().total()*_laserScanRaw.data().elemSize() + - _groundCellsCompressed.total()*_groundCellsCompressed.elemSize() + - _groundCellsRaw.total()*_groundCellsRaw.elemSize() + - _obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() + - _obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+ - _emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() + - _emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+ + (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) + + (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) + + (_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) + + (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) + + (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) + + (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) + + (_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) + + (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) + + (_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) + + (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) + + (_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) + + (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+ + (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) + + (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+ _keypoints.size() * sizeof(cv::KeyPoint) + _keypoints3D.size() * sizeof(cv::Point3f) + - _descriptors.total()*_descriptors.elemSize(); + (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize()); } void SensorData::clearCompressedData(bool images, bool scan, bool userData) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 53f843b2b8..ee08ab6804 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -348,7 +348,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo total += _words.size() * (sizeof(int)*2+sizeof(std::multimap::iterator)) + sizeof(std::multimap); total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector); total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector); - total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); + total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map::iterator)) + sizeof(std::map); if(withSensorData) { diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index 9039640207..46f9f25619 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -32,7 +34,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) : return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -54,15 +56,13 @@ PyDetector::PyDetector(const ParametersMap & parameters) : if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyDetector::~PyDetector() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { @@ -72,8 +72,6 @@ PyDetector::~PyDetector() { Py_DECREF(pModule_); } - - unlock(); } void PyDetector::parseParameters(const ParametersMap & parameters) @@ -102,7 +100,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag return keypoints; } - lock(); + pybind11::gil_scoped_acquire acquire; if(!pFunc_) { @@ -116,7 +114,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(result); @@ -129,7 +127,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -141,7 +139,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(pFunc); @@ -149,7 +147,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } UDEBUG("init time = %fs", timer.ticks()); @@ -167,7 +165,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -220,8 +218,6 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag Py_DECREF(pImageBuffer); } - unlock(); - return keypoints; } diff --git a/corelib/src/python/PyDetector.h b/corelib/src/python/PyDetector.h index 4512b8eb02..5f292abdc5 100644 --- a/corelib/src/python/PyDetector.h +++ b/corelib/src/python/PyDetector.h @@ -18,7 +18,7 @@ namespace rtabmap { -class PyDetector : public Feature2D, public PythonInterface +class PyDetector : public Feature2D { public: PyDetector(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index 446f28c961..aac482efe1 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -39,7 +41,7 @@ PyMatcher::PyMatcher( return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -59,15 +61,13 @@ PyMatcher::PyMatcher( if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyMatcher::~PyMatcher() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { Py_DECREF(pFunc_); @@ -76,7 +76,6 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } - unlock(); } std::vector PyMatcher::match( @@ -104,7 +103,7 @@ std::vector PyMatcher::match( imageSize.width>0 && imageSize.height>0) { - lock(); + pybind11::gil_scoped_acquire acquire; UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0); @@ -120,7 +119,7 @@ std::vector PyMatcher::match( if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(result); @@ -133,7 +132,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -145,7 +144,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(pFunc); @@ -153,7 +152,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } UDEBUG("init time = %fs", timer.ticks()); @@ -216,7 +215,7 @@ std::vector PyMatcher::match( if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -260,7 +259,6 @@ std::vector PyMatcher::match( UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks()); } - unlock(); } else { diff --git a/corelib/src/python/PyMatcher.h b/corelib/src/python/PyMatcher.h index 2af25c75cc..da04cd466f 100644 --- a/corelib/src/python/PyMatcher.h +++ b/corelib/src/python/PyMatcher.h @@ -16,7 +16,7 @@ namespace rtabmap { -class PyMatcher : public PythonInterface +class PyMatcher { public: PyMatcher(const std::string & pythonMatcherPath, diff --git a/corelib/src/python/PythonInterface.cpp b/corelib/src/python/PythonInterface.cpp index 7e9afb48b3..0e2d987980 100644 --- a/corelib/src/python/PythonInterface.cpp +++ b/corelib/src/python/PythonInterface.cpp @@ -8,83 +8,26 @@ #include #include #include +#include namespace rtabmap { -UMutex PythonInterface::mutex_; -int PythonInterface::refCount_ = 0; -PyThreadState * PythonInterface::mainThreadState_ = 0; -unsigned long PythonInterface::mainThreadID_ = 0; - -PythonInterface::PythonInterface() : - threadState_(0) +PythonInterface::PythonInterface() { - UScopeMutex lockM(mutex_); - if(refCount_ == 0) - { - UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId()); - // initialize Python - Py_Initialize(); - - // initialize thread support - PyEval_InitThreads(); - Py_DECREF(PyImport_ImportModule("threading")); - - //release the GIL, store thread state, set the current thread state to NULL - mainThreadState_ = PyEval_SaveThread(); - UASSERT(mainThreadState_); - mainThreadID_ = UThread::currentThreadId(); - } - - ++refCount_; + UINFO("Initialize python interpreter"); + guard_ = new pybind11::scoped_interpreter(); + pybind11::module::import("threading"); + release_ = new pybind11::gil_scoped_release(); } PythonInterface::~PythonInterface() { - UScopeMutex lock(mutex_); - if(refCount_>0 && --refCount_==0) - { - // shut down the interpreter - UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId()); - PyEval_RestoreThread(mainThreadState_); - Py_Finalize(); - } -} - -void PythonInterface::lock() -{ - mutex_.lock(); - - UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - if(UThread::currentThreadId() == mainThreadID_) - { - PyEval_RestoreThread(mainThreadState_); - } - else - { - // create a thread state object for this thread - threadState_ = PyThreadState_New(mainThreadState_->interp); - UASSERT(threadState_); - PyEval_RestoreThread(threadState_); - } -} - -void PythonInterface::unlock() -{ - if(UThread::currentThreadId() == mainThreadID_) - { - mainThreadState_ = PyEval_SaveThread(); - } - else - { - PyThreadState_Clear(threadState_); - PyThreadState_DeleteCurrent(); - } - UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - mutex_.unlock(); + UINFO("Finalize python interpreter"); + delete release_; + delete guard_; } -std::string PythonInterface::getTraceback() +std::string getPythonTraceback() { // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated From 760faf4072c4ab6b62310bc5f8e5cd06492f6811 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 30 Jan 2024 19:04:01 +0100 Subject: [PATCH 03/13] fix mismatch kps and desc --- corelib/src/python/PyDetector.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index 46f9f25619..49c64bea45 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -207,6 +207,13 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone(); descriptors_.push_back(descriptor); } + if (keypoints.size() != descriptors_.rows) + { + UWARN("keypoints size is not match descriptors size %d vs %d", static_cast(keypoints.size()), descriptors_.rows); + keypoints.clear(); + descriptors_ = cv::Mat(); + } + } } else From 040c814d1394471132b81db2316c21d865670448 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Fri, 9 Feb 2024 17:01:53 +0100 Subject: [PATCH 04/13] not limitKps when using pyDetector --- corelib/src/Features2d.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 5df9f05bc8..88aa68c18e 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -734,7 +734,10 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize); std::vector sub_keypoints; sub_keypoints = this->generateKeypointsImpl(image, roi, mask); - limitKeypoints(sub_keypoints, maxFeatures); + if (this->getType() != Feature2D::Type::kFeaturePyDetector) + { + limitKeypoints(sub_keypoints, maxFeatures); + } if(roi.x || roi.y) { // Adjust keypoint position to raw image From e72632c5fc691b88489d334b02dfc74b8789394a Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Wed, 21 Feb 2024 11:40:51 +0100 Subject: [PATCH 05/13] save index and reuse --- corelib/include/rtabmap/core/FlannIndex.h | 17 +- corelib/include/rtabmap/core/Memory.h | 3 +- corelib/include/rtabmap/core/VWDictionary.h | 50 +-- corelib/src/FlannIndex.cpp | 227 +++++++++++-- corelib/src/Memory.cpp | 7 +- corelib/src/Rtabmap.cpp | 12 +- corelib/src/VWDictionary.cpp | 344 +++++++++++++------- corelib/src/python/PyDetector.cpp | 2 +- 8 files changed, 484 insertions(+), 178 deletions(-) diff --git a/corelib/include/rtabmap/core/FlannIndex.h b/corelib/include/rtabmap/core/FlannIndex.h index a1cd253e56..220bd8a723 100644 --- a/corelib/include/rtabmap/core/FlannIndex.h +++ b/corelib/include/rtabmap/core/FlannIndex.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define CORELIB_SRC_FLANNINDEX_H_ #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines +#include #include #include @@ -40,11 +41,19 @@ class RTABMAP_CORE_EXPORT FlannIndex FlannIndex(); virtual ~FlannIndex(); + // serialize and save methods + void save(const std::filesystem::path& path); + void serialize(const std::filesystem::path& filename); + + // deserialize and load methods + void load(const std::filesystem::path& dir); + void deserialize(const std::filesystem::path& file); + void release(); - size_t indexedFeatures() const; + [[nodiscard]] size_t indexedFeatures() const; // return Bytes - size_t memoryUsed() const; + [[nodiscard]] size_t memoryUsed() const; // Note that useDistanceL1 doesn't have any effect if LSH is used void buildLinearIndex( @@ -71,8 +80,8 @@ class RTABMAP_CORE_EXPORT FlannIndex bool isBuilt(); - int featuresType() const {return featuresType_;} - int featuresDim() const {return featuresDim_;} + [[nodiscard]] int featuresType() const {return featuresType_;} + [[nodiscard]] int featuresDim() const {return featuresDim_;} std::vector addPoints(const cv::Mat & features); diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index a0f86e89a3..e34210744d 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/SensorData.h" #include "rtabmap/core/Link.h" #include "rtabmap/core/Features2d.h" +#include #include #include #include @@ -225,7 +226,7 @@ class RTABMAP_CORE_EXPORT Memory void dumpMemoryTree(const char * fileNameTree) const; virtual void dumpMemory(std::string directory) const; virtual void dumpSignatures(const char * fileNameSign, bool words3D) const; - void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const; + void dumpDictionary(std::string_view dir) const; unsigned long getMemoryUsed() const; //Bytes void generateGraph(const std::string & fileName, const std::set & ids = std::set()); diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index fbfa7ba542..a8745e2ba8 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -29,11 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines +#include #include #include #include #include #include +#include #include "rtabmap/core/Parameters.h" namespace rtabmap @@ -74,7 +76,8 @@ class RTABMAP_CORE_EXPORT VWDictionary } public: - VWDictionary(const ParametersMap & parameters = ParametersMap()); + explicit VWDictionary(const ParametersMap & parameters = ParametersMap()); + void setSavedIndex(const std::filesystem::path& dir); virtual ~VWDictionary(); virtual void parseParameters(const ParametersMap & parameters); @@ -86,60 +89,65 @@ class RTABMAP_CORE_EXPORT VWDictionary int signatureId); virtual void addWord(VisualWord * vw); - std::vector findNN(const std::list & vws) const; - std::vector findNN(const cv::Mat & descriptors) const; + [[nodiscard]] std::vector findNN(const std::list & vws) const; + [[nodiscard]] std::vector findNN(const cv::Mat & queryIn) const; void addWordRef(int wordId, int signatureId); void removeAllWordRef(int wordId, int signatureId); - const VisualWord * getWord(int id) const; - VisualWord * getUnusedWord(int id) const; + [[nodiscard]] const VisualWord * getWord(int id) const; + [[nodiscard]] VisualWord * getUnusedWord(int id) const; void setLastWordId(int id) {_lastWordId = id;} - const std::map & getVisualWords() const {return _visualWords;} - float getNndrRatio() const {return _nndrRatio;} - unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();} - int getLastIndexedWordId() const; - int getTotalActiveReferences() const {return _totalActiveReferences;} - unsigned int getIndexedWordsCount() const; - unsigned int getIndexMemoryUsed() const; // KB - unsigned long getMemoryUsed() const; //Bytes + [[nodiscard]] const std::map & getVisualWords() const {return _visualWords;} + [[nodiscard]] float getNndrRatio() const {return _nndrRatio;} + [[nodiscard]] unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();} + [[nodiscard]] int getLastIndexedWordId() const; + [[nodiscard]] int getTotalActiveReferences() const {return _totalActiveReferences;} + [[nodiscard]] unsigned int getIndexedWordsCount() const; + [[nodiscard]] unsigned int getIndexMemoryUsed() const; // KB + [[nodiscard]] unsigned long getMemoryUsed() const; //Bytes bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized - bool isIncremental() const {return _incrementalDictionary;} - bool isIncrementalFlann() const {return _incrementalFlann;} + [[nodiscard]] bool isIncremental() const {return _incrementalDictionary;} + [[nodiscard]] bool isIncrementalFlann() const {return _incrementalFlann;} void setIncrementalDictionary(); void setFixedDictionary(const std::string & dictionaryPath); void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const; void clear(bool printWarningsIfNotEmpty = true); - std::vector getUnusedWords() const; - std::vector getUnusedWordIds() const; - unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();} + [[nodiscard]] std::vector getUnusedWords() const; + [[nodiscard]] std::vector getUnusedWordIds() const; + [[nodiscard]] unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();} void removeWords(const std::vector & words); // caller must delete the words void deleteUnusedWords(); + void save(std::string_view dir) const; + void load(std::string_view dir); + void loadMapIndex(const std::filesystem::path& file); + public: static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat = true); static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat = true); protected: int getNextId(); - -protected: + void saveVars(const std::filesystem::path& filename) const; std::map _visualWords; // int _totalActiveReferences; // keep track of all references for updating the common signature private: + bool _useSavedFile{false}; bool _incrementalDictionary; bool _incrementalFlann; float _rebalancingFactor; bool _byteToFloat; float _nndrRatio; + std::string save_dir_; std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db) std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db) bool _newWordsComparedTogether; int _lastWordId; bool useDistanceL1_; - FlannIndex * _flannIndex; + FlannIndex * _flannIndex; // save cv::Mat _dataTree; NNStrategy _strategy; std::map _mapIndexId; diff --git a/corelib/src/FlannIndex.cpp b/corelib/src/FlannIndex.cpp index a593257780..141ff33892 100644 --- a/corelib/src/FlannIndex.cpp +++ b/corelib/src/FlannIndex.cpp @@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { FlannIndex::FlannIndex(): - index_(0), + index_(nullptr), nextIndex_(0), featuresType_(0), featuresDim_(0), @@ -47,6 +47,33 @@ FlannIndex::~FlannIndex() this->release(); } +void FlannIndex::save(const std::filesystem::path& path) +{ + UDEBUG(""); + if(!index_) + { + UERROR("Can not save FlannIndex when index_ is nullptr"); + return; + } + + if (featuresType_ == CV_8UC1) + { + static_cast>*>(index_)->save(path); + } + else if (useDistanceL1_) + { + static_cast>*>(index_)->save(path); + } + else if (featuresDim_ <= 3) + { + static_cast>*>(index_)->save(path); + } + else + { + static_cast>*>(index_)->save(path); + } +} + void FlannIndex::release() { UDEBUG(""); @@ -54,24 +81,24 @@ void FlannIndex::release() { if(featuresType_ == CV_8UC1) { - delete (rtflann::Index >*)index_; + delete static_cast >*>(index_); } else { if(useDistanceL1_) { - delete (rtflann::Index >*)index_; + delete static_cast >*>(index_); } else if(featuresDim_ <= 3) { - delete (rtflann::Index >*)index_; + delete static_cast>*>(index_); } else { - delete (rtflann::Index >*)index_; + delete static_cast >*>(index_); } } - index_ = 0; + index_ = nullptr; } nextIndex_ = 0; isLSH_ = false; @@ -80,6 +107,118 @@ void FlannIndex::release() UDEBUG(""); } +void FlannIndex::deserialize(const std::filesystem::path& file) +{ + std::ifstream inFile(file, std::ios::binary); + if (!inFile.is_open()) { + throw std::runtime_error("Unable to open file for reading"); + } + + int mapSize; + inFile.read(reinterpret_cast(&mapSize), sizeof(mapSize)); + inFile.read(reinterpret_cast(&nextIndex_), sizeof(nextIndex_)); + inFile.read(reinterpret_cast(&featuresType_), sizeof(featuresType_)); + inFile.read(reinterpret_cast(&featuresDim_), sizeof(featuresDim_)); + inFile.read(reinterpret_cast(&isLSH_), sizeof(isLSH_)); + inFile.read(reinterpret_cast(&useDistanceL1_), sizeof(useDistanceL1_)); + inFile.read(reinterpret_cast(&rebalancingFactor_), sizeof(rebalancingFactor_)); + + addedDescriptors_.clear(); + for (int i = 0; i < mapSize; ++i) { + // Read the size of the compressed data + int compressedSize; + inFile.read(reinterpret_cast(&compressedSize), sizeof(compressedSize)); + std::vector compressedBuffer(compressedSize); + inFile.read(compressedBuffer.data(), compressedSize); + + // Decompress + size_t decompressedSize = LZ4_compressBound(compressedSize); // Estimate decompressed size + std::vector decompressedBuffer(decompressedSize); + [[maybe_unused]]int actualDecompressedSize = LZ4_decompress_safe(compressedBuffer.data(), decompressedBuffer.data(), compressedSize, (int)decompressedSize); + // UDEBUG("Size decriptor after decompressed: %ld", actualDecompressedSize); + + // Extract data from decompressed buffer + char* dataPtr = decompressedBuffer.data(); + int key, rows, cols, type, channels; + memcpy(&key, dataPtr, sizeof(key)); dataPtr += sizeof(key); + memcpy(&rows, dataPtr, sizeof(rows)); dataPtr += sizeof(rows); + memcpy(&cols, dataPtr, sizeof(cols)); dataPtr += sizeof(cols); + memcpy(&type, dataPtr, sizeof(type)); dataPtr += sizeof(type); + memcpy(&channels, dataPtr, sizeof(channels)); dataPtr += sizeof(channels); + cv::Mat mat(rows, cols, type, dataPtr); + addedDescriptors_[key] = mat.clone(); // Use clone if necessary + } +} + +// Define a function to build the index +template +rtflann::Index* buildIndex(const rtflann::IndexParams& params) +{ + return new rtflann::Index(params); +} + +void FlannIndex::load(const std::filesystem::path& dir) +{ + auto start = std::chrono::high_resolution_clock::now(); + auto var_file = dir / "vars.bin"; + deserialize(var_file); + + // Define function pointers for index building functions + if(index_) + { + if(featuresType_ == CV_8UC1) + { + delete static_cast >*>(index_); + } + else + { + if(useDistanceL1_) + { + delete static_cast >*>(index_); + } + else if(featuresDim_ <= 3) + { + delete static_cast>*>(index_); + } + else + { + delete static_cast >*>(index_); + } + } + index_ = nullptr; + } + + auto index_file = dir / "index.bin"; + rtflann::IndexParams params; + params["filename"] = index_file.string(); + params["algorithm"] = rtflann::FLANN_INDEX_SAVED; + params["save_dataset"] = true; + + if(featuresType_ == CV_8UC1) + { + index_ = buildIndex>(params); + assert(index_); + } + else + { + if(useDistanceL1_) + { + index_ = buildIndex>(params); + } + else if(featuresDim_ <= 3) + { + index_ = buildIndex>(params); + } + else + { + index_ = buildIndex>(params); + } + } + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + std::cout << "Done build index with saved index at: " << index_file.string() << " within " << duration.count() << " ms" << std::endl; +} + size_t FlannIndex::indexedFeatures() const { if(!index_) @@ -146,7 +285,7 @@ void FlannIndex::buildLinearIndex( { UDEBUG(""); this->release(); - UASSERT(index_ == 0); + UASSERT(index_ == nullptr); UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); featuresType_ = features.type(); featuresDim_ = features.cols; @@ -154,7 +293,7 @@ void FlannIndex::buildLinearIndex( rebalancingFactor_ = rebalancingFactor; rtflann::LinearIndexParams params; - + params["save_dataset"] = true; if(featuresType_ == CV_8UC1) { rtflann::Matrix dataset(features.data, features.rows, features.cols); @@ -207,7 +346,7 @@ void FlannIndex::buildKDTreeIndex( { UDEBUG(""); this->release(); - UASSERT(index_ == 0); + UASSERT(index_ == nullptr); UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); featuresType_ = features.type(); featuresDim_ = features.cols; @@ -215,6 +354,7 @@ void FlannIndex::buildKDTreeIndex( rebalancingFactor_ = rebalancingFactor; rtflann::KDTreeIndexParams params(trees); + params["save_dataset"] = true; if(featuresType_ == CV_8UC1) { @@ -269,7 +409,7 @@ void FlannIndex::buildKDTreeSingleIndex( { UDEBUG(""); this->release(); - UASSERT(index_ == 0); + UASSERT(index_ == nullptr); UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); featuresType_ = features.type(); featuresDim_ = features.cols; @@ -331,15 +471,18 @@ void FlannIndex::buildLSHIndex( { UDEBUG(""); this->release(); - UASSERT(index_ == 0); + UASSERT(index_ == nullptr); UASSERT(features.type() == CV_8UC1); featuresType_ = features.type(); featuresDim_ = features.cols; useDistanceL1_ = true; rebalancingFactor_ = rebalancingFactor; + auto params = rtflann::LshIndexParams(12, 20, 2); + params["save_dataset"] = true; + rtflann::Matrix dataset(features.data, features.rows, features.cols); - index_ = new rtflann::Index >(dataset, rtflann::LshIndexParams(12, 20, 2)); + index_ = new rtflann::Index >(dataset, params); ((rtflann::Index >*)index_)->buildIndex(); // incremental FLANN: we should add all headers separately in case we remove @@ -362,7 +505,7 @@ void FlannIndex::buildLSHIndex( bool FlannIndex::isBuilt() { - return index_!=0; + return index_!=nullptr; } std::vector FlannIndex::addPoints(const cv::Mat & features) @@ -370,7 +513,7 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) if(!index_) { UERROR("Flann index not yet created!"); - return std::vector(); + return {}; } UASSERT(features.type() == featuresType_); UASSERT(features.cols == featuresDim_); @@ -379,7 +522,7 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) if(featuresType_ == CV_8UC1) { rtflann::Matrix points(features.data, features.rows, features.cols); - rtflann::Index > * index = (rtflann::Index >*)index_; + auto * index = (rtflann::Index >*)index_; removedPts = index->removedCount(); index->addPoints(points, 0); // Rebuild index if it is now X times in size @@ -396,7 +539,7 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) rtflann::Matrix points((float*)features.data, features.rows, features.cols); if(useDistanceL1_) { - rtflann::Index > * index = (rtflann::Index >*)index_; + auto * index = (rtflann::Index >*)index_; removedPts = index->removedCount(); index->addPoints(points, 0); // Rebuild index if it doubles in size @@ -410,7 +553,7 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) } else if(featuresDim_ <= 3) { - rtflann::Index > * index = (rtflann::Index >*)index_; + auto * index = (rtflann::Index >*)index_; removedPts = index->removedCount(); index->addPoints(points, 0); // Rebuild index if it doubles in size @@ -424,7 +567,7 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) } else { - rtflann::Index > * index = (rtflann::Index >*)index_; + auto * index = (rtflann::Index >*)index_; removedPts = index->removedCount(); index->addPoints(points, 0); // Rebuild index if it doubles in size @@ -442,9 +585,9 @@ std::vector FlannIndex::addPoints(const cv::Mat & features) { UASSERT(removedPts == removedIndexes_.size()); // clean not used features - for(std::list::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter) + for(int & removedIndexe : removedIndexes_) { - addedDescriptors_.erase(*iter); + addedDescriptors_.erase(removedIndexe); } removedIndexes_.clear(); } @@ -491,7 +634,7 @@ void FlannIndex::removePoint(unsigned int index) ((rtflann::Index >*)index_)->removePoint(index); } - removedIndexes_.push_back(index); + removedIndexes_.push_back((int)index); } void FlannIndex::knnSearch( @@ -592,4 +735,46 @@ void FlannIndex::radiusSearch( } } +void FlannIndex::serialize(const std::filesystem::path& filename) +{ + std::ofstream outFile(filename, std::ios::binary); + if (!outFile.is_open()) { + throw std::runtime_error("Unable to open file for writing"); + } + int mapSize = (int)addedDescriptors_.size(); + outFile.write(reinterpret_cast(&mapSize), sizeof(mapSize)); + outFile.write(reinterpret_cast(&nextIndex_), sizeof(nextIndex_)); + outFile.write(reinterpret_cast(&featuresType_), sizeof(featuresType_)); + outFile.write(reinterpret_cast(&featuresDim_), sizeof(featuresDim_)); + outFile.write(reinterpret_cast(&isLSH_), sizeof(isLSH_)); + outFile.write(reinterpret_cast(&useDistanceL1_), sizeof(useDistanceL1_)); + outFile.write(reinterpret_cast(&rebalancingFactor_), sizeof(rebalancingFactor_)); + for (const auto& entry : addedDescriptors_) { + // Prepare data for compression + std::vector buffer; + int key = entry.first; + const cv::Mat& m = entry.second; + int rows = m.rows; + int cols = m.cols; + int type = m.type(); + int channels = m.channels(); + buffer.insert(buffer.end(), reinterpret_cast(&key), reinterpret_cast(&key) + sizeof(key)); + buffer.insert(buffer.end(), reinterpret_cast(&rows), reinterpret_cast(&rows) + sizeof(rows)); + buffer.insert(buffer.end(), reinterpret_cast(&cols), reinterpret_cast(&cols) + sizeof(cols)); + buffer.insert(buffer.end(), reinterpret_cast(&type), reinterpret_cast(&type) + sizeof(type)); + buffer.insert(buffer.end(), reinterpret_cast(&channels), reinterpret_cast(&channels) ) + sizeof(channels); + int dataSize = rows * cols * (int)m.elemSize(); + buffer.insert(buffer.end(), reinterpret_cast(m.data), reinterpret_cast(m.data) + dataSize); + + // Compress the buffer + size_t compressedSize = LZ4_compressBound((int)buffer.size()); + std::vector compressedBuffer(compressedSize); + int actualCompressedSize = LZ4_compress_default(buffer.data(), compressedBuffer.data(), (int)buffer.size(), (int)compressedSize); + + // Write the size of the compressed data followed by the compressed data itself + outFile.write(reinterpret_cast(&actualCompressedSize), sizeof(actualCompressedSize)); + outFile.write(compressedBuffer.data(), actualCompressedSize); + } + outFile.close(); +} } /* namespace rtabmap */ diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 91e5b5ebf8..9db104186b 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -367,6 +367,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) // Now load the dictionary if we have a connection if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Loading dictionary...")); + _vwd->setSavedIndex(std::filesystem::path(_dbDriver->getUrl()).parent_path() / "vocabulary"); UDEBUG("Loading dictionary..."); if(loadAllNodesInWM) { @@ -3549,17 +3550,17 @@ void Memory::removeVirtualLinks(int signatureId) void Memory::dumpMemory(std::string directory) const { UINFO("Dumping memory to directory \"%s\"", directory.c_str()); - this->dumpDictionary((directory+"/DumpMemoryWordRef.txt").c_str(), (directory+"/DumpMemoryWordDesc.txt").c_str()); + this->dumpDictionary(directory); this->dumpSignatures((directory + "/DumpMemorySign.txt").c_str(), false); this->dumpSignatures((directory + "/DumpMemorySign3.txt").c_str(), true); this->dumpMemoryTree((directory + "/DumpMemoryTree.txt").c_str()); } -void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const +void Memory::dumpDictionary(std::string_view dir) const { if(_vwd) { - _vwd->exportDictionary(fileNameRef, fileNameDesc); + _vwd->save(dir); } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index b4561b3747..8a3c9182a3 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -318,7 +318,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas _databasePath = databasePath; if(!_databasePath.empty()) { - UASSERT(UFile::getExtension(_databasePath).compare("db") == 0); + UASSERT(UFile::getExtension(_databasePath) == "db"); UINFO("Using database \"%s\".", _databasePath.c_str()); } else @@ -4691,14 +4691,8 @@ void Rtabmap::dumpData() const UDEBUG(""); if(_memory) { - if(this->getWorkingDir().empty()) - { - UERROR("Working directory not set."); - } - else - { - _memory->dumpMemory(this->getWorkingDir()); - } + auto working_dir = std::filesystem::path(_databasePath).parent_path(); + _memory->dumpDictionary((working_dir / "vocabulary").string()); } } diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index fc1bb172f8..8916fce146 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -30,13 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Signature.h" #include "rtabmap/core/DBDriver.h" -#include "rtabmap/core/Parameters.h" #include "rtabmap/core/FlannIndex.h" #include "rtabmap/utilite/UtiLite.h" -#include - #if CV_MAJOR_VERSION < 3 #ifdef HAVE_OPENCV_GPU #include @@ -48,9 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif -#include -#include - #define KDTREE_SIZE 4 #define KNN_CHECKS 32 @@ -99,7 +93,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) bool incrementalDictionary = _incrementalDictionary; if((iter=parameters.find(Parameters::kKpDictionaryPath())) != parameters.end()) { - _newDictionaryPath = (*iter).second.c_str(); + _newDictionaryPath = (*iter).second; } if((iter=parameters.find(Parameters::kKpIncrementalDictionary())) != parameters.end()) { @@ -110,7 +104,8 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) bool treeUpdated = false; if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end()) { - NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str()); + // auto nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str()); + auto nnStrategy = static_cast(std::strtol((*iter).second.c_str(), nullptr, 10)); treeUpdated = this->setNNStrategy(nnStrategy); } if(!treeUpdated && byteToFloat!=_byteToFloat && _strategy == kNNFlannKdTree) @@ -129,12 +124,32 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) _incrementalDictionary = incrementalDictionary; } +void VWDictionary::setSavedIndex(const std::filesystem::path& dir) +{ + // check if saved files are needed + _useSavedFile = std::filesystem::exists(dir / "index.bin") && + std::filesystem::exists(dir / "vars.bin") && + std::filesystem::exists(dir / "VWDictionary.bin"); + + // set directory if files are existed + save_dir_ = dir; + + if(_useSavedFile) + { + UINFO("Use saved files at %s to load vocabulary!", save_dir_.c_str()); + } + else + { + UINFO("Can not find necessary files at %s to load vocabulary, rebuild flannIndex from database", save_dir_.c_str()); + } +} + void VWDictionary::setIncrementalDictionary() { if(!_incrementalDictionary) { _incrementalDictionary = true; - if(_visualWords.size()) + if(!_visualWords.empty()) { UWARN("Incremental dictionary set: already loaded visual words (%d) from the fixed dictionary will be included in the incremental one.", _visualWords.size()); } @@ -148,24 +163,24 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) UDEBUG(""); if(!dictionaryPath.empty()) { - if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) || - _visualWords.size() == 0) + if((!_incrementalDictionary && _dictionaryPath != dictionaryPath) || + _visualWords.empty()) { UINFO("incremental=%d, oldPath=%s newPath=%s, visual words=%d", _incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size()); - if(UFile::getExtension(dictionaryPath).compare("db") == 0) + if(UFile::getExtension(dictionaryPath) == "db") { UWARN("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str()); DBDriver * driver = DBDriver::create(); if(driver->openConnection(dictionaryPath, false)) { driver->load(this, false); - for(std::map::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter) + for(auto & _visualWord : _visualWords) { - iter->second->setSaved(true); + _visualWord.second->setSaved(true); } - _incrementalDictionary = _visualWords.size()==0; + _incrementalDictionary = _visualWords.empty(); driver->closeConnection(false); } else @@ -190,11 +205,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) std::getline(file, str); strList = uSplitNumChar(str); int dimension = 0; - for(std::list::iterator iter = strList.begin(); iter != strList.end(); ++iter) + for(auto & iter : strList) { - if(uIsDigit(iter->at(0))) + if(uIsDigit(iter.at(0))) { - dimension = std::atoi(iter->c_str()); + dimension = static_cast(std::strtol(iter.c_str(), nullptr, 10)); break; } } @@ -214,8 +229,9 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) if((int)strList.size() == dimension+1) { //first one is the visual word id - std::list::iterator iter = strList.begin(); - int id = std::atoi(iter->c_str()); + auto iter = strList.begin(); + auto id = static_cast(std::strtol(iter->c_str(), nullptr, 10)); + // int id = std::atoi(iter->c_str()); cv::Mat descriptor(1, dimension, CV_32F); ++iter; int i=0; @@ -230,7 +246,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) UERROR("Loaded word has not the same size (%d) than descriptor size previously detected (%d).", i, dimension); } - VisualWord * vw = new VisualWord(id, descriptor, 0); + auto * vw = new VisualWord(id, descriptor, 0); vw->setSaved(true); _visualWords.insert(_visualWords.end(), std::pair(id, vw)); _notIndexedWords.insert(_notIndexedWords.end(), id); @@ -241,7 +257,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) UWARN("Cannot parse line \"%s\"", str.c_str()); } } - if(_visualWords.size()) + if(!_visualWords.empty()) { UWARN("Loaded %d words!", (int)_visualWords.size()); } @@ -254,9 +270,9 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) file.close(); } - if(_visualWords.size() == 0) + if(_visualWords.empty()) { - _incrementalDictionary = _visualWords.size()==0; + _incrementalDictionary = _visualWords.empty(); UWARN("No words loaded, cannot set a fixed dictionary.", (int)_visualWords.size()); } else @@ -277,7 +293,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath) UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size()); } } - else if(_incrementalDictionary && _visualWords.size()) + else if(_incrementalDictionary && !_visualWords.empty()) { UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size()); } @@ -346,7 +362,7 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy) int VWDictionary::getLastIndexedWordId() const { - if(_mapIndexId.size()) + if(!_mapIndexId.empty()) { return _mapIndexId.rbegin()->second; } @@ -370,7 +386,7 @@ unsigned long VWDictionary::getMemoryUsed() const { long memoryUsage = sizeof(VWDictionary); memoryUsage += getIndexMemoryUsed(); - memoryUsage += _dataTree.total()*_dataTree.elemSize(); + memoryUsage += long( _dataTree.total()*_dataTree.elemSize() ); if(!_visualWords.empty()) { memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::map::iterator)) + sizeof(std::map); @@ -411,7 +427,7 @@ cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteTo for(int i=0; i(i); + auto * ptrOut = descriptorsOut.ptr(i); for(int j=0; j(i); + const auto * ptrIn = descriptorsIn.ptr(i); unsigned char * ptrOut = descriptorsOut.ptr(i); for(int j=0; jsetFixedDictionary(_newDictionaryPath); - if(!_incrementalDictionary && !_notIndexedWords.size()) + if(!_incrementalDictionary && _notIndexedWords.empty()) { // No need to update the search index if we // use a fixed dictionary and the index is @@ -482,29 +499,30 @@ void VWDictionary::update() } } - if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) + if(!_notIndexedWords.empty() || _visualWords.empty() || !_removedIndexedWords.empty()) { if(_incrementalFlann && _strategy < kNNBruteForce && - _visualWords.size()) + !_visualWords.empty()) { ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size()); - for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) + for(int _removedIndexedWord : _removedIndexedWords) { - UASSERT(uContains(_mapIdIndex, *iter)); - UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter))); - _flannIndex->removePoint(_mapIdIndex.at(*iter)); - _mapIndexId.erase(_mapIdIndex.at(*iter)); - _mapIdIndex.erase(*iter); + UASSERT(uContains(_mapIdIndex, _removedIndexedWord)); + UASSERT(uContains(_mapIndexId, _mapIdIndex.at(_removedIndexedWord))); + _flannIndex->removePoint(_mapIdIndex.at(_removedIndexedWord)); + _mapIndexId.erase(_mapIdIndex.at(_removedIndexedWord)); + _mapIdIndex.erase(_removedIndexedWord); } - ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size()); + UINFO("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size()); - if(_notIndexedWords.size()) + // not rebuild tree when init + if(!_notIndexedWords.empty() && !_useSavedFile) { - ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size()); - for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + UINFO("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size()); + for(const int& notIndexedWord : _notIndexedWords) { - VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + VisualWord* w = uValue(_visualWords, notIndexedWord, (VisualWord*)nullptr); UASSERT(w); cv::Mat descriptor; @@ -553,7 +571,7 @@ void VWDictionary::update() UASSERT(descriptor.cols == _flannIndex->featuresDim()); UASSERT(descriptor.type() == _flannIndex->featuresType()); UASSERT(descriptor.rows == 1); - index = _flannIndex->addPoints(descriptor).front(); + index = (int)_flannIndex->addPoints(descriptor).front(); } std::pair::iterator, bool> inserted; inserted = _mapIndexId.insert(std::pair(index, w->id())); @@ -561,21 +579,29 @@ void VWDictionary::update() inserted = _mapIdIndex.insert(std::pair(w->id(), index)); UASSERT(inserted.second); } - ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size()); + UINFO("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size()); + } + + // load tree and mapId when files are provided + // load only once initing -> set _useSavedFile = false after inited + if (_useSavedFile) + { + load(save_dir_); + _useSavedFile = false; } } else if(_strategy >= kNNBruteForce && - _notIndexedWords.size() && - _removedIndexedWords.size() == 0 && - _visualWords.size() && + !_notIndexedWords.empty() && + _removedIndexedWords.empty() && + !_visualWords.empty() && _dataTree.rows) { //just add not indexed words int i = _dataTree.rows; _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); - for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + for(int _notIndexedWord : _notIndexedWords) { - VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + VisualWord* w = uValue(_visualWords, _notIndexedWord, (VisualWord*)nullptr); UASSERT(w); UASSERT(w->getDescriptor().cols == _dataTree.cols); UASSERT(w->getDescriptor().type() == _dataTree.type()); @@ -593,7 +619,7 @@ void VWDictionary::update() _dataTree = cv::Mat(); _flannIndex->release(); - if(_visualWords.size()) + if(!_visualWords.empty()) { UTimer timer; timer.start(); @@ -625,9 +651,9 @@ void VWDictionary::update() UASSERT(dim > 0); // Create the data matrix - _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F - std::map::const_iterator iter = _visualWords.begin(); - for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + _dataTree = cv::Mat((int)_visualWords.size(), dim, type); // SURF descriptors are CV_32F + auto iter = _visualWords.begin(); + for(auto i=0; i < (int)_visualWords.size(); ++i, ++iter) { cv::Mat descriptor; if(iter->second->getDescriptor().type() == CV_8U) @@ -649,7 +675,7 @@ void VWDictionary::update() UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str()); UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str()); - descriptor.copyTo(_dataTree.row(i)); + descriptor.copyTo(_dataTree.row((int)i)); _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); _mapIdIndex.insert(_mapIdIndex.end(), std::pair(iter->second->id(), i)); } @@ -677,7 +703,7 @@ void VWDictionary::update() ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); } } - UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)", + UINFO("Dictionary updated! (size=%d added=%d removed=%d)", _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); } else @@ -687,6 +713,9 @@ void VWDictionary::update() _notIndexedWords.clear(); _removedIndexedWords.clear(); UDEBUG(""); + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + std::cout << __PRETTY_FUNCTION__ << " tooks: " << duration.count() << " ms" << std::endl; } void VWDictionary::clear(bool printWarningsIfNotEmpty) @@ -694,18 +723,18 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty) ULOGGER_DEBUG(""); if(printWarningsIfNotEmpty) { - if(_visualWords.size() && _incrementalDictionary) + if(!_visualWords.empty() && _incrementalDictionary) { UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size()); } - if(_notIndexedWords.size()) + if(!_notIndexedWords.empty()) { UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size()); } } - for(std::map::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i) + for(auto & _visualWord : _visualWords) { - delete (*i).second; + delete _visualWord.second; } _visualWords.clear(); _notIndexedWords.clear(); @@ -727,7 +756,7 @@ int VWDictionary::getNextId() void VWDictionary::addWordRef(int wordId, int signatureId) { - VisualWord * vw = 0; + VisualWord * vw = nullptr; vw = uValue(_visualWords, wordId, vw); if(vw) { @@ -744,12 +773,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId) void VWDictionary::removeAllWordRef(int wordId, int signatureId) { - VisualWord * vw = 0; + VisualWord * vw = nullptr; vw = uValue(_visualWords, wordId, vw); if(vw) { _totalActiveReferences -= vw->removeAllRef(signatureId); - if(vw->getReferences().size() == 0) + if(vw->getReferences().empty()) { _unusedWords.insert(std::pair(vw->id(), vw)); } @@ -778,7 +807,7 @@ std::list VWDictionary::addNewWords( // verify we have the same features int dim = 0; int type = -1; - if(_visualWords.size()) + if(!_visualWords.empty()) { dim = _visualWords.begin()->second->getDescriptor().cols; type = _visualWords.begin()->second->getDescriptor().type(); @@ -858,13 +887,13 @@ std::list VWDictionary::addNewWords( if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH) { - _flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS); + _flannIndex->knnSearch(descriptors, results, dists, (int)k, KNN_CHECKS); } else if(_strategy == kNNBruteForce) { bruteForce = true; cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); - matcher.knnMatch(descriptors, _dataTree, matches, k); + matcher.knnMatch(descriptors, _dataTree, matches, (int)k); } else if(_strategy == kNNBruteForceGPU) { @@ -953,12 +982,12 @@ std::list VWDictionary::addNewWords( } } } - else if(bruteForce && matches.size()) + else if(bruteForce && !matches.empty()) { - for(unsigned int j=0; j= 0.0f && id != 0) { if(isL2NotSqr) @@ -983,10 +1012,10 @@ std::list VWDictionary::addNewWords( UASSERT(descriptors.cols == newWords.cols && descriptors.type() == newWords.type()); matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1); UASSERT(matchesNewWords.size() == 1); - for(unsigned int j=0; j= 0.0f && id != 0) { fullResults.insert(std::pair(d, id)); @@ -1001,7 +1030,7 @@ std::list VWDictionary::addNewWords( if(_incrementalDictionary) { bool badDist = false; - if(fullResults.size() == 0) + if(fullResults.empty()) { badDist = true; } @@ -1024,7 +1053,7 @@ std::list VWDictionary::addNewWords( if(badDist) { // use original descriptor - VisualWord * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId); + auto * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId); _visualWords.insert(_visualWords.end(), std::pair(vw->id(), vw)); _notIndexedWords.insert(_notIndexedWords.end(), vw->id()); newWords.push_back(descriptors.row(i)); @@ -1047,7 +1076,7 @@ std::list VWDictionary::addNewWords( wordIds.push_back(fullResults.begin()->second); } } - else if(fullResults.size()) + else if(!fullResults.empty()) { // If the dictionary is not incremental, just take the nearest word ++dupWordsCountFromDict; @@ -1063,7 +1092,7 @@ std::list VWDictionary::addNewWords( dupWordsCountFromDict+dupWordsCountFromLast, dupWordsCountFromLast); UDEBUG("total time %fs", timer.ticks()); - _totalActiveReferences += _notIndexedWords.size(); + _totalActiveReferences += (int)_notIndexedWords.size(); return wordIds; } @@ -1072,7 +1101,7 @@ std::vector VWDictionary::findNN(const std::list & vws) const UTimer timer; timer.start(); - if(_visualWords.size() && vws.size()) + if(!_visualWords.empty() && !vws.empty()) { int type = (*vws.begin())->getDescriptor().type(); int dim = (*vws.begin())->getDescriptor().cols; @@ -1092,8 +1121,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const // fill the request matrix int index = 0; VisualWord * vw; - cv::Mat query(vws.size(), dim, type); - for(std::list::const_iterator iter=vws.begin(); iter!=vws.end(); ++iter, ++index) + cv::Mat query((int)vws.size(), dim, type); + for(auto iter=vws.begin(); iter!=vws.end(); ++iter, ++index) { vw = *iter; UASSERT(vw); @@ -1116,7 +1145,7 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const std::vector resultIds(queryIn.rows, 0); unsigned int k=2; // k nearest neighbor - if(_visualWords.size() && queryIn.rows) + if(!_visualWords.empty() && queryIn.rows) { // verify we have the same features int dim = _visualWords.begin()->second->getDescriptor().cols; @@ -1185,13 +1214,13 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH) { - _flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS); + _flannIndex->knnSearch(query, results, dists, (int)k, KNN_CHECKS); } else if(_strategy == kNNBruteForce) { bruteForce = true; cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); - matcher.knnMatch(query, _dataTree, matches, k); + matcher.knnMatch(query, _dataTree, matches, (int)k); } else if(_strategy == kNNBruteForceGPU) { @@ -1256,10 +1285,10 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const std::vector > matchesNotIndexed; if(!_notIndexedWords.empty()) { - cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type()); + cv::Mat dataNotIndexed = cv::Mat::zeros((int)_notIndexedWords.size(), query.cols, query.type()); unsigned int index = 0; VisualWord * vw; - for(std::set::iterator iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index) + for(auto iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index) { vw = _visualWords.at(*iter); @@ -1279,8 +1308,8 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const { descriptor = vw->getDescriptor(); } - UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type()); - descriptor.copyTo(dataNotIndexed.row(index)); + UASSERT(vw != nullptr && descriptor.cols == query.cols && descriptor.type() == query.type()); + descriptor.copyTo(dataNotIndexed.row((int)index)); mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair(index, vw->id())); } // Find nearest neighbor @@ -1315,12 +1344,12 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const } } } - else if(bruteForce && matches.size()) + else if(bruteForce && !matches.empty()) { - for(unsigned int j=0; j= 0.0f && id != 0) { if(isL2NotSqr) @@ -1334,12 +1363,12 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const } // not indexed.. - if(matchesNotIndexed.size()) + if(!matchesNotIndexed.empty()) { - for(unsigned int j=0; j= 0.0f && id != 0) { fullResults.insert(std::pair(d, id)); @@ -1354,7 +1383,7 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const if(_incrementalDictionary) { bool badDist = false; - if(fullResults.size() == 0) + if(fullResults.empty()) { badDist = true; } @@ -1379,7 +1408,7 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const resultIds[i] = fullResults.begin()->second; // Accepted } } - else if(fullResults.size()) + else if(!fullResults.empty()) { //Just take the nearest if the dictionary is not incremental resultIds[i] = fullResults.begin()->second; // Accepted @@ -1396,7 +1425,7 @@ void VWDictionary::addWord(VisualWord * vw) { _visualWords.insert(std::pair(vw->id(), vw)); _notIndexedWords.insert(vw->id()); - if(vw->getReferences().size()) + if(!vw->getReferences().empty()) { _totalActiveReferences += uSum(uValues(vw->getReferences())); } @@ -1413,12 +1442,12 @@ void VWDictionary::addWord(VisualWord * vw) const VisualWord * VWDictionary::getWord(int id) const { - return uValue(_visualWords, id, (VisualWord *)0); + return uValue(_visualWords, id, (VisualWord *)nullptr); } VisualWord * VWDictionary::getUnusedWord(int id) const { - return uValue(_unusedWords, id, (VisualWord *)0); + return uValue(_unusedWords, id, (VisualWord *)nullptr); } std::vector VWDictionary::getUnusedWords() const @@ -1434,13 +1463,13 @@ std::vector VWDictionary::getUnusedWordIds() const void VWDictionary::removeWords(const std::vector & words) { //UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size()); - for(unsigned int i=0; iid()); - _unusedWords.erase(words[i]->id()); - if(_notIndexedWords.erase(words[i]->id()) == 0) + _visualWords.erase(word->id()); + _unusedWords.erase(word->id()); + if(_notIndexedWords.erase(word->id()) == 0) { - _removedIndexedWords.insert(words[i]->id()); + _removedIndexedWords.insert(word->id()); } } } @@ -1449,9 +1478,9 @@ void VWDictionary::deleteUnusedWords() { std::vector unusedWords = uValues(_unusedWords); removeWords(unusedWords); - for(unsigned int i=0; i::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter) + for(auto _visualWord : _visualWords) { // References if(foutRef) { - fprintf(foutRef, "%d ", (*iter).first); - const std::map ref = (*iter).second->getReferences(); - for(std::map::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter) + fprintf(foutRef, "%d ", _visualWord.first); + const std::map ref = _visualWord.second->getReferences(); + for(auto jter : ref) { - for(int i=0; i<(*jter).second; ++i) + for(int i=0; igetDescriptor().data; - int dim = (*iter).second->getDescriptor().cols; + fprintf(foutDesc, "%d ", _visualWord.first); + const auto * desc = (const float *)_visualWord.second->getDescriptor().data; + int dim = _visualWord.second->getDescriptor().cols; for(int i=0; isave(index_path); + UINFO("saved index and its data points at: %s", index_path.c_str()); + + // serialization binary map: Index <-> Id + std::filesystem::path mapIdexId = std::filesystem::path(dir) / "VWDictionary.bin"; + UINFO("serializing class at : %s", mapIdexId.c_str()); + saveVars(mapIdexId); + UINFO("serialized class at : %s", mapIdexId.c_str()); + + // serialization flannIndex class + std::filesystem::path flannIndex_path = std::filesystem::path( dir ) / "vars.bin"; + UINFO("serializing class at : %s", flannIndex_path.c_str()); + _flannIndex->serialize(flannIndex_path); + UINFO("serialized class at : %s", flannIndex_path.c_str()); +} + +void VWDictionary::load(std::string_view dir) +{ + delete _flannIndex; + _flannIndex = new rtabmap::FlannIndex() ; + _flannIndex->load(dir); + loadMapIndex(std::filesystem::path(dir) / "VWDictionary.bin"); +} + +void VWDictionary::loadMapIndex(const std::filesystem::path& file) +{ + std::ifstream inFile(file, std::ios::binary); + if (!inFile.is_open()) { + throw std::runtime_error("Unable to open file for reading"); + } + + int size, key, value; + inFile.read(reinterpret_cast(&size), sizeof(size)); + _mapIndexId.clear(); + _mapIdIndex.clear(); + + for (int i = 0; i < size; ++i) { + inFile.read(reinterpret_cast(&key), sizeof(key)); + inFile.read(reinterpret_cast(&value), sizeof(value)); + _mapIndexId[key] = value; + _mapIdIndex[value] = key; + } + + inFile.close(); +} + +void VWDictionary::saveVars(const std::filesystem::path& filename) const +{ + std::ofstream outFile(filename, std::ios::binary); + if (!outFile.is_open()) { + throw std::runtime_error("Unable to open file for writing"); + } + + int size = (int)_mapIndexId.size(); + outFile.write(reinterpret_cast(&size), sizeof(size)); + for (const auto& pair : _mapIndexId) { + outFile.write(reinterpret_cast(&pair.first), sizeof(pair.first)); + outFile.write(reinterpret_cast(&pair.second), sizeof(pair.second)); + } + + outFile.close(); +} } // namespace rtabmap diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index 49c64bea45..d5ee6cb5a2 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -207,7 +207,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone(); descriptors_.push_back(descriptor); } - if (keypoints.size() != descriptors_.rows) + if (keypoints.size() != (size_t)descriptors_.rows) { UWARN("keypoints size is not match descriptors size %d vs %d", static_cast(keypoints.size()), descriptors_.rows); keypoints.clear(); From 6f26b215fb46efc360147ac9335e44f766b577c9 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Wed, 21 Feb 2024 12:43:55 +0100 Subject: [PATCH 06/13] compile with c++17 --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 295d106a3a..4a18d8d157 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,6 +33,9 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION}) SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") ####### COMPILATION PARAMS ####### +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release: IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles") IF("${CMAKE_BUILD_TYPE}" STREQUAL "") From 3f352758ba9fa4ebe7540ab3e1165ae3bd344fb4 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Thu, 22 Feb 2024 15:40:39 +0100 Subject: [PATCH 07/13] suppress log --- corelib/src/VWDictionary.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 8916fce146..60855d8c46 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -715,7 +715,7 @@ void VWDictionary::update() UDEBUG(""); auto end = std::chrono::high_resolution_clock::now(); auto duration = std::chrono::duration_cast(end - start); - std::cout << __PRETTY_FUNCTION__ << " tooks: " << duration.count() << " ms" << std::endl; + UINFO("Update took: %d ms", duration.count()); } void VWDictionary::clear(bool printWarningsIfNotEmpty) From 0d201a34ed6b53fe124c8a51022f20b6a642ca39 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Thu, 29 Feb 2024 13:09:13 +0100 Subject: [PATCH 08/13] check tmp file --- corelib/src/VWDictionary.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 60855d8c46..d077b8146e 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -129,7 +129,8 @@ void VWDictionary::setSavedIndex(const std::filesystem::path& dir) // check if saved files are needed _useSavedFile = std::filesystem::exists(dir / "index.bin") && std::filesystem::exists(dir / "vars.bin") && - std::filesystem::exists(dir / "VWDictionary.bin"); + std::filesystem::exists(dir / "VWDictionary.bin") && + !std::filesystem::exists(dir / "tmp.txt"); // set directory if files are existed save_dir_ = dir; From cd6c4bae74d42a8846f552d283c67b1dfd987fcf Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 23 Apr 2024 10:33:11 +0200 Subject: [PATCH 09/13] Revert "fix ros gtsam 4.2a9 build (#1108)" This reverts commit 59046f1de279427f32343ccda2a5f70a915d926e. --- corelib/src/optimizer/OptimizerGTSAM.cpp | 8 ---- corelib/src/optimizer/gtsam/GravityFactor.h | 44 ++++--------------- corelib/src/optimizer/gtsam/XYFactor.h | 7 +-- corelib/src/optimizer/gtsam/XYZFactor.h | 14 +----- .../optimizer/vertigo/gtsam/DerivedValue.h | 7 +-- .../vertigo/gtsam/betweenFactorMaxMix.h | 4 +- .../vertigo/gtsam/betweenFactorSwitchable.h | 12 ----- .../vertigo/gtsam/switchVariableLinear.h | 17 +------ .../vertigo/gtsam/switchVariableSigmoid.h | 15 +------ 9 files changed, 17 insertions(+), 111 deletions(-) diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index ac2afff0a4..ff256493f0 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -527,11 +527,7 @@ std::map OptimizerGTSAM::optimize( { float x,y,z,roll,pitch,yaw; std::map tmpPoses; -#if GTSAM_VERSION_NUMERIC >= 40200 - for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#endif { if(iter->value.dim() > 1) { @@ -634,11 +630,7 @@ std::map OptimizerGTSAM::optimize( optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); float x,y,z,roll,pitch,yaw; -#if GTSAM_VERSION_NUMERIC >= 40200 - for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#else for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#endif { if(iter->value.dim() > 1) { diff --git a/corelib/src/optimizer/gtsam/GravityFactor.h b/corelib/src/optimizer/gtsam/GravityFactor.h index 02c7a08ffc..f292072f13 100644 --- a/corelib/src/optimizer/gtsam/GravityFactor.h +++ b/corelib/src/optimizer/gtsam/GravityFactor.h @@ -63,21 +63,15 @@ class GravityFactor { /** vector of errors */ Vector attitudeError(const Rot3& p, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalJacobian<2,3> H = {}) const; -#else - OptionalJacobian<2,3> H = boost::none) const; -#endif + OptionalJacobian<2,3> H = boost::none) const; /** Serialization function */ -#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); } -#endif }; /** @@ -91,11 +85,8 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { public: /// shorthand for a smart pointer to a factor -#if GTSAM_VERSION_NUMERIC >= 40300 - typedef std::shared_ptr shared_ptr; -#else typedef boost::shared_ptr shared_ptr; -#endif + /// Typedef to this class typedef Rot3GravityFactor This; @@ -120,13 +111,10 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { -#if GTSAM_VERSION_NUMERIC >= 40300 - return std::static_pointer_cast( -#else return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } -#endif + /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -136,11 +124,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /** vector of errors */ virtual Vector evaluateError(const Rot3& nRb, // -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else - boost::optional H = boost::none) const { -#endif + boost::optional H = boost::none) const { return attitudeError(nRb, H); } Unit3 nZ() const { @@ -151,7 +135,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { } private: -#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 + /** Serialization function */ friend class boost::serialization::access; template @@ -161,7 +145,6 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } -#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -180,11 +163,8 @@ class Pose3GravityFactor: public NoiseModelFactor1, public: /// shorthand for a smart pointer to a factor -#if GTSAM_VERSION_NUMERIC >= 40300 - typedef std::shared_ptr shared_ptr; -#else typedef boost::shared_ptr shared_ptr; -#endif + /// Typedef to this class typedef Pose3GravityFactor This; @@ -209,11 +189,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { -#if GTSAM_VERSION_NUMERIC >= 40300 - return std::static_pointer_cast( -#else return boost::static_pointer_cast( -#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -226,11 +202,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, /** vector of errors */ virtual Vector evaluateError(const Pose3& nTb, // -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else boost::optional H = boost::none) const { -#endif Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; @@ -247,7 +219,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, } private: -#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 + /** Serialization function */ friend class boost::serialization::access; template @@ -257,7 +229,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } -#endif + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/corelib/src/optimizer/gtsam/XYFactor.h b/corelib/src/optimizer/gtsam/XYFactor.h index 8a7c65585c..d13a3bc8a0 100644 --- a/corelib/src/optimizer/gtsam/XYFactor.h +++ b/corelib/src/optimizer/gtsam/XYFactor.h @@ -41,12 +41,7 @@ class XYFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose2 // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const VALUE& p, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else - boost::optional H = boost::none) const { -#endif + gtsam::Vector evaluateError(const VALUE& p, boost::optional H = boost::none) const { // note that use boost optional like a pointer // only calculate jacobian matrix when non-null pointer exists diff --git a/corelib/src/optimizer/gtsam/XYZFactor.h b/corelib/src/optimizer/gtsam/XYZFactor.h index 42e55d0b83..d4ed63c01a 100644 --- a/corelib/src/optimizer/gtsam/XYZFactor.h +++ b/corelib/src/optimizer/gtsam/XYZFactor.h @@ -41,24 +41,14 @@ class XYZFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const gtsam::Pose3& p, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else - boost::optional H = boost::none) const { -#endif + gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional H = boost::none) const { if(H) { p.translation(H); } return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } - gtsam::Vector evaluateError(const gtsam::Point3& p, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H = OptionalNone) const { -#else - boost::optional H = boost::none) const { -#endif + gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional H = boost::none) const { return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h index ecf5fcad25..66cf6954e9 100644 --- a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h +++ b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h @@ -72,15 +72,10 @@ class DerivedValue : public gtsam::Value { /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ -#if GTSAM_VERSION_NUMERIC >= 40300 - virtual std::shared_ptr clone() const { - return std::make_shared(static_cast(*this)); - } - #else virtual boost::shared_ptr clone() const { return boost::make_shared(static_cast(*this)); } -#endif + /// equals implementing generic Value interface virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const { // Cast the base class Value pointer to a derived class pointer diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h index 40538fc0f0..0ee996f64f 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h @@ -12,7 +12,7 @@ #include #include -#if GTSAM_VERSION_NUMERIC >= 40100 +#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) namespace gtsam { gtsam::Matrix inverse(const gtsam::Matrix & matrix) { @@ -49,7 +49,7 @@ namespace vertigo { double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); double l1 = nu1 * exp(-0.5*m1); -#if GTSAM_VERSION_NUMERIC >= 40100 +#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) double m2 = nullHypothesisModel->squaredMahalanobisDistance(error); #else double m2 = nullHypothesisModel->distance(error); diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index 1bfe91ea4f..ab5a443840 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h @@ -30,15 +30,9 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone) const -#else boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const -#endif { // calculate error @@ -70,15 +64,9 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone) const -#else boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const -#endif { // calculate error diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index e95e0b568b..2260ea6077 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -76,13 +76,8 @@ namespace vertigo { /** between operation */ inline SwitchVariableLinear between(const SwitchVariableLinear& l2, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1=OptionalNone, - OptionalMatrixType H2=OptionalNone) const { -#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { -#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableLinear(l2.value() - value()); @@ -121,19 +116,11 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, -#if GTSAM_VERSION_NUMERIC >= 40300 - ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { -#else - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { -#endif + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other); } static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, -#if GTSAM_VERSION_NUMERIC >= 40300 - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { -#else - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { -#endif + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { return g.retract(v); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 79e1fca99d..674e897ac7 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -76,13 +76,8 @@ namespace vertigo { /** between operation */ inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, -#if GTSAM_VERSION_NUMERIC >= 40300 - OptionalMatrixType H1=OptionalNone, - OptionalMatrixType H2=OptionalNone) const { -#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { -#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableSigmoid(l2.value() - value()); @@ -122,19 +117,11 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, -#if GTSAM_VERSION_NUMERIC >= 40300 - ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { -#else - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { -#endif + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other); } static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, -#if GTSAM_VERSION_NUMERIC >= 40300 - ChartJacobian H1 = {}, ChartJacobian H2 = {}) { -#else ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { -#endif return g.retract(v); } }; From 35fc5999e7f7e8077c2a70ae87dc2b81f10a9258 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 23 Apr 2024 10:33:21 +0200 Subject: [PATCH 10/13] Revert "Fixed superglue deadlock on standalone (#896). Fixed some elemSize opencv asserts in debug build (876)" This reverts commit f7e4b38dd712c7d4a8d9fb86928497602f8b3dd6. --- CMakeLists.txt | 1 - .../include/rtabmap/core/PythonInterface.h | 26 +++---- corelib/src/CMakeLists.txt | 1 - corelib/src/DBDriverSqlite3.cpp | 6 +- corelib/src/SensorData.cpp | 30 ++++---- corelib/src/Signature.cpp | 2 +- corelib/src/python/PyDetector.cpp | 26 ++++--- corelib/src/python/PyDetector.h | 2 +- corelib/src/python/PyMatcher.cpp | 24 +++--- corelib/src/python/PyMatcher.h | 2 +- corelib/src/python/PythonInterface.cpp | 77 ++++++++++++++++--- 11 files changed, 129 insertions(+), 68 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a18d8d157..1ddde68be6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -392,7 +392,6 @@ IF(WITH_PYTHON) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy) IF(Python3_FOUND) MESSAGE(STATUS "Found Python3") - FIND_PACKAGE(pybind11 REQUIRED) ENDIF(Python3_FOUND) ENDIF(WITH_PYTHON) diff --git a/corelib/include/rtabmap/core/PythonInterface.h b/corelib/include/rtabmap/core/PythonInterface.h index f2f3a6a272..2216cb5ffa 100644 --- a/corelib/include/rtabmap/core/PythonInterface.h +++ b/corelib/include/rtabmap/core/PythonInterface.h @@ -11,30 +11,30 @@ #include #include - -namespace pybind11 { -class scoped_interpreter; -class gil_scoped_release; -} +#include namespace rtabmap { -/** - * Create a single PythonInterface on main thread at - * global scope before any Python classes. - */ class PythonInterface { public: PythonInterface(); virtual ~PythonInterface(); +protected: + std::string getTraceback(); // should be called between lock() and unlock() + void lock(); + void unlock(); + private: - pybind11::scoped_interpreter* guard_; - pybind11::gil_scoped_release* release_; -}; + static UMutex mutex_; + static int refCount_; -std::string getPythonTraceback(); +protected: + static PyThreadState * mainThreadState_; + static unsigned long mainThreadID_; + PyThreadState * threadState_; +}; } diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 25ef96a08d..7e6cd99687 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -209,7 +209,6 @@ IF(WITH_PYTHON AND Python3_FOUND) ${LIBRARIES} Python3::Python Python3::NumPy - pybind11::embed ) SET(SRC_FILES ${SRC_FILES} diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 280c228acd..453c52201e 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) { _memoryUsedEstimate += (*i)->getMemoryUsed(); // raw data are not kept in database - _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); stepNode(ppStmt, *i); } diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 147e854d69..0c0fddbbc9 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector & keypoints, const unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes { return sizeof(SensorData) + - (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) + - (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) + - (_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) + - (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) + - (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) + - (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) + - (_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) + - (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) + - (_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) + - (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) + - (_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) + - (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+ - (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) + - (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+ + _imageCompressed.total()*_imageCompressed.elemSize() + + _imageRaw.total()*_imageRaw.elemSize() + + _depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() + + _depthOrRightRaw.total()*_depthOrRightRaw.elemSize() + + _userDataCompressed.total()*_userDataCompressed.elemSize() + + _userDataRaw.total()*_userDataRaw.elemSize() + + _laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() + + _laserScanRaw.data().total()*_laserScanRaw.data().elemSize() + + _groundCellsCompressed.total()*_groundCellsCompressed.elemSize() + + _groundCellsRaw.total()*_groundCellsRaw.elemSize() + + _obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() + + _obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+ + _emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() + + _emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+ _keypoints.size() * sizeof(cv::KeyPoint) + _keypoints3D.size() * sizeof(cv::Point3f) + - (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize()); + _descriptors.total()*_descriptors.elemSize(); } void SensorData::clearCompressedData(bool images, bool scan, bool userData) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index ee08ab6804..53f843b2b8 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -348,7 +348,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo total += _words.size() * (sizeof(int)*2+sizeof(std::multimap::iterator)) + sizeof(std::multimap); total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector); total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector); - total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); + total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map::iterator)) + sizeof(std::map); if(withSensorData) { diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index d5ee6cb5a2..a532085467 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -10,8 +10,6 @@ #include #include -#include - #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -34,7 +32,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) : return; } - pybind11::gil_scoped_acquire acquire; + lock(); std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -56,13 +54,15 @@ PyDetector::PyDetector(const ParametersMap & parameters) : if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); } + + unlock(); } PyDetector::~PyDetector() { - pybind11::gil_scoped_acquire acquire; + lock(); if(pFunc_) { @@ -72,6 +72,8 @@ PyDetector::~PyDetector() { Py_DECREF(pModule_); } + + unlock(); } void PyDetector::parseParameters(const ParametersMap & parameters) @@ -100,7 +102,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag return keypoints; } - pybind11::gil_scoped_acquire acquire; + lock(); if(!pFunc_) { @@ -114,7 +116,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return keypoints; } Py_DECREF(result); @@ -127,7 +129,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -139,7 +141,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return keypoints; } Py_DECREF(pFunc); @@ -147,7 +149,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return keypoints; } UDEBUG("init time = %fs", timer.ticks()); @@ -165,7 +167,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); } else { @@ -225,6 +227,8 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag Py_DECREF(pImageBuffer); } + unlock(); + return keypoints; } diff --git a/corelib/src/python/PyDetector.h b/corelib/src/python/PyDetector.h index 5f292abdc5..4512b8eb02 100644 --- a/corelib/src/python/PyDetector.h +++ b/corelib/src/python/PyDetector.h @@ -18,7 +18,7 @@ namespace rtabmap { -class PyDetector : public Feature2D +class PyDetector : public Feature2D, public PythonInterface { public: PyDetector(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index aac482efe1..446f28c961 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -10,8 +10,6 @@ #include #include -#include - #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -41,7 +39,7 @@ PyMatcher::PyMatcher( return; } - pybind11::gil_scoped_acquire acquire; + lock(); std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -61,13 +59,15 @@ PyMatcher::PyMatcher( if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); } + + unlock(); } PyMatcher::~PyMatcher() { - pybind11::gil_scoped_acquire acquire; + lock(); if(pFunc_) { Py_DECREF(pFunc_); @@ -76,6 +76,7 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } + unlock(); } std::vector PyMatcher::match( @@ -103,7 +104,7 @@ std::vector PyMatcher::match( imageSize.width>0 && imageSize.height>0) { - pybind11::gil_scoped_acquire acquire; + lock(); UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0); @@ -119,7 +120,7 @@ std::vector PyMatcher::match( if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return matches; } Py_DECREF(result); @@ -132,7 +133,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -144,7 +145,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return matches; } Py_DECREF(pFunc); @@ -152,7 +153,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); return matches; } UDEBUG("init time = %fs", timer.ticks()); @@ -215,7 +216,7 @@ std::vector PyMatcher::match( if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getPythonTraceback().c_str()); + UERROR("%s", getTraceback().c_str()); } else { @@ -259,6 +260,7 @@ std::vector PyMatcher::match( UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks()); } + unlock(); } else { diff --git a/corelib/src/python/PyMatcher.h b/corelib/src/python/PyMatcher.h index da04cd466f..2af25c75cc 100644 --- a/corelib/src/python/PyMatcher.h +++ b/corelib/src/python/PyMatcher.h @@ -16,7 +16,7 @@ namespace rtabmap { -class PyMatcher +class PyMatcher : public PythonInterface { public: PyMatcher(const std::string & pythonMatcherPath, diff --git a/corelib/src/python/PythonInterface.cpp b/corelib/src/python/PythonInterface.cpp index 0e2d987980..7e9afb48b3 100644 --- a/corelib/src/python/PythonInterface.cpp +++ b/corelib/src/python/PythonInterface.cpp @@ -8,26 +8,83 @@ #include #include #include -#include namespace rtabmap { -PythonInterface::PythonInterface() +UMutex PythonInterface::mutex_; +int PythonInterface::refCount_ = 0; +PyThreadState * PythonInterface::mainThreadState_ = 0; +unsigned long PythonInterface::mainThreadID_ = 0; + +PythonInterface::PythonInterface() : + threadState_(0) { - UINFO("Initialize python interpreter"); - guard_ = new pybind11::scoped_interpreter(); - pybind11::module::import("threading"); - release_ = new pybind11::gil_scoped_release(); + UScopeMutex lockM(mutex_); + if(refCount_ == 0) + { + UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId()); + // initialize Python + Py_Initialize(); + + // initialize thread support + PyEval_InitThreads(); + Py_DECREF(PyImport_ImportModule("threading")); + + //release the GIL, store thread state, set the current thread state to NULL + mainThreadState_ = PyEval_SaveThread(); + UASSERT(mainThreadState_); + mainThreadID_ = UThread::currentThreadId(); + } + + ++refCount_; } PythonInterface::~PythonInterface() { - UINFO("Finalize python interpreter"); - delete release_; - delete guard_; + UScopeMutex lock(mutex_); + if(refCount_>0 && --refCount_==0) + { + // shut down the interpreter + UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId()); + PyEval_RestoreThread(mainThreadState_); + Py_Finalize(); + } +} + +void PythonInterface::lock() +{ + mutex_.lock(); + + UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); + if(UThread::currentThreadId() == mainThreadID_) + { + PyEval_RestoreThread(mainThreadState_); + } + else + { + // create a thread state object for this thread + threadState_ = PyThreadState_New(mainThreadState_->interp); + UASSERT(threadState_); + PyEval_RestoreThread(threadState_); + } +} + +void PythonInterface::unlock() +{ + if(UThread::currentThreadId() == mainThreadID_) + { + mainThreadState_ = PyEval_SaveThread(); + } + else + { + PyThreadState_Clear(threadState_); + PyThreadState_DeleteCurrent(); + } + UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); + mutex_.unlock(); } -std::string getPythonTraceback() +std::string PythonInterface::getTraceback() { // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated From bece9334e209581a1492da3818c4c4f17db55f9e Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 23 Apr 2024 10:46:10 +0200 Subject: [PATCH 11/13] Merge master origin --- .appveyor.yml | 5 +- .devcontainer/devcontainer.json | 8 + .github/workflows/cmake-ros.yml | 27 +- .github/workflows/cmake.yml | 5 +- .github/workflows/docker.yml | 86 +- .github/workflows/scheduled-stats.yml | 16 + CMakeLists.txt | 132 +- README.md | 28 +- RTABMapConfig.cmake.in | 12 + Version.h.in | 1 + app/android/jni/CameraARCore.cpp | 162 +- app/android/jni/CameraARCore.h | 13 +- app/android/jni/CameraAREngine.cpp | 146 +- app/android/jni/CameraAREngine.h | 9 +- app/android/jni/CameraMobile.cpp | 342 +- app/android/jni/CameraMobile.h | 36 +- app/android/jni/CameraTango.cpp | 43 +- app/android/jni/CameraTango.h | 9 +- app/android/jni/RTABMapApp.cpp | 243 +- app/android/jni/RTABMapApp.h | 9 +- app/android/jni/background_renderer.cc | 6 +- app/android/jni/background_renderer.h | 8 +- app/android/libs/.gitignore | 5 +- .../introlab/rtabmap/ARCoreSharedCamera.java | 18 +- app/ios/RTABMapApp.xcodeproj/project.pbxproj | 8 +- app/ios/RTABMapApp/NativeWrapper.cpp | 11 +- app/ios/RTABMapApp/NativeWrapper.hpp | 3 +- app/ios/RTABMapApp/RTABMap.swift | 11 +- app/ios/RTABMapApp/ViewController.swift | 1 - app/ios/RTABMapApp/install_deps.sh | 2 +- app/ios/Settings.bundle/License.plist | 2 +- app/ios/Settings.bundle/Mapping.plist | 104 +- app/ios/Settings.bundle/Root.plist | 4 +- app/src/CMakeLists.txt | 172 +- app/src/main.cpp | 9 +- archive/2022-IlluminationInvariant/README.md | 6 +- cmake_modules/FindORB_SLAM.cmake | 4 + cmake_modules/FindSqlite3.cmake | 30 - corelib/include/rtabmap/core/Camera.h | 56 +- corelib/include/rtabmap/core/CameraEvent.h | 63 +- corelib/include/rtabmap/core/CameraInfo.h | 46 +- corelib/include/rtabmap/core/CameraThread.h | 127 +- corelib/include/rtabmap/core/DBDriver.h | 9 + .../include/rtabmap/core/DBDriverSqlite3.h | 7 + corelib/include/rtabmap/core/DBReader.h | 4 +- .../include/rtabmap/core/GlobalDescriptor.h | 2 +- .../rtabmap/core/GlobalDescriptorExtractor.h | 73 + corelib/include/rtabmap/core/GlobalMap.h | 102 + corelib/include/rtabmap/core/Graph.h | 12 + corelib/include/rtabmap/core/IMUFilter.h | 3 +- corelib/include/rtabmap/core/IMUThread.h | 7 + corelib/include/rtabmap/core/LaserScan.h | 11 +- corelib/include/rtabmap/core/Lidar.h | 57 + corelib/include/rtabmap/core/LocalGrid.h | 90 + corelib/include/rtabmap/core/LocalGridMaker.h | 115 + corelib/include/rtabmap/core/Memory.h | 8 +- corelib/include/rtabmap/core/OccupancyGrid.h | 146 +- corelib/include/rtabmap/core/OctoMap.h | 225 +- corelib/include/rtabmap/core/Odometry.h | 1 + corelib/include/rtabmap/core/OdometryInfo.h | 3 + corelib/include/rtabmap/core/Optimizer.h | 3 +- corelib/include/rtabmap/core/Parameters.h | 174 +- .../include/rtabmap/core/PythonInterface.h | 26 +- .../include/rtabmap/core/RegistrationIcp.h | 2 + .../include/rtabmap/core/RegistrationVis.h | 3 + corelib/include/rtabmap/core/Rtabmap.h | 3 + corelib/include/rtabmap/core/SensorCapture.h | 93 + .../include/rtabmap/core/SensorCaptureInfo.h | 82 + .../rtabmap/core/SensorCaptureThread.h | 216 + corelib/include/rtabmap/core/SensorEvent.h | 94 + corelib/include/rtabmap/core/Signature.h | 5 +- corelib/include/rtabmap/core/Statistics.h | 1 + .../rtabmap/core/camera/CameraDepthAI.h | 49 +- .../rtabmap/core/camera/CameraFreenect.h | 2 +- .../rtabmap/core/camera/CameraFreenect2.h | 2 +- .../rtabmap/core/camera/CameraImages.h | 2 +- .../include/rtabmap/core/camera/CameraK4A.h | 2 +- .../include/rtabmap/core/camera/CameraK4W2.h | 2 +- .../rtabmap/core/camera/CameraMyntEye.h | 2 +- .../rtabmap/core/camera/CameraOpenNI2.h | 2 +- .../rtabmap/core/camera/CameraOpenNICV.h | 2 +- .../rtabmap/core/camera/CameraOpenni.h | 2 +- .../rtabmap/core/camera/CameraRGBDImages.h | 2 +- .../rtabmap/core/camera/CameraRealSense.h | 2 +- .../rtabmap/core/camera/CameraRealSense2.h | 7 +- .../rtabmap/core/camera/CameraStereoDC1394.h | 2 +- .../core/camera/CameraStereoFlyCapture2.h | 2 +- .../rtabmap/core/camera/CameraStereoImages.h | 2 +- .../rtabmap/core/camera/CameraStereoTara.h | 2 +- .../rtabmap/core/camera/CameraStereoVideo.h | 2 +- .../rtabmap/core/camera/CameraStereoZed.h | 13 +- .../rtabmap/core/camera/CameraStereoZedOC.h | 2 +- .../include/rtabmap/core/camera/CameraVideo.h | 2 +- .../rtabmap/core/global_map/CloudMap.h | 64 + .../include/rtabmap/core/global_map/GridMap.h | 69 + .../rtabmap/core/global_map/OccupancyGrid.h | 69 + .../include/rtabmap/core/global_map/OctoMap.h | 227 + .../{OccupancyGrid.hpp => LocalMapMaker.hpp} | 8 +- .../include/rtabmap/core/lidar/LidarVLP16.h | 94 + .../{OdometryORBSLAM.h => OdometryORBSLAM2.h} | 28 +- .../rtabmap/core/odometry/OdometryORBSLAM3.h | 71 + .../rtabmap/core/odometry/OdometryOpenVINS.h | 12 +- .../rtabmap/core/optimizer/OptimizerGTSAM.h | 33 +- corelib/include/rtabmap/core/util2d.h | 27 + corelib/include/rtabmap/core/util3d.h | 73 + .../rtabmap/core/util3d_motion_estimation.h | 9 +- .../rtabmap/core/util3d_registration.h | 12 +- corelib/src/CMakeLists.txt | 57 +- corelib/src/Camera.cpp | 97 +- corelib/src/CameraThread.cpp | 678 - corelib/src/DBDriver.cpp | 10 + corelib/src/DBDriverSqlite3.cpp | 166 +- corelib/src/DBReader.cpp | 6 +- corelib/src/Features2d.cpp | 28 +- corelib/src/GlobalDescriptorExtractor.cpp | 76 + corelib/src/GlobalMap.cpp | 169 + corelib/src/Graph.cpp | 144 +- corelib/src/IMUThread.cpp | 73 +- corelib/src/LaserScan.cpp | 116 +- corelib/src/Link.cpp | 2 +- corelib/src/LocalGrid.cpp | 126 + corelib/src/LocalGridMaker.cpp | 587 + corelib/src/Memory.cpp | 189 +- corelib/src/OccupancyGrid.cpp | 1667 -- corelib/src/Odometry.cpp | 102 +- corelib/src/OdometryThread.cpp | 10 +- corelib/src/Optimizer.cpp | 41 +- corelib/src/Parameters.cpp | 53 +- corelib/src/Recovery.cpp | 2 +- corelib/src/RegistrationIcp.cpp | 15 +- corelib/src/RegistrationVis.cpp | 63 +- corelib/src/Rtabmap.cpp | 734 +- corelib/src/RtabmapThread.cpp | 10 +- corelib/src/SensorCapture.cpp | 114 + corelib/src/SensorCaptureThread.cpp | 1041 + corelib/src/SensorData.cpp | 30 +- corelib/src/Signature.cpp | 67 +- corelib/src/Transform.cpp | 16 +- corelib/src/camera/CameraDepthAI.cpp | 807 +- corelib/src/camera/CameraFreenect.cpp | 2 +- corelib/src/camera/CameraFreenect2.cpp | 2 +- corelib/src/camera/CameraImages.cpp | 2 +- corelib/src/camera/CameraK4A.cpp | 2 +- corelib/src/camera/CameraK4W2.cpp | 2 +- corelib/src/camera/CameraMyntEye.cpp | 2 +- corelib/src/camera/CameraOpenNI2.cpp | 2 +- corelib/src/camera/CameraOpenNICV.cpp | 2 +- corelib/src/camera/CameraOpenni.cpp | 2 +- corelib/src/camera/CameraRGBDImages.cpp | 2 +- corelib/src/camera/CameraRealSense.cpp | 2 +- corelib/src/camera/CameraRealSense2.cpp | 35 +- corelib/src/camera/CameraStereoDC1394.cpp | 2 +- .../src/camera/CameraStereoFlyCapture2.cpp | 2 +- corelib/src/camera/CameraStereoImages.cpp | 2 +- corelib/src/camera/CameraStereoTara.cpp | 2 +- corelib/src/camera/CameraStereoVideo.cpp | 2 +- corelib/src/camera/CameraStereoZed.cpp | 138 +- corelib/src/camera/CameraStereoZedOC.cpp | 2 +- corelib/src/camera/CameraVideo.cpp | 2 +- corelib/src/global_map/CloudMap.cpp | 153 + corelib/src/global_map/GridMap.cpp | 485 + corelib/src/global_map/OccupancyGrid.cpp | 682 + corelib/src/{ => global_map}/OctoMap.cpp | 361 +- corelib/src/icp/libpointmatcher.h | 22 +- corelib/src/lidar/LidarVLP16.cpp | 378 + corelib/src/odometry/OdometryF2F.cpp | 8 +- corelib/src/odometry/OdometryF2M.cpp | 18 +- corelib/src/odometry/OdometryMSCKF.cpp | 41 +- ...ometryORBSLAM.cpp => OdometryORBSLAM2.cpp} | 171 +- corelib/src/odometry/OdometryORBSLAM3.cpp | 589 + corelib/src/odometry/OdometryOpenVINS.cpp | 702 +- corelib/src/optimizer/OptimizerG2O.cpp | 306 +- corelib/src/optimizer/OptimizerGTSAM.cpp | 415 +- corelib/src/optimizer/OptimizerTORO.cpp | 4 +- corelib/src/optimizer/gtsam/GravityFactor.h | 42 +- corelib/src/optimizer/gtsam/XYFactor.h | 7 +- corelib/src/optimizer/gtsam/XYZFactor.h | 14 +- .../toro3d/{dmatrix.hh => dmatrix.h} | 0 .../toro3d/{posegraph.hh => posegraph.h} | 0 corelib/src/optimizer/toro3d/posegraph2.cpp | 3 +- .../toro3d/{posegraph2.hh => posegraph2.h} | 4 +- corelib/src/optimizer/toro3d/posegraph3.cpp | 3 +- .../toro3d/{posegraph3.hh => posegraph3.h} | 4 +- .../{transformation2.hh => transformation2.h} | 0 .../{transformation3.hh => transformation3.h} | 3 +- .../src/optimizer/toro3d/treeoptimizer2.cpp | 3 +- .../{treeoptimizer2.hh => treeoptimizer2.h} | 2 +- .../src/optimizer/toro3d/treeoptimizer3.cpp | 3 +- .../{treeoptimizer3.hh => treeoptimizer3.h} | 2 +- .../toro3d/treeoptimizer3_iteration.cpp | 2 +- .../optimizer/vertigo/gtsam/DerivedValue.h | 6 + .../vertigo/gtsam/betweenFactorMaxMix.h | 4 +- .../vertigo/gtsam/betweenFactorSwitchable.h | 20 +- .../vertigo/gtsam/switchVariableLinear.h | 17 +- .../vertigo/gtsam/switchVariableSigmoid.h | 15 +- .../pcl18/surface/impl/texture_mapping.hpp | 4 - corelib/src/python/PyDescriptor.cpp | 218 + corelib/src/python/PyDescriptor.h | 38 + corelib/src/python/PyDetector.cpp | 26 +- corelib/src/python/PyDetector.h | 11 +- corelib/src/python/PyMatcher.cpp | 24 +- corelib/src/python/PyMatcher.h | 2 +- corelib/src/python/PythonInterface.cpp | 77 +- corelib/src/python/rtabmap_netvlad.py | 83 + corelib/src/stereo/StereoBM.cpp | 8 + corelib/src/stereo/StereoSGBM.cpp | 8 + corelib/src/superpoint_torch/SuperPoint.cc | 197 +- corelib/src/util2d.cpp | 201 + corelib/src/util3d.cpp | 439 +- corelib/src/util3d_filtering.cpp | 58 +- corelib/src/util3d_mapping.cpp | 4 - corelib/src/util3d_motion_estimation.cpp | 329 +- corelib/src/util3d_registration.cpp | 30 +- corelib/src/util3d_surface.cpp | 71 +- data/presets/camera_tof_icp.ini | 31 + data/presets/lidar3d_icp.ini | 46 + docker/bionic/Dockerfile | 31 +- .../bionic/android/rtabmap_apiXX/rtabmap.bash | 2 +- docker/bionic/nvidia/Dockerfile | 13 - docker/focal-foxy/Dockerfile | 150 +- docker/focal-foxy/deps/Dockerfile | 176 + docker/focal-foxy/deps/ros_entrypoint.sh | 6 + docker/focal-foxy/hooks/build | 2 - docker/focal-foxy/hooks/post_push | 2 - docker/focal-foxy/nvidia/Dockerfile | 13 - docker/focal/Dockerfile | 149 +- docker/focal/deps/Dockerfile | 175 + docker/focal/hooks/build | 2 - docker/focal/hooks/post_push | 2 - docker/focal/nvidia/Dockerfile | 13 - docker/jammy-iron/Dockerfile | 37 + docker/jammy-iron/deps/Dockerfile | 88 + docker/jammy-iron/deps/ros_entrypoint.sh | 6 + docker/jammy/Dockerfile | 61 +- docker/jammy/deps/Dockerfile | 100 + docker/jammy/deps/ros_entrypoint.sh | 6 + docker/jammy/hooks/build | 2 - docker/jammy/hooks/post_push | 2 - docker/jammy/nvidia/Dockerfile | 13 - docker/jfr2018/Dockerfile | 2 +- docker/jfr2018/latest/Dockerfile | 48 +- ...tabmap_msckf_ubuntu_16_compatibility.patch | 13 + ...map_opencv310_backward_compatibility.patch | 13 + docker/jfr2018/run_kitti_datasets.sh | 2 +- docker/latest_deps/Dockerfile | 31 +- examples/CMakeLists.txt | 1 + examples/LidarMapping/CMakeLists.txt | 34 + examples/LidarMapping/MapBuilder.h | 340 + examples/LidarMapping/main.cpp | 240 + examples/NoEventsExample/CMakeLists.txt | 16 +- examples/RGBDMapping/CMakeLists.txt | 16 +- examples/RGBDMapping/MapBuilder.h | 17 +- examples/RGBDMapping/main.cpp | 18 +- examples/WifiMapping/CMakeLists.txt | 16 +- examples/WifiMapping/MapBuilder.h | 6 +- examples/WifiMapping/MapBuilderWifi.h | 2 +- examples/WifiMapping/main.cpp | 16 +- guilib/include/rtabmap/gui/CameraViewer.h | 1 + guilib/include/rtabmap/gui/CloudViewer.h | 8 + guilib/include/rtabmap/gui/DatabaseViewer.h | 24 +- .../include/rtabmap/gui/ExportCloudsDialog.h | 4 + guilib/include/rtabmap/gui/GraphViewer.h | 5 + guilib/include/rtabmap/gui/ImageView.h | 7 +- .../include/rtabmap/gui/LinkRefiningDialog.h | 79 + guilib/include/rtabmap/gui/MainWindow.h | 22 +- .../include/rtabmap/gui/PreferencesDialog.h | 27 +- guilib/include/rtabmap/gui/StatsToolBox.h | 3 +- guilib/include/rtabmap/utilite/UPlot.h | 8 +- guilib/src/AboutDialog.cpp | 15 + guilib/src/CMakeLists.txt | 71 +- guilib/src/CalibrationDialog.cpp | 8 +- guilib/src/CameraViewer.cpp | 15 +- guilib/src/CloudViewer.cpp | 176 +- guilib/src/CreateSimpleCalibrationDialog.cpp | 2 +- guilib/src/DataRecorder.cpp | 8 +- guilib/src/DatabaseViewer.cpp | 1084 +- guilib/src/ExportCloudsDialog.cpp | 102 +- guilib/src/GraphViewer.cpp | 107 +- guilib/src/GuiLib.qrc | 1 + guilib/src/ImageView.cpp | 72 +- guilib/src/LinkRefiningDialog.cpp | 156 + guilib/src/MainWindow.cpp | 526 +- guilib/src/ParametersToolBox.cpp | 19 +- guilib/src/PreferencesDialog.cpp | 769 +- guilib/src/StatsToolBox.cpp | 46 +- guilib/src/images/oakdpro.png | Bin 0 -> 8691 bytes guilib/src/ui/DatabaseViewer.ui | 755 +- guilib/src/ui/aboutDialog.ui | 870 +- guilib/src/ui/exportCloudsDialog.ui | 566 +- guilib/src/ui/linkRefiningDialog.ui | 191 + guilib/src/ui/mainWindow.ui | 57 +- guilib/src/ui/preferencesDialog.ui | 15960 +++++++++------- guilib/src/utilite/UPlot.cpp | 39 +- package.xml | 6 +- tools/CMakeLists.txt | 1 + tools/Calibration/main.cpp | 6 +- tools/CameraRGBD/main.cpp | 4 +- tools/DataRecorder/main.cpp | 9 +- tools/DatabaseViewer/main.cpp | 13 +- tools/DetectMoreLoopClosures/main.cpp | 8 +- tools/EurocDataset/CMakeLists.txt | 17 +- tools/EurocDataset/main.cpp | 13 +- tools/Info/main.cpp | 44 +- tools/KittiDataset/main.cpp | 13 +- tools/LidarViewer/CMakeLists.txt | 32 + tools/LidarViewer/main.cpp | 144 + tools/OdometryViewer/main.cpp | 4 +- tools/Report/main.cpp | 335 +- tools/Reprocess/main.cpp | 65 +- tools/RgbdDataset/main.cpp | 35 +- utilite/include/rtabmap/utilite/UStl.h | 2 - 311 files changed, 27758 insertions(+), 15083 deletions(-) create mode 100644 .devcontainer/devcontainer.json create mode 100644 .github/workflows/scheduled-stats.yml delete mode 100644 cmake_modules/FindSqlite3.cmake create mode 100644 corelib/include/rtabmap/core/GlobalDescriptorExtractor.h create mode 100644 corelib/include/rtabmap/core/GlobalMap.h create mode 100644 corelib/include/rtabmap/core/Lidar.h create mode 100644 corelib/include/rtabmap/core/LocalGrid.h create mode 100644 corelib/include/rtabmap/core/LocalGridMaker.h create mode 100644 corelib/include/rtabmap/core/SensorCapture.h create mode 100644 corelib/include/rtabmap/core/SensorCaptureInfo.h create mode 100644 corelib/include/rtabmap/core/SensorCaptureThread.h create mode 100644 corelib/include/rtabmap/core/SensorEvent.h create mode 100644 corelib/include/rtabmap/core/global_map/CloudMap.h create mode 100644 corelib/include/rtabmap/core/global_map/GridMap.h create mode 100644 corelib/include/rtabmap/core/global_map/OccupancyGrid.h create mode 100644 corelib/include/rtabmap/core/global_map/OctoMap.h rename corelib/include/rtabmap/core/impl/{OccupancyGrid.hpp => LocalMapMaker.hpp} (96%) create mode 100644 corelib/include/rtabmap/core/lidar/LidarVLP16.h rename corelib/include/rtabmap/core/odometry/{OdometryORBSLAM.h => OdometryORBSLAM2.h} (79%) create mode 100644 corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h delete mode 100644 corelib/src/CameraThread.cpp create mode 100644 corelib/src/GlobalDescriptorExtractor.cpp create mode 100644 corelib/src/GlobalMap.cpp create mode 100644 corelib/src/LocalGrid.cpp create mode 100644 corelib/src/LocalGridMaker.cpp delete mode 100644 corelib/src/OccupancyGrid.cpp create mode 100644 corelib/src/SensorCapture.cpp create mode 100644 corelib/src/SensorCaptureThread.cpp create mode 100644 corelib/src/global_map/CloudMap.cpp create mode 100644 corelib/src/global_map/GridMap.cpp create mode 100644 corelib/src/global_map/OccupancyGrid.cpp rename corelib/src/{ => global_map}/OctoMap.cpp (72%) create mode 100644 corelib/src/lidar/LidarVLP16.cpp rename corelib/src/odometry/{OdometryORBSLAM.cpp => OdometryORBSLAM2.cpp} (87%) create mode 100644 corelib/src/odometry/OdometryORBSLAM3.cpp rename corelib/src/optimizer/toro3d/{dmatrix.hh => dmatrix.h} (100%) rename corelib/src/optimizer/toro3d/{posegraph.hh => posegraph.h} (100%) rename corelib/src/optimizer/toro3d/{posegraph2.hh => posegraph2.h} (98%) rename corelib/src/optimizer/toro3d/{posegraph3.hh => posegraph3.h} (98%) rename corelib/src/optimizer/toro3d/{transformation2.hh => transformation2.h} (100%) rename corelib/src/optimizer/toro3d/{transformation3.hh => transformation3.h} (99%) rename corelib/src/optimizer/toro3d/{treeoptimizer2.hh => treeoptimizer2.h} (99%) rename corelib/src/optimizer/toro3d/{treeoptimizer3.hh => treeoptimizer3.h} (99%) create mode 100644 corelib/src/python/PyDescriptor.cpp create mode 100644 corelib/src/python/PyDescriptor.h create mode 100644 corelib/src/python/rtabmap_netvlad.py create mode 100644 data/presets/camera_tof_icp.ini create mode 100644 data/presets/lidar3d_icp.ini delete mode 100644 docker/bionic/nvidia/Dockerfile create mode 100644 docker/focal-foxy/deps/Dockerfile create mode 100644 docker/focal-foxy/deps/ros_entrypoint.sh delete mode 100644 docker/focal-foxy/hooks/build delete mode 100644 docker/focal-foxy/hooks/post_push delete mode 100644 docker/focal-foxy/nvidia/Dockerfile create mode 100644 docker/focal/deps/Dockerfile delete mode 100644 docker/focal/hooks/build delete mode 100644 docker/focal/hooks/post_push delete mode 100644 docker/focal/nvidia/Dockerfile create mode 100644 docker/jammy-iron/Dockerfile create mode 100644 docker/jammy-iron/deps/Dockerfile create mode 100644 docker/jammy-iron/deps/ros_entrypoint.sh create mode 100644 docker/jammy/deps/Dockerfile create mode 100644 docker/jammy/deps/ros_entrypoint.sh delete mode 100644 docker/jammy/hooks/build delete mode 100644 docker/jammy/hooks/post_push delete mode 100644 docker/jammy/nvidia/Dockerfile create mode 100644 docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch create mode 100644 docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch create mode 100644 examples/LidarMapping/CMakeLists.txt create mode 100644 examples/LidarMapping/MapBuilder.h create mode 100644 examples/LidarMapping/main.cpp create mode 100644 guilib/include/rtabmap/gui/LinkRefiningDialog.h create mode 100644 guilib/src/LinkRefiningDialog.cpp create mode 100644 guilib/src/images/oakdpro.png create mode 100644 guilib/src/ui/linkRefiningDialog.ui create mode 100644 tools/LidarViewer/CMakeLists.txt create mode 100644 tools/LidarViewer/main.cpp diff --git a/.appveyor.yml b/.appveyor.yml index a77f758a7e..4dc21dc680 100644 --- a/.appveyor.yml +++ b/.appveyor.yml @@ -17,6 +17,9 @@ init: - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64 install: + # To download from google drive + - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH% + - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 # Qt - set QTDIR=C:\Qt\5.10.1\msvc2015_64 # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation @@ -73,7 +76,7 @@ install: - ps: "ls \"C:/Program Files/PCL\"" - set PATH=%PATH%;C:\Program Files\PCL\bin # zlib - - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip + - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files' - ECHO "Installed zlib:" - ps: "ls \"C:/Program Files/zlib\"" diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000000..476d9718b7 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,8 @@ +{ + "image": "introlab3it/rtabmap:20.04", + "customizations": { + "vscode": { + "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"] + } + } +} diff --git a/.github/workflows/cmake-ros.yml b/.github/workflows/cmake-ros.yml index a3a663103d..e9fd45b76e 100644 --- a/.github/workflows/cmake-ros.yml +++ b/.github/workflows/cmake-ros.yml @@ -21,36 +21,29 @@ jobs: name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }} runs-on: ${{ matrix.os }} strategy: + fail-fast: false matrix: - ros_distribution: [melodic, noetic, foxy, humble, rolling] + ros_distribution: [ noetic, humble, iron] include: - - ros_distribution: 'melodic' - os: ubuntu-18.04 - ros_distribution: 'noetic' os: ubuntu-20.04 - - ros_distribution: 'foxy' - os: ubuntu-20.04 - ros_distribution: 'humble' os: ubuntu-22.04 - - ros_distribution: 'rolling' + - ros_distribution: 'iron' os: ubuntu-22.04 - steps: - - name: Workaround dpkg grub-efi-amd64-signed error - run: | - sudo apt-mark hold grub-efi-amd64-signed - - - uses: ros-tooling/setup-ros@v0.5 + steps: + - uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distribution }} + - uses: actions/checkout@v4 + - name: Install dependencies run: | - sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distribution }}-rtabmap - sudo apt-get -y remove ros-${{ matrix.ros_distribution }}-rtabmap - - - uses: actions/checkout@v2 + source /opt/ros/${{ matrix.ros_distribution }}/setup.bash + rosdep update + rosdep install --from-paths ${{github.workspace}} -y - name: Configure CMake run: | diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index cfad708574..f29220c3af 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -16,8 +16,9 @@ jobs: name: ${{ matrix.os }} runs-on: ${{ matrix.os }} strategy: + fail-fast: false matrix: - os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04] + os: [ubuntu-22.04, ubuntu-20.04] steps: - name: Install dependencies @@ -26,7 +27,7 @@ jobs: sudo apt-get update sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Configure CMake run: | diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 9f0494d577..1e3740f30a 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -6,22 +6,75 @@ on: - 'master' jobs: - docker: + docker_deps: runs-on: ubuntu-latest strategy: + fail-fast: false matrix: - docker_tag: [xenial, bionic, focal, focal-foxy, jammy, android23, android24, android26, android30] + docker_tag: [focal-deps, jammy-deps, jammy-iron-deps] include: - - docker_tag: xenial + - docker_tag: focal-deps docker_tags: | - introlab3it/rtabmap:xenial - introlab3it/rtabmap:16.04 - docker_args: | - NOT_USED=0 + introlab3it/rtabmap:focal-deps + docker_platforms: | + linux/amd64 + linux/arm64 + linux/arm/v7 + docker_path: 'focal/deps' + - docker_tag: jammy-deps + docker_tags: | + introlab3it/rtabmap:jammy-deps docker_platforms: | linux/amd64 - docker_path: 'xenial' + linux/arm64 + docker_path: 'jammy/deps' + - docker_tag: jammy-iron-deps + docker_tags: | + introlab3it/rtabmap:jammy-iron-deps + docker_platforms: | + linux/amd64 + docker_path: 'jammy-iron/deps' + + steps: + - + name: Checkout + uses: actions/checkout@v2 + - + name: Set up QEMU + uses: docker/setup-qemu-action@v1 + with: + platforms: all + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + - + name: Login to DockerHub + uses: docker/login-action@v1 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - + name: Build and push + uses: docker/build-push-action@v2 + with: + context: . + push: true + platforms: ${{ matrix.docker_platforms }} + file: ./docker/${{ matrix.docker_path }}/Dockerfile + tags: ${{ matrix.docker_tags }} + cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }} + cache-to: type=inline + + docker: + needs: docker_deps + runs-on: ubuntu-latest + + strategy: + fail-fast: false + matrix: + docker_tag: [bionic, focal, jammy, jammy-iron, android23, android24, android26, android30] + include: - docker_tag: bionic docker_tags: | introlab3it/rtabmap:bionic @@ -42,27 +95,26 @@ jobs: docker_platforms: | linux/amd64 linux/arm64 + linux/arm/v7 docker_path: 'focal' - - docker_tag: focal-foxy + - docker_tag: jammy docker_tags: | - introlab3it/rtabmap:focal-foxy - introlab3it/rtabmap:20.04-foxy + introlab3it/rtabmap:jammy + introlab3it/rtabmap:22.04 docker_args: | NOT_USED=0 docker_platforms: | linux/amd64 linux/arm64 - docker_path: 'focal-foxy' - - docker_tag: jammy + docker_path: 'jammy' + - docker_tag: jammy-iron docker_tags: | - introlab3it/rtabmap:jammy - introlab3it/rtabmap:22.04 + introlab3it/rtabmap:jammy-iron docker_args: | NOT_USED=0 docker_platforms: | linux/amd64 - linux/arm64 - docker_path: 'jammy' + docker_path: 'jammy-iron' - docker_tag: android23 docker_tags: | introlab3it/rtabmap:android23 diff --git a/.github/workflows/scheduled-stats.yml b/.github/workflows/scheduled-stats.yml new file mode 100644 index 0000000000..f836c14626 --- /dev/null +++ b/.github/workflows/scheduled-stats.yml @@ -0,0 +1,16 @@ +name: RTAB-Map Scheduled Stats Extraction From GitHub + +on: + workflow_dispatch: + schedule: + - cron: '0 5 * * *' +jobs: + get_stats: + runs-on: ubuntu-latest + steps: + - name: Update Stats + uses: introlab/github-stats-action@v1 + with: + github-stats-token: ${{ secrets.STATS_TOKEN }} + google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }} + spreadsheet-id: ${{ secrets.SPREADSHEET_ID }} diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ddde68be6..eb36792fc4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # Top-Level CmakeLists.txt -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.14) PROJECT( RTABMap ) SET(PROJECT_PREFIX rtabmap) @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 21) -SET(RTABMAP_PATCH_VERSION 0) +SET(RTABMAP_PATCH_VERSION 5) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -206,11 +206,12 @@ option(WITH_REALSENSE_SLAM "Include RealSenseSlam support" ON) option(WITH_REALSENSE2 "Include RealSense support" ON) option(WITH_MYNTEYE "Include mynteye-s support" ON) option(WITH_DEPTHAI "Include depthai-core support" OFF) -option(WITH_OCTOMAP "Include Octomap support" ON) +option(WITH_OCTOMAP "Include OctoMap support" ON) +option(WITH_GRIDMAP "Include GridMap support" ON) option(WITH_CPUTSDF "Include CPUTSDF support" OFF) option(WITH_OPENCHISEL "Include open_chisel support" OFF) option(WITH_ALICE_VISION "Include AliceVision support" OFF) -option(WITH_FOVIS "Include FOVIS support" OFF) +option(WITH_FOVIS "Include FOVIS supp++ort" OFF) option(WITH_VISO2 "Include VISO2 support" OFF) option(WITH_DVO "Include DVO support" OFF) option(WITH_ORB_SLAM "Include ORB_SLAM2 or ORB_SLAM3 support" OFF) @@ -221,7 +222,7 @@ option(WITH_OPENVINS "Include OpenVINS support" OFF) option(WITH_MADGWICK "Include Madgwick IMU filtering support" ON) option(WITH_FASTCV "Include FastCV support" ON) option(WITH_OPENMP "Include OpenMP support" ON) -option(WITH_OPENGV "Include OpenGV support" OFF) +option(WITH_OPENGV "Include OpenGV support" ON) IF(MOBILE_BUILD) option(PCL_OMP "With PCL OMP implementations" OFF) ELSE() @@ -231,7 +232,7 @@ ENDIF() set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.") set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6) -FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) +FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) IF(WITH_QT) FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) @@ -254,10 +255,10 @@ endif() FIND_PACKAGE(ZLIB REQUIRED QUIET) -FIND_PACKAGE(Sqlite3 QUIET) -IF(Sqlite3_FOUND) - MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}") -ENDIF(Sqlite3_FOUND) +FIND_PACKAGE(SQLite3 QUIET) +IF(SQLite3_FOUND) + MESSAGE(STATUS "Found SQLite3: ${SQLite3_INCLUDE_DIRS} ${SQLite3_LIBRARIES}") +ENDIF(SQLite3_FOUND) if(NOT "${PCL_LIBRARIES}" STREQUAL "") # fix libproj.so not found on Xenial @@ -296,14 +297,20 @@ SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF FALSE) IF(WITH_QT) FIND_PACKAGE(VTK) IF(NOT VTK_FOUND) - MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.") + MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.") ENDIF(NOT VTK_FOUND) # If Qt is here, the GUI will be built IF(NOT(${VTK_MAJOR_VERSION} LESS 9)) - IF(NOT VTK_QT_VERSION) - MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.") - ENDIF() + IF(NOT VTK_QT_VERSION) + MESSAGE(FATAL_ERROR "WITH_QT option is ON, but VTK ${VTK_MAJOR_VERSION} has not been built with Qt support, disabling Qt.") + ENDIF() + + option(VTK_GLOBAL_WARNING_DISPLAY "Show VTK warning display on runtime" OFF) + IF(NOT VTK_GLOBAL_WARNING_DISPLAY) + ADD_DEFINITIONS(-DVTK_GLOBAL_WARNING_DISPLAY_OFF) + ENDIF() + MESSAGE(STATUS "VTK>=9 detected, will use VTK_QT_VERSION=${VTK_QT_VERSION} for Qt version.") IF(${VTK_QT_VERSION} EQUAL 6) FIND_PACKAGE(Qt6 COMPONENTS Widgets Core Gui OpenGL PrintSupport QUIET OPTIONAL_COMPONENTS Svg) @@ -328,6 +335,11 @@ IF(WITH_QT) ENDIF() IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + # For VCPKG build, set those global variables to off, + # we will enable them for jsut specific targets + set(CMAKE_AUTOMOC OFF) + set(CMAKE_AUTORCC OFF) + set(CMAKE_AUTOUIC OFF) IF("${VTK_MAJOR_VERSION}" EQUAL 5) FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 ELSE() @@ -392,6 +404,7 @@ IF(WITH_PYTHON) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy) IF(Python3_FOUND) MESSAGE(STATUS "Found Python3") + FIND_PACKAGE(pybind11 REQUIRED) ENDIF(Python3_FOUND) ENDIF(WITH_PYTHON) @@ -521,7 +534,14 @@ ENDIF(WITH_CVSBA) IF(WITH_POINTMATCHER) find_package(libpointmatcher QUIET) IF(libpointmatcher_FOUND) - MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}") + MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}") + string(FIND "${libpointmatcher_LIBRARIES}" "libnabo" value) + IF(value EQUAL -1) + # Find libnabo (Issue #1117): + find_package(libnabo REQUIRED PATHS ${LIBNABO_INSTALL_DIR}) + message(STATUS "libnabo found, version ${libnabo_VERSION} (Config mode)") + SET(libpointmatcher_LIBRARIES "${libpointmatcher_LIBRARIES};libnabo::nabo") + ENDIF(value EQUAL -1) ENDIF(libpointmatcher_FOUND) ENDIF(WITH_POINTMATCHER) @@ -634,7 +654,7 @@ IF(WITH_MYNTEYE) ENDIF(WITH_MYNTEYE) IF(WITH_DEPTHAI) - FIND_PACKAGE(depthai 2 QUIET) + FIND_PACKAGE(depthai 2.24 QUIET) IF(depthai_FOUND) MESSAGE(STATUS "Found depthai-core (targets)") ENDIF(depthai_FOUND) @@ -650,6 +670,13 @@ IF(WITH_OCTOMAP) ENDIF(octomap_FOUND) ENDIF(WITH_OCTOMAP) +IF(WITH_GRIDMAP) + FIND_PACKAGE(grid_map_core QUIET) + IF(grid_map_core_FOUND) + MESSAGE(STATUS "Found grid_map_core ${grid_map_core_VERSION}: ${grid_map_core_INCLUDE_DIRS}") + ENDIF(grid_map_core_FOUND) +ENDIF(WITH_GRIDMAP) + IF(WITH_CPUTSDF) FIND_PACKAGE(CPUTSDF QUIET) IF(CPUTSDF_FOUND) @@ -769,7 +796,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND) ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND) IF(NOT MSVC) - IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1)) + IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND) # Qt6 requires c++17 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17) @@ -780,8 +807,8 @@ IF(NOT MSVC) message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.") ENDIF() ENDIF() - IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))) - #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11 + IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND (msckf_vio_FOUND OR loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)) + #MSCKF_VIO, LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) IF(COMPILER_SUPPORTS_CXX14) @@ -792,22 +819,7 @@ IF(NOT MSVC) ENDIF() ENDIF() - IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "17") AND NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND ( - G2O_FOUND OR - GTSAM_FOUND OR - CERES_FOUND OR - ZED_FOUND OR - ZEDOC_FOUND OR - ANDROID OR - RealSense_FOUND OR - realsense2_FOUND OR - ORB_SLAM_FOUND OR - okvis_FOUND OR - open_chisel_FOUND OR - msckf_vio_FOUND OR - vins_FOUND OR - ov_msckf_FOUND OR - libpointmatcher_FOUND)) + IF(NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "17") AND NOT ("${CMAKE_CXX_STANDARD}" STREQUAL "14")) #Newest versions require std11 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) @@ -822,6 +834,7 @@ IF(NOT MSVC) ENDIF() ENDIF() + ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) IF(Qt6_FOUND OR Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) @@ -882,9 +895,9 @@ ENDIF() IF(NOT MRPT_FOUND) SET(MRPT "//") ENDIF(NOT MRPT_FOUND) -IF(NOT CERES_FOUND) +IF(NOT WITH_CERES OR NOT CERES_FOUND) SET(CERES "//") -ENDIF(NOT CERES_FOUND) +ENDIF(NOT WITH_CERES OR NOT CERES_FOUND) IF(NOT WITH_TORO) SET(TORO "//") ENDIF(NOT WITH_TORO) @@ -906,9 +919,9 @@ ENDIF(NOT Open3D_FOUND) IF(NOT FastCV_FOUND) SET(FASTCV "//") ENDIF(NOT FastCV_FOUND) -IF(NOT opengv_FOUND) +IF(NOT opengv_FOUND OR NOT WITH_OPENGV) SET(OPENGV "//") -ENDIF(NOT opengv_FOUND) +ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV) IF(NOT PDAL_FOUND) SET(PDAL "//") ENDIF(NOT PDAL_FOUND) @@ -980,10 +993,10 @@ IF(NOT mynteye_FOUND) SET(MYNTEYE "//") ENDIF(NOT mynteye_FOUND) IF(NOT depthai_FOUND) - SET(CONF_DEPTH_AI OFF) + SET(CONF_WITH_DEPTH_AI 0) SET(DEPTHAI "//") ELSE() - SET(CONF_DEPTH_AI ON) + SET(CONF_WITH_DEPTH_AI 1) ENDIF() IF(NOT octomap_FOUND) SET(OCTOMAP "//") @@ -991,6 +1004,12 @@ IF(NOT octomap_FOUND) ELSE() SET(CONF_WITH_OCTOMAP 1) ENDIF() +IF(NOT grid_map_core_FOUND) + SET(GRIDMAP "//") + SET(CONF_WITH_GRIDMAP 0) +ELSE() + SET(CONF_WITH_GRIDMAP 1) +ENDIF() IF(NOT CPUTSDF_FOUND) SET(CPUTSDF "//") ENDIF() @@ -1032,6 +1051,9 @@ IF(NOT TORCH_FOUND) ENDIF() IF(NOT WITH_PYTHON OR NOT Python3_FOUND) SET(PYTHON "//") + SET(CONF_WITH_PYTHON 0) +ELSE() + SET(CONF_WITH_PYTHON 1) ENDIF() IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF) SET(CONF_VTK_QT true) @@ -1070,6 +1092,7 @@ ENDIF(BUILD_EXAMPLES) ####################### # Uninstall target, for "make uninstall" ####################### +IF (NOT TARGET uninstall) CONFIGURE_FILE( "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in" "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" @@ -1077,6 +1100,7 @@ CONFIGURE_FILE( ADD_CUSTOM_TARGET(uninstall "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") +ENDIF() #### # Global Export Target @@ -1311,10 +1335,10 @@ ELSE() MESSAGE(STATUS " With Qt = NO (Qt not found)") ENDIF() -IF(Sqlite3_FOUND) +IF(SQLite3_FOUND) MESSAGE(STATUS " With external SQLite3 = YES (License: Public Domain)") ELSE() -MESSAGE(STATUS " With external SQLite3 = NO (sqlite3 not found, internal version is used for convenience)") +MESSAGE(STATUS " With external SQLite3 = NO (SQLite3 not found, internal version is used for convenience)") ENDIF() IF(WITH_ORB_OCTREE) @@ -1385,12 +1409,8 @@ ELSE() MESSAGE(STATUS " *With GTSAM = NO (GTSAM not found)") ENDIF() -IF(CERES_FOUND) -IF(WITH_CERES) +IF(WITH_CERES AND CERES_FOUND) MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD)") -ELSE() -MESSAGE(STATUS " *With Ceres ${Ceres_VERSION} = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)") -ENDIF() ELSEIF(NOT WITH_CERES) MESSAGE(STATUS " *With Ceres = NO (WITH_CERES=OFF)") ELSE() @@ -1449,7 +1469,7 @@ ELSE() MESSAGE(STATUS " With Open3D = NO (Open3D not found)") ENDIF() -IF(opengv_FOUND) +IF(opengv_FOUND AND WITH_OPENGV) MESSAGE(STATUS " With OpenGV ${opengv_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_OPENGV) MESSAGE(STATUS " With OpenGV = NO (WITH_OPENGV=OFF)") @@ -1460,11 +1480,19 @@ ENDIF() MESSAGE(STATUS "") MESSAGE(STATUS " Reconstruction Approaches:") IF(octomap_FOUND) -MESSAGE(STATUS " With OCTOMAP ${octomap_VERSION} = YES (License: BSD)") +MESSAGE(STATUS " With OctoMap ${octomap_VERSION} = YES (License: BSD)") +ELSEIF(NOT WITH_OCTOMAP) +MESSAGE(STATUS " With OctoMap = NO (WITH_OCTOMAP=OFF)") +ELSE() +MESSAGE(STATUS " With OctoMap = NO (octomap not found)") +ENDIF() + +IF(grid_map_core_FOUND) +MESSAGE(STATUS " With GridMap ${grid_map_core_VERSION} = YES (License: BSD)") ELSEIF(NOT WITH_OCTOMAP) -MESSAGE(STATUS " With OCTOMAP = NO (WITH_OCTOMAP=OFF)") +MESSAGE(STATUS " With GridMap = NO (WITH_GRIDMAP=OFF)") ELSE() -MESSAGE(STATUS " With OCTOMAP = NO (octomap not found)") +MESSAGE(STATUS " With GridMap = NO (grid_map_core not found)") ENDIF() IF(CPUTSDF_FOUND) diff --git a/README.md b/README.md index a89d37efa2..7e17ddfcac 100644 --- a/README.md +++ b/README.md @@ -4,11 +4,15 @@ rtabmap [![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap) [![Release][release-image]][releases] +[![Downloads][downloads-image]][downloads] [![License][license-image]][license] -[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat +[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat [releases]: https://github.com/introlab/rtabmap/releases +[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads +[downloads]: https://github.com/introlab/rtabmap/releases + [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat [license]: https://github.com/introlab/rtabmap/blob/master/LICENSE @@ -50,27 +54,29 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated - - - - - + - - - - + + + + + + + + +
ROS 1MelodicBuild Status
ROS 1 Noetic Build Status
ROS 2FoxyBuild Status
Humble Build Status
IronBuild Status
Rolling Build Status
Docker + rtabmap + Docker Pulls
- diff --git a/RTABMapConfig.cmake.in b/RTABMapConfig.cmake.in index 0f156f0142..b4c51e2a68 100644 --- a/RTABMapConfig.cmake.in +++ b/RTABMapConfig.cmake.in @@ -42,10 +42,22 @@ IF(@CONF_WITH_K4A@) ENDIF() ENDIF() +IF(@CONF_WITH_DEPTH_AI@) + find_dependency(depthai 2) +ENDIF() + IF(@CONF_WITH_OCTOMAP@) find_dependency(octomap) ENDIF() +IF(@CONF_WITH_GRIDMAP@) + find_dependency(grid_map_core) +ENDIF() + +IF(@CONF_WITH_PYTHON@) + find_dependency(Python3 COMPONENTS Interpreter Development NumPy) +ENDIF() + # Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies) set(RTABMap_INCLUDE_DIRS "") set(RTABMap_LIBRARIES "") diff --git a/Version.h.in b/Version.h.in index 97e04b77e0..6dfc1977c4 100644 --- a/Version.h.in +++ b/Version.h.in @@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @MYNTEYE@#define RTABMAP_MYNTEYE @DEPTHAI@#define RTABMAP_DEPTHAI @OCTOMAP@#define RTABMAP_OCTOMAP +@GRIDMAP@#define RTABMAP_GRIDMAP @CPUTSDF@#define RTABMAP_CPUTSDF @ALICE_VISION@#define RTABMAP_ALICE_VISION @OPENCHISEL@#define RTABMAP_OPENCHISEL diff --git a/app/android/jni/CameraARCore.cpp b/app/android/jni/CameraARCore.cpp index 526c177926..717b5ba511 100644 --- a/app/android/jni/CameraARCore.cpp +++ b/app/android/jni/CameraARCore.cpp @@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR } } -SensorData CameraARCore::captureImage(CameraInfo * info) +SensorData CameraARCore::updateDataOnRender(Transform & pose) { UScopeMutex lock(arSessionMutex_); //LOGI("Capturing image..."); + pose.setNull(); SensorData data; if(!arSession_) { @@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info) if (geometry_changed != 0 || !uvs_initialized_) { ArFrame_transformCoordinates2d( arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES, - BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED, + BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED, transformed_uvs_); UASSERT(transformed_uvs_); uvs_initialized_ = true; @@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info) ArTrackingState camera_tracking_state; ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - Transform pose; CameraModel model; if(camera_tracking_state == AR_TRACKING_STATE_TRACKING) { @@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info) float pose_raw[7]; ArCamera_getPose(arSession_, ar_camera, arPose_); ArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); + poseArCore = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - Transform poseArCore = pose; - if(pose.isNull()) + if(poseArCore.isNull()) { LOGE("CameraARCore: Pose is null"); } - else - { - this->poseReceived(pose); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - info->odomPose = pose; - } // Get calibration parameters float fx,fy, cx, cy; @@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info) data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp); data.setFeatures(kpts, kpts3, cv::Mat()); + + if(!poseArCore.isNull()) + { + pose = poseArCore; + this->poseReceived(pose, stamp); + // adjust origin + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } + } } } else @@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info) ArCamera_release(ar_camera); return data; - -} - -void CameraARCore::capturePoseOnly() -{ - UScopeMutex lock(arSessionMutex_); - //LOGI("Capturing image..."); - - if(!arSession_) - { - return; - } - - if(textureId_ != 0) - { - glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - ArSession_setCameraTextureName(arSession_, textureId_); - } - - // Update session to get current frame and render camera background. - if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) { - LOGE("CameraARCore::capturePoseOnly() ArSession_update error"); - return; - } - - // If display rotation changed (also includes view size change), we need to - // re-query the uv coordinates for the on-screen portion of the camera image. - int32_t geometry_changed = 0; - ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed); - if (geometry_changed != 0 || !uvs_initialized_) { - ArFrame_transformCoordinates2d( - arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES, - BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED, - transformed_uvs_); - UASSERT(transformed_uvs_); - uvs_initialized_ = true; - } - - ArCamera* ar_camera; - ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); - - ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_)); - ArCamera_getProjectionMatrix(arSession_, ar_camera, - /*near=*/0.1f, /*far=*/100.f, - glm::value_ptr(projectionMatrix_)); - - // adjust origin - if(!getOriginOffset().isNull()) - { - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } - - ArTrackingState camera_tracking_state; - ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - - Transform pose; - CameraModel model; - if(camera_tracking_state == AR_TRACKING_STATE_TRACKING) - { - // pose in OpenGL coordinates - float pose_raw[7]; - ArCamera_getPose(arSession_, ar_camera, arPose_); - ArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - if(!pose.isNull()) - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - } - - int32_t is_depth_supported = 0; - ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported); - - if(is_depth_supported) - { - LOGD("Acquire depth image!"); - ArImage * depthImage = nullptr; - ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage); - - ArImageFormat format; - ArImage_getFormat(arSession_, depthImage, &format); - if(format == AR_IMAGE_FORMAT_DEPTH16) - { - LOGD("Depth format detected!"); - int planeCount; - ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount); - LOGD("planeCount=%d", planeCount); - UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str()); - const uint8_t *data = nullptr; - int len = 0; - int stride; - int width; - int height; - ArImage_getWidth(arSession_, depthImage, &width); - ArImage_getHeight(arSession_, depthImage, &height); - ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride); - ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len); - - LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride); - - cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone(); - - float fx,fy, cx, cy; - int32_t rgb_width, rgb_height; - ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_); - ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy); - ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy); - ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height); - - float scaleX = (float)width / (float)rgb_width; - float scaleY = (float)height / (float)rgb_height; - CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height)); - this->setOcclusionImage(occlusionImage, occlusionModel); - } - ArImage_release(depthImage); - } - } - - ArCamera_release(ar_camera); } } /* namespace rtabmap */ diff --git a/app/android/jni/CameraARCore.h b/app/android/jni/CameraARCore.h index ead1791d5c..429fa3fc6c 100644 --- a/app/android/jni/CameraARCore.h +++ b/app/android/jni/CameraARCore.h @@ -63,23 +63,14 @@ class CameraARCore : public CameraMobile { CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false); virtual ~CameraARCore(); - bool uvsInitialized() const {return uvs_initialized_;} - const float* uvsTransformed() const {return transformed_uvs_;} - void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;} - virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - void setupGL(); - virtual void close(); // close Tango connection + virtual void close(); // close ARCore connection virtual std::string getSerial() const; - GLuint getTextureId() const {return textureId_;} - - void imageCallback(AImageReader *reader); protected: - virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread - virtual void capturePoseOnly(); + virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread private: rtabmap::Transform getPoseAtTimestamp(double timestamp); diff --git a/app/android/jni/CameraAREngine.cpp b/app/android/jni/CameraAREngine.cpp index 2d1cd88c38..5d10ecf917 100644 --- a/app/android/jni/CameraAREngine.cpp +++ b/app/android/jni/CameraAREngine.cpp @@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri deviceTColorCamera_ = opticalRotation; - // Required as ArSession_update does some off-screen OpenGL stuff... - HwArSession_setCameraTextureName(arSession_, textureId_); - if (HwArSession_resume(arSession_) != HWAR_SUCCESS) { UERROR("Cannot resume camera!"); @@ -169,38 +166,87 @@ void CameraAREngine::close() CameraMobile::close(); } -SensorData CameraAREngine::captureImage(CameraInfo * info) +void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) +{ + CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height); + if(arSession_) + { + int ret = static_cast(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation + if (ret > 3) { + ret -= 4; + } + + HwArSession_setDisplayGeometry(arSession_, ret, width, height); + } +} + +SensorData CameraAREngine::updateDataOnRender(Transform & pose) { UScopeMutex lock(arSessionMutex_); //LOGI("Capturing image..."); + pose.setNull(); SensorData data; if(!arSession_) { return data; } + if(textureId_ == 0) + { + glGenTextures(1, &textureId_); + glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + if(textureId_!=0) + HwArSession_setCameraTextureName(arSession_, textureId_); + // Update session to get current frame and render camera background. if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) { LOGE("CameraAREngine::captureImage() ArSession_update error"); return data; } + // If display rotation changed (also includes view size change), we need to + // re-query the uv coordinates for the on-screen portion of the camera image. + int32_t geometry_changed = 0; + HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed); + if (geometry_changed != 0 || !uvs_initialized_) { + HwArFrame_transformDisplayUvCoords( + arSession_, arFrame_, + BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView, + transformed_uvs_); + UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", + transformed_uvs_[0], transformed_uvs_[1], + transformed_uvs_[2], transformed_uvs_[3], + transformed_uvs_[4], transformed_uvs_[5], + transformed_uvs_[6], transformed_uvs_[7]); + UASSERT(transformed_uvs_); + uvs_initialized_ = true; + } + HwArCamera* ar_camera; HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); + HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_)); + HwArCamera_getProjectionMatrix(arSession_, ar_camera, + /*near=*/0.1f, /*far=*/100.f, + glm::value_ptr(projectionMatrix_)); + + // adjust origin + if(!getOriginOffset().isNull()) + { + viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); + } + HwArTrackingState camera_tracking_state; HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - Transform pose; if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING) { - // pose in OpenGL coordinates - float pose_raw[7]; - HwArCamera_getPose(arSession_, ar_camera, arPose_); - HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - // Get calibration parameters // FIXME: Hard-coded as getting intrinsics with the api fails float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744; @@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info) double stamp = double(timestamp_ns)/10e8; CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight)); data = SensorData(outputRGB, outputDepth, model, 0, stamp); + + // pose in OpenGL coordinates + float pose_raw[7]; + HwArCamera_getPose(arSession_, ar_camera, arPose_); + HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); + pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); + if(pose.isNull()) + { + LOGE("CameraAREngine: Pose is null"); + } + else + { + pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + this->poseReceived(pose, stamp); + // adjust origin + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } + } } } else @@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info) } HwArCamera_release(ar_camera); - - if(pose.isNull()) - { - LOGE("CameraAREngine: Pose is null"); - } - else - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - info->odomPose = pose; - } return data; } -void CameraAREngine::capturePoseOnly() -{ - UScopeMutex lock(arSessionMutex_); - //LOGI("Capturing image..."); - - SensorData data; - if(!arSession_) - { - return; - } - - // Update session to get current frame and render camera background. - if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) { - LOGE("CameraARCore::captureImage() ArSession_update error"); - return; - } - - HwArCamera* ar_camera; - HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); - - HwArTrackingState camera_tracking_state; - HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - - Transform pose; - CameraModel model; - if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING) - { - // pose in OpenGL coordinates - float pose_raw[7]; - HwArCamera_getPose(arSession_, ar_camera, arPose_); - HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - if(!pose.isNull()) - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - } - } - - HwArCamera_release(ar_camera); -} - } /* namespace rtabmap */ diff --git a/app/android/jni/CameraAREngine.h b/app/android/jni/CameraAREngine.h index d8d1264d95..03f592191e 100644 --- a/app/android/jni/CameraAREngine.h +++ b/app/android/jni/CameraAREngine.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -48,13 +49,14 @@ class CameraAREngine : public CameraMobile { CameraAREngine(void* env, void* context, void* activity, bool smoothing = false); virtual ~CameraAREngine(); + virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - virtual void close(); // close Tango connection + virtual void close(); // close AREngine connection virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); - virtual void capturePoseOnly(); + virtual SensorData updateDataOnRender(Transform & pose); private: rtabmap::Transform getPoseAtTimestamp(double timestamp); @@ -69,7 +71,6 @@ class CameraAREngine : public CameraMobile { HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr; HwArPose * arPose_ = nullptr; bool arInstallRequested_; - GLuint textureId_; UMutex arSessionMutex_; }; diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index a3ca0e9657..cd7c4f140e 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform( CameraMobile::CameraMobile(bool smoothing) : Camera(10), deviceTColorCamera_(Transform::getIdentity()), - spinOncePreviousStamp_(0.0), textureId_(0), uvs_initialized_(false), - previousStamp_(0.0), stampEpochOffset_(0.0), smoothing_(smoothing), colorCameraToDisplayRotation_(ROTATION_0), @@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &) void CameraMobile::close() { - previousPose_.setNull(); - previousStamp_ = 0.0; + firstFrame_ = true; lastKnownGPS_ = GPS(); lastEnvSensors_.clear(); originOffset_ = Transform(); originUpdate_ = false; - pose_ = Transform(); + dataPose_ = Transform(); data_ = SensorData(); if(textureId_ != 0) @@ -97,35 +94,107 @@ void CameraMobile::close() void CameraMobile::resetOrigin() { - previousPose_.setNull(); - previousStamp_ = 0.0; + firstFrame_ = true; lastKnownGPS_ = GPS(); lastEnvSensors_.clear(); - pose_ = Transform(); + dataPose_ = Transform(); data_ = SensorData(); originUpdate_ = true; } -void CameraMobile::poseReceived(const Transform & pose) +bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) +{ + pose.setNull(); + + int maxWaitTimeMs = maxWaitTime * 1000; + + // Interpolate pose + if(!poseBuffer_.empty()) + { + poseMutex_.lock(); + int waitTry = 0; + while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) + { + poseMutex_.unlock(); + ++waitTry; + uSleep(1); + poseMutex_.lock(); + } + if(poseBuffer_.rbegin()->first < stamp) + { + if(maxWaitTimeMs > 0) + { + UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first); + } + else + { + UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first); + } + } + else + { + std::map::const_iterator iterB = poseBuffer_.lower_bound(stamp); + std::map::const_iterator iterA = iterB; + if(iterA != poseBuffer_.begin()) + { + iterA = --iterA; + } + if(iterB == poseBuffer_.end()) + { + iterB = --iterB; + } + if(iterA == iterB && stamp == iterA->first) + { + pose = iterA->second; + } + else if(stamp >= iterA->first && stamp <= iterB->first) + { + pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second); + } + else // stamp < iterA->first + { + UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); + } + } + poseMutex_.unlock(); + } + return !pose.isNull(); +} + +void CameraMobile::poseReceived(const Transform & pose, double deviceStamp) { if(!pose.isNull()) { - // send pose of the camera (without optical rotation) - Transform p = pose*deviceTColorCamera_; + Transform p = pose; if(originUpdate_) { originOffset_ = p.translation().inverse(); originUpdate_ = false; } + + if(stampEpochOffset_ == 0.0) + { + stampEpochOffset_ = UTimer::now() - deviceStamp; + } + + double epochStamp = stampEpochOffset_ + deviceStamp; if(!originOffset_.isNull()) { - this->post(new PoseEvent(originOffset_*p)); + p = originOffset_*p; } - else + { - this->post(new PoseEvent(p)); + UScopeMutex lock(poseMutex_); + poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p)); + if(poseBuffer_.size() > 1000) + { + poseBuffer_.erase(poseBuffer_.begin()); + } } + + // send pose of the camera (with optical rotation) + this->post(new PoseEvent(p * deviceTColorCamera_)); } } @@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps) lastKnownGPS_ = gps; } -void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord) +void CameraMobile::addEnvSensor(int type, float value) +{ + lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value))); +} + +void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord) { - LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); + UScopeMutex lock(dataMutex_); + + bool notify = !data_.isValid(); + + LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); data_ = data; - pose_ = pose; + dataPose_ = pose; viewMatrix_ = viewMatrix; projectionMatrix_ = projectionMatrix; @@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons // adjust origin if(!originOffset_.isNull()) { - pose_ = originOffset_ * pose_; + dataPose_ = originOffset_ * dataPose_; viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); } @@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons uvs_initialized_ = true; } - LOGD("CameraMobile::setData textureId_=%d", (int)textureId_); + LOGD("CameraMobile::update textureId_=%d", (int)textureId_); if(textureId_ != 0 && texCoord != 0) { @@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons return; } } -} -void CameraMobile::addEnvSensor(int type, float value) -{ - lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value))); + postUpdate(); + + if(notify) + { + dataReady_.release(); + } } -void CameraMobile::spinOnce() +void CameraMobile::updateOnRender() { - if(!this->isRunning()) + UScopeMutex lock(dataMutex_); + bool notify = !data_.isValid(); + + data_ = updateDataOnRender(dataPose_); + + if(data_.isValid()) { - bool ignoreFrame = false; - //float rate = 10.0f; // maximum 10 FPS for image data - double now = UTimer::now(); - /*if(rate>0.0f) - { - if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) || - ((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate)) - { - ignoreFrame = true; - } - }*/ + postUpdate(); - if(!ignoreFrame) - { - spinOnceFrameRateTimer_.start(); - spinOncePreviousStamp_ = now; - mainLoop(); - } - else + if(notify) { - // just send pose - capturePoseOnly(); + dataReady_.release(); } } } -void CameraMobile::mainLoopBegin() +SensorData CameraMobile::updateDataOnRender(Transform & pose) { - double t = cameraStartedTime_.elapsed(); - if(t < 5.0) - { - uSleep((5.0-t)*1000); // just to make sure that the camera is started - } + LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() " + "should be overridden by inherited classes. Returning empty data!\n"); + return SensorData(); } -void CameraMobile::mainLoop() +void CameraMobile::postUpdate() { - CameraInfo info; - SensorData data = this->captureImage(&info); - - if(data.isValid() && !info.odomPose.isNull()) + if(data_.isValid()) { - if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0) + if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0) { - data.setGPS(lastKnownGPS_); + data_.setGPS(lastKnownGPS_); } else if(lastKnownGPS_.stamp()>0.0) { - LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp()); + LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp()); } if(lastEnvSensors_.size()) { - data.setEnvSensors(lastEnvSensors_); + data_.setEnvSensors(lastEnvSensors_); lastEnvSensors_.clear(); } - if(smoothing_ && !data.depthRaw().empty()) + if(smoothing_ && !data_.depthRaw().empty()) { //UTimer t; - data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR)); + data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR)); //LOGD("Bilateral filtering, time=%fs", t.ticks()); } @@ -273,15 +336,15 @@ void CameraMobile::mainLoop() { UDEBUG("ROTATION_90"); cv::Mat rgb, depth; - cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type()); - cv::flip(data.imageRaw(),rgb,1); + cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type()); + cv::flip(data_.imageRaw(),rgb,1); cv::transpose(rgb,rgbt); rgb = rgbt; - cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type()); - cv::flip(data.depthRaw(),depth,1); + cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type()); + cv::flip(data_.depthRaw(),depth,1); cv::transpose(depth,deptht); depth = deptht; - CameraModel model = data.cameraModels()[0]; + CameraModel model = data_.cameraModels()[0]; cv::Size sizet(model.imageHeight(), model.imageWidth()); model = CameraModel( model.fy(), @@ -290,25 +353,25 @@ void CameraMobile::mainLoop() model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0)); model.setImageSize(sizet); - data.setRGBDImage(rgb, depth, model); + data_.setRGBDImage(rgb, depth, model); - std::vector keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i0?model.imageHeight()-model.cy():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0)); model.setImageSize(sizet); - data.setRGBDImage(rgb, depth, model); + data_.setRGBDImage(rgb, depth, model); - std::vector keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i(3,3) *= 0.01; - info.reg.covariance.at(4,4) *= 0.01; - info.reg.covariance.at(5,5) *= 0.01; + // linear cov = 0.0001 + info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001); + if(!firstFrame_) + { + // angular cov = 0.000001 + info->odomCovariance.at(3,3) *= 0.01; + info->odomCovariance.at(4,4) *= 0.01; + info->odomCovariance.at(5,5) *= 0.01; + } + info->odomPose = dataPose_; } - LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001); - this->post(new OdometryEvent(data, pose, info)); - previousPose_ = pose; - previousStamp_ = data.stamp(); - } - else if(!this->isKilled() && info.odomPose.isNull()) - { - LOGW("Odometry lost"); - this->post(new OdometryEvent()); - } -} -SensorData CameraMobile::captureImage(CameraInfo * info) -{ - if(info) + firstFrame_ = false; + } + else { - info->odomPose = pose_; + UWARN("CameraMobile::captureImage() invalid data!"); } - return data_; + return data; } LaserScan CameraMobile::scanFromPointCloudData( @@ -432,23 +488,25 @@ LaserScan CameraMobile::scanFromPointCloudData( //get color from rgb image cv::Point3f org= pt; pt = util3d::transformPoint(pt, opticalRotationInv); - int u,v; - model.reproject(pt.x, pt.y, pt.z, u, v); - unsigned char r=255,g=255,b=255; - if(model.inFrame(u, v)) + if(pt.z > 0) { - b=rgb.at(v,u).val[0]; - g=rgb.at(v,u).val[1]; - r=rgb.at(v,u).val[2]; - if(kpts) - kpts->push_back(cv::KeyPoint(u,v,kptsSize)); - if(kpts3D) - kpts3D->push_back(org); - - *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16); - ++oi; + int u,v; + model.reproject(pt.x, pt.y, pt.z, u, v); + unsigned char r=255,g=255,b=255; + if(model.inFrame(u, v)) + { + b=rgb.at(v,u).val[0]; + g=rgb.at(v,u).val[1]; + r=rgb.at(v,u).val[2]; + if(kpts) + kpts->push_back(cv::KeyPoint(u,v,kptsSize)); + if(kpts3D) + kpts3D->push_back(org); + + *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16); + ++oi; + } } - //confidence //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16); diff --git a/app/android/jni/CameraMobile.h b/app/android/jni/CameraMobile.h index 19662d7e39..8df249342b 100644 --- a/app/android/jni/CameraMobile.h +++ b/app/android/jni/CameraMobile.h @@ -68,7 +68,7 @@ class PoseEvent: public UEvent Transform pose_; }; -class CameraMobile : public Camera, public UThread, public UEventsSender { +class CameraMobile : public Camera, public UEventsSender { public: static const float bilateralFilteringSigmaS; static const float bilateralFilteringSigmaR; @@ -93,14 +93,20 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { // abstract functions virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - virtual void close(); // inherited classes should call its parent in their close(). + virtual void close(); // inherited classes should call its parent at the end of their close(). virtual std::string getSerial() const {return "CameraMobile";} + void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord); + void updateOnRender(); + const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame void resetOrigin(); virtual bool isCalibrated() const; - void poseReceived(const Transform & pose); // in rtabmap frame + virtual bool odomProvided() const { return true; } + virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time + void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch) + double getStampEpochOffset() const {return stampEpochOffset_;} const CameraModel & getCameraModel() const {return model_;} const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;} @@ -108,10 +114,7 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;} void setGPS(const GPS & gps); void addEnvSensor(int type, float value); - void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord); - void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing - GLuint getTextureId() {return textureId_;} bool uvsInitialized() const {return uvs_initialized_;} const float* uvsTransformed() const {return transformed_uvs_;} @@ -122,17 +125,15 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; } protected: - virtual SensorData captureImage(CameraInfo * info = 0); - virtual void capturePoseOnly() {} + virtual SensorData updateDataOnRender(Transform & pose); - virtual void mainLoopBegin(); - virtual void mainLoop(); +private: + virtual SensorData captureImage(SensorCaptureInfo * info = 0); + void postUpdate(); // Should be called while being protected by dataMutex_ protected: CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame - UTimer spinOnceFrameRateTimer_; - double spinOncePreviousStamp_; GLuint textureId_; glm::mat4 viewMatrix_; @@ -141,9 +142,7 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { bool uvs_initialized_ = false; private: - Transform previousPose_; - double previousStamp_; - UTimer cameraStartedTime_; + bool firstFrame_; double stampEpochOffset_; bool smoothing_; ScreenRotation colorCameraToDisplayRotation_; @@ -152,8 +151,13 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { Transform originOffset_; bool originUpdate_; + USemaphore dataReady_; + UMutex dataMutex_; SensorData data_; - Transform pose_; + Transform dataPose_; + + UMutex poseMutex_; + std::map poseBuffer_; // cv::Mat occlusionImage_; CameraModel occlusionModel_; diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 6408588d44..5498014dea 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose) if(pose->status_code == TANGO_POSE_VALID) { CameraTango* app = static_cast(context); - app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world); + app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp); } } @@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) //LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols); UASSERT(cloud.type() == CV_32FC4); - boost::mutex::scoped_lock lock(dataMutex_); + boost::mutex::scoped_lock lock(tangoDataMutex_); // From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames // "In the current version of Project Tango Tablet RGB IR camera @@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) if(dt >= 0.0 && dt < 0.5) { - bool notify = !data_.isValid(); + bool notify = !tangoData_.isValid(); cv::Mat tangoImage = tangoColor_; cv::Mat rgb; @@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) else { LOGE("Not supported color format : %d.", tangoColorType); - data_ = SensorData(); + tangoData_ = SensorData(); return; } @@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) if(rawScanPublished_) { - data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp); + tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp); } else { - data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp); + tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp); } - data_.setGroundTruth(odom); + tangoData_.setGroundTruth(odom); } else { LOGE("Could not get depth and rgb images!?!"); - data_ = SensorData(); + tangoData_ = SensorData(); return; } if(notify) { - dataReady_.release(); + tangoDataReady_.release(); } LOGD("process cloud received %fs", timer.ticks()); } @@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times { //LOGD("RGB received! %fs", timestamp); - boost::mutex::scoped_lock lock(dataMutex_); + boost::mutex::scoped_lock lock(tangoDataMutex_); tangoColor_ = tangoImage.clone(); tangoColorStamp_ = timestamp; @@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp) return pose; } -SensorData CameraTango::captureImage(CameraInfo * info) +SensorData CameraTango::updateDataOnRender(Transform & pose) { //LOGI("Capturing image..."); + pose.setNull(); if(textureId_ == 0) { glGenTextures(1, &textureId_); @@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info) if (status == TANGO_SUCCESS) { - if(info) - { - info->odomPose = getPoseAtTimestamp(video_overlay_timestamp); - } + pose = getPoseAtTimestamp(video_overlay_timestamp); int rotation = static_cast(getScreenRotation()) + 1; // remove 90deg camera rotation if (rotation > 3) { @@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info) } SensorData data; - if(dataReady_.acquireTry(1)) + if(tangoDataReady_.acquireTry(1)) { - boost::mutex::scoped_lock lock(dataMutex_); - data = data_; - data_ = SensorData(); - if(info) - { - info->odomPose = data.groundTruth(); - data.setGroundTruth(Transform()); - } + boost::mutex::scoped_lock lock(tangoDataMutex_); + data = tangoData_; + tangoData_ = SensorData(); + pose = data.groundTruth(); + data.setGroundTruth(Transform()); } return data; diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h index 055222f9f4..6dc0366592 100644 --- a/app/android/jni/CameraTango.h +++ b/app/android/jni/CameraTango.h @@ -52,7 +52,6 @@ class CameraTango : public CameraMobile { virtual void close(); // close Tango connection virtual std::string getSerial() const; rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const; - void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;} void setDecimation(int value) {decimation_ = value;} void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;} @@ -61,7 +60,7 @@ class CameraTango : public CameraMobile { void tangoEventReceived(int type, const char * key, const char * value); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData updateDataOnRender(Transform & pose); private: rtabmap::Transform getPoseAtTimestamp(double timestamp); @@ -71,12 +70,12 @@ class CameraTango : public CameraMobile { bool colorCamera_; int decimation_; bool rawScanPublished_; - SensorData data_; + SensorData tangoData_; cv::Mat tangoColor_; int tangoColorType_; double tangoColorStamp_; - boost::mutex dataMutex_; - USemaphore dataReady_; + boost::mutex tangoDataMutex_; + USemaphore tangoDataReady_; cv::Mat fisheyeRectifyMapX_; cv::Mat fisheyeRectifyMapY_; }; diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 51b731a4fe..e8b0b6115c 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -202,6 +203,7 @@ RTABMapApp::RTABMapApp() : #endif cameraDriver_(0), camera_(0), + sensorCaptureThread_(0), rtabmapThread_(0), rtabmap_(0), logHandler_(0), @@ -216,6 +218,7 @@ RTABMapApp::RTABMapApp() : cameraColor_(true), fullResolution_(false), appendMode_(true), + useExternalLidar_(false), maxCloudDepth_(2.5), minCloudDepth_(0.0), cloudDensityLevel_(1), @@ -537,7 +540,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) + if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty())) { int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows); @@ -649,9 +652,9 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe UWARN("Cloud %d is empty", id); } } - else + else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty()) { - UERROR("Failed to uncompress data!"); + UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size()); status=-2; } } @@ -885,7 +888,7 @@ bool RTABMapApp::startCamera() #endif LOGW("startCamera() camera driver=%d", cameraDriver_); boost::mutex::scoped_lock lock(cameraMutex_); - + if(cameraDriver_ == 0) // Tango { #ifdef RTABMAP_TANGO @@ -937,6 +940,19 @@ bool RTABMapApp::startCamera() LOGI("Start camera thread"); cameraJustInitialized_ = true; + if(useExternalLidar_) + { + rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true); + lidar->init(); + camera_->setImageRate(0); // if lidar, to get close camera synchronization + sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity()); + sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true); + } + else + { + sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_); + } + sensorCaptureThread_->start(); return true; } UERROR("Failed camera initialization!"); @@ -948,13 +964,12 @@ void RTABMapApp::stopCamera() LOGI("stopCamera()"); { boost::mutex::scoped_lock lock(cameraMutex_); - if(camera_!=0) + if(sensorCaptureThread_!=0) { - camera_->join(true); - camera_->close(); - delete camera_; + sensorCaptureThread_->join(true); + delete sensorCaptureThread_; // camera_ is closed and deleted inside + sensorCaptureThread_ = 0; camera_ = 0; - poseBuffer_.clear(); } } { @@ -1241,7 +1256,7 @@ int RTABMapApp::Render() std::list rtabmapEvents; try { - if(camera_ == 0) + if(sensorCaptureThread_ == 0) { // We are not doing continous drawing, just measure single draw fpsTime_.restart(); @@ -1272,49 +1287,45 @@ int RTABMapApp::Render() { if(cameraDriver_ <= 2) { - camera_->spinOnce(); + camera_->updateOnRender(); } #ifdef DEBUG_RENDERING_PERFORMANCE - LOGW("Camera spinOnce %fs", time.ticks()); + LOGW("Camera updateOnRender %fs", time.ticks()); #endif - - if(cameraDriver_ != 2) + if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0) + { + main_scene_.background_renderer_ = new BackgroundRenderer(); + main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2); + } + if(camera_->uvsInitialized()) { - if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0) + uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed(); + ((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix); + if(graphOptimization_ && !mapToOdom_.isIdentity()) { - main_scene_.background_renderer_ = new BackgroundRenderer(); - main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1); + rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world; + arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); } - if(camera_->uvsInitialized()) + } + if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + { + rtabmap::CameraModel occlusionModel; + cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel); + + if(occlusionModel.isValidForProjection()) { - uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed(); - ((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix); - if(graphOptimization_ && !mapToOdom_.isIdentity()) - { - rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world; - arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); - } + pcl::IndicesPtr indices(new std::vector); + int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); + cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); + occlusionMesh.cloud.reset(new pcl::PointCloud()); + pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); + occlusionMesh.indices = indices; + occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_); } - if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + else if(!occlusionImage.empty()) { - rtabmap::CameraModel occlusionModel; - cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel); - - if(occlusionModel.isValidForProjection()) - { - pcl::IndicesPtr indices(new std::vector); - int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); - cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); - occlusionMesh.cloud.reset(new pcl::PointCloud()); - pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); - occlusionMesh.indices = indices; - occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_); - } - else if(!occlusionImage.empty()) - { - UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight()); - } + UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight()); } } #ifdef DEBUG_RENDERING_PERFORMANCE @@ -1334,14 +1345,14 @@ int RTABMapApp::Render() } } - rtabmap::OdometryEvent odomEvent; + rtabmap::SensorEvent sensorEvent; { - boost::mutex::scoped_lock lock(odomMutex_); - if(odomEvents_.size()) + boost::mutex::scoped_lock lock(sensorMutex_); + if(sensorEvents_.size()) { - LOGI("Process odom events"); - odomEvent = odomEvents_.back(); - odomEvents_.clear(); + LOGI("Process sensor events"); + sensorEvent = sensorEvents_.back(); + sensorEvents_.clear(); if(cameraJustInitialized_) { notifyCameraStarted = true; @@ -1361,7 +1372,7 @@ int RTABMapApp::Render() { main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl); } - if(camera_!=0 && cameraJustInitialized_) + if(sensorCaptureThread_!=0 && cameraJustInitialized_) { notifyCameraStarted = true; cameraJustInitialized_ = false; @@ -1562,9 +1573,9 @@ int RTABMapApp::Render() if(clearSceneOnNextRender_) { LOGI("Clearing all rendering data..."); - odomMutex_.lock(); - odomEvents_.clear(); - odomMutex_.unlock(); + sensorMutex_.lock(); + sensorEvents_.clear(); + sensorMutex_.unlock(); poseMutex_.lock(); poseEvents_.clear(); @@ -1800,7 +1811,7 @@ int RTABMapApp::Render() // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) + if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty())) { int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); @@ -2004,26 +2015,26 @@ int RTABMapApp::Render() } else { - main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0); + main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0); //just process the last one - if(!odomEvent.pose().isNull()) + if(!sensorEvent.info().odomPose.isNull()) { if(odomCloudShown_ && !trajectoryMode_) { - if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty()) + if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty()) { pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())) + if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty())) { - int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows); - cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); + int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows); + cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); } else { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -2034,10 +2045,10 @@ int RTABMapApp::Render() if(cloud->size() && indices->size()) { LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", - odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows, - odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows, + sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows, + sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows, (int)cloud->width, (int)cloud->height); - main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose()); + main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose); main_scene_.setCloudVisible(-1, true); } else @@ -2127,7 +2138,7 @@ int RTABMapApp::Render() lastPostRenderEventTime_ = UTimer::now(); - if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0) + if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0) { UERROR("TangoPoseEventNotReceived"); UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6))); @@ -2319,7 +2330,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled) void RTABMapApp::setGraphOptimization(bool enabled) { graphOptimization_ = enabled; - if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0) + if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0) { std::map poses; std::multimap links; @@ -3709,19 +3720,12 @@ void RTABMapApp::postCameraPoseEvent( if(qx==0 && qy==0 && qz==0 && qw==0) { // Lost! clear buffer - poseBuffer_.clear(); camera_->resetOrigin(); // we are lost, create new session on next valid frame return; } rtabmap::Transform pose(x,y,z,qx,qy,qz,qw); pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - camera_->poseReceived(pose); - - poseBuffer_.insert(std::make_pair(stamp, pose)); - if(poseBuffer_.size() > 1000) - { - poseBuffer_.erase(poseBuffer_.begin()); - } + camera_->poseReceived(pose, stamp); } } @@ -3833,66 +3837,41 @@ void RTABMapApp::postOdometryEvent( { pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + rtabmap::Transform poseWithOriginOffset = pose; + if(!camera_->getOriginOffset().isNull()) + { + poseWithOriginOffset = camera_->getOriginOffset() * pose; + } + // Registration depth to rgb if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp)) { UTimer time; rtabmap::Transform motion = rtabmap::Transform::getIdentity(); - if(depthStamp != stamp && !poseBuffer_.empty()) + if(depthStamp != stamp) { // Interpolate pose - if(!poseBuffer_.empty()) + rtabmap::Transform poseDepth; + cv::Mat cov; + if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0)) + { + UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp); + } + else { - if(poseBuffer_.rbegin()->first < depthStamp) - { - UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first); - } - else - { - std::map::const_iterator iterB = poseBuffer_.lower_bound(depthStamp); - std::map::const_iterator iterA = iterB; - rtabmap::Transform poseDepth; - if(iterA != poseBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == poseBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && depthStamp == iterA->first) - { - poseDepth = iterA->second; - } - else if(depthStamp >= iterA->first && depthStamp <= iterB->first) - { - poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second); - } - else if(depthStamp < iterA->first) - { - UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first); - } - else - { - UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first); - } - if(!poseDepth.isNull()) - { #ifndef DISABLE_LOG - UDEBUG("poseRGB =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp); - UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp); + UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp); + UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp); #endif - motion = pose.inverse()*poseDepth; - // transform in camera frame + motion = poseWithOriginOffset.inverse()*poseDepth; + // transform in camera frame #ifndef DISABLE_LOG - UDEBUG("motion=%s", motion.prettyPrint().c_str()); + UDEBUG("motion=%s", motion.prettyPrint().c_str()); #endif - motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation(); + motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation(); #ifndef DISABLE_LOG - UDEBUG("motion=%s", motion.prettyPrint().c_str()); + UDEBUG("motion=%s", motion.prettyPrint().c_str()); #endif - } - } } } rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame; @@ -3941,11 +3920,6 @@ void RTABMapApp::postOdometryEvent( if(!outputDepth.empty()) { - rtabmap::Transform poseWithOriginOffset = pose; - if(!camera_->getOriginOffset().isNull()) - { - poseWithOriginOffset = camera_->getOriginOffset() * pose; - } rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth())); depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform()); camera_->setOcclusionImage(outputDepth, depthModel); @@ -3971,8 +3945,7 @@ void RTABMapApp::postOdometryEvent( texCoords[5] = t5; texCoords[6] = t6; texCoords[7] = t7; - camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0); - camera_->spinOnce(); + camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0); } } } @@ -3989,17 +3962,17 @@ void RTABMapApp::postOdometryEvent( bool RTABMapApp::handleEvent(UEvent * event) { - if(camera_!=0) + if(sensorCaptureThread_!=0) { // called from events manager thread, so protect the data - if(event->getClassName().compare("OdometryEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - LOGI("Received OdometryEvent!"); - if(odomMutex_.try_lock()) + LOGI("Received SensorEvent!"); + if(sensorMutex_.try_lock()) { - odomEvents_.clear(); - odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event))); - odomMutex_.unlock(); + sensorEvents_.clear(); + sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event))); + sensorMutex_.unlock(); } } if(event->getClassName().compare("RtabmapEvent") == 0) diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 01fde945d8..40c9d66215 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "util.h" #include "ProgressionStatus.h" +#include #include +#include #include #include #include @@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler { private: int cameraDriver_; rtabmap::CameraMobile * camera_; + rtabmap::SensorCaptureThread * sensorCaptureThread_; rtabmap::RtabmapThread * rtabmapThread_; rtabmap::Rtabmap * rtabmap_; rtabmap::LogHandler * logHandler_; @@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler { bool cameraColor_; bool fullResolution_; bool appendMode_; + bool useExternalLidar_; float maxCloudDepth_; float minCloudDepth_; int cloudDensityLevel_; @@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler { UTimer fpsTime_; std::list rtabmapEvents_; - std::list odomEvents_; + std::list sensorEvents_; std::list poseEvents_; - std::map poseBuffer_; rtabmap::Transform mapToOdom_; boost::mutex cameraMutex_; boost::mutex rtabmapMutex_; boost::mutex meshesMutex_; - boost::mutex odomMutex_; + boost::mutex sensorMutex_; boost::mutex poseMutex_; boost::mutex renderingMutex_; diff --git a/app/android/jni/background_renderer.cc b/app/android/jni/background_renderer.cc index 9908c00243..23e0fea5e9 100644 --- a/app/android/jni/background_renderer.cc +++ b/app/android/jni/background_renderer.cc @@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes) } void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) { - static_assert(std::extent::value == kNumVertices * 2, "Incorrect kVertices length"); + static_assert(std::extent::value == kNumVertices * 2, "Incorrect kVertices length"); GLuint program = shaderPrograms_[depthTexture>0?1:0]; @@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept else #endif glBindTexture(GL_TEXTURE_2D, texture_id_); - + if(depthTexture>0) { // Texture activate unit 1 @@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept GLuint attributeVertices = glGetAttribLocation(program, "a_Position"); GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord"); - glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices); + glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice); glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord); glEnableVertexAttribArray(attributeVertices); diff --git a/app/android/jni/background_renderer.h b/app/android/jni/background_renderer.h index d696a9c936..8c07aa8a96 100644 --- a/app/android/jni/background_renderer.h +++ b/app/android/jni/background_renderer.h @@ -28,9 +28,15 @@ #include "util.h" -static const GLfloat BackgroundRenderer_kVertices[] = { +static const GLfloat BackgroundRenderer_kVerticesDevice[] = { -1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f, }; +//static const GLfloat BackgroundRenderer_kVerticesView[] = { +// 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, +//}; +static const GLfloat BackgroundRenderer_kVerticesView[] = { + 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, +}; static const GLfloat BackgroundRenderer_kTexCoord[] = { 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, }; diff --git a/app/android/libs/.gitignore b/app/android/libs/.gitignore index d392f0e82c..86d0cb2726 100644 --- a/app/android/libs/.gitignore +++ b/app/android/libs/.gitignore @@ -1 +1,4 @@ -*.jar +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java index 986fcbf6fa..456bf6160d 100644 --- a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java +++ b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java @@ -115,6 +115,9 @@ public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSp // Image reader that continuously processes CPU images. public TOF_ImageReader mTOFImageReader = new TOF_ImageReader(); private boolean mTOFAvailable = false; + + ByteBuffer mPreviousDepth = null; + double mPreviousDepthStamp = 0.0; public boolean isDepthSupported() {return mTOFAvailable;} @@ -757,7 +760,6 @@ public void run() { double stamp = (double)frame.getTimestamp()/10e8; if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp)); RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp); - CameraIntrinsics intrinsics = camera.getImageIntrinsics(); try{ Image image = frame.acquireCameraImage(); @@ -813,6 +815,12 @@ public void run() { depth = mTOFImageReader.depth16_raw; depthStamp = (double)mTOFImageReader.timestamp/10e8; } + + if(mPreviousDepth == null) + { + mPreviousDepth = depth; + mPreviousDepthStamp = depthStamp; + } if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f", mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp)); @@ -825,13 +833,17 @@ public void run() { rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(), depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(), stamp, - depthStamp, + depthStamp>stamp?mPreviousDepthStamp:depthStamp, y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), - depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, + depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, points, points.limit()/4, viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0], p[0], p[5], p[8], p[9], p[10], p[11], p[14], texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]); + + + mPreviousDepthStamp = depthStamp; + mPreviousDepth = depth; } else { diff --git a/app/ios/RTABMapApp.xcodeproj/project.pbxproj b/app/ios/RTABMapApp.xcodeproj/project.pbxproj index 22dae846d5..ac31de3e37 100644 --- a/app/ios/RTABMapApp.xcodeproj/project.pbxproj +++ b/app/ios/RTABMapApp.xcodeproj/project.pbxproj @@ -1007,7 +1007,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.20.19; + MARKETING_VERSION = 0.21.0; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; PRODUCT_NAME = "$(TARGET_NAME)"; @@ -1020,7 +1020,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"", "\"$(SRCROOT)/../android/jni/tango-gl/include\"", "\"$(SRCROOT)/../android/jni/third-party/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"", @@ -1064,7 +1064,7 @@ "$(PROJECT_DIR)/RTABMapApp/Libraries/lib", "$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib", ); - MARKETING_VERSION = 0.20.19; + MARKETING_VERSION = 0.21.0; ONLY_ACTIVE_ARCH = YES; OTHER_CFLAGS = ""; PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap; @@ -1078,7 +1078,7 @@ "\"$(SRCROOT)/RTABMapApp/Libraries/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"", "\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.11\"", - "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.20\"", + "\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.21\"", "\"$(SRCROOT)/../android/jni/tango-gl/include\"", "\"$(SRCROOT)/../android/jni/third-party/include\"", "\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"", diff --git a/app/ios/RTABMapApp/NativeWrapper.cpp b/app/ios/RTABMapApp/NativeWrapper.cpp index 145e3a8e30..27d5ec2f2c 100644 --- a/app/ios/RTABMapApp/NativeWrapper.cpp +++ b/app/ios/RTABMapApp/NativeWrapper.cpp @@ -284,11 +284,11 @@ void setCameraNative(const void *object, int type) { } void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw) + float x, float y, float z, float qx, float qy, float qz, float qw, double stamp) { if(object) { - native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0); + native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp); } else { @@ -489,13 +489,6 @@ void setGridVisibleNative(const void *object, bool visible) else UERROR("object is null!"); } -void setRawScanSavedNative(const void *object, bool enabled) -{ - if(object) - native(object)->setRawScanSaved(enabled); - else - UERROR("object is null!"); -} void setFullResolutionNative(const void *object, bool enabled) { if(object) diff --git a/app/ios/RTABMapApp/NativeWrapper.hpp b/app/ios/RTABMapApp/NativeWrapper.hpp index 574295117b..7aa11d93af 100644 --- a/app/ios/RTABMapApp/NativeWrapper.hpp +++ b/app/ios/RTABMapApp/NativeWrapper.hpp @@ -67,7 +67,7 @@ bool startCameraNative(const void *object); void stopCameraNative(const void *object); void setCameraNative(const void *object, int type); void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw); + float x, float y, float z, float qx, float qy, float qz, float qw, double stamp); void postOdometryEventNative(const void *object, float x, float y, float z, float qx, float qy, float qz, float qw, float fx, float fy, float cx, float cy, @@ -97,7 +97,6 @@ void setGraphOptimizationNative(const void *object, bool enabled); void setNodesFilteringNative(const void *object, bool enabled); void setGraphVisibleNative(const void *object, bool visible); void setGridVisibleNative(const void *object, bool visible); -void setRawScanSavedNative(const void *object, bool enabled); void setFullResolutionNative(const void *object, bool enabled); void setSmoothingNative(const void *object, bool enabled); void setAppendModeNative(const void *object, bool enabled); diff --git a/app/ios/RTABMapApp/RTABMap.swift b/app/ios/RTABMapApp/RTABMap.swift index 52430941b4..dc7a602edf 100644 --- a/app/ios/RTABMapApp/RTABMap.swift +++ b/app/ios/RTABMapApp/RTABMap.swift @@ -216,18 +216,18 @@ class RTABMap { setCameraNative(native_rtabmap, Int32(type)) } - func postCameraPoseEvent(pose: simd_float4x4) { + func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) { let rotation = GLKMatrix3( m: (pose[0,0], pose[0,1], pose[0,2], pose[1,0], pose[1,1], pose[1,2], pose[2,0], pose[2,1], pose[2,2])) let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w) + postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp) } func notifyLost() { // a null transform will make rtabmap creating a new session - postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0) + postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0) } func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) { @@ -239,7 +239,7 @@ class RTABMap { let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w) + postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp) let confMap = frame.sceneDepth?.confidenceMap let depthMap = frame.sceneDepth?.depthMap @@ -405,9 +405,6 @@ class RTABMap { func setGridVisible(visible: Bool) { setGridVisibleNative(native_rtabmap, visible) } - func setRawScanSaved(enabled: Bool) { - setRawScanSavedNative(native_rtabmap, enabled) - } func setFullResolution(enabled: Bool) { setFullResolutionNative(native_rtabmap, enabled) } diff --git a/app/ios/RTABMapApp/ViewController.swift b/app/ios/RTABMapApp/ViewController.swift index dabfe15063..40ebfbaca9 100644 --- a/app/ios/RTABMapApp/ViewController.swift +++ b/app/ios/RTABMapApp/ViewController.swift @@ -1319,7 +1319,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP // update preference rtabmap!.setOnlineBlending(enabled: defaults.bool(forKey: "Blending")); rtabmap!.setNodesFiltering(enabled: defaults.bool(forKey: "NodesFiltering")); - rtabmap!.setRawScanSaved(enabled: defaults.bool(forKey: "SaveRawScan")); rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode")); rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing")); rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode")); diff --git a/app/ios/RTABMapApp/install_deps.sh b/app/ios/RTABMapApp/install_deps.sh index 5c0b6de250..0e8debd167 100755 --- a/app/ios/RTABMapApp/install_deps.sh +++ b/app/ios/RTABMapApp/install_deps.sh @@ -122,7 +122,7 @@ git clone https://github.com/PointCloudLibrary/pcl.git cd pcl git checkout tags/pcl-1.11.1 # patch -curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch +curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch git apply pcl_1_11_1_vtk_ios_support.patch mkdir build cd build diff --git a/app/ios/Settings.bundle/License.plist b/app/ios/Settings.bundle/License.plist index 3c600aa4a3..dc827e966a 100644 --- a/app/ios/Settings.bundle/License.plist +++ b/app/ios/Settings.bundle/License.plist @@ -13,7 +13,7 @@ ======= RTAB-Map ======= RTAB-Map - https://github.com/introlab/rtabmap -Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. +Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved. Copyright (c) XXX, contributors, all rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/app/ios/Settings.bundle/Mapping.plist b/app/ios/Settings.bundle/Mapping.plist index f84ed83b9c..0f2dc059b6 100644 --- a/app/ios/Settings.bundle/Mapping.plist +++ b/app/ios/Settings.bundle/Mapping.plist @@ -552,50 +552,50 @@ DefaultValue - - Type - PSGroupSpecifier - FooterText - Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on. - - - Type - PSMultiValueSpecifier - Title - Maximum odometry cache size - Key - MaximumOdometryCacheSize - DefaultValue - 10 - Titles - - 500 - 200 - 100 - 75 - 50 - 40 - 30 - 20 - 10 - 5 - Disabled - - Values - - 500 - 200 - 100 - 75 - 50 - 40 - 30 - 20 - 10 - 5 - 0 - - + + Type + PSGroupSpecifier + FooterText + Used only in localization mode (when clicking First-P. View during visualization). This is used to get smoother localizations and to verify localization transforms (when Max Optimization Error is not disabled) to make sure we don't teleport to a location very similar to one we previously localized on. + + + Type + PSMultiValueSpecifier + Title + Maximum odometry cache size + Key + MaximumOdometryCacheSize + DefaultValue + 10 + Titles + + 500 + 200 + 100 + 75 + 50 + 40 + 30 + 20 + 10 + 5 + Disabled + + Values + + 500 + 200 + 100 + 75 + 50 + 40 + 30 + 20 + 10 + 5 + 0 + + Type PSGroupSpecifier @@ -752,22 +752,6 @@ DefaultValue - - Type - PSGroupSpecifier - FooterText - Save raw point clouds in database. - - - Type - PSToggleSwitchSpecifier - Title - Save Raw Scan - Key - SaveRawScan - DefaultValue - - Type PSGroupSpecifier diff --git a/app/ios/Settings.bundle/Root.plist b/app/ios/Settings.bundle/Root.plist index 39b71e84be..bc9360fe97 100644 --- a/app/ios/Settings.bundle/Root.plist +++ b/app/ios/Settings.bundle/Root.plist @@ -454,7 +454,7 @@ FooterText - Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. + Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved. Title About Type @@ -462,7 +462,7 @@ DefaultValue - 0.20.19 + 0.21.0 Key Version Title diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt index f0cef72c3a..7ec7e01c3b 100644 --- a/app/src/CMakeLists.txt +++ b/app/src/CMakeLists.txt @@ -24,7 +24,11 @@ IF(WIN32) ELSE( MINGW ) SET(SRC_FILES ${SRC_FILES} ${PROJECT_NAME}.rc) ENDIF( MINGW ) -ENDIF(WIN32) +ENDIF(WIN32) + +IF(BUILD_AS_BUNDLE) + ADD_DEFINITIONS("-DBUILD_AS_BUNDLE") +ENDIF() # Add binary IF(APPLE AND BUILD_AS_BUNDLE) @@ -61,15 +65,18 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE) IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}") - SET(plugin_dest_dir bin) + SET(plugin_dest_dir bin/plugins) SET(qtconf_dest_dir bin) - SET(openni2_dest_dir bin) + SET(thirdparty_dest_dir bin) IF(APPLE) - SET(plugin_dest_dir MacOS) + SET(plugin_dest_dir MacOS/plugins) + IF(Qt6_FOUND) + SET(plugin_dest_dir PlugIns) + ENDIF() SET(qtconf_dest_dir Resources) - SET(openni2_dest_dir MacOS) - SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}") + SET(thirdparty_dest_dir MacOS) + SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/MacOS/${CMAKE_BUNDLE_NAME}") ENDIF(APPLE) IF(OpenNI2_FOUND) @@ -85,11 +92,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) ENDIF() INSTALL(DIRECTORY "${OpenNI2_BIN_DIR}/OpenNI2" - DESTINATION ${openni2_dest_dir} + DESTINATION ${thirdparty_dest_dir} COMPONENT runtime REGEX ".*pdb" EXCLUDE) INSTALL(FILES "${OpenNI2_BIN_DIR}/OpenNI.ini" - DESTINATION ${openni2_dest_dir} + DESTINATION ${thirdparty_dest_dir} COMPONENT runtime) ENDIF(OpenNI2_FOUND) @@ -98,7 +105,7 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) IF(WIN32) file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR) INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" - DESTINATION ${plugin_dest_dir} + DESTINATION ${thirdparty_dest_dir} COMPONENT runtime) ENDIF(WIN32) ENDIF(k4a_FOUND) @@ -113,7 +120,7 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}") MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}") INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL} - DESTINATION ${plugin_dest_dir} + DESTINATION ${thirdparty_dest_dir} COMPONENT runtime) ELSE() MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.") @@ -128,82 +135,94 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) file(GENERATE OUTPUT ${deploy_script} CONTENT " # Including the file pointed to by QT_DEPLOY_SUPPORT ensures the generated # deployment script has access to qt_deploy_runtime_dependencies() - include(\"${QT_DEPLOY_SUPPORT}\") + include(\"${QT_DEPLOY_SUPPORT}\") qt_deploy_runtime_dependencies( EXECUTABLE \"${APPS}\" + PLUGINS_DIR ${plugin_dest_dir} + GENERATE_QT_CONF + NO_TRANSLATIONS VERBOSE - PLUGINS_DIR ${plugin_dest_dir}/plugins )") + IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Debug") install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appDebug.cmake" - CONFIGURATIONS Debug - COMPONENT runtime) - install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake" - CONFIGURATIONS Release - COMPONENT runtime) - install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake" - CONFIGURATIONS RelWithDebInfo - COMPONENT runtime) - install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake" - CONFIGURATIONS MinSizeRel - COMPONENT runtime) + CONFIGURATIONS Debug + COMPONENT runtime) + ENDIF() + IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "Release") + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelease.cmake" + CONFIGURATIONS Release + COMPONENT runtime) + ENDIF() + IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appRelWithDebInfo.cmake" + CONFIGURATIONS RelWithDebInfo + COMPONENT runtime) + ENDIF() + IF("${CMAKE_BUILD_TYPE}" STREQUAL "" OR "${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") + install(SCRIPT "${CMAKE_CURRENT_BINARY_DIR}/deploy_appMinSizeRel.cmake" + CONFIGURATIONS MinSizeRel + COMPONENT runtime) + ENDIF() ELSEIF(Qt5_FOUND) - #Qt5 - foreach(plugin ${Qt5Gui_PLUGINS}) - get_target_property(plugin_loc ${plugin} LOCATION) - get_filename_component(plugin_dir ${plugin_loc} DIRECTORY) - string(REPLACE "plugins" ";" loc_list ${plugin_dir}) - list(GET loc_list 1 plugin_type) - IF(NOT plugin_root) - get_filename_component(plugin_root ${plugin_dir} DIRECTORY) - ENDIF(NOT plugin_root) - #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"") - INSTALL(FILES ${plugin_loc} - DESTINATION ${plugin_dest_dir}/plugins${plugin_type} - COMPONENT runtime) - endforeach() - IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) - IF(WIN32) - SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll") - ELSEIF(APPLE) - SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib") - ENDIF() - IF(EXISTS ${plugin_loc}) - get_filename_component(plugin_dir ${plugin_loc} DIRECTORY) - string(REPLACE "plugins" ";" loc_list ${plugin_dir}) - list(GET loc_list 1 plugin_type) - INSTALL(FILES ${plugin_loc} - DESTINATION ${plugin_dest_dir}/plugins${plugin_type} - COMPONENT runtime) - #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"") - ENDIF(EXISTS ${plugin_loc}) - ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) + #Qt5 + foreach(plugin ${Qt5Gui_PLUGINS}) + get_target_property(plugin_loc ${plugin} LOCATION) + get_filename_component(plugin_dir ${plugin_loc} DIRECTORY) + string(REPLACE "plugins" ";" loc_list ${plugin_dir}) + list(GET loc_list 1 plugin_type) + IF(NOT plugin_root) + get_filename_component(plugin_root ${plugin_dir} DIRECTORY) + ENDIF(NOT plugin_root) + #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"") + INSTALL(FILES ${plugin_loc} + DESTINATION ${plugin_dest_dir}${plugin_type} + COMPONENT runtime) + endforeach() + IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) + IF(WIN32) + SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll") + ELSEIF(APPLE) + SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib") + ENDIF() + IF(EXISTS ${plugin_loc}) + get_filename_component(plugin_dir ${plugin_loc} DIRECTORY) + string(REPLACE "plugins" ";" loc_list ${plugin_dir}) + list(GET loc_list 1 plugin_type) + INSTALL(FILES ${plugin_loc} + DESTINATION ${plugin_dest_dir}/plugins${plugin_type} + COMPONENT runtime) + #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}${plugin_type}\"") + ENDIF(EXISTS ${plugin_loc}) + ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0) ELSEIF(QT_PLUGINS_DIR) # Qt4 # Install needed Qt plugins by copying directories from the qt installation - # One can cull what gets copied by using 'REGEX "..." EXCLUDE' - # Exclude debug libraries - INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats" - DESTINATION ${plugin_dest_dir}/plugins - COMPONENT runtime - REGEX ".*d4.dll" EXCLUDE - REGEX ".*d4.a" EXCLUDE) + # One can cull what gets copied by using 'REGEX "..." EXCLUDE' + # Exclude debug libraries + INSTALL(DIRECTORY "${QT_PLUGINS_DIR}/imageformats" + DESTINATION ${plugin_dest_dir} + COMPONENT runtime + REGEX ".*d4.dll" EXCLUDE + REGEX ".*d4.a" EXCLUDE) ENDIF() - # install a qt.conf file - # this inserts some cmake code into the install script to write the file - SET(QT_CONF_FILE [Paths]\nPlugins=plugins) - IF(APPLE) - SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins) - ENDIF(APPLE) - INSTALL(CODE " - file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\") - " COMPONENT runtime) - + IF(Qt5_FOUND OR QT4_FOUND) + # install a qt.conf file + # this inserts some cmake code into the install script to write the file + SET(QT_CONF_FILE [Paths]\nPlugins=plugins) + IF(APPLE) + SET(QT_CONF_FILE [Paths]\nPlugins=MacOS/plugins) + ENDIF(APPLE) + INSTALL(CODE " + file(WRITE \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${qtconf_dest_dir}/qt.conf\" \"${QT_CONF_FILE}\") + " COMPONENT runtime) + ENDIF() + # directories to look for dependencies - SET(DIRS ${QT_LIBRARY_DIRS} ${PROJECT_SOURCE_DIR}/bin) + SET(DIRS "${QT_LIBRARY_DIRS}" "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/lib") IF(APPLE) - SET(DIRS ${DIRS} /usr/local /usr/local/lib) - ENDIF(APPLE) - + SET(DIRS ${DIRS} /usr/local /usr/local/lib /opt/homebrew /opt/homebrew/lib /opt/homebrew/lib/gcc/current) + ENDIF(APPLE) + # Now the work of copying dependencies into the bundle/package # The quotes are escaped and variables to use at install time have their $ escaped # An alternative is the do a configure_file() on a script and use install(SCRIPT ...). @@ -211,10 +230,11 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) # over. # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them). install(CODE " - file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") + file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") set(BU_CHMOD_BUNDLE_ITEMS ON) include(\"BundleUtilities\") fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\") - " COMPONENT runtime) + " COMPONENT runtime) + ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) diff --git a/app/src/main.cpp b/app/src/main.cpp index 1d77f8f093..914a3496dc 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) +#include +#endif + using namespace rtabmap; int main(int argc, char* argv[]) @@ -51,8 +55,9 @@ int main(int argc, char* argv[]) CoInitialize(nullptr); #endif -#if VTK_MAJOR_VERSION >= 8 - vtkObject::GlobalWarningDisplayOff(); +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) + // needed to ensure appropriate OpenGL context is created for VTK rendering. + QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); #endif /* Create tasks */ diff --git a/archive/2022-IlluminationInvariant/README.md b/archive/2022-IlluminationInvariant/README.md index 2dd021b774..041c21767e 100644 --- a/archive/2022-IlluminationInvariant/README.md +++ b/archive/2022-IlluminationInvariant/README.md @@ -25,14 +25,14 @@ The following image shows when we do the same localization experiment at differe We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated. -* [Images](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EV8F4PZUxOxLhwAyEehlzKwBjF-9xNuxR32Q4mUjx5u-rA?e=eCJ3TW): +* [Images](https://drive.google.com/file/d/1fUm1m8oW6q8qlThx7BjrBH2vrVbNQ9bQ/view?usp=drive_link): * `rgb`: folder containing *.jpg color camera images * `depth`: folder containing *.png 16bits mm depth images * `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`. * `device_poses.txt`: VIO poses of each image in `device` frame * `camera_poses.txt`: VIO poses of each image in `camera` frame -* [RTAB-Map Databases](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY) - +* [RTAB-Map Databases](https://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link) +* Dataset now also available on [Federated Research Data Repository (FRDR)](https://doi.org/10.20383/103.0931) (if links above don't work) ## How reproduce results shown in the paper diff --git a/cmake_modules/FindORB_SLAM.cmake b/cmake_modules/FindORB_SLAM.cmake index 1e89348b73..f9ccdc0d57 100644 --- a/cmake_modules/FindORB_SLAM.cmake +++ b/cmake_modules/FindORB_SLAM.cmake @@ -12,6 +12,7 @@ find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/incl find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib) find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib) find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH) +find_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus NO_DEFAULT_PATH) find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH) find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH) @@ -21,6 +22,9 @@ IF(ORB_SLAM2_LIBRARY) ELSEIF(ORB_SLAM3_LIBRARY) SET(ORB_SLAM_VERSION 3) SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY}) + IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) # ORB_SLAM3 v1 + SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR}) + ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) ENDIF() IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY) diff --git a/cmake_modules/FindSqlite3.cmake b/cmake_modules/FindSqlite3.cmake deleted file mode 100644 index a5e5a10cf7..0000000000 --- a/cmake_modules/FindSqlite3.cmake +++ /dev/null @@ -1,30 +0,0 @@ -# - Find Sqlite3 -# This module finds an installed Sqlite3 package. -# -# It sets the following variables: -# Sqlite3_FOUND - Set to false, or undefined, if Sqlite3 isn't found. -# Sqlite3_INCLUDE_DIR - The Sqlite3 include directory. -# Sqlite3_LIBRARY - The Sqlite3 library to link against. - -FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR}) - -FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR}) - -IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY) - SET(Sqlite3_FOUND TRUE) - SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR}) - SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY}) -ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY) - -IF (Sqlite3_FOUND) - # show which Sqlite3 was found only if not quiet - IF (NOT Sqlite3_FIND_QUIETLY) - MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}") - ENDIF (NOT Sqlite3_FIND_QUIETLY) -ELSE (Sqlite3_FOUND) - # fatal error if Sqlite3 is required but not found - IF (Sqlite3_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find Sqlite3") - ENDIF (Sqlite3_FIND_REQUIRED) -ENDIF (Sqlite3_FOUND) - diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index 9c324cd786..a267f140e8 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/CameraInfo.h" -#include -#include -#include -#include - -class UDirectory; -class UTimer; +#include +#include namespace rtabmap { +class IMUFilter; + /** * Class Camera * */ -class RTABMAP_CORE_EXPORT Camera +class RTABMAP_CORE_EXPORT Camera : public SensorCapture { public: virtual ~Camera(); - SensorData takeImage(CameraInfo * info = 0); + + 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 + bool isInterIMUPublishing() const {return publishInterIMU_;} bool initFromFile(const std::string & calibrationPath); - virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0; virtual bool isCalibrated() const = 0; - virtual std::string getSerial() const = 0; - virtual bool odomProvided() const { return false; } - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; } - - //getters - float getImageRate() const {return _imageRate;} - const Transform & getLocalTransform() const {return _localTransform;} - //setters - void setImageRate(float imageRate) {_imageRate = imageRate;} - void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} - - void resetTimer(); protected: /** * Constructor @@ -78,19 +63,16 @@ class RTABMAP_CORE_EXPORT Camera */ Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); - /** - * returned rgb and depth images should be already rectified if calibration was loaded - */ - virtual SensorData captureImage(CameraInfo * info = 0) = 0; + virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0; - int getNextSeqID() {return ++_seq;} + void postInterIMU(const IMU & imu, double stamp); + +private: + virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);} private: - float _imageRate; - Transform _localTransform; - cv::Size _targetImageSize; - UTimer * _frameRateTimer; - int _seq; + IMUFilter * imuFilter_; + bool publishInterIMU_; }; diff --git a/corelib/include/rtabmap/core/CameraEvent.h b/corelib/include/rtabmap/core/CameraEvent.h index cdb6c8eb84..5f58b73112 100644 --- a/corelib/include/rtabmap/core/CameraEvent.h +++ b/corelib/include/rtabmap/core/CameraEvent.h @@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/CameraInfo.h" - -namespace rtabmap -{ - -class CameraEvent : - public UEvent -{ -public: - enum Code { - kCodeData, - kCodeNoMoreImages - }; - -public: - CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) : - UEvent(kCodeData), - data_(image, seq, stamp) - { - cameraInfo_.cameraName = cameraName; - } - - CameraEvent() : - UEvent(kCodeNoMoreImages) - { - } - - CameraEvent(const SensorData & data) : - UEvent(kCodeData), - data_(data) - { - } - - CameraEvent(const SensorData & data, const std::string & cameraName) : - UEvent(kCodeData), - data_(data) - { - cameraInfo_.cameraName = cameraName; - } - CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) : - UEvent(kCodeData), - data_(data), - cameraInfo_(cameraInfo) - { - } - - // Image or descriptors - const SensorData & data() const {return data_;} - const std::string & cameraName() const {return cameraInfo_.cameraName;} - const CameraInfo & info() const {return cameraInfo_;} - - virtual ~CameraEvent() {} - virtual std::string getClassName() const {return std::string("CameraEvent");} - -private: - SensorData data_; - CameraInfo cameraInfo_; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorEvent.h" diff --git a/corelib/include/rtabmap/core/CameraInfo.h b/corelib/include/rtabmap/core/CameraInfo.h index 0b3681543c..409af719ae 100644 --- a/corelib/include/rtabmap/core/CameraInfo.h +++ b/corelib/include/rtabmap/core/CameraInfo.h @@ -27,48 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -namespace rtabmap -{ - -class CameraInfo -{ - -public: - CameraInfo() : - cameraName(""), - id(0), - stamp(0.0), - timeCapture(0.0f), - timeDisparity(0.0f), - timeMirroring(0.0f), - timeStereoExposureCompensation(0.0f), - timeImageDecimation(0.0f), - timeScanFromDepth(0.0f), - timeUndistortDepth(0.0f), - timeBilateralFiltering(0.0f), - timeTotal(0.0f), - odomCovariance(cv::Mat::eye(6,6,CV_64FC1)) - { - } - virtual ~CameraInfo() {} - - std::string cameraName; - int id; - double stamp; - float timeCapture; - float timeDisparity; - float timeMirroring; - float timeStereoExposureCompensation; - float timeImageDecimation; - float timeScanFromDepth; - float timeUndistortDepth; - float timeBilateralFiltering; - float timeTotal; - Transform odomPose; - cv::Mat odomCovariance; - std::vector odomVelocity; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorCaptureInfo.h" diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 8dd40469ce..0ca8c2ec6d 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -27,129 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include -#include -#include - -namespace clams -{ -class DiscreteDepthDistortionModel; -} - -namespace rtabmap -{ - -class Camera; -class CameraInfo; -class SensorData; -class StereoDense; -class IMUFilter; - -/** - * Class CameraThread - * - */ -class RTABMAP_CORE_EXPORT CameraThread : - public UThread, - public UEventsSender -{ -public: - // ownership transferred - CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap()); - /** - * @param camera the camera to take images from - * @param odomSensor an odometry sensor to get a pose - * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame - */ - CameraThread(Camera * camera, - Camera * odomSensor, - const Transform & extrinsics, - double poseTimeOffset = 0.0, - float poseScaleFactor = 1.0f, - bool odomAsGt = false, - const ParametersMap & parameters = ParametersMap()); - CameraThread(Camera * camera, - bool odomAsGt, - const ParametersMap & parameters = ParametersMap()); - virtual ~CameraThread(); - - void setMirroringEnabled(bool enabled) {_mirroring = enabled;} - void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} - void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} - void setImageDecimation(int decimation) {_imageDecimation = decimation;} - void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} - void setImageRate(float imageRate); - void setDistortionModel(const std::string & path); - void enableBilateralFiltering(float sigmaS, float sigmaR); - void disableBilateralFiltering() {_bilateralFiltering = false;} - void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); - void disableIMUFiltering(); - - // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. - RTABMAP_DEPRECATED void setScanParameters( - bool fromDepth, - int downsampleStep, // decimation of the depth image in case the scan is from depth image - float rangeMin, - float rangeMax, - float voxelSize, - int normalsK, - int normalsRadius, - bool forceGroundNormalsUp); - void setScanParameters( - bool fromDepth, - int downsampleStep=1, // decimation of the depth image in case the scan is from depth image - float rangeMin=0.0f, - float rangeMax=0.0f, - float voxelSize = 0.0f, - int normalsK = 0, - int normalsRadius = 0.0f, - float groundNormalsUp = 0.0f); - - void postUpdate(SensorData * data, CameraInfo * info = 0) const; - - //getters - bool isPaused() const {return !this->isRunning();} - bool isCapturing() const {return this->isRunning();} - bool odomProvided() const; - - Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted - Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted - -private: - virtual void mainLoopBegin(); - virtual void mainLoop(); - virtual void mainLoopKill(); - -private: - Camera * _camera; - Camera * _odomSensor; - Transform _extrinsicsOdomToCamera; - bool _odomAsGt; - double _poseTimeOffset; - float _poseScaleFactor; - bool _mirroring; - bool _stereoExposureCompensation; - bool _colorOnly; - int _imageDecimation; - bool _stereoToDepth; - bool _scanFromDepth; - int _scanDownsampleStep; - float _scanRangeMin; - float _scanRangeMax; - float _scanVoxelSize; - int _scanNormalsK; - float _scanNormalsRadius; - float _scanForceGroundNormalsUp; - StereoDense * _stereoDense; - clams::DiscreteDepthDistortionModel * _distortionModel; - bool _bilateralFiltering; - float _bilateralSigmaS; - float _bilateralSigmaR; - IMUFilter * _imuFilter; - bool _imuBaseFrameConversion; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorCaptureThread.h" diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 1335189272..f18121f92e 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -96,6 +96,10 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode const cv::Mat & empty, float cellSize, const cv::Point3f & viewpoint); + void updateCalibration( + int nodeId, + const std::vector & models, + const std::vector & stereoModels); void updateDepthImage(int nodeId, const cv::Mat & image); void updateLaserScan(int nodeId, const LaserScan & scan); @@ -231,6 +235,11 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode float cellSize, const cv::Point3f & viewpoint) const = 0; + virtual void updateCalibrationQuery( + int nodeId, + const std::vector & models, + const std::vector & stereoModels) const = 0; + virtual void updateDepthImageQuery( int nodeId, const cv::Mat & image) const = 0; diff --git a/corelib/include/rtabmap/core/DBDriverSqlite3.h b/corelib/include/rtabmap/core/DBDriverSqlite3.h index 463e916817..9b61dbc4e8 100644 --- a/corelib/include/rtabmap/core/DBDriverSqlite3.h +++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h @@ -96,6 +96,11 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver { float cellSize, const cv::Point3f & viewpoint) const; + virtual void updateCalibrationQuery( + int nodeId, + const std::vector & models, + const std::vector & stereoModels) const; + virtual void updateDepthImageQuery( int nodeId, const cv::Mat & image) const; @@ -153,6 +158,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver { std::string queryStepNode() const; std::string queryStepImage() const; std::string queryStepDepth() const; + std::string queryStepCalibrationUpdate() const; std::string queryStepDepthUpdate() const; std::string queryStepScanUpdate() const; std::string queryStepSensorData() const; @@ -165,6 +171,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver { void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const; void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const; void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; + void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector & models, const std::vector & stereoModels) const; void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const; void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const; void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const; diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index f6fc97c00d..5bdc0e2008 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -86,10 +86,10 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera { const DBDriver * driver() const {return _dbDriver;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: - SensorData getNextData(CameraInfo * info = 0); + SensorData getNextData(SensorCaptureInfo * info = 0); private: std::list _paths; diff --git a/corelib/include/rtabmap/core/GlobalDescriptor.h b/corelib/include/rtabmap/core/GlobalDescriptor.h index 2dcff1b048..9940043e06 100644 --- a/corelib/include/rtabmap/core/GlobalDescriptor.h +++ b/corelib/include/rtabmap/core/GlobalDescriptor.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h new file mode 100644 index 0000000000..7a95940392 --- /dev/null +++ b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h @@ -0,0 +1,73 @@ +/* +Copyright (c) 2010-2024, 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 GLOBAL_DESCRIPTOR_EXTRACTOR_H_ +#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include "rtabmap/core/Parameters.h" +#include "rtabmap/core/SensorData.h" + + +namespace rtabmap { + +// Feature2D +class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor { +public: + enum Type { + kUndef=0, + kPyDescriptor=1}; + + static std::string typeName(Type type) + { + switch(type){ + case kPyDescriptor: + return "PyDescriptor"; + default: + return "Unknown"; + } + } + + static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap()); + static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience + +public: + virtual ~GlobalDescriptorExtractor(); + + virtual GlobalDescriptor extract(const SensorData & data) const = 0; + + virtual void parseParameters(const ParametersMap & parameters) {} + virtual GlobalDescriptorExtractor::Type getType() const = 0; + +protected: + GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap()); +}; + +} + +#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */ diff --git a/corelib/include/rtabmap/core/GlobalMap.h b/corelib/include/rtabmap/core/GlobalMap.h new file mode 100644 index 0000000000..140921328e --- /dev/null +++ b/corelib/include/rtabmap/core/GlobalMap.h @@ -0,0 +1,102 @@ +/* +Copyright (c) 2010-2023, 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 SRC_MAP_H_ +#define SRC_MAP_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT GlobalMap +{ +public: + inline static float logodds(double probability) + { + return (float) log(probability/(1-probability)); + } + + inline static double probability(double logodds) + { + return 1. - ( 1. / (1. + exp(logodds))); + } + +public: + virtual ~GlobalMap(); + + bool update(const std::map & poses); // return true if map has changed + + virtual void clear(); + + float getCellSize() const {return cellSize_;} + float getUpdateError() const {return updateError_;} + const std::map & addedNodes() const {return addedNodes_;} + + void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];} + void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];} + void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];} + void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];} + + virtual unsigned long getMemoryUsed() const; + +protected: + GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()); + + virtual void assemble(const std::list > & newPoses) = 0; + + const std::map & cache() const {return cache_->localGrids();} + + const std::map & assembledNodes() const {return addedNodes_;} + bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();} + void addAssembledNode(int id, const Transform & pose); + +protected: + float cellSize_; + float updateError_; + + float occupancyThr_; + float logOddsHit_; + float logOddsMiss_; + float logOddsClampingMin_; + float logOddsClampingMax_; + + double minValues_[3]; + double maxValues_[3]; + +private: + const LocalGridCache * cache_; + std::map addedNodes_; +}; + +} /* namespace rtabmap */ + +#endif /* SRC_MAP_H_ */ diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 1d53fd0d44..324323d252 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -136,6 +136,12 @@ std::multimap::iterator RTABMAP_CORE_EXPORT findLink( int to, bool checkBothWays = true, Link::Type type = Link::kUndef); +std::multimap >::iterator RTABMAP_CORE_EXPORT findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); std::multimap::iterator RTABMAP_CORE_EXPORT findLink( std::multimap & links, int from, @@ -147,6 +153,12 @@ std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( int to, bool checkBothWays = true, Link::Type type = Link::kUndef); +std::multimap >::const_iterator RTABMAP_CORE_EXPORT findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays = true, + Link::Type type = Link::kUndef); std::multimap::const_iterator RTABMAP_CORE_EXPORT findLink( const std::multimap & links, int from, diff --git a/corelib/include/rtabmap/core/IMUFilter.h b/corelib/include/rtabmap/core/IMUFilter.h index f93d3f9179..6bd97d4776 100644 --- a/corelib/include/rtabmap/core/IMUFilter.h +++ b/corelib/include/rtabmap/core/IMUFilter.h @@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ #define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class IMUFilter +class RTABMAP_CORE_EXPORT IMUFilter { public: enum Type { diff --git a/corelib/include/rtabmap/core/IMUThread.h b/corelib/include/rtabmap/core/IMUThread.h index c2d912fb55..a7cc834770 100644 --- a/corelib/include/rtabmap/core/IMUThread.h +++ b/corelib/include/rtabmap/core/IMUThread.h @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include +#include #include #include #include @@ -39,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { +class IMUFilter; + /** * Class IMUThread * @@ -53,6 +56,8 @@ class RTABMAP_CORE_EXPORT IMUThread : bool init(const std::string & path); void setRate(int rate); + void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); + void disableIMUFiltering(); private: virtual void mainLoopBegin(); @@ -65,6 +70,8 @@ class RTABMAP_CORE_EXPORT IMUThread : UTimer frameRateTimer_; double captureDelay_; double previousStamp_; + IMUFilter * _imuFilter; + bool _imuBaseFrameConversion; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/LaserScan.h b/corelib/include/rtabmap/core/LaserScan.h index 33999a7b59..0e76fdcb76 100644 --- a/corelib/include/rtabmap/core/LaserScan.h +++ b/corelib/include/rtabmap/core/LaserScan.h @@ -47,7 +47,8 @@ class RTABMAP_CORE_EXPORT LaserScan kXYZRGB=7, kXYZNormal=8, kXYZINormal=9, - kXYZRGBNormal=10}; + kXYZRGBNormal=10, + kXYZIT=11}; static std::string formatName(const Format & format); static int channels(const Format & format); @@ -55,6 +56,7 @@ class RTABMAP_CORE_EXPORT LaserScan static bool isScanHasNormals(const Format & format); static bool isScanHasRGB(const Format & format); static bool isScanHasIntensity(const Format & format); + static bool isScanHasTime(const Format & format); static LaserScan backwardCompatibility( const cv::Mat & oldScanFormat, int maxPoints = 0, @@ -121,22 +123,27 @@ class RTABMAP_CORE_EXPORT LaserScan float angleMin() const {return angleMin_;} float angleMax() const {return angleMax_;} float angleIncrement() const {return angleIncrement_;} + void setLocalTransform(const Transform & t) {localTransform_ = t;} Transform localTransform() const {return localTransform_;} bool empty() const {return data_.empty();} bool isEmpty() const {return data_.empty();} - int size() const {return data_.cols;} + int size() const {return data_.total();} int dataType() const {return data_.type();} bool is2d() const {return isScan2d(format_);} bool hasNormals() const {return isScanHasNormals(format_);} bool hasRGB() const {return isScanHasRGB(format_);} bool hasIntensity() const {return isScanHasIntensity(format_);} + bool hasTime() const {return isScanHasTime(format_);} bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;} + bool isOrganized() const {return data_.rows > 1;} LaserScan clone() const; + LaserScan densify() const; int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;} int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;} int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;} + int getTimeOffset() const {return hasTime()?4:-1;} float & field(unsigned int pointIndex, unsigned int channelOffset); diff --git a/corelib/include/rtabmap/core/Lidar.h b/corelib/include/rtabmap/core/Lidar.h new file mode 100644 index 0000000000..fd4b821556 --- /dev/null +++ b/corelib/include/rtabmap/core/Lidar.h @@ -0,0 +1,57 @@ +/* +Copyright (c) 2010-2022, 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/rtabmap_core_export.h" // DLL export/import defines +#include + +namespace rtabmap +{ + +/** + * Class Lidar + * + */ +class RTABMAP_CORE_EXPORT Lidar : public SensorCapture +{ +public: + virtual ~Lidar() {} + +protected: + /** + * Constructor + * + * @param lidarRate the frame rate (Hz), 0 for fast as the lidar can + * @param localTransform the transform from base frame to lidar frame + */ + Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) : + SensorCapture(lidarRate, localTransform) {} +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/LocalGrid.h b/corelib/include/rtabmap/core/LocalGrid.h new file mode 100644 index 0000000000..0b9c24c71a --- /dev/null +++ b/corelib/include/rtabmap/core/LocalGrid.h @@ -0,0 +1,90 @@ +/* +Copyright (c) 2010-2023, 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 SRC_LOCALGRID_H_ +#define SRC_LOCALGRID_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT LocalGrid +{ +public: + LocalGrid(const cv::Mat & ground, + const cv::Mat & obstacles, + const cv::Mat & empty, + float cellSize, + const cv::Point3f & viewPoint = cv::Point3f(0,0,0)); + virtual ~LocalGrid() {} + bool is3D() const; +public: + cv::Mat groundCells; + cv::Mat obstacleCells; + cv::Mat emptyCells; + float cellSize; + cv::Point3f viewPoint; +}; + +class RTABMAP_CORE_EXPORT LocalGridCache +{ +public: + LocalGridCache() {} + virtual ~LocalGridCache() {} + + void add(int nodeId, + const cv::Mat & ground, + const cv::Mat & obstacles, + const cv::Mat & empty, + float cellSize, + const cv::Point3f & viewPoint = cv::Point3f(0,0,0)); + + void add(int nodeId, const LocalGrid & localGrid); + + bool shareTo(int nodeId, LocalGridCache & anotherCache) const; + + unsigned long getMemoryUsed() const; + void clear(bool temporaryOnly = false); + + size_t size() const {return localGrids_.size();} + bool empty() const {return localGrids_.empty();} + const std::map & localGrids() const {return localGrids_;} + + std::map::const_iterator find(int nodeId) const {return localGrids_.find(nodeId);} + std::map::const_iterator begin() const {return localGrids_.begin();} + std::map::const_iterator end() const {return localGrids_.end();} + +private: + std::map localGrids_; +}; + +} /* namespace rtabmap */ + +#endif /* SRC_LOCALGRID_H_ */ diff --git a/corelib/include/rtabmap/core/LocalGridMaker.h b/corelib/include/rtabmap/core/LocalGridMaker.h new file mode 100644 index 0000000000..1cf057ee87 --- /dev/null +++ b/corelib/include/rtabmap/core/LocalGridMaker.h @@ -0,0 +1,115 @@ +/* +Copyright (c) 2010-2023, 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 SRC_LOCAL_MAP_H_ +#define SRC_LOCAL_MAP_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include + +#include +#include +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT LocalGridMaker +{ +public: + LocalGridMaker(const ParametersMap & parameters = ParametersMap()); + virtual ~LocalGridMaker(); + virtual void parseParameters(const ParametersMap & parameters); + + float getCellSize() const {return cellSize_;} + bool isGridFromDepth() const {return occupancySensor_;} + bool isMapFrameProjection() const {return projMapFrame_;} + + template + typename pcl::PointCloud::Ptr segmentCloud( + const typename pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const Transform & pose, + const cv::Point3f & viewPoint, + pcl::IndicesPtr & groundIndices, // output cloud indices + pcl::IndicesPtr & obstaclesIndices, // output cloud indices + pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices + + void createLocalMap( + const Signature & node, + cv::Mat & groundCells, + cv::Mat & obstacleCells, + cv::Mat & emptyCells, + cv::Point3f & viewPoint); + + void createLocalMap( + const LaserScan & cloud, + const Transform & pose, + cv::Mat & groundCells, + cv::Mat & obstacleCells, + cv::Mat & emptyCells, + cv::Point3f & viewPointInOut) const; + +protected: + ParametersMap parameters_; + + unsigned int cloudDecimation_; + float rangeMax_; + float rangeMin_; + std::vector roiRatios_; + float footprintLength_; + float footprintWidth_; + float footprintHeight_; + int scanDecimation_; + float cellSize_; + bool preVoxelFiltering_; + int occupancySensor_; + bool projMapFrame_; + float maxObstacleHeight_; + int normalKSearch_; + float groundNormalsUp_; + float maxGroundAngle_; + float clusterRadius_; + int minClusterSize_; + bool flatObstaclesDetected_; + float minGroundHeight_; + float maxGroundHeight_; + bool normalsSegmentation_; + bool grid3D_; + bool groundIsObstacle_; + float noiseFilteringRadius_; + int noiseFilteringMinNeighbors_; + bool scan2dUnknownSpaceFilled_; + bool rayTracing_; +}; + +} /* namespace rtabmap */ + +#include + +#endif /* SRC_MAP_H_ */ diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index e34210744d..4bb7ffe438 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -58,8 +58,9 @@ class RegistrationInfo; class RegistrationIcp; class RegistrationVis; class Stereo; -class OccupancyGrid; +class LocalGridMaker; class MarkerDetector; +class GlobalDescriptorExtractor; class RTABMAP_CORE_EXPORT Memory { @@ -331,6 +332,7 @@ class RTABMAP_CORE_EXPORT Memory bool _rehearsalWeightIgnoredWhileMoving; bool _useOdometryFeatures; bool _useOdometryGravity; + bool _rotateImagesUpsideUp; bool _createOccupancyGrid; int _visMaxFeatures; bool _imagesAlreadyRectified; @@ -372,9 +374,11 @@ class RTABMAP_CORE_EXPORT Memory RegistrationIcp * _registrationIcpMulti; RegistrationVis * _registrationVis; - OccupancyGrid * _occupancy; + LocalGridMaker * _localMapMaker; MarkerDetector * _markerDetector; + + GlobalDescriptorExtractor * _globalDescriptorExtractor; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/OccupancyGrid.h b/corelib/include/rtabmap/core/OccupancyGrid.h index 04617c4bfe..83bc2e2799 100644 --- a/corelib/include/rtabmap/core/OccupancyGrid.h +++ b/corelib/include/rtabmap/core/OccupancyGrid.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,144 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef CORELIB_SRC_OCCUPANCYGRID_H_ -#define CORELIB_SRC_OCCUPANCYGRID_H_ +#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ -#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include -#include -#include - -namespace rtabmap { - -class RTABMAP_CORE_EXPORT OccupancyGrid -{ -public: - inline static float logodds(double probability) - { - return (float) log(probability/(1-probability)); - } - - inline static double probability(double logodds) - { - return 1. - ( 1. / (1. + exp(logodds))); - } - -public: - OccupancyGrid(const ParametersMap & parameters = ParametersMap()); - void parseParameters(const ParametersMap & parameters); - void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map & poses); - void setCellSize(float cellSize); - float getCellSize() const {return cellSize_;} - void setCloudAssembling(bool enabled); - float getMinMapSize() const {return minMapSize_;} - bool isGridFromDepth() const {return occupancySensor_;} - bool isFullUpdate() const {return fullUpdate_;} - float getUpdateError() const {return updateError_;} - bool isMapFrameProjection() const {return projMapFrame_;} - const std::map & addedNodes() const {return addedNodes_;} - int cacheSize() const {return (int)cache_.size();} - const std::map, cv::Mat> > & getCache() const {return cache_;} - - template - typename pcl::PointCloud::Ptr segmentCloud( - const typename pcl::PointCloud::Ptr & cloud, - const pcl::IndicesPtr & indices, - const Transform & pose, - const cv::Point3f & viewPoint, - pcl::IndicesPtr & groundIndices, // output cloud indices - pcl::IndicesPtr & obstaclesIndices, // output cloud indices - pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices - - void createLocalMap( - const Signature & node, - cv::Mat & groundCells, - cv::Mat & obstacleCells, - cv::Mat & emptyCells, - cv::Point3f & viewPoint); - - void createLocalMap( - const LaserScan & cloud, - const Transform & pose, - cv::Mat & groundCells, - cv::Mat & obstacleCells, - cv::Mat & emptyCells, - cv::Point3f & viewPointInOut) const; - - void clear(); - void addToCache( - int nodeId, - const cv::Mat & ground, - const cv::Mat & obstacles, - const cv::Mat & empty); - bool update(const std::map & poses); // return true if map has changed - cv::Mat getMap(float & xMin, float & yMin) const; - cv::Mat getProbMap(float & xMin, float & yMin) const; - const pcl::PointCloud::Ptr & getMapGround() const {return assembledGround_;} - const pcl::PointCloud::Ptr & getMapObstacles() const {return assembledObstacles_;} - const pcl::PointCloud::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;} - - unsigned long getMemoryUsed() const; - -private: - ParametersMap parameters_; - unsigned int cloudDecimation_; - float cloudMaxDepth_; - float cloudMinDepth_; - std::vector roiRatios_; - float footprintLength_; - float footprintWidth_; - float footprintHeight_; - int scanDecimation_; - float cellSize_; - bool preVoxelFiltering_; - int occupancySensor_; - bool projMapFrame_; - float maxObstacleHeight_; - int normalKSearch_; - float groundNormalsUp_; - float maxGroundAngle_; - float clusterRadius_; - int minClusterSize_; - bool flatObstaclesDetected_; - float minGroundHeight_; - float maxGroundHeight_; - bool normalsSegmentation_; - bool grid3D_; - bool groundIsObstacle_; - float noiseFilteringRadius_; - int noiseFilteringMinNeighbors_; - bool scan2dUnknownSpaceFilled_; - bool rayTracing_; - bool fullUpdate_; - float minMapSize_; - bool erode_; - float footprintRadius_; - float updateError_; - float occupancyThr_; - float probHit_; - float probMiss_; - float probClampingMin_; - float probClampingMax_; - - std::map, cv::Mat> > cache_; //, empty> > - cv::Mat map_; - cv::Mat mapInfo_; - std::map > cellCount_; // - float xMin_; - float yMin_; - std::map addedNodes_; - - bool cloudAssembling_; - pcl::PointCloud::Ptr assembledGround_; - pcl::PointCloud::Ptr assembledObstacles_; - pcl::PointCloud::Ptr assembledEmptyCells_; -}; +/* + * Deprecated header, use the one below directly! + */ -} +#include -#include -#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */ +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ */ diff --git a/corelib/include/rtabmap/core/OctoMap.h b/corelib/include/rtabmap/core/OctoMap.h index 770177175f..2be979a5c6 100644 --- a/corelib/include/rtabmap/core/OctoMap.h +++ b/corelib/include/rtabmap/core/OctoMap.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,223 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef SRC_OCTOMAP_H_ -#define SRC_OCTOMAP_H_ +#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ -#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace rtabmap { - -// forward declaraton for "friend" -class RtabmapColorOcTree; - -class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode -{ -public: - enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100}; - -public: - friend class RtabmapColorOcTree; // needs access to node children (inherited) - - RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {} - RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {} - - void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;} - void setOccupancyType(char type) {type_=type;} - void setPointRef(const octomap::point3d & point) {pointRef_ = point;} - int getNodeRefId() const {return nodeRefId_;} - int getOccupancyType() const {return type_;} - const octomap::point3d & getPointRef() const {return pointRef_;} - - // following methods defined for octomap < 1.8 compatibility - RtabmapColorOcTreeNode* getChild(unsigned int i); - const RtabmapColorOcTreeNode* getChild(unsigned int i) const; - bool pruneNode(); - void expandNode(); - bool createChild(unsigned int i); - - void updateOccupancyTypeChildren(); - -private: - int nodeRefId_; - int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground - octomap::point3d pointRef_; -}; - -// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode -class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase { - - public: - /// Default constructor, sets resolution of leafs - RtabmapColorOcTree(double resolution); - virtual ~RtabmapColorOcTree() {} - - /// virtual constructor: creates a new object of same type - /// (Covariant return type requires an up-to-date compiler) - RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); } - - std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg - - /** - * Prunes a node when it is collapsible. This overloaded - * version only considers the node occupancy for pruning, - * different colors of child nodes are ignored. - * @return true if pruning was successful - */ - virtual bool pruneNode(RtabmapColorOcTreeNode* node); - - virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const; - - // set node color at given key or coordinate. Replaces previous color. - RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r, - uint8_t g, uint8_t b); - - RtabmapColorOcTreeNode* setNodeColor(float x, float y, - float z, uint8_t r, - uint8_t g, uint8_t b) { - octomap::OcTreeKey key; - if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; - return setNodeColor(key,r,g,b); - } - - // integrate color measurement at given key or coordinate. Average with previous color - RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r, - uint8_t g, uint8_t b); - - RtabmapColorOcTreeNode* averageNodeColor(float x, float y, - float z, uint8_t r, - uint8_t g, uint8_t b) { - octomap:: OcTreeKey key; - if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; - return averageNodeColor(key,r,g,b); - } - - // integrate color measurement at given key or coordinate. Average with previous color - RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r, - uint8_t g, uint8_t b); - - RtabmapColorOcTreeNode* integrateNodeColor(float x, float y, - float z, uint8_t r, - uint8_t g, uint8_t b) { - octomap::OcTreeKey key; - if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; - return integrateNodeColor(key,r,g,b); - } - - // update inner nodes, sets color to average child color - void updateInnerOccupancy(); - - protected: - void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth); - - /** - * Static member object which ensures that this OcTree's prototype - * ends up in the classIDMapping only once. You need this as a - * static member in any derived octree class in order to read .ot - * files through the AbstractOcTree factory. You should also call - * ensureLinking() once from the constructor. - */ - class StaticMemberInitializer{ - public: - StaticMemberInitializer(); - - /** - * Dummy function to ensure that MSVC does not drop the - * StaticMemberInitializer, causing this tree failing to register. - * Needs to be called from the constructor of this octree. - */ - void ensureLinking() {}; - }; - /// static member to ensure static initialization (only once) - static StaticMemberInitializer RtabmapColorOcTreeMemberInit; - - }; - -class RTABMAP_CORE_EXPORT OctoMap { -public: - OctoMap(const ParametersMap & parameters = ParametersMap()); - - const std::map & addedNodes() const {return addedNodes_;} - void addToCache(int nodeId, - const pcl::PointCloud::Ptr & ground, - const pcl::PointCloud::Ptr & obstacles, - const pcl::PointXYZ & viewPoint); - void addToCache(int nodeId, - const cv::Mat & ground, - const cv::Mat & obstacles, - const cv::Mat & empty, - const cv::Point3f & viewPoint); - bool update(const std::map & poses); // return true if map has changed - - const RtabmapColorOcTree * octree() const {return octree_;} - - pcl::PointCloud::Ptr createCloud( - unsigned int treeDepth = 0, - std::vector * obstacleIndices = 0, - std::vector * emptyIndices = 0, - std::vector * groundIndices = 0, - bool originalRefPoints = true, - std::vector * frontierIndices = 0, - std::vector * cloudProb = 0) const; - - cv::Mat createProjectionMap( - float & xMin, - float & yMin, - float & gridCellSize, - float minGridSize = 0.0f, - unsigned int treeDepth = 0); - - bool writeBinary(const std::string & path); - - virtual ~OctoMap(); - void clear(); - - void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];} - void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];} - - void setMaxRange(float value) {rangeMax_ = value;} - void setRayTracing(bool enabled) {rayTracing_ = enabled;} - bool hasColor() const {return hasColor_;} - - static std::unordered_set findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition); - static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set & EmptyNodes,std::queue& positionToExplore); - static bool isNodeVisited(std::unordered_set const & EmptyNodes,octomap::OcTreeKey const key); - static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition); - static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition); - -private: - void updateMinMax(const octomap::point3d & point); +/* + * Deprecated header, use the one below directly! + */ -private: - std::map, cv::Mat> > cache_; // [id: < , empty>] - std::map::Ptr, const pcl::PointCloud::Ptr> > cacheClouds_; // [id: ] - std::map cacheViewPoints_; - RtabmapColorOcTree * octree_; - std::map addedNodes_; - bool hasColor_; - bool fullUpdate_; - float updateError_; - float rangeMax_; - bool rayTracing_; - unsigned int emptyFloodFillDepth_; - double minValues_[3]; - double maxValues_[3]; -}; +#include -} /* namespace rtabmap */ -#endif /* SRC_OCTOMAP_H_ */ +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ */ diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 258be65836..8b89767309 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -111,6 +111,7 @@ class RTABMAP_CORE_EXPORT Odometry bool _alignWithGround; bool _publishRAMUsage; bool _imagesAlreadyRectified; + bool _deskewing; Transform _pose; int _resetCurrentCount; double previousStamp_; diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index 451a2d9e72..19b3e86c06 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -50,6 +50,7 @@ class OdometryInfo localBundleConstraints(0), localBundleTime(0), keyFrameAdded(false), + timeDeskewing(0.0f), timeEstimation(0.0f), timeParticleFiltering(0.0f), stamp(0), @@ -76,6 +77,7 @@ class OdometryInfo output.localBundlePoses = localBundlePoses; output.localBundleModels = localBundleModels; output.keyFrameAdded = keyFrameAdded; + output.timeDeskewing = timeDeskewing; output.timeEstimation = timeEstimation; output.timeParticleFiltering = timeParticleFiltering; output.stamp = stamp; @@ -105,6 +107,7 @@ class OdometryInfo std::map localBundlePoses; std::map > localBundleModels; bool keyFrameAdded; + float timeDeskewing; float timeEstimation; float timeParticleFiltering; double stamp; diff --git a/corelib/include/rtabmap/core/Optimizer.h b/corelib/include/rtabmap/core/Optimizer.h index aec3734ce5..23051312ee 100644 --- a/corelib/include/rtabmap/core/Optimizer.h +++ b/corelib/include/rtabmap/core/Optimizer.h @@ -79,8 +79,7 @@ class RTABMAP_CORE_EXPORT Optimizer const std::map & posesIn, const std::multimap & linksIn, std::map & posesOut, - std::multimap & linksOut, - bool adjustPosesWithConstraints = true) const; + std::multimap & linksOut) const; public: virtual ~Optimizer() {} diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 95c201a14f..4173a0570d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -231,6 +231,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them."); RTABMAP_PARAM(Mem, UseOdomGravity, bool, false, uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str())); RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix."); + RTABMAP_PARAM(Mem, GlobalDescriptorStrategy, int, 0, "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor"); + RTABMAP_PARAM(Mem, RotateImagesUpsideUp, bool, false, "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet."); // KeypointMemory (Keypoint-based) RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); @@ -357,6 +359,7 @@ class RTABMAP_CORE_EXPORT Parameters 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, 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())); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails."); @@ -376,6 +379,8 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(RGBD, MarkerDetection, bool, false, "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters."); RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links."); RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str())); + RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); + RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); // Local/Proximity loop closure detection RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM."); @@ -433,6 +438,9 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(g2o, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly."); RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); + RTABMAP_PARAM(GTSAM, Incremental, bool, false, uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str())); + RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold, double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info."); + 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"); @@ -455,6 +463,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str())); RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization."); + RTABMAP_PARAM(Odom, Deskewing, bool, true, "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided)."); // Odometry Frame-to-Map RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); @@ -525,12 +534,19 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket."); // Odometry ORB_SLAM2 - 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."); - RTABMAP_PARAM(OdomORBSLAM, Fps, float, 0.0, "Camera FPS."); - RTABMAP_PARAM(OdomORBSLAM, MaxFeatures, int, 1000, "Maximum ORB features extracted per frame."); - RTABMAP_PARAM(OdomORBSLAM, MapSize, int, 3000, "Maximum size of the feature map (0 means infinite)."); + 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."); + RTABMAP_PARAM(OdomORBSLAM, Fps, float, 0.0, "Camera FPS (0 to estimate from input data)."); + RTABMAP_PARAM(OdomORBSLAM, MaxFeatures, int, 1000, "Maximum ORB features extracted per frame."); + RTABMAP_PARAM(OdomORBSLAM, MapSize, int, 3000, "Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2."); + RTABMAP_PARAM(OdomORBSLAM, Inertial, bool, false, "Enable IMU. Only supported with ORB_SLAM3."); + RTABMAP_PARAM(OdomORBSLAM, GyroNoise, double, 0.01, "IMU gyroscope \"white noise\"."); + RTABMAP_PARAM(OdomORBSLAM, AccNoise, double, 0.1, "IMU accelerometer \"white noise\"."); + RTABMAP_PARAM(OdomORBSLAM, GyroWalk, double, 0.000001, "IMU gyroscope \"random walk\"."); + RTABMAP_PARAM(OdomORBSLAM, AccWalk, double, 0.0001, "IMU accelerometer \"random walk\"."); + RTABMAP_PARAM(OdomORBSLAM, SamplingRate, double, 0, "IMU sampling rate (0 to estimate from input data)."); + // Odometry OKVIS RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath, "", "Path of OKVIS config file."); @@ -575,6 +591,68 @@ class RTABMAP_CORE_EXPORT Parameters // Odometry VINS RTABMAP_PARAM_STR(OdomVINS, ConfigPath, "", "Path of VINS 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"); + RTABMAP_PARAM(OdomOpenVINS, UseKLT, bool, true, "If true we will use KLT, otherwise use a ORB descriptor + robust matching"); + RTABMAP_PARAM(OdomOpenVINS, NumPts, int, 200, "Number of points (per camera) we will extract and try to track"); + RTABMAP_PARAM(OdomOpenVINS, MinPxDist, int, 15, "Eistance between features (features near each other provide less information)"); + RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d, bool, false, "If we should perform 1d triangulation instead of 3d"); + RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures, bool, true, "If we should perform Levenberg-Marquardt refinement"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns, int, 5, "Max runs for Levenberg-Marquardt"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline, double, 40, "Max baseline ratio to accept triangulated features"); + RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber, double, 10000, "Max condition number of linear triangulation matrix accept triangulated features"); + + RTABMAP_PARAM(OdomOpenVINS, UseFEJ, bool, true, "If first-estimate Jacobians should be used (enable for good consistency)"); + RTABMAP_PARAM(OdomOpenVINS, Integration, int, 1, "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics, bool, false, "Bool to determine whether or not to calibrate imu-to-camera pose"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics, bool, false, "Bool to determine whether or not to calibrate camera intrinsics"); + RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset, bool, false, "Bool to determine whether or not to calibrate camera to IMU time offset"); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics, bool, false, "Bool to determine whether or not to calibrate the IMU intrinsics"); + RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity, bool, false, "Bool to determine whether or not to calibrate the Gravity sensitivity"); + RTABMAP_PARAM(OdomOpenVINS, MaxClones, int, 11, "Max clone size of sliding window"); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAM, int, 50, "Max number of estimated SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate, int, 25, "Max number of SLAM features we allow to be included in a single EKF update."); + RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate, int, 50, "Max number of MSCKF features we will use at a given image timestep."); + RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF, int, 0, "What representation our features are in (msckf features)"); + RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM, int, 4, "What representation our features are in (slam features)"); + RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay, double, 0.0, "Delay, in seconds, that we should wait from init before we start estimating SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, GravityMag, double, 9.81, "Gravity magnitude in the global frame (i.e. should be 9.81 typically)"); + RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath, "", "Mask for left image"); + RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath, "", "Mask for right image"); + + RTABMAP_PARAM(OdomOpenVINS, InitWindowTime, double, 2.0, "Amount of time we will initialize over (seconds)"); + RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh, double, 1.0, "Variance threshold on our acceleration to be classified as moving"); + RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity, double, 10.0, "Max disparity to consider the platform stationary (dependent on resolution)"); + RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures, int, 50, "How many features to track during initialization (saves on computation)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynUse, bool, false, "If dynamic initialization should be used"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib, bool, false, "If we should optimize calibration during intialization (not recommended)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter, int, 50, "How many iterations the MLE refinement should use (zero to skip the MLE)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime, double, 0.05, "How many seconds the MLE should be completed in"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads, int, 6, "How many threads the MLE should use"); + RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose, int, 6, "Number of poses to use within our window time (evenly spaced)"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg, double, 10.0, "Orientation change needed to try to init"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri, double, 10.0, "What to inflate the recovered q_GtoI covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel, double, 100.0, "What to inflate the recovered v_IinG covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg, double, 10.0, "What to inflate the recovered bias_g covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa, double, 100.0, "What to inflate the recovered bias_a covariance by"); + RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond, double, 1e-15, "Reciprocal condition number thresh for info inversion"); + + RTABMAP_PARAM(OdomOpenVINS, TryZUPT, bool, true, "If we should try to use zero velocity update"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier, double, 0.0, "Chi2 multiplier for zero velocity"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy, double, 0.1, "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier, double, 10.0, "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity, double, 0.5, "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)"); + RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning, bool, false, "If we should only use the zupt at the very beginning static initialization phase"); + + RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01, "[m/s^2/sqrt(Hz)] (accel \"white noise\")"); + RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk, double, 0.001, "[m/s^3/sqrt(Hz)] (accel bias diffusion)"); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity, double, 0.001, "[rad/s/sqrt(Hz)] (gyro \"white noise\")"); + RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk, double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)"); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx, double, 1.0, "Pixel noise for MSCKF features"); + RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier, double, 1.0, "Chi2 multiplier for MSCKF features"); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx, double, 1.0, "Pixel noise for SLAM features"); + RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier, double, 1.0, "Chi2 multiplier for SLAM features"); + // Odometry Open3D RTABMAP_PARAM(OdomOpen3D, MaxDepth, float, 3.0, "Maximum depth."); RTABMAP_PARAM(OdomOpen3D, Method, int, 0, "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid."); @@ -585,54 +663,57 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Reg, Force3DoF, bool, false, "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0."); // Visual registration parameters - RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); - RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); - RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); + RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms)."); + RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #else - RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); + RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str())); #endif - RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPVarianceMedianRatio, int, 4, uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str()); + RTABMAP_PARAM(Vis, PnPSplitLinearCovComponents, bool, false, uFormat("[%s = 1] Compute variance for each linear component instead of using the combined XYZ variance for all linear components.", kVisEstimationType().c_str()).c_str()); - RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); - RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); - RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); - RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); + RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str())); + RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); + RTABMAP_PARAM(Vis, MeanInliersDistance, float, 0.0, "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled."); + RTABMAP_PARAM(Vis, MinInliersDistribution, float, 0.0, "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled."); - RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform."); + 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"); #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"); #endif - RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); - RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); - RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); - RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); - RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); - RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); - RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); - RTABMAP_PARAM(Vis, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); - RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); - RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); - RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); + RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit)."); + RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features."); + RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); + RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix()."); + RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining."); + RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix()."); + RTABMAP_PARAM(Vis, GridRows, int, 1, uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + RTABMAP_PARAM(Vis, GridCols, int, 1, uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str())); + RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); + RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorGuessWinSize, int, 40, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); + RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str())); #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM) - RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #else - RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); + RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres."); #endif // Features matching approaches @@ -646,6 +727,10 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(GMS, WithScale, bool, false, "Take scale transformation into account."); RTABMAP_PARAM(GMS, ThresholdFactor, double, 6.0, "The higher, the less matches."); + // Global descriptor approaches + RTABMAP_PARAM_STR(PyDescriptor, Path, "", "Path to python script file (see available ones in rtabmap/corelib/src/pydescriptor/*). See the header to see where the script should be used."); + RTABMAP_PARAM(PyDescriptor, Dim, int, 4096, "Descriptor dimension."); + // ICP registration parameters #ifdef RTABMAP_POINTMATCHER RTABMAP_PARAM(Icp, Strategy, int, 1, "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare)."); @@ -663,10 +748,12 @@ class RTABMAP_CORE_EXPORT Parameters #else RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences."); #endif - RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations."); + RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true, "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence."); + RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations."); RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution."); RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str())); + RTABMAP_PARAM(Icp, FiltersEnabled, int, 3, "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both."); #ifdef RTABMAP_POINTMATCHER RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP."); #else @@ -762,8 +849,6 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors."); RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str())); RTABMAP_PARAM(Grid, RayTracing, bool, false, uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str())); - - RTABMAP_PARAM(GridGlobal, FullUpdate, bool, true, "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not."); RTABMAP_PARAM(GridGlobal, UpdateError, float, 0.01, "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value."); RTABMAP_PARAM(GridGlobal, FootprintRadius, float, 0.0, "Footprint radius (m) used to clear all obstacles under the graph."); RTABMAP_PARAM(GridGlobal, MinSize, float, 0.0, "Minimum map size (m)."); @@ -848,6 +933,7 @@ class RTABMAP_CORE_EXPORT Parameters static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false); static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false); + static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false); static void writeINI(const std::string & configFile, const ParametersMap & parameters); /** diff --git a/corelib/include/rtabmap/core/PythonInterface.h b/corelib/include/rtabmap/core/PythonInterface.h index 2216cb5ffa..f2f3a6a272 100644 --- a/corelib/include/rtabmap/core/PythonInterface.h +++ b/corelib/include/rtabmap/core/PythonInterface.h @@ -11,31 +11,31 @@ #include #include -#include + +namespace pybind11 { +class scoped_interpreter; +class gil_scoped_release; +} namespace rtabmap { +/** + * Create a single PythonInterface on main thread at + * global scope before any Python classes. + */ class PythonInterface { public: PythonInterface(); virtual ~PythonInterface(); -protected: - std::string getTraceback(); // should be called between lock() and unlock() - void lock(); - void unlock(); - private: - static UMutex mutex_; - static int refCount_; - -protected: - static PyThreadState * mainThreadState_; - static unsigned long mainThreadID_; - PyThreadState * threadState_; + pybind11::scoped_interpreter* guard_; + pybind11::gil_scoped_release* release_; }; +std::string getPythonTraceback(); + } #endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */ diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index e5d512f381..0735cfae1b 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -64,10 +64,12 @@ class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration float _rangeMin; float _rangeMax; float _maxCorrespondenceDistance; + bool _reciprocalCorrespondences; int _maxIterations; float _epsilon; float _correspondenceRatio; bool _force4DoF; + int _filtersEnabled; bool _pointToPlane; int _pointToPlaneK; float _pointToPlaneRadius; diff --git a/corelib/include/rtabmap/core/RegistrationVis.h b/corelib/include/rtabmap/core/RegistrationVis.h index 51e3a19acb..68c176f0db 100644 --- a/corelib/include/rtabmap/core/RegistrationVis.h +++ b/corelib/include/rtabmap/core/RegistrationVis.h @@ -82,7 +82,10 @@ class RTABMAP_CORE_EXPORT RegistrationVis : public Registration float _PnPReprojError; int _PnPFlags; int _PnPRefineIterations; + int _PnPVarMedianRatio; float _PnPMaxVar; + bool _PnPSplitLinearCovarianceComponents; + unsigned int _multiSamplingPolicy; int _correspondencesApproach; int _flowWinSize; int _flowIterations; diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 1120b728c4..d4b69c1fde 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -322,10 +322,13 @@ class RTABMAP_CORE_EXPORT Rtabmap int _pathStuckIterations; float _pathLinearVelocity; float _pathAngularVelocity; + bool _forceOdom3doF; bool _restartAtOrigin; bool _loopCovLimited; bool _loopGPS; int _maxOdomCacheSize; + bool _localizationSmoothing; + double _localizationPriorInf; bool _createGlobalScanMap; float _markerPriorsLinearVariance; float _markerPriorsAngularVariance; diff --git a/corelib/include/rtabmap/core/SensorCapture.h b/corelib/include/rtabmap/core/SensorCapture.h new file mode 100644 index 0000000000..20ee01b1a2 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCapture.h @@ -0,0 +1,93 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +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 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/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include "rtabmap/core/SensorData.h" +#include +#include +#include +#include + +class UDirectory; +class UTimer; + +namespace rtabmap +{ + +/** + * Class Camera + * + */ +class RTABMAP_CORE_EXPORT SensorCapture +{ +public: + virtual ~SensorCapture(); + SensorData takeData(SensorCaptureInfo * info = 0); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0; + virtual std::string getSerial() const = 0; + virtual bool odomProvided() const { return false; } + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; } + + //getters + float getFrameRate() const {return _frameRate;} + const Transform & getLocalTransform() const {return _localTransform;} + + //setters + void setFrameRate(float frameRate) {_frameRate = frameRate;} + void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} + + void resetTimer(); +protected: + /** + * Constructor + * + * @param frameRate the frame rate (Hz), 0 for fast as the sensor can + * @param localTransform the transform from base frame to sensor frame + */ + SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity()); + + /** + * returned rgb and depth images should be already rectified if calibration was loaded + */ + virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0; + + int getNextSeqID() {return ++_seq;} + +private: + float _frameRate; + Transform _localTransform; + UTimer * _frameRateTimer; + int _seq; +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorCaptureInfo.h b/corelib/include/rtabmap/core/SensorCaptureInfo.h new file mode 100644 index 0000000000..2b0b017ea5 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCaptureInfo.h @@ -0,0 +1,82 @@ +/* +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. +*/ + +#pragma once + +#include "rtabmap/core/Transform.h" +#include + +namespace rtabmap +{ + +class SensorCaptureInfo +{ + +public: + SensorCaptureInfo() : + cameraName(""), + id(0), + stamp(0.0), + timeCapture(0.0f), + timeDeskewing(0.0f), + timeDisparity(0.0f), + timeMirroring(0.0f), + timeStereoExposureCompensation(0.0f), + timeImageDecimation(0.0f), + timeHistogramEqualization(0.0f), + timeScanFromDepth(0.0f), + timeUndistortDepth(0.0f), + timeBilateralFiltering(0.0f), + timeTotal(0.0f), + odomCovariance(cv::Mat::eye(6,6,CV_64FC1)) + { + } + virtual ~SensorCaptureInfo() {} + + std::string cameraName; + int id; + double stamp; + float timeCapture; + float timeDeskewing; + float timeDisparity; + float timeMirroring; + float timeStereoExposureCompensation; + float timeImageDecimation; + float timeHistogramEqualization; + float timeScanFromDepth; + float timeUndistortDepth; + float timeBilateralFiltering; + float timeTotal; + Transform odomPose; + cv::Mat odomCovariance; + std::vector odomVelocity; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorCaptureThread.h b/corelib/include/rtabmap/core/SensorCaptureThread.h new file mode 100644 index 0000000000..5013381f92 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCaptureThread.h @@ -0,0 +1,216 @@ +/* +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. +*/ + +#pragma once + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include +#include + +namespace clams +{ +class DiscreteDepthDistortionModel; +} + +namespace rtabmap +{ + +class Camera; +class Lidar; +class SensorCapture; +class SensorCaptureInfo; +class SensorData; +class StereoDense; +class IMUFilter; +class Feature2D; + +/** + * Class CameraThread + * + */ +class RTABMAP_CORE_EXPORT SensorCaptureThread : + public UThread, + public UEventsSender +{ +public: + // ownership transferred + SensorCaptureThread( + Camera * camera, + const ParametersMap & parameters = ParametersMap()); + /** + * @param camera the camera to take images from + * @param odomSensor an odometry sensor to get a pose (can be again the camera) + * @param odomAsGt set odometry sensor pose as ground truth instead of odometry + * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame (without optical rotation) + */ + SensorCaptureThread( + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + */ + SensorCaptureThread( + Lidar * lidar, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing + */ + SensorCaptureThread( + Lidar * lidar, + Camera * camera, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar) + */ + SensorCaptureThread( + Lidar * lidar, + SensorCapture * odomSensor, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param camera the camera to take images from + * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar) + * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation) + */ + SensorCaptureThread( + Lidar * lidar, + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + virtual ~SensorCaptureThread(); + + void setMirroringEnabled(bool enabled) {_mirroring = enabled;} + void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} + void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} + void setImageDecimation(int decimation) {_imageDecimation = decimation;} + void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;} + void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} + void setFrameRate(float frameRate); + RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);} + void setDistortionModel(const std::string & path); + void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;} + void enableBilateralFiltering(float sigmaS, float sigmaR); + void disableBilateralFiltering() {_bilateralFiltering = false;} + void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); + void disableIMUFiltering(); + void enableFeatureDetection(const ParametersMap & parameters = ParametersMap()); + void disableFeatureDetection(); + + // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. + RTABMAP_DEPRECATED void setScanParameters( + bool fromDepth, + int downsampleStep, // decimation of the depth image in case the scan is from depth image + float rangeMin, + float rangeMax, + float voxelSize, + int normalsK, + float normalsRadius, + bool forceGroundNormalsUp, + bool deskewing); + void setScanParameters( + bool fromDepth, + int downsampleStep=1, // decimation of the depth image in case the scan is from depth image + float rangeMin=0.0f, + float rangeMax=0.0f, + float voxelSize = 0.0f, + int normalsK = 0, + float normalsRadius = 0.0f, + float groundNormalsUp = 0.0f, + bool deskewing = false); + + void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const; + + //getters + bool isPaused() const {return !this->isRunning();} + bool isCapturing() const {return this->isRunning();} + bool odomProvided() const; + + Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted + SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted + Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted + +private: + virtual void mainLoopBegin(); + virtual void mainLoop(); + virtual void mainLoopKill(); + +private: + Camera * _camera; + SensorCapture * _odomSensor; + Lidar * _lidar; + Transform _extrinsicsOdomToCamera; + bool _odomAsGt; + double _poseTimeOffset; + float _poseScaleFactor; + double _poseWaitTime; + bool _mirroring; + bool _stereoExposureCompensation; + bool _colorOnly; + int _imageDecimation; + int _histogramMethod; + bool _stereoToDepth; + bool _scanDeskewing; + bool _scanFromDepth; + int _scanDownsampleStep; + float _scanRangeMin; + float _scanRangeMax; + float _scanVoxelSize; + int _scanNormalsK; + float _scanNormalsRadius; + float _scanForceGroundNormalsUp; + StereoDense * _stereoDense; + clams::DiscreteDepthDistortionModel * _distortionModel; + bool _bilateralFiltering; + float _bilateralSigmaS; + float _bilateralSigmaR; + IMUFilter * _imuFilter; + bool _imuBaseFrameConversion; + Feature2D * _featureDetector; + bool _depthAsMask; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorEvent.h b/corelib/include/rtabmap/core/SensorEvent.h new file mode 100644 index 0000000000..240f5f0010 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorEvent.h @@ -0,0 +1,94 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include "rtabmap/core/SensorData.h" + +namespace rtabmap +{ + +class SensorEvent : + public UEvent +{ +public: + enum Code { + kCodeData, + kCodeNoMoreImages + }; + +public: + SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) : + UEvent(kCodeData), + data_(image, seq, stamp) + { + sensorCaptureInfo_.cameraName = cameraName; + } + + SensorEvent() : + UEvent(kCodeNoMoreImages) + { + } + + SensorEvent(const SensorData & data) : + UEvent(kCodeData), + data_(data) + { + } + + SensorEvent(const SensorData & data, const std::string & cameraName) : + UEvent(kCodeData), + data_(data) + { + sensorCaptureInfo_.cameraName = cameraName; + } + SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) : + UEvent(kCodeData), + data_(data), + sensorCaptureInfo_(sensorCaptureInfo) + { + } + + // Image or descriptors + const SensorData & data() const {return data_;} + const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;} + const SensorCaptureInfo & info() const {return sensorCaptureInfo_;} + + virtual ~SensorEvent() {} + virtual std::string getClassName() const {return std::string("SensorEvent");} + +private: + SensorData data_; + SensorCaptureInfo sensorCaptureInfo_; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorEvent CameraEvent; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index 1e393cb235..9dbbe532f3 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -90,9 +90,10 @@ class RTABMAP_CORE_EXPORT Signature void removeLink(int idTo); void removeVirtualLinks(); - void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));} + void addLandmark(const Link & landmark); const std::map & getLandmarks() const {return _landmarks;} - void removeLandmarks() {_landmarks.clear();} + void removeLandmarks(); + void removeLandmark(int landmarkId); void setSaved(bool saved) {_saved = saved;} void setModified(bool modified) {_modified = modified; _linksModified = modified;} diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 743b46bf04..66d0f358c0 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics RTABMAP_STATS(Timing, Memory_update, ms); RTABMAP_STATS(Timing, Neighbor_link_refining, ms); RTABMAP_STATS(Timing, Proximity_by_time, ms); + RTABMAP_STATS(Timing, Proximity_by_space_search, ms); RTABMAP_STATS(Timing, Proximity_by_space_visual, ms); RTABMAP_STATS(Timing, Proximity_by_space, ms); RTABMAP_STATS(Timing, Cleaning_neighbors, ms); diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 85b59fb2f9..fa359ad098 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -49,39 +49,64 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : public: CameraDepthAI( - const std::string & deviceSerial = "", + const std::string & mxidOrName = "", int resolution = 1, // 0=720p, 1=800p, 2=400p float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraDepthAI(); - void setOutputDepth(bool enabled, int confidence = 200); - void setIMUFirmwareUpdate(bool enabled); - void setIMUPublished(bool published); + void setOutputMode(int outputMode = 0); + void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5); + void setExtendedDisparity(bool extendedDisparity); + void setSubpixelMode(bool enabled, int fractionalBits = 3); + void setCompanding(bool enabled, int width=96); + void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f); + void setIMU(bool imuPublished, bool publishInterIMU); + void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f); + void setDetectFeatures(int detectFeatures = 0); + void setBlobPath(const std::string & blobPath); + void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000); + void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_DEPTHAI StereoCameraModel stereoModel_; + cv::Size targetSize_; Transform imuLocalTransform_; - std::string deviceSerial_; - bool outputDepth_; - int depthConfidence_; + std::string mxidOrName_; + int outputMode_; + int confThreshold_; + int lrcThreshold_; int resolution_; - bool imuFirmwareUpdate_; + bool extendedDisparity_; + int subpixelFractionalBits_; + int compandingWidth_; + bool useSpecTranslation_; + float alphaScaling_; bool imuPublished_; + bool publishInterIMU_; + float dotIntensity_; + float floodIntensity_; + int detectFeatures_; + bool useHarrisDetector_; + float minDistance_; + int numTargetFeatures_; + float threshold_; + bool nms_; + int nmsRadius_; + std::string blobPath_; std::shared_ptr device_; - std::shared_ptr leftQueue_; - std::shared_ptr rightOrDepthQueue_; - std::shared_ptr imuQueue_; + std::shared_ptr cameraQueue_; std::map accBuffer_; std::map gyroBuffer_; + UMutex imuMutex_; #endif }; diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect.h b/corelib/include/rtabmap/core/camera/CameraFreenect.h index 3373e04e45..4c7c62ffe9 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect.h @@ -61,7 +61,7 @@ class RTABMAP_CORE_EXPORT CameraFreenect : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FREENECT diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect2.h b/corelib/include/rtabmap/core/camera/CameraFreenect2.h index 70210c0069..d0516074c5 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect2.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect2.h @@ -77,7 +77,7 @@ class RTABMAP_CORE_EXPORT CameraFreenect2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FREENECT2 diff --git a/corelib/include/rtabmap/core/camera/CameraImages.h b/corelib/include/rtabmap/core/camera/CameraImages.h index 0a38c52c2d..9b7134dc99 100644 --- a/corelib/include/rtabmap/core/camera/CameraImages.h +++ b/corelib/include/rtabmap/core/camera/CameraImages.h @@ -118,7 +118,7 @@ class RTABMAP_CORE_EXPORT CameraImages : } protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: bool readPoses( diff --git a/corelib/include/rtabmap/core/camera/CameraK4A.h b/corelib/include/rtabmap/core/camera/CameraK4A.h index b942ea2086..0629af4693 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4A.h +++ b/corelib/include/rtabmap/core/camera/CameraK4A.h @@ -63,7 +63,7 @@ class RTABMAP_CORE_EXPORT CameraK4A : void setPreferences(int rgb_resolution, int framerate, int depth_resolution); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: void close(); diff --git a/corelib/include/rtabmap/core/camera/CameraK4W2.h b/corelib/include/rtabmap/core/camera/CameraK4W2.h index 26581471ae..a47a6378a4 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4W2.h +++ b/corelib/include/rtabmap/core/camera/CameraK4W2.h @@ -72,7 +72,7 @@ class RTABMAP_CORE_EXPORT CameraK4W2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: void close(); diff --git a/corelib/include/rtabmap/core/camera/CameraMyntEye.h b/corelib/include/rtabmap/core/camera/CameraMyntEye.h index 648d0846c8..d6037946a7 100644 --- a/corelib/include/rtabmap/core/camera/CameraMyntEye.h +++ b/corelib/include/rtabmap/core/camera/CameraMyntEye.h @@ -67,7 +67,7 @@ class RTABMAP_CORE_EXPORT CameraMyntEye : public Camera /** * returned rgb and depth images should be already rectified if calibration was loaded */ - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_MYNTEYE diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h index 32d14a4625..dffbcfef98 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h @@ -69,7 +69,7 @@ class RTABMAP_CORE_EXPORT CameraOpenNI2 : void setDepthDecimation(int decimation); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_OPENNI2 diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h index 41a0669acf..9cb467d13c 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h @@ -51,7 +51,7 @@ class RTABMAP_CORE_EXPORT CameraOpenNICV : virtual std::string getSerial() const {return "";} // unknown with OpenCV protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: bool _asus; diff --git a/corelib/include/rtabmap/core/camera/CameraOpenni.h b/corelib/include/rtabmap/core/camera/CameraOpenni.h index 31af10515e..37a4425921 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenni.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenni.h @@ -85,7 +85,7 @@ class RTABMAP_CORE_EXPORT CameraOpenni : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: pcl::Grabber* interface_; diff --git a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h index 03189a09ff..ebc8870b14 100644 --- a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h +++ b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h @@ -53,7 +53,7 @@ class RTABMAP_CORE_EXPORT CameraRGBDImages : virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: CameraImages cameraDepth_; diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense.h b/corelib/include/rtabmap/core/camera/CameraRealSense.h index bf9b4de5c9..9bfd01c8d0 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense.h @@ -72,7 +72,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense : virtual bool odomProvided() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_REALSENSE diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index e8e02e4847..47788f0f42 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -68,7 +68,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : virtual bool isCalibrated() const; virtual std::string getSerial() const; virtual bool odomProvided() const; - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance); + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // parameters are set during initialization // D400 series @@ -77,7 +77,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : void setResolution(int width, int height, int fps = 30); void setDepthResolution(int width, int height, int fps = 30); void setGlobalTimeSync(bool enabled); - void publishInterIMU(bool enabled); + /** * Dual mode (D400+T265 or L500+T265) * @param enabled enable dual mode @@ -105,7 +105,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : #endif protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_REALSENSE2 @@ -142,7 +142,6 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : int cameraDepthHeight_; int cameraDepthFps_; bool globalTimeSync_; - bool publishInterIMU_; bool dualMode_; Transform dualExtrinsics_; std::string jsonConfig_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h index 13832204b5..025566b0e9 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h @@ -51,7 +51,7 @@ class RTABMAP_CORE_EXPORT CameraStereoDC1394 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_DC1394 diff --git a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h index 093f40865c..3b42788486 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h @@ -53,7 +53,7 @@ class RTABMAP_CORE_EXPORT CameraStereoFlyCapture2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FLYCAPTURE2 diff --git a/corelib/include/rtabmap/core/camera/CameraStereoImages.h b/corelib/include/rtabmap/core/camera/CameraStereoImages.h index ed3a3ae303..75b43bbf68 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h @@ -64,7 +64,7 @@ class RTABMAP_CORE_EXPORT CameraStereoImages : virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: CameraImages * camera2_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoTara.h b/corelib/include/rtabmap/core/camera/CameraStereoTara.h index d6a655e4cf..72f5e12265 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoTara.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoTara.h @@ -60,7 +60,7 @@ public Camera virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: cv::VideoCapture capture_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h index 90042e05c3..b2e17d518d 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h @@ -71,7 +71,7 @@ class RTABMAP_CORE_EXPORT CameraStereoVideo : void setResolution(int width, int height) {_width=width, _height=height;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: cv::VideoCapture capture_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index 0f5f2591ec..640eec6c5a 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -45,11 +45,11 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : { public: static bool available(); - + static int sdkVersion(); public: CameraStereoZed( int deviceId, - int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA + int resolution = 6, // 0=HD2K, 1=HD1080, 2=HD1200, 3=HD720, 4=SVGA, 5=VGA, 6=AUTO int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, @@ -61,7 +61,7 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : int texturenessConfidenceThr = 90); // introduced with ZED SDK 3 CameraStereoZed( const std::string & svoFilePath, - int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY + int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, bool computeOdometry = false, @@ -76,12 +76,12 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : virtual bool isCalibrated() const; virtual std::string getSerial() const; virtual bool odomProvided() const; - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance); + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0); - void publishInterIMU(bool enabled); + void postInterIMUPublic(const IMU & imu, double stamp); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_ZED @@ -100,7 +100,6 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : bool computeOdometry_; bool lost_; bool force3DoF_; - bool publishInterIMU_; ZedIMUThread * imuPublishingThread_; #endif }; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h index 5f31ea960e..2399b7b174 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h @@ -63,7 +63,7 @@ class RTABMAP_CORE_EXPORT CameraStereoZedOC : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_ZEDOC diff --git a/corelib/include/rtabmap/core/camera/CameraVideo.h b/corelib/include/rtabmap/core/camera/CameraVideo.h index da7124937f..d34e0e9b45 100644 --- a/corelib/include/rtabmap/core/camera/CameraVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraVideo.h @@ -64,7 +64,7 @@ class RTABMAP_CORE_EXPORT CameraVideo : void setResolution(int width, int height) {_width=width, _height=height;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: // File type diff --git a/corelib/include/rtabmap/core/global_map/CloudMap.h b/corelib/include/rtabmap/core/global_map/CloudMap.h new file mode 100644 index 0000000000..bf63bc05bf --- /dev/null +++ b/corelib/include/rtabmap/core/global_map/CloudMap.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 CORELIB_SRC_CLOUDMAP_H_ +#define CORELIB_SRC_CLOUDMAP_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include + +#include +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT CloudMap : public GlobalMap +{ +public: + CloudMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()); + + virtual void clear(); + + const pcl::PointCloud::Ptr & getMapGround() const {return assembledGround_;} + const pcl::PointCloud::Ptr & getMapObstacles() const {return assembledObstacles_;} + const pcl::PointCloud::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;} + + unsigned long getMemoryUsed() const; + +protected: + virtual void assemble(const std::list > & newPoses); + +private: + pcl::PointCloud::Ptr assembledGround_; + pcl::PointCloud::Ptr assembledObstacles_; + pcl::PointCloud::Ptr assembledEmptyCells_; +}; + +} + +#endif /* CORELIB_SRC_CLOUDMAP_H_ */ diff --git a/corelib/include/rtabmap/core/global_map/GridMap.h b/corelib/include/rtabmap/core/global_map/GridMap.h new file mode 100644 index 0000000000..1849856add --- /dev/null +++ b/corelib/include/rtabmap/core/global_map/GridMap.h @@ -0,0 +1,69 @@ +/* +Copyright (c) 2010-2023, 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_SRC_GRIDMAP_H_ +#define CORELIB_SRC_GRIDMAP_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include +#include + +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT GridMap : public GlobalMap +{ +public: + GridMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()); + + virtual void clear(); + + const grid_map::GridMap & gridMap() const {return gridMap_;} + + cv::Mat createHeightMap(float & xMin, float & yMin, float & cellSize) const; + cv::Mat createColorMap(float & xMin, float & yMin, float & cellSize) const; + pcl::PointCloud::Ptr createTerrainCloud() const; + pcl::PolygonMesh::Ptr createTerrainMesh() const; + +protected: + virtual void assemble(const std::list > & newPoses); + +private: + cv::Mat toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const; + +private: + grid_map::GridMap gridMap_; + float minMapSize_; +}; + +} + +#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */ diff --git a/corelib/include/rtabmap/core/global_map/OccupancyGrid.h b/corelib/include/rtabmap/core/global_map/OccupancyGrid.h new file mode 100644 index 0000000000..915c3dc1e0 --- /dev/null +++ b/corelib/include/rtabmap/core/global_map/OccupancyGrid.h @@ -0,0 +1,69 @@ +/* +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 CORELIB_SRC_OCCUPANCYGRID_H_ +#define CORELIB_SRC_OCCUPANCYGRID_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include + +#include +#include + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT OccupancyGrid : public GlobalMap +{ +public: + OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()); + void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map & poses); + float getMinMapSize() const {return minMapSize_;} + + virtual void clear(); + + cv::Mat getMap(float & xMin, float & yMin) const; + cv::Mat getProbMap(float & xMin, float & yMin) const; + + unsigned long getMemoryUsed() const; + +protected: + virtual void assemble(const std::list > & newPoses); + +private: + cv::Mat map_; + cv::Mat mapInfo_; + std::map > cellCount_; // + + float minMapSize_; + bool erode_; + float footprintRadius_; +}; + +} + +#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */ diff --git a/corelib/include/rtabmap/core/global_map/OctoMap.h b/corelib/include/rtabmap/core/global_map/OctoMap.h new file mode 100644 index 0000000000..53f3308967 --- /dev/null +++ b/corelib/include/rtabmap/core/global_map/OctoMap.h @@ -0,0 +1,227 @@ +/* +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 SRC_OCTOMAP_H_ +#define SRC_OCTOMAP_H_ + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace rtabmap { + +// forward declaraton for "friend" +class RtabmapColorOcTree; + +class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode +{ +public: + enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100}; + +public: + friend class RtabmapColorOcTree; // needs access to node children (inherited) + + RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {} + RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {} + + void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;} + void setOccupancyType(char type) {type_=type;} + void setPointRef(const octomap::point3d & point) {pointRef_ = point;} + int getNodeRefId() const {return nodeRefId_;} + int getOccupancyType() const {return type_;} + const octomap::point3d & getPointRef() const {return pointRef_;} + + // following methods defined for octomap < 1.8 compatibility + RtabmapColorOcTreeNode* getChild(unsigned int i); + const RtabmapColorOcTreeNode* getChild(unsigned int i) const; + bool pruneNode(); + void expandNode(); + bool createChild(unsigned int i); + + void updateOccupancyTypeChildren(); + +private: + int nodeRefId_; + int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground + octomap::point3d pointRef_; +}; + +// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode +class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase { + + public: + /// Default constructor, sets resolution of leafs + RtabmapColorOcTree(double resolution); + virtual ~RtabmapColorOcTree() {} + + /// virtual constructor: creates a new object of same type + /// (Covariant return type requires an up-to-date compiler) + RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); } + + std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg + + /** + * Prunes a node when it is collapsible. This overloaded + * version only considers the node occupancy for pruning, + * different colors of child nodes are ignored. + * @return true if pruning was successful + */ + virtual bool pruneNode(RtabmapColorOcTreeNode* node); + + virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const; + + // set node color at given key or coordinate. Replaces previous color. + RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r, + uint8_t g, uint8_t b); + + RtabmapColorOcTreeNode* setNodeColor(float x, float y, + float z, uint8_t r, + uint8_t g, uint8_t b) { + octomap::OcTreeKey key; + if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; + return setNodeColor(key,r,g,b); + } + + // integrate color measurement at given key or coordinate. Average with previous color + RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r, + uint8_t g, uint8_t b); + + RtabmapColorOcTreeNode* averageNodeColor(float x, float y, + float z, uint8_t r, + uint8_t g, uint8_t b) { + octomap:: OcTreeKey key; + if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; + return averageNodeColor(key,r,g,b); + } + + // integrate color measurement at given key or coordinate. Average with previous color + RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r, + uint8_t g, uint8_t b); + + RtabmapColorOcTreeNode* integrateNodeColor(float x, float y, + float z, uint8_t r, + uint8_t g, uint8_t b) { + octomap::OcTreeKey key; + if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL; + return integrateNodeColor(key,r,g,b); + } + + // update inner nodes, sets color to average child color + void updateInnerOccupancy(); + + protected: + void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth); + + /** + * Static member object which ensures that this OcTree's prototype + * ends up in the classIDMapping only once. You need this as a + * static member in any derived octree class in order to read .ot + * files through the AbstractOcTree factory. You should also call + * ensureLinking() once from the constructor. + */ + class StaticMemberInitializer{ + public: + StaticMemberInitializer(); + + /** + * Dummy function to ensure that MSVC does not drop the + * StaticMemberInitializer, causing this tree failing to register. + * Needs to be called from the constructor of this octree. + */ + void ensureLinking() {}; + }; + /// static member to ensure static initialization (only once) + static StaticMemberInitializer RtabmapColorOcTreeMemberInit; + + }; + +class RTABMAP_CORE_EXPORT OctoMap : public GlobalMap { +public: + OctoMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()); + + const RtabmapColorOcTree * octree() const {return octree_;} + + pcl::PointCloud::Ptr createCloud( + unsigned int treeDepth = 0, + std::vector * obstacleIndices = 0, + std::vector * emptyIndices = 0, + std::vector * groundIndices = 0, + bool originalRefPoints = true, + std::vector * frontierIndices = 0, + std::vector * cloudProb = 0) const; + + cv::Mat createProjectionMap( + float & xMin, + float & yMin, + float & gridCellSize, + float minGridSize = 0.0f, + unsigned int treeDepth = 0); + + bool writeBinary(const std::string & path); + + virtual ~OctoMap(); + virtual void clear(); + virtual unsigned long getMemoryUsed() const; + + bool hasColor() const {return hasColor_;} + + static std::unordered_set findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition); + static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set & EmptyNodes,std::queue& positionToExplore); + static bool isNodeVisited(std::unordered_set const & EmptyNodes,octomap::OcTreeKey const key); + static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition); + static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition); + +protected: + virtual void assemble(const std::list > & newPoses); + +private: + void updateMinMax(const octomap::point3d & point); + +private: + RtabmapColorOcTree * octree_; + bool hasColor_; + float rangeMax_; + bool rayTracing_; + unsigned int emptyFloodFillDepth_; +}; + +} /* namespace rtabmap */ + +#endif /* SRC_OCTOMAP_H_ */ diff --git a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp b/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp similarity index 96% rename from corelib/include/rtabmap/core/impl/OccupancyGrid.hpp rename to corelib/include/rtabmap/core/impl/LocalMapMaker.hpp index a614a3d5a8..a844d450b2 100644 --- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp +++ b/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp @@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ -#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ +#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_ +#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_ #include #include @@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { template -typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( +typename pcl::PointCloud::Ptr LocalGridMaker::segmentCloud( const typename pcl::PointCloud::Ptr & cloudIn, const pcl::IndicesPtr & indicesIn, const Transform & pose, @@ -205,4 +205,4 @@ typename pcl::PointCloud::Ptr OccupancyGrid::segmentCloud( } -#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */ +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_ */ diff --git a/corelib/include/rtabmap/core/lidar/LidarVLP16.h b/corelib/include/rtabmap/core/lidar/LidarVLP16.h new file mode 100644 index 0000000000..bb8df04bdb --- /dev/null +++ b/corelib/include/rtabmap/core/lidar/LidarVLP16.h @@ -0,0 +1,94 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +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 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_LIDAR_LIDARVLP16_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include + +#include +#include + +namespace rtabmap { + +struct PointXYZIT { + float x; + float y; + float z; + float i; + float t; +}; + +class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber { +public: + LidarVLP16( + const std::string& pcapFile, + bool organized = false, + bool stampLast = true, + float frameRate = 0.0f, + Transform localTransform = Transform::getIdentity()); + LidarVLP16( + const boost::asio::ip::address& ipAddress, + const std::uint16_t port = 2368, + bool organized = false, + bool useHostTime = true, + bool stampLast = true, + float frameRate = 0.0f, + Transform localTransform = Transform::getIdentity()); + virtual ~LidarVLP16(); + + SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);} + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override; + virtual std::string getSerial() const override {return getName();} + + void setOrganized(bool enable); + +private: + void buildTimings(bool dualMode); + virtual void toPointClouds (HDLDataPacket *dataPacket) override; + +protected: + virtual SensorData captureData(SensorCaptureInfo * info = 0) override; + +private: + // timing offset lookup table + std::vector< std::vector > timingOffsets_; + bool timingOffsetsDualMode_; + double startSweepTime_; + double startSweepTimeHost_; + bool organized_; + bool useHostTime_; + bool stampLast_; + SensorData lastScan_; + std::vector > accumulatedScans_; + USemaphore scanReady_; + UMutex lastScanMutex_; +}; + +} /* namespace rtabmap */ + +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ */ diff --git a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h similarity index 79% rename from corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h rename to corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h index a0af8bad03..0a0adeff4c 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h +++ b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h @@ -25,48 +25,38 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef ODOMETRYORBSLAM_H_ -#define ODOMETRYORBSLAM_H_ +#ifndef ODOMETRYORBSLAM2_H_ +#define ODOMETRYORBSLAM2_H_ #include -#if RTABMAP_ORB_SLAM == 3 -namespace ORB_SLAM3 { -#else -namespace ORB_SLAM2 { -#endif -class System; -} -class ORBSLAMSystem; +class ORBSLAM2System; namespace rtabmap { -class RTABMAP_CORE_EXPORT OdometryORBSLAM : public Odometry +class RTABMAP_CORE_EXPORT OdometryORBSLAM2 : public Odometry { public: - OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); - virtual ~OdometryORBSLAM(); + OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryORBSLAM2(); virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;} - virtual bool canProcessAsyncIMU() const; private: virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); private: -#ifdef RTABMAP_ORB_SLAM - ORBSLAMSystem * orbslam_; +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 + ORBSLAM2System * orbslam_; bool firstFrame_; Transform originLocalTransform_; Transform previousPose_; - bool useIMU_; - Transform imuLocalTransform_; #endif }; } -#endif /* ODOMETRYORBSLAM_H_ */ +#endif /* ODOMETRYORBSLAM2_H_ */ diff --git a/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h new file mode 100644 index 0000000000..2bb47f047a --- /dev/null +++ b/corelib/include/rtabmap/core/odometry/OdometryORBSLAM3.h @@ -0,0 +1,71 @@ +/* +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 ODOMETRYORBSLAM3_H_ +#define ODOMETRYORBSLAM3_H_ + +#include + +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 +#include +#endif + +namespace rtabmap { + +class RTABMAP_CORE_EXPORT OdometryORBSLAM3 : public Odometry +{ +public: + OdometryORBSLAM3(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); + virtual ~OdometryORBSLAM3(); + + virtual void reset(const Transform & initialPose = Transform::getIdentity()); + virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;} + virtual bool canProcessAsyncIMU() const; + +private: + virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); + + bool init(const rtabmap::CameraModel & model1, const rtabmap::CameraModel & model2, double stamp, bool stereo, double baseline); +private: +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + ORB_SLAM3::System * orbslam_; + bool firstFrame_; + Transform originLocalTransform_; + Transform previousPose_; + bool useIMU_; + Transform imuLocalTransform_; + ParametersMap parameters_; + std::vector orbslamImus_; + double lastImuStamp_; + double lastImageStamp_; +#endif + +}; + +} + +#endif /* ODOMETRYORBSLAM_H3_ */ diff --git a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h index ed0aa2de2c..1d17e74558 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOpenVINS.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ov_msckf { class VioManager; +struct VioManagerOptions; } namespace rtabmap { @@ -40,7 +41,6 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry { public: OdometryOpenVINS(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()); - virtual ~OdometryOpenVINS(); virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual Odometry::Type getType() {return Odometry::kTypeOpenVINS;} @@ -52,12 +52,12 @@ class RTABMAP_CORE_EXPORT OdometryOpenVINS : public Odometry private: #ifdef RTABMAP_OPENVINS - ov_msckf::VioManager * vioManager_; + std::unique_ptr vioManager_; + std::unique_ptr params_; bool initGravity_; - Transform previousPose_; - Transform previousLocalTransform_; - Transform imuLocalTransform_; - std::map imuBuffer_; + Transform previousPoseInv_; + Transform imuLocalTransformInv_; + Eigen::Matrix Phi_; #endif }; diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h index 5cec5c2415..2d3eb6ba01 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h @@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +namespace gtsam { + class ISAM2; +} + namespace rtabmap { class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer @@ -38,13 +42,8 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer static bool available(); public: - OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) : - Optimizer(parameters), - optimizer_(Parameters::defaultGTSAMOptimizer()) - { - parseParameters(parameters); - } - virtual ~OptimizerGTSAM() {} + OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()); + virtual ~OptimizerGTSAM(); virtual Type type() const {return kTypeGTSAM;} @@ -60,7 +59,25 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer int * iterationsDone = 0); private: - int optimizer_; + int internalOptimizerType_; + + gtsam::ISAM2 * isam2_; + struct ConstraintToFactor { + ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice) + { + from = _from; + to = _to; + factorIndice = _factorIndice; + } + int from; + int to; + std::uint64_t factorIndice; + }; + + std::vector lastAddedConstraints_; + int lastSwitchId_; + std::set addedPoses_; + std::pair lastRootFactorIndex_; }; } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index 008309573b..3565921386 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/corelib/include/rtabmap/core/util2d.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap @@ -156,6 +157,32 @@ cv::Mat RTABMAP_CORE_EXPORT exposureFusion( void RTABMAP_CORE_EXPORT HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ); +void RTABMAP_CORE_EXPORT NMS( + const std::vector & ptsIn, + const cv::Mat & descriptorsIn, + std::vector & ptsOut, + cv::Mat & descriptorsOut, + int border, int dist_thresh, int img_width, int img_height); + +/** + * @brief Rotate images and camera model so that the top of the image is up. + * + * The roll value of local transform of the camera model is used to estimate + * if the images have to be rotated. If there is a pitch value higher than + * 45 deg, the original images and camera model will be returned (no rotation will happen). + * The return local transform of the camera model is updated accordingly. The distortion + * model is ignored and won't be transfered to modified camera model, so this function + * expects already rectified images. + * + * @param model a valid camera model + * @param rgb a rgb/grayscale image (set cv::Mat() if not used) + * @param depth a depth image (set cv::Mat() if not used) + */ +void RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary( + CameraModel & model, + cv::Mat & rgb, + cv::Mat & depth); + } // namespace util3d } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5077083906..9cba8724da 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -144,6 +144,43 @@ pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages std::vector * validIndices = 0, const ParametersMap & parameters = ParametersMap()); +/** + * Create a XYZ cloud from the images contained in SensorData, one for each camera + * + * @param sensorData, the sensor data. + * @param decimation, images are decimated by this factor before projecting points to 3D. The factor + * should be a factor of the image width and height. + * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud). + * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud). + * @param validIndices, the indices of valid points in the cloud + * @param stereoParameters, stereo optional parameters (in case it is stereo data) + * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected. + * @return XYZ cloud(s), one per camera + */ +std::vector::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData( + const SensorData & sensorData, + int decimation = 1, + float maxDepth = 0.0f, + float minDepth = 0.0f, + std::vector * validIndices = 0, + const ParametersMap & stereoParameters = ParametersMap(), + const std::vector & roiRatios = std::vector()); // ignored for stereo + +/** + * Create a XYZ cloud from the images contained in SensorData. If there is only one camera, + * the returned cloud is organized. Otherwise, all NaN + * points are removed and the cloud will be dense. + * + * @param sensorData, the sensor data. + * @param decimation, images are decimated by this factor before projecting points to 3D. The factor + * should be a factor of the image width and height. + * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud). + * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud). + * @param validIndices, the indices of valid points in the cloud + * @param stereoParameters, stereo optional parameters (in case it is stereo data) + * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected. + * @return a XYZ cloud. + */ pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData( const SensorData & sensorData, int decimation = 1, @@ -153,6 +190,28 @@ pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData( const ParametersMap & stereoParameters = ParametersMap(), const std::vector & roiRatios = std::vector()); // ignored for stereo +/** + * Create an RGB cloud from the images contained in SensorData, one for each camera + * + * @param sensorData, the sensor data. + * @param decimation, images are decimated by this factor before projecting points to 3D. The factor + * should be a factor of the image width and height. + * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud). + * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud). + * @param validIndices, the indices of valid points in the cloud + * @param stereoParameters, stereo optional parameters (in case it is stereo data) + * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected. + * @return RGB cloud(s), one per camera + */ +std::vector::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData( + const SensorData & sensorData, + int decimation = 1, + float maxDepth = 0.0f, + float minDepth = 0.0f, + std::vector * validIndices = 0, + const ParametersMap & stereoParameters = ParametersMap(), + const std::vector & roiRatios = std::vector()); // ignored for stereo + /** * Create an RGB cloud from the images contained in SensorData. If there is only one camera, * the returned cloud is organized. Otherwise, all NaN @@ -164,6 +223,7 @@ pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData( * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud). * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud). * @param validIndices, the indices of valid points in the cloud + * @param stereoParameters, stereo optional parameters (in case it is stereo data) * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected. * @return a RGB cloud. */ @@ -395,6 +455,19 @@ RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT loadC int downsampleStep = 1, float voxelSize = 0.0f); +/** + * @brief Lidar deskewing + * @param input lidar, format should have time channel + * @param input stamp of the lidar + * @param velocity in base frame + * @param velocity stamp at which it has been computed + * @return lidar deskewed + */ +LaserScan RTABMAP_CORE_EXPORT deskew( + const LaserScan & input, + double inputStamp, + const rtabmap::Transform & velocity); + } // namespace util3d } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/util3d_motion_estimation.h b/corelib/include/rtabmap/core/util3d_motion_estimation.h index e859c7ac69..ed500e25e0 100644 --- a/corelib/include/rtabmap/core/util3d_motion_estimation.h +++ b/corelib/include/rtabmap/core/util3d_motion_estimation.h @@ -48,28 +48,33 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D( double reprojError = 5., int flagsPnP = 0, int pnpRefineIterations = 1, + int varianceMedianRatio = 4, float maxVariance = 0, const Transform & guess = Transform::getIdentity(), const std::map & words3B = std::map(), cv::Mat * covariance = 0, // mean reproj error if words3B is not set std::vector * matchesOut = 0, - std::vector * inliersOut = 0); + std::vector * inliersOut = 0, + bool splitLinearCovarianceComponents = false); Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D( const std::map & words3A, const std::map & words2B, const std::vector & cameraModels, + unsigned int samplingPolicy = 0, // 0=AUTO, 1=ANY, 2=HOMOGENEOUS int minInliers = 10, int iterations = 100, double reprojError = 5., int flagsPnP = 0, int pnpRefineIterations = 1, + int varianceMedianRatio = 4, float maxVariance = 0, const Transform & guess = Transform::getIdentity(), const std::map & words3B = std::map(), cv::Mat * covariance = 0, // mean reproj error if words3B is not set std::vector * matchesOut = 0, - std::vector * inliersOut = 0); + std::vector * inliersOut = 0, + bool splitLinearCovarianceComponents = false); Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D( const std::map & words3A, diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index fa74801d58..639c8e890e 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -65,26 +65,30 @@ void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( double maxCorrespondenceDistance, double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference double & variance, - int & correspondencesOut); + int & correspondencesOut, + bool reciprocal); void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, // <=0 means that we don't care about normal angle difference double & variance, - int & correspondencesOut); + int & correspondencesOut, + bool reciprocal); void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut); + int & correspondencesOut, + bool reciprocal); void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudA, const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut); + int & correspondencesOut, + bool reciprocal); Transform RTABMAP_CORE_EXPORT icp( const pcl::PointCloud::ConstPtr & cloud_source, diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 7e6cd99687..fd66c948a3 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -13,8 +13,10 @@ SET(SRC_FILES Recovery.cpp + SensorCapture.cpp + SensorCaptureThread.cpp + Camera.cpp - CameraThread.cpp CameraModel.cpp camera/CameraFreenect.cpp @@ -39,6 +41,8 @@ SET(SRC_FILES camera/CameraMyntEye.cpp camera/CameraDepthAI.cpp + lidar/LidarVLP16.cpp + EpipolarGeometry.cpp VisualWord.cpp VWDictionary.cpp @@ -87,7 +91,8 @@ SET(SRC_FILES odometry/OdometryViso2.cpp odometry/OdometryDVO.cpp odometry/OdometryOkvis.cpp - odometry/OdometryORBSLAM.cpp + odometry/OdometryORBSLAM2.cpp + odometry/OdometryORBSLAM3.cpp odometry/OdometryLOAM.cpp odometry/OdometryFLOAM.cpp odometry/OdometryMSCKF.cpp @@ -106,10 +111,16 @@ SET(SRC_FILES stereo/StereoBM.cpp stereo/StereoSGBM.cpp - OccupancyGrid.cpp + GlobalMap.cpp + LocalGridMaker.cpp + LocalGrid.cpp + global_map/OccupancyGrid.cpp + global_map/CloudMap.cpp MarkerDetector.cpp + GlobalDescriptorExtractor.cpp + GainCompensator.cpp rtflann/ext/lz4.c @@ -168,14 +179,14 @@ SET(PUBLIC_LIBRARIES ${PCL_LIBRARIES} ) -IF(Sqlite3_FOUND) +IF(SQLite3_FOUND) SET(INCLUDE_DIRS ${INCLUDE_DIRS} - ${Sqlite3_INCLUDE_DIRS} + ${SQLite3_INCLUDE_DIRS} ) SET(LIBRARIES ${LIBRARIES} - ${Sqlite3_LIBRARIES} + ${SQLite3_LIBRARIES} ) ELSE() SET(SRC_FILES @@ -205,16 +216,21 @@ IF(TORCH_FOUND) ENDIF(TORCH_FOUND) IF(WITH_PYTHON AND Python3_FOUND) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} Python3::Python Python3::NumPy ) + SET(LIBRARIES + ${LIBRARIES} + pybind11::embed + ) SET(SRC_FILES ${SRC_FILES} python/PythonInterface.cpp python/PyMatcher.cpp python/PyDetector.cpp + python/PyDescriptor.cpp ) SET(INCLUDE_DIRS ${TORCH_INCLUDE_DIRS} @@ -585,10 +601,32 @@ IF(octomap_FOUND) ENDIF() SET(SRC_FILES ${SRC_FILES} - OctoMap.cpp + global_map/OctoMap.cpp ) ENDIF(octomap_FOUND) +IF(grid_map_core_FOUND) + IF(TARGET grid_map_core) + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} + grid_map_core + ) + ELSE() + SET(PUBLIC_INCLUDE_DIRS + ${PUBLIC_INCLUDE_DIRS} + ${grid_map_core_INCLUDE_DIRS} + ) + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} + ${grid_map_core_LIBRARIES} + ) + ENDIF() + SET(SRC_FILES + ${SRC_FILES} + global_map/GridMap.cpp + ) +ENDIF(grid_map_core_FOUND) + IF(AliceVision_FOUND) SET(LIBRARIES ${LIBRARIES} @@ -750,6 +788,7 @@ foreach(arg ${RESOURCES}) get_filename_component(filename ${arg} NAME) string(REPLACE "." "_" output ${filename}) set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h") + set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON) endforeach(arg ${RESOURCES}) #MESSAGE(STATUS "RESOURCES = ${RESOURCES}") diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index ef90651ad6..b9503c85ee 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -26,42 +26,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/Camera.h" +#include "rtabmap/core/IMUFilter.h" -#include -#include #include -#include #include #include -#include - -#include - -#include -#include +#include namespace rtabmap { Camera::Camera(float imageRate, const Transform & localTransform) : - _imageRate(imageRate), - _localTransform(localTransform*CameraModel::opticalRotation()), - _targetImageSize(0,0), - _frameRateTimer(new UTimer()), - _seq(0) -{ -} + SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()), + imuFilter_(0), + publishInterIMU_(false) +{} Camera::~Camera() { - UDEBUG(""); - delete _frameRateTimer; - UDEBUG(""); -} - -void Camera::resetTimer() -{ - _frameRateTimer->start(); + delete imuFilter_; } bool Camera::initFromFile(const std::string & calibrationPath) @@ -69,54 +52,32 @@ bool Camera::initFromFile(const std::string & calibrationPath) return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()); } -SensorData Camera::takeImage(CameraInfo * info) +void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter) { - bool warnFrameRateTooHigh = false; - float actualFrameRate = 0; - float imageRate = _imageRate; - if(imageRate>0) - { - int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); - if(sleepTime > 2) - { - uSleep(sleepTime-2); - } - else if(sleepTime < 0) - { - warnFrameRateTooHigh = true; - actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); - } - - // Add precision at the cost of a small overhead - while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001) - { - // - } - - double slept = _frameRateTimer->getElapsedTime(); - _frameRateTimer->start(); - UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate)); - } + publishInterIMU_ = enabled; + delete imuFilter_; + imuFilter_ = filter; +} - UTimer timer; - SensorData data = this->captureImage(info); - double captureTime = timer.ticks(); - if(warnFrameRateTooHigh) - { - UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.", - imageRate, actualFrameRate, captureTime); - } - else - { - UDEBUG("Time capturing image = %fs", captureTime); - } - if(info) +void Camera::postInterIMU(const IMU & imu, double stamp) +{ + if(imuFilter_) { - info->id = data.id(); - info->stamp = data.stamp(); - info->timeCapture = captureTime; + imuFilter_->update( + imu.angularVelocity()[0], imu.angularVelocity()[1], imu.angularVelocity()[2], + imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2], + stamp); + cv::Vec4d q; + imuFilter_->getOrientation(q[0],q[1],q[2],q[3]); + UEventsManager::post(new IMUEvent(IMU( + q, cv::Mat(), + imu.angularVelocity(), imu.angularVelocityCovariance(), + imu.linearAcceleration(), imu.linearAccelerationCovariance(), + imu.localTransform()), + stamp)); + return; } - return data; + UEventsManager::post(new IMUEvent(imu, stamp)); } } // namespace rtabmap diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp deleted file mode 100644 index 68abc3b498..0000000000 --- a/corelib/src/CameraThread.cpp +++ /dev/null @@ -1,678 +0,0 @@ -/* -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. -*/ - -#include "rtabmap/core/CameraThread.h" -#include "rtabmap/core/Camera.h" -#include "rtabmap/core/CameraEvent.h" -#include "rtabmap/core/CameraRGBD.h" -#include "rtabmap/core/util2d.h" -#include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_surface.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/StereoDense.h" -#include "rtabmap/core/DBReader.h" -#include "rtabmap/core/IMUFilter.h" -#include "rtabmap/core/clams/discrete_depth_distortion_model.h" -#include -#include -#include - -#include - -namespace rtabmap -{ - -// ownership transferred -CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) : - _camera(camera), - _odomSensor(0), - _odomAsGt(false), - _poseTimeOffset(0.0), - _poseScaleFactor(1.0f), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false) -{ - UASSERT(_camera != 0); -} - -// ownership transferred -CameraThread::CameraThread( - Camera * camera, - Camera * odomSensor, - const Transform & extrinsics, - double poseTimeOffset, - float poseScaleFactor, - bool odomAsGt, - const ParametersMap & parameters) : - _camera(camera), - _odomSensor(odomSensor), - _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()), - _odomAsGt(odomAsGt), - _poseTimeOffset(poseTimeOffset), - _poseScaleFactor(poseScaleFactor), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false) -{ - UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull()); - UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); - UDEBUG("_poseTimeOffset =%f", _poseTimeOffset); - UDEBUG("_poseScaleFactor =%f", _poseScaleFactor); - UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false"); -} - -// ownership transferred -CameraThread::CameraThread( - Camera * camera, - bool odomAsGt, - const ParametersMap & parameters) : - _camera(camera), - _odomSensor(0), - _odomAsGt(odomAsGt), - _poseTimeOffset(0.0), - _poseScaleFactor(1.0f), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false) -{ - UASSERT(_camera != 0); - UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false"); -} - -CameraThread::~CameraThread() -{ - join(true); - delete _camera; - delete _odomSensor; - delete _distortionModel; - delete _stereoDense; - delete _imuFilter; -} - -void CameraThread::setImageRate(float imageRate) -{ - if(_camera) - { - _camera->setImageRate(imageRate); - } -} - -void CameraThread::setDistortionModel(const std::string & path) -{ - if(_distortionModel) - { - delete _distortionModel; - _distortionModel = 0; - } - if(!path.empty()) - { - _distortionModel = new clams::DiscreteDepthDistortionModel(); - _distortionModel->load(path); - if(!_distortionModel->isValid()) - { - UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str()); - delete _distortionModel; - _distortionModel = 0; - } - } -} - -void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR) -{ - UASSERT(sigmaS > 0.0f && sigmaR > 0.0f); - _bilateralFiltering = true; - _bilateralSigmaS = sigmaS; - _bilateralSigmaR = sigmaR; -} - -void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) -{ - delete _imuFilter; - _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters); - _imuBaseFrameConversion = baseFrameConversion; -} - -void CameraThread::disableIMUFiltering() -{ - delete _imuFilter; - _imuFilter = 0; -} - -void CameraThread::setScanParameters( - bool fromDepth, - int downsampleStep, - float rangeMin, - float rangeMax, - float voxelSize, - int normalsK, - int normalsRadius, - bool forceGroundNormalsUp) -{ - setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f); -} - -void CameraThread::setScanParameters( - bool fromDepth, - int downsampleStep, // decimation of the depth image in case the scan is from depth image - float rangeMin, - float rangeMax, - float voxelSize, - int normalsK, - int normalsRadius, - float groundNormalsUp) -{ - _scanFromDepth = fromDepth; - _scanDownsampleStep=downsampleStep; - _scanRangeMin = rangeMin; - _scanRangeMax = rangeMax; - _scanVoxelSize = voxelSize; - _scanNormalsK = normalsK; - _scanNormalsRadius = normalsRadius; - _scanForceGroundNormalsUp = groundNormalsUp; -} - -bool CameraThread::odomProvided() const -{ - return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided())); -} - -void CameraThread::mainLoopBegin() -{ - ULogger::registerCurrentThread("Camera"); - _camera->resetTimer(); -} - -void CameraThread::mainLoop() -{ - UTimer totalTime; - CameraInfo info; - SensorData data = _camera->takeImage(&info); - - if(_odomSensor) - { - Transform pose; - Transform poseToLeftCam; - cv::Mat covariance; - if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance)) - { - info.odomPose = pose; - info.odomCovariance = covariance; - if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f) - { - info.odomPose.x() *= _poseScaleFactor; - info.odomPose.y() *= _poseScaleFactor; - info.odomPose.z() *= _poseScaleFactor; - } - // Adjust local transform of the camera based on the pose frame - if(!data.cameraModels().empty()) - { - UASSERT(data.cameraModels().size()==1); - CameraModel model = data.cameraModels()[0]; - model.setLocalTransform(_extrinsicsOdomToCamera); - data.setCameraModel(model); - } - else if(!data.stereoCameraModels().empty()) - { - UASSERT(data.stereoCameraModels().size()==1); - StereoCameraModel model = data.stereoCameraModels()[0]; - model.setLocalTransform(_extrinsicsOdomToCamera); - data.setStereoCameraModel(model); - } - } - else - { - UWARN("Could not get pose at stamp %f", data.stamp()); - } - } - - if(_odomAsGt && !info.odomPose.isNull()) - { - data.setGroundTruth(info.odomPose); - info.odomPose.setNull(); - } - - if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set - { - postUpdate(&data, &info); - info.cameraName = _camera->getSerial(); - info.timeTotal = totalTime.ticks(); - this->post(new CameraEvent(data, info)); - } - else if(!this->isKilled()) - { - UWARN("no more images..."); - this->kill(); - this->post(new CameraEvent()); - } -} - -void CameraThread::mainLoopKill() -{ - if(dynamic_cast(_camera) != 0) - { - int i=20; - while(i-->0) - { - uSleep(100); - if(!this->isKilled()) - { - break; - } - } - if(this->isKilled()) - { - //still in killed state, maybe a deadlock - UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked " - "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. " - "Note that rtabmap should link on libusb of libfreenect2. " - "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\""); - } - - } -} - -void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const -{ - UASSERT(dataPtr!=0); - SensorData & data = *dataPtr; - if(_colorOnly) - { - if(!data.depthRaw().empty()) - { - data.setRGBDImage(data.imageRaw(), cv::Mat(), data.cameraModels()); - } - else if(!data.rightRaw().empty()) - { - std::vector models; - for(size_t i=0; igetWidth() >= data.depthRaw().cols && - _distortionModel->getHeight() >= data.depthRaw().rows && - _distortionModel->getWidth() % data.depthRaw().cols == 0 && - _distortionModel->getHeight() % data.depthRaw().rows == 0) - { - cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures. - _distortionModel->undistort(depth); - data.setRGBDImage(data.imageRaw(), depth, data.cameraModels()); - } - else - { - UERROR("Distortion model size is %dx%d but depth image is %dx%d!", - _distortionModel->getWidth(), _distortionModel->getHeight(), - data.depthRaw().cols, data.depthRaw().rows); - } - if(info) info->timeUndistortDepth = timer.ticks(); - } - - if(_bilateralFiltering && !data.depthRaw().empty()) - { - UTimer timer; - data.setRGBDImage(data.imageRaw(), util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR), data.cameraModels()); - if(info) info->timeBilateralFiltering = timer.ticks(); - } - - if(_imageDecimation>1 && !data.imageRaw().empty()) - { - UDEBUG(""); - UTimer timer; - if(!data.depthRaw().empty() && - !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0)) - { - UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! " - "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows); - } - else - { - cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation); - - int depthDecimation = _imageDecimation; - if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols) - { - depthDecimation = 1; - } - else - { - depthDecimation = 2; - while(data.depthOrRightRaw().rows / depthDecimation > image.rows || - data.depthOrRightRaw().cols / depthDecimation > image.cols || - data.depthOrRightRaw().rows % depthDecimation != 0 || - data.depthOrRightRaw().cols % depthDecimation != 0) - { - ++depthDecimation; - } - UDEBUG("depthDecimation=%d", depthDecimation); - } - cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation); - - std::vector models = data.cameraModels(); - for(unsigned int i=0; i stereoModels = data.stereoCameraModels(); - for(unsigned int i=0; itimeImageDecimation = timer.ticks(); - } - if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1) - { - if(data.cameraModels().size() == 1) - { - UDEBUG(""); - UTimer timer; - cv::Mat tmpRgb; - cv::flip(data.imageRaw(), tmpRgb, 1); - - CameraModel tmpModel = data.cameraModels()[0]; - if(data.cameraModels()[0].cx()) - { - tmpModel = CameraModel( - data.cameraModels()[0].fx(), - data.cameraModels()[0].fy(), - float(data.imageRaw().cols) - data.cameraModels()[0].cx(), - data.cameraModels()[0].cy(), - data.cameraModels()[0].localTransform(), - data.cameraModels()[0].Tx(), - data.cameraModels()[0].imageSize()); - } - cv::Mat tmpDepth = data.depthOrRightRaw(); - if(!data.depthRaw().empty()) - { - cv::flip(data.depthRaw(), tmpDepth, 1); - } - data.setRGBDImage(tmpRgb, tmpDepth, tmpModel); - if(info) info->timeMirroring = timer.ticks(); - } - else - { - UWARN("Mirroring is not implemented for multiple cameras or stereo..."); - } - } - - if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty()) - { - if(data.stereoCameraModels().size()==1) - { -#if CV_MAJOR_VERSION < 3 - UWARN("Stereo exposure compensation not implemented for OpenCV version under 3."); -#else - UDEBUG(""); - UTimer timer; - cv::Ptr compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN); - std::vector topLeftCorners(2, cv::Point(0,0)); - std::vector images; - std::vector masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255))); - images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ)); - images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ)); - compensator->feed(topLeftCorners, images, masks); - cv::Mat imgLeft = data.imageRaw().clone(); - compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]); - cv::Mat imgRight = data.rightRaw().clone(); - compensator->apply(1, cv::Point(0,0), imgRight, masks[1]); - data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]); - cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get(); - UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]); - if(info) info->timeStereoExposureCompensation = timer.ticks(); -#endif - } - else - { - UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras..."); - } - } - - if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty()) - { - if(data.stereoCameraModels().size()==1) - { - UDEBUG(""); - UTimer timer; - cv::Mat depth = util2d::depthFromDisparity( - _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()), - data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].baseline()); - // set Tx for stereo bundle adjustment (when used) - CameraModel model = CameraModel( - data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].left().fy(), - data.stereoCameraModels()[0].left().cx(), - data.stereoCameraModels()[0].left().cy(), - data.stereoCameraModels()[0].localTransform(), - -data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].left().imageSize()); - data.setRGBDImage(data.imageRaw(), depth, model); - if(info) info->timeDisparity = timer.ticks(); - } - else - { - UWARN("Stereo to depth is not implemented for multiple stereo cameras..."); - } - } - if(_scanFromDepth && - data.cameraModels().size() && - data.cameraModels().at(0).isValidForProjection() && - !data.depthRaw().empty()) - { - UDEBUG(""); - if(data.laserScanRaw().isEmpty()) - { - UASSERT(_scanDownsampleStep >= 1); - UTimer timer; - pcl::IndicesPtr validIndices(new std::vector); - pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( - data, - _scanDownsampleStep, - _scanRangeMax, - _scanRangeMin, - validIndices.get()); - float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep); - LaserScan scan; - const Transform & baseToScan = data.cameraModels()[0].localTransform(); - if(validIndices->size()) - { - if(_scanVoxelSize>0.0f) - { - cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize); - float ratio = float(cloud->size()) / float(validIndices->size()); - maxPoints = ratio * maxPoints; - } - else if(!cloud->is_dense) - { - pcl::PointCloud::Ptr denseCloud(new pcl::PointCloud); - pcl::copyPointCloud(*cloud, *validIndices, *denseCloud); - cloud = denseCloud; - } - - if(cloud->size()) - { - if(_scanNormalsK>0 || _scanNormalsRadius>0.0f) - { - Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint); - pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); - pcl::concatenateFields(*cloud, *normals, *cloudNormals); - scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); - } - else - { - scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse()); - } - } - } - data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, baseToScan)); - if(info) info->timeScanFromDepth = timer.ticks(); - } - else - { - UWARN("Option to create laser scan from depth image is enabled, but " - "there is already a laser scan in the captured sensor data. Scan from " - "depth will not be created."); - } - } - else if(!data.laserScanRaw().isEmpty()) - { - UDEBUG(""); - // filter the scan after registration - data.setLaserScan(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp)); - } - - // IMU filtering - if(_imuFilter && !data.imu().empty()) - { - if(data.imu().angularVelocity()[0] == 0 && - data.imu().angularVelocity()[1] == 0 && - data.imu().angularVelocity()[2] == 0 && - data.imu().linearAcceleration()[0] == 0 && - data.imu().linearAcceleration()[1] == 0 && - data.imu().linearAcceleration()[2] == 0) - { - UWARN("IMU's acc and gyr values are null! Please disable IMU filtering."); - } - else - { - // Transform IMU data in base_link to correctly initialize yaw - IMU imu = data.imu(); - if(_imuBaseFrameConversion) - { - UASSERT(!data.imu().localTransform().isNull()); - imu.convertToBaseFrame(); - - } - _imuFilter->update( - imu.angularVelocity()[0], - imu.angularVelocity()[1], - imu.angularVelocity()[2], - imu.linearAcceleration()[0], - imu.linearAcceleration()[1], - imu.linearAcceleration()[2], - data.stamp()); - double qx,qy,qz,qw; - _imuFilter->getOrientation(qx,qy,qz,qw); - - data.setIMU(IMU( - cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1), - imu.angularVelocity(), imu.angularVelocityCovariance(), - imu.linearAcceleration(), imu.linearAccelerationCovariance(), - imu.localTransform())); - - UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)", - data.imu().orientation()[0], - data.imu().orientation()[1], - data.imu().orientation()[2], - data.imu().orientation()[3], - data.imu().angularVelocity()[0], - data.imu().angularVelocity()[1], - data.imu().angularVelocity()[2], - data.imu().linearAcceleration()[0], - data.imu().linearAcceleration()[1], - data.imu().linearAcceleration()[2], - data.stamp()); - } - } -} - -} // namespace rtabmap diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 7138632cf1..4639bcba0f 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -502,6 +502,16 @@ void DBDriver::updateOccupancyGrid( _dbSafeAccessMutex.unlock(); } +void DBDriver::updateCalibration(int nodeId, const std::vector & models, const std::vector & stereoModels) +{ + _dbSafeAccessMutex.lock(); + this->updateCalibrationQuery( + nodeId, + models, + stereoModels); + _dbSafeAccessMutex.unlock(); +} + void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image) { _dbSafeAccessMutex.lock(); diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 453c52201e..909aeca565 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list & signatures) { _memoryUsedEstimate += (*i)->getMemoryUsed(); // raw data are not kept in database - _memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); - _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); + _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); stepNode(ppStmt, *i); } @@ -4615,6 +4615,39 @@ void DBDriverSqlite3::updateOccupancyGridQuery( } } +void DBDriverSqlite3::updateCalibrationQuery( + int nodeId, + const std::vector & models, + const std::vector & stereoModels) const +{ + UDEBUG(""); + if(_ppDb) + { + std::string type; + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + + // Create query + std::string query = queryStepCalibrationUpdate(); + rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // step calibration + stepCalibrationUpdate(ppStmt, + nodeId, + models, + stereoModels); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + UDEBUG("Time=%fs", timer.ticks()); + } +} + void DBDriverSqlite3::updateDepthImageQuery( int nodeId, const cv::Mat & image) const @@ -5771,6 +5804,131 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); } +std::string DBDriverSqlite3::queryStepCalibrationUpdate() const +{ + UASSERT(uStrNumCmp(_version, "0.10.0") >= 0); + return "UPDATE Data SET calibration=? WHERE id=?;"; +} +void DBDriverSqlite3::stepCalibrationUpdate( + sqlite3_stmt * ppStmt, + int nodeId, + const std::vector & models, + const std::vector & stereoModels) const +{ + if(!ppStmt) + { + UFATAL(""); + } + + int rc = SQLITE_OK; + int index = 1; + + // calibration + std::vector calibrationData; + std::vector calibration; + // multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(models.size() && models[0].isValidForProjection()) + { + if(uStrNumCmp(_version, "0.18.0") >= 0) + { + for(unsigned int i=0; i data = models[i].serialize(); + UASSERT(!data.empty()); + unsigned int oldSize = calibrationData.size(); + calibrationData.resize(calibrationData.size() + data.size()); + memcpy(calibrationData.data()+oldSize, data.data(), data.size()); + } + } + else if(uStrNumCmp(_version, "0.11.2") >= 0) + { + calibration.resize(models.size() * (6+Transform().size())); + for(unsigned int i=0; i= 0) + { + for(unsigned int i=0; i data = stereoModels[i].serialize(); + UASSERT(!data.empty()); + unsigned int oldSize = calibrationData.size(); + calibrationData.resize(calibrationData.size() + data.size()); + memcpy(calibrationData.data()+oldSize, data.data(), data.size()); + } + } + else + { + UASSERT_MSG(stereoModels.size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str()); + const Transform & localTransform = stereoModels[0].left().localTransform(); + calibration.resize(7+localTransform.size()); + calibration[0] = stereoModels[0].left().fx(); + calibration[1] = stereoModels[0].left().fy(); + calibration[2] = stereoModels[0].left().cx(); + calibration[3] = stereoModels[0].left().cy(); + calibration[4] = stereoModels[0].baseline(); + calibration[5] = stereoModels[0].left().imageWidth(); + calibration[6] = stereoModels[0].left().imageHeight(); + memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float)); + } + } + + if(calibrationData.size()) + { + rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC); + } + else if(calibration.size()) + { + rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC); + } + else + { + rc = sqlite3_bind_null(ppStmt, index++); + } + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + //id + rc = sqlite3_bind_int(ppStmt, index++, nodeId); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + //step + rc=sqlite3_step(ppStmt); + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + rc = sqlite3_reset(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); +} + std::string DBDriverSqlite3::queryStepDepthUpdate() const { if(uStrNumCmp(_version, "0.10.0") < 0) @@ -6570,7 +6728,7 @@ void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt, //data std::vector dataBytes = rtabmap::compressData(descriptor.data()); - if(infoBytes.empty()) + if(dataBytes.empty()) { rc = sqlite3_bind_null(ppStmt, index++); } diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 83b66c592a..6eb27f796b 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/DBReader.h" #include "rtabmap/core/DBDriver.h" @@ -34,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/core/util3d.h" @@ -268,7 +268,7 @@ std::string DBReader::getSerial() const return "DBReader"; } -SensorData DBReader::captureImage(CameraInfo * info) +SensorData DBReader::captureImage(SensorCaptureInfo * info) { SensorData data = this->getNextData(info); if(data.id()>0 && _stopId>0 && data.id() > _stopId) @@ -370,7 +370,7 @@ SensorData DBReader::captureImage(CameraInfo * info) return data; } -SensorData DBReader::getNextData(CameraInfo * info) +SensorData DBReader::getNextData(SensorCaptureInfo * info) { SensorData data; if(_dbDriver) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 88aa68c18e..add45c75cc 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -732,22 +732,22 @@ std::vector Feature2D::generateKeypoints(const cv::Mat & image, co for (int j = 0; j sub_keypoints; - sub_keypoints = this->generateKeypointsImpl(image, roi, mask); + std::vector subKeypoints; + subKeypoints = this->generateKeypointsImpl(image, roi, mask); if (this->getType() != Feature2D::Type::kFeaturePyDetector) { - limitKeypoints(sub_keypoints, maxFeatures); + limitKeypoints(subKeypoints, maxFeatures); } if(roi.x || roi.y) { // Adjust keypoint position to raw image - for(std::vector::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter) + for(std::vector::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter) { iter->pt.x += roi.x; iter->pt.y += roi.y; } } - keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() ); + keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() ); } } UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)", @@ -2122,6 +2122,24 @@ std::vector ORBOctree::generateKeypointsImpl(const cv::Mat & image (*_orb)(imgRoi, maskRoi, keypoints, descriptors_); + // OrbOctree ignores the mask, so we have to apply it manually here + if(!keypoints.empty() && !maskRoi.empty()) + { + std::vector validKeypoints; + validKeypoints.reserve(keypoints.size()); + cv::Mat validDescriptors; + for(size_t i=0; i(keypoints[i].pt.y+roi.y, keypoints[i].pt.x+roi.x) != 0) + { + validKeypoints.push_back(keypoints[i]); + validDescriptors.push_back(descriptors_.row(i)); + } + } + keypoints = validKeypoints; + descriptors_ = validDescriptors; + } + if((int)keypoints.size() > this->getMaxFeatures()) { limitKeypoints(keypoints, descriptors_, this->getMaxFeatures()); diff --git a/corelib/src/GlobalDescriptorExtractor.cpp b/corelib/src/GlobalDescriptorExtractor.cpp new file mode 100644 index 0000000000..00fdfc13dd --- /dev/null +++ b/corelib/src/GlobalDescriptorExtractor.cpp @@ -0,0 +1,76 @@ +/* +Copyright (c) 2010-2024, 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. +*/ + + +#include "rtabmap/core/GlobalDescriptorExtractor.h" + +#ifdef RTABMAP_PYTHON +#include "python/PyDescriptor.h" +#endif + +namespace rtabmap { + +GlobalDescriptorExtractor::GlobalDescriptorExtractor(const ParametersMap & parameters) +{ +} +GlobalDescriptorExtractor::~GlobalDescriptorExtractor() +{ +} +GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMap & parameters) +{ + int type = Parameters::defaultMemGlobalDescriptorStrategy(); + Parameters::parse(parameters, Parameters::kMemGlobalDescriptorStrategy(), type); + return create((GlobalDescriptorExtractor::Type)type, parameters); +} +GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters) +{ + UDEBUG("Creating global descriptor of type %d", (int)type); +#ifndef RTABMAP_PYTHON + if(type == GlobalDescriptorExtractor::kPyDescriptor) + { + UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support."); + type = GlobalDescriptorExtractor::kUndef; + } +#endif + + GlobalDescriptorExtractor * GlobalDescriptorExtractor = 0; + switch(type) + { +#ifdef RTABMAP_PYTHON + case GlobalDescriptorExtractor::kPyDescriptor: + GlobalDescriptorExtractor = new PyDescriptor(parameters); + break; +#endif + default: + type = GlobalDescriptorExtractor::kUndef; + break; + } + return GlobalDescriptorExtractor; +} + +} + diff --git a/corelib/src/GlobalMap.cpp b/corelib/src/GlobalMap.cpp new file mode 100644 index 0000000000..256aaeda13 --- /dev/null +++ b/corelib/src/GlobalMap.cpp @@ -0,0 +1,169 @@ +/* +Copyright (c) 2010-2023, 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. +*/ + +#include +#include +#include + +namespace rtabmap { + +GlobalMap::GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters) : + cellSize_(Parameters::defaultGridCellSize()), + updateError_(Parameters::defaultGridGlobalUpdateError()), + occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()), + logOddsHit_(logodds(Parameters::defaultGridGlobalProbHit())), + logOddsMiss_(logodds(Parameters::defaultGridGlobalProbMiss())), + logOddsClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())), + logOddsClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())), + cache_(cache) +{ + UASSERT(cache_); + + minValues_[0] = minValues_[1] = minValues_[2] = 0.0; + maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; + + Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_); + UASSERT(cellSize_>0.0f); + + Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_); + + UDEBUG("cellSize_ =%f", cellSize_); + UDEBUG("updateError_ =%f", updateError_); + + // Probabilistic parameters + Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_); + if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), logOddsHit_)) + { + logOddsHit_ = logodds(logOddsHit_); + UASSERT_MSG(logOddsHit_ >= 0.0f, uFormat("probHit_=%f",logOddsHit_).c_str()); + } + if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), logOddsMiss_)) + { + logOddsMiss_ = logodds(logOddsMiss_); + UASSERT_MSG(logOddsMiss_ <= 0.0f, uFormat("probMiss_=%f",logOddsMiss_).c_str()); + } + if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), logOddsClampingMin_)) + { + logOddsClampingMin_ = logodds(logOddsClampingMin_); + } + if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), logOddsClampingMax_)) + { + logOddsClampingMax_ = logodds(logOddsClampingMax_); + } + UASSERT(logOddsClampingMax_ > logOddsClampingMin_); +} + +GlobalMap::~GlobalMap() +{ + clear(); +} + +void GlobalMap::clear() +{ + UDEBUG("Clearing"); + addedNodes_.clear(); + minValues_[0] = minValues_[1] = minValues_[2] = 0.0; + maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; +} + +unsigned long GlobalMap::getMemoryUsed() const +{ + unsigned long memoryUsage = 0; + + memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map::iterator)) + sizeof(std::map); + + return memoryUsage; +} + +bool GlobalMap::update(const std::map & poses) +{ + UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); + + // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes. + bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified) + bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map + float updateErrorSqrd = updateError_*updateError_; + for(std::map::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) + { + std::map::const_iterator jter = poses.find(iter->first); + if(jter != poses.end()) + { + graphChanged = false; + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd) + { + graphOptimized = true; + } + } + else + { + UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first); + } + } + + if(graphOptimized || graphChanged) + { + // clear all but keep cache + clear(); + } + + std::list > orderedPoses; + + // add old poses that were not in the current map (they were just retrieved from LTM) + for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) + { + if(!isNodeAssembled(iter->first)) + { + UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first); + orderedPoses.push_back(*iter); + } + } + + // insert zero after + if(poses.find(0) != poses.end()) + { + orderedPoses.push_back(std::make_pair(-1, poses.at(0))); + } + + if(!orderedPoses.empty()) + { + assemble(orderedPoses); + } + + return !orderedPoses.empty(); +} + +void GlobalMap::addAssembledNode(int id, const Transform & pose) +{ + if(id > 0) + { + uInsert(addedNodes_, std::make_pair(id, pose)); + } +} + + +} // namespace rtabmap diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index ebf92f28aa..da85efcc0c 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -430,7 +430,7 @@ bool importPoses( else if(format == 1 || format==10 || format==11) // rgbd-slam format { std::list strList = uSplit(str); - if((strList.size() == 8 && format!=11) || (strList.size() == 9 && format==11)) + if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11)) { double stamp = uStr2Double(strList.front()); strList.pop_front(); @@ -902,7 +902,7 @@ void computeMaxGraphErrors( float & maxAngularError, const Link ** maxLinearErrorLink, const Link ** maxAngularErrorLink, - bool for3DoF) + bool force3DoF) { maxLinearErrorRatio = -1; maxAngularErrorRatio = -1; @@ -912,17 +912,44 @@ void computeMaxGraphErrors( UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size()); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - // ignore links with high variance, priors and landmarks - if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark) + // ignore priors + if(iter->second.from() != iter->second.to()) { Transform t1 = uValue(poses, iter->second.from(), Transform()); Transform t2 = uValue(poses, iter->second.to(), Transform()); + + if( t1.isNull() || + t2.isNull() || + !t1.isInvertible() || + !t2.isInvertible()) + { + UWARN("Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)", + iter->second.from(), + t1.prettyPrint().c_str(), + iter->second.to(), + t2.prettyPrint().c_str()); + + if(maxLinearErrorLink) + { + *maxLinearErrorLink = 0; + } + if(maxAngularErrorLink) + { + *maxAngularErrorLink = 0; + } + maxLinearErrorRatio = -1; + maxAngularErrorRatio = -1; + maxLinearError = -1; + maxAngularError = -1; + return; + } + Transform t = t1.inverse()*t2; float linearError = uMax3( fabs(iter->second.transform().x() - t.x()), fabs(iter->second.transform().y() - t.y()), - for3DoF?0:fabs(iter->second.transform().z() - t.z())); + force3DoF?0:fabs(iter->second.transform().z() - t.z())); UASSERT(iter->second.transVariance(false)>0.0); float stddevLinear = sqrt(iter->second.transVariance(false)); float linearErrorRatio = linearError/stddevLinear; @@ -936,25 +963,30 @@ void computeMaxGraphErrors( } } - float opt_roll,opt_pitch,opt_yaw; - float link_roll,link_pitch,link_yaw; - t.getEulerAngles(opt_roll, opt_pitch, opt_yaw); - iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); - float angularError = uMax3( - for3DoF?0:fabs(opt_roll - link_roll), - for3DoF?0:fabs(opt_pitch - link_pitch), - fabs(opt_yaw - link_yaw)); - angularError = angularError>M_PI?2*M_PI-angularError:angularError; - UASSERT(iter->second.rotVariance(false)>0.0); - float stddevAngular = sqrt(iter->second.rotVariance(false)); - float angularErrorRatio = angularError/stddevAngular; - if(angularErrorRatio > maxAngularErrorRatio) + // For landmark links, don't compute angular error if it doesn't estimate orientation + if(iter->second.type() != Link::kLandmark || + 1.0 / static_cast(iter->second.infMatrix().at(5,5)) < 9999.0) { - maxAngularError = angularError; - maxAngularErrorRatio = angularErrorRatio; - if(maxAngularErrorLink) + float opt_roll,opt_pitch,opt_yaw; + float link_roll,link_pitch,link_yaw; + t.getEulerAngles(opt_roll, opt_pitch, opt_yaw); + iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); + float angularError = uMax3( + force3DoF?0:fabs(opt_roll - link_roll), + force3DoF?0:fabs(opt_pitch - link_pitch), + fabs(opt_yaw - link_yaw)); + angularError = angularError>M_PI?2*M_PI-angularError:angularError; + UASSERT(iter->second.rotVariance(false)>0.0); + float stddevAngular = sqrt(iter->second.rotVariance(false)); + float angularErrorRatio = angularError/stddevAngular; + if(angularErrorRatio > maxAngularErrorRatio) { - *maxAngularErrorLink = &iter->second; + maxAngularError = angularError; + maxAngularErrorRatio = angularErrorRatio; + if(maxAngularErrorLink) + { + *maxAngularErrorLink = &iter->second; + } } } } @@ -1022,6 +1054,39 @@ std::multimap::iterator findLink( return links.end(); } +std::multimap >::iterator findLink( + std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + + if(checkBothWays) + { + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + } + return links.end(); +} + std::multimap::iterator findLink( std::multimap & links, int from, @@ -1086,6 +1151,39 @@ std::multimap::const_iterator findLink( return links.end(); } +std::multimap >::const_iterator findLink( + const std::multimap > & links, + int from, + int to, + bool checkBothWays, + Link::Type type) +{ + std::multimap >::const_iterator iter = links.find(from); + while(iter != links.end() && iter->first == from) + { + if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + + if(checkBothWays) + { + // let's try to -> from + iter = links.find(to); + while(iter != links.end() && iter->first == to) + { + if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second)) + { + return iter; + } + ++iter; + } + } + return links.end(); +} + std::multimap::const_iterator findLink( const std::multimap & links, int from, @@ -2218,7 +2316,7 @@ std::map findNearestPoses( { foundPoses.insert(*poses.find(iter->first)); } - UDEBUG("found nodes=%d", (int)foundPoses.size()); + UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k); return foundPoses; } diff --git a/corelib/src/IMUThread.cpp b/corelib/src/IMUThread.cpp index 2397390cbf..58072ed83c 100644 --- a/corelib/src/IMUThread.cpp +++ b/corelib/src/IMUThread.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/IMUThread.h" #include "rtabmap/core/IMU.h" +#include "rtabmap/core/IMUFilter.h" #include #include #include @@ -38,13 +39,16 @@ IMUThread::IMUThread(int rate, const Transform & localTransform) : rate_(rate), localTransform_(localTransform), captureDelay_(0.0), - previousStamp_(0.0) + previousStamp_(0.0), + _imuFilter(0), + _imuBaseFrameConversion(false) { } IMUThread::~IMUThread() { imuFile_.close(); + delete _imuFilter; } bool IMUThread::init(const std::string & path) @@ -81,6 +85,19 @@ void IMUThread::setRate(int rate) rate_ = rate; } +void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) +{ + delete _imuFilter; + _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters); + _imuBaseFrameConversion = baseFrameConversion; +} + +void IMUThread::disableIMUFiltering() +{ + delete _imuFilter; + _imuFilter = 0; +} + void IMUThread::mainLoopBegin() { ULogger::registerCurrentThread("IMU"); @@ -141,6 +158,60 @@ void IMUThread::mainLoop() previousStamp_ = stamp; IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_); + + // IMU filtering + if(_imuFilter && !imu.empty()) + { + if(imu.angularVelocity()[0] == 0 && + imu.angularVelocity()[1] == 0 && + imu.angularVelocity()[2] == 0 && + imu.linearAcceleration()[0] == 0 && + imu.linearAcceleration()[1] == 0 && + imu.linearAcceleration()[2] == 0) + { + UWARN("IMU's acc and gyr values are null! Please disable IMU filtering."); + } + else + { + // Transform IMU data in base_link to correctly initialize yaw + if(_imuBaseFrameConversion) + { + UASSERT(!imu.localTransform().isNull()); + imu.convertToBaseFrame(); + + } + _imuFilter->update( + imu.angularVelocity()[0], + imu.angularVelocity()[1], + imu.angularVelocity()[2], + imu.linearAcceleration()[0], + imu.linearAcceleration()[1], + imu.linearAcceleration()[2], + stamp); + double qx,qy,qz,qw; + _imuFilter->getOrientation(qx,qy,qz,qw); + + imu = IMU( + cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1), + imu.angularVelocity(), imu.angularVelocityCovariance(), + imu.linearAcceleration(), imu.linearAccelerationCovariance(), + imu.localTransform()); + + UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)", + imu.orientation()[0], + imu.orientation()[1], + imu.orientation()[2], + imu.orientation()[3], + imu.angularVelocity()[0], + imu.angularVelocity()[1], + imu.angularVelocity()[2], + imu.linearAcceleration()[0], + imu.linearAcceleration()[1], + imu.linearAcceleration()[2], + stamp); + } + } + this->post(new IMUEvent(imu, stamp)); } else if(!this->isKilled()) diff --git a/corelib/src/LaserScan.cpp b/corelib/src/LaserScan.cpp index eb7a2cb77f..7e875a9834 100644 --- a/corelib/src/LaserScan.cpp +++ b/corelib/src/LaserScan.cpp @@ -65,6 +65,9 @@ std::string LaserScan::formatName(const Format & format) case kXYZRGBNormal: name = "XYZRGBNormal"; break; + case kXYZIT: + name = "XYZIT"; + break; default: name = "Unknown"; break; @@ -88,6 +91,7 @@ int LaserScan::channels(const Format & format) channels = 4; break; case kXYNormal: + case kXYZIT: channels = 5; break; case kXYZNormal: @@ -119,7 +123,11 @@ bool LaserScan::isScanHasRGB(const Format & format) } bool LaserScan::isScanHasIntensity(const Format & format) { - return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal; + return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT; +} +bool LaserScan::isScanHasTime(const Format & format) +{ + return format==kXYZIT; } LaserScan LaserScan::backwardCompatibility( @@ -213,7 +221,14 @@ LaserScan::LaserScan( const LaserScan & scan, int maxPoints, float maxRange, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { UASSERT(scan.empty() || scan.format() != kUnknown); init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform); @@ -224,7 +239,14 @@ LaserScan::LaserScan( int maxPoints, float maxRange, Format format, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform); } @@ -234,7 +256,14 @@ LaserScan::LaserScan( int maxPoints, float maxRange, Format format, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform); } @@ -246,7 +275,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { UASSERT(scan.empty() || scan.format() != kUnknown); init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); @@ -260,7 +296,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); } @@ -273,7 +316,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); } @@ -289,8 +339,7 @@ void LaserScan::init( int maxPoints, const Transform & localTransform) { - UASSERT(data.empty() || data.rows == 1); - UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7)); + UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7)); UASSERT(!localTransform.isNull()); bool is2D = false; @@ -307,6 +356,10 @@ void LaserScan::init( // 3D scan UASSERT(rangeMax>=rangeMin); maxPoints_ = maxPoints; + if(maxPoints_ == 0 && data.rows>1) + { + maxPoints_ = data.rows * data.cols; + } } data_ = data; @@ -320,18 +373,18 @@ void LaserScan::init( if(!data.empty() && !isCompressed()) { - if(is2D && data_.cols > maxPoints_) + if(is2D && (int)data_.total() > maxPoints_) { - UWARN("The number of points (%d) in the scan is over the maximum " + UWARN("The number of points (%ld) in the scan is over the maximum " "points (%d) defined by angle settings (min=%f max=%f inc=%f). " "The scan info may be wrong!", - data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_); + data_.total(), maxPoints_, angleMin_, angleMax_, angleIncrement_); } - else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_) + else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_) { - UDEBUG("The number of points (%d) in the scan is over the maximum " + UDEBUG("The number of points (%ld) in the scan is over the maximum " "points (%d) defined by max points setting.", - data_.cols, maxPoints_); + data_.total(), maxPoints_); } if(format == kUnknown) @@ -350,7 +403,7 @@ void LaserScan::init( UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); - UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); + UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); } @@ -366,11 +419,36 @@ LaserScan LaserScan::clone() const return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone()); } +LaserScan LaserScan::densify() const +{ + if(!isOrganized()) + { + return *this; + } + cv::Mat output(1, data_.total(), data_.type()); + int oi = 0; + for(int i=0; i(i, j); + float * outputPtr = output.ptr(0, oi); + if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2])))) + { + memcpy(outputPtr, ptr, data_.elemSize()); + ++oi; + } + } + } + return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone()); +} + float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset) { - UASSERT(pointIndex < (unsigned int)data_.cols); + UASSERT(pointIndex < (unsigned int)data_.total()); UASSERT(channelOffset < (unsigned int)data_.channels()); - return data_.ptr(0, pointIndex)[channelOffset]; + unsigned int row = pointIndex / data_.cols; + return data_.ptr(row, pointIndex - row * data_.cols)[channelOffset]; } LaserScan & LaserScan::operator+=(const LaserScan & scan) @@ -381,7 +459,7 @@ LaserScan & LaserScan::operator+=(const LaserScan & scan) LaserScan LaserScan::operator+(const LaserScan & scan) { - UASSERT(this->empty() || scan.empty() || this->format() == scan.format()); + UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized())); LaserScan dest; if(!scan.empty()) { diff --git a/corelib/src/Link.cpp b/corelib/src/Link.cpp index 4b776eb73d..ad15e03fa0 100644 --- a/corelib/src/Link.cpp +++ b/corelib/src/Link.cpp @@ -163,7 +163,7 @@ cv::Mat Link::uncompressUserDataConst() const Link Link::merge(const Link & link, Type outputType) const { - UASSERT(to_ == link.from()); + UASSERT_MSG(to_ == link.from(), uFormat("merging this=%d->%d to link=%d->%d", from_, to_, link.from(), link.to()).c_str()); UASSERT(outputType != Link::kUndef); UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull())); UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); diff --git a/corelib/src/LocalGrid.cpp b/corelib/src/LocalGrid.cpp new file mode 100644 index 0000000000..77e98781a6 --- /dev/null +++ b/corelib/src/LocalGrid.cpp @@ -0,0 +1,126 @@ +/* +Copyright (c) 2010-2023, 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. +*/ + +#include +#include +#include + +namespace rtabmap { + +LocalGrid::LocalGrid(const cv::Mat & groundIn, + const cv::Mat & obstaclesIn, + const cv::Mat & emptyIn, + float cellSizeIn, + const cv::Point3f & viewPointIn) : + groundCells(groundIn), + obstacleCells(obstaclesIn), + emptyCells(emptyIn), + cellSize(cellSizeIn), + viewPoint(viewPointIn) +{ + UASSERT(cellSize > 0.0f); +} + +bool LocalGrid::is3D() const +{ + return (groundCells.empty() || groundCells.type() == CV_32FC3 || groundCells.type() == CV_32FC(4) || groundCells.type() == CV_32FC(6)) && + (obstacleCells.empty() || obstacleCells.type() == CV_32FC3 || obstacleCells.type() == CV_32FC(4) || obstacleCells.type() == CV_32FC(6)) && + (emptyCells.empty() || emptyCells.type() == CV_32FC3 || emptyCells.type() == CV_32FC(4) || emptyCells.type() == CV_32FC(6)); +} + +void LocalGridCache::add(int nodeId, + const cv::Mat & ground, + const cv::Mat & obstacles, + const cv::Mat & empty, + float cellSize, + const cv::Point3f & viewPoint) +{ + add(nodeId, LocalGrid(ground, obstacles, empty, cellSize, viewPoint)); +} + +void LocalGridCache::add(int nodeId, const LocalGrid & localGrid) +{ + UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)", + nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels()); + if(nodeId < 0) + { + UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId); + return; + } + uInsert(localGrids_, std::make_pair(nodeId==0?-1:nodeId, localGrid)); +} + +bool LocalGridCache::shareTo(int nodeId, LocalGridCache & anotherCache) const +{ + if(uContains(localGrids_, nodeId) && !uContains(anotherCache.localGrids(), nodeId)) + { + const LocalGrid & localGrid = localGrids_.at(nodeId); + anotherCache.add(nodeId, localGrid.groundCells, localGrid.obstacleCells, localGrid.emptyCells, localGrid.cellSize, localGrid.viewPoint); + return true; + } + return false; +} + +unsigned long LocalGridCache::getMemoryUsed() const +{ + unsigned long memoryUsage = 0; + memoryUsage += localGrids_.size()*(sizeof(int) + sizeof(LocalGrid) + sizeof(std::map::iterator)) + sizeof(std::map); + for(std::map::const_iterator iter=localGrids_.begin(); iter!=localGrids_.end(); ++iter) + { + memoryUsage += iter->second.groundCells.total() * iter->second.groundCells.elemSize(); + memoryUsage += iter->second.obstacleCells.total() * iter->second.obstacleCells.elemSize(); + memoryUsage += iter->second.emptyCells.total() * iter->second.emptyCells.elemSize(); + memoryUsage += sizeof(int); + memoryUsage += sizeof(cv::Point3f); + } + return memoryUsage; +} + +void LocalGridCache::clear(bool temporaryOnly) +{ + if(temporaryOnly) + { + //clear only negative ids + for(std::map::iterator iter=localGrids_.begin(); iter!=localGrids_.end();) + { + if(iter->first < 0) + { + localGrids_.erase(iter++); + } + else + { + break; + } + } + } + else + { + localGrids_.clear(); + } +} + +} // namespace rtabmap diff --git a/corelib/src/LocalGridMaker.cpp b/corelib/src/LocalGridMaker.cpp new file mode 100644 index 0000000000..14525bb5f2 --- /dev/null +++ b/corelib/src/LocalGridMaker.cpp @@ -0,0 +1,587 @@ +/* +Copyright (c) 2010-2023, 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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef RTABMAP_OCTOMAP +#include +#endif + +#include + +namespace rtabmap { + +LocalGridMaker::LocalGridMaker(const ParametersMap & parameters) : + parameters_(parameters), + cloudDecimation_(Parameters::defaultGridDepthDecimation()), + rangeMax_(Parameters::defaultGridRangeMax()), + rangeMin_(Parameters::defaultGridRangeMin()), + //roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters() + footprintLength_(Parameters::defaultGridFootprintLength()), + footprintWidth_(Parameters::defaultGridFootprintWidth()), + footprintHeight_(Parameters::defaultGridFootprintHeight()), + scanDecimation_(Parameters::defaultGridScanDecimation()), + cellSize_(Parameters::defaultGridCellSize()), + preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()), + occupancySensor_(Parameters::defaultGridSensor()), + projMapFrame_(Parameters::defaultGridMapFrameProjection()), + maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()), + normalKSearch_(Parameters::defaultGridNormalK()), + groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()), + maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f), + clusterRadius_(Parameters::defaultGridClusterRadius()), + minClusterSize_(Parameters::defaultGridMinClusterSize()), + flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()), + minGroundHeight_(Parameters::defaultGridMinGroundHeight()), + maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()), + normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()), + grid3D_(Parameters::defaultGrid3D()), + groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()), + noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()), + noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()), + scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()), + rayTracing_(Parameters::defaultGridRayTracing()) +{ + this->parseParameters(parameters); +} + +LocalGridMaker::~LocalGridMaker() +{ +} + +void LocalGridMaker::parseParameters(const ParametersMap & parameters) +{ + uInsert(parameters_, parameters); + + Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_); + Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_); + if(cloudDecimation_ == 0) + { + cloudDecimation_ = 1; + } + Parameters::parse(parameters, Parameters::kGridRangeMin(), rangeMin_); + Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_); + Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_); + Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_); + Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_); + Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_); + Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_); + UASSERT(cellSize_>0.0f); + + Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_); + Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_); + Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_); + Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_); + Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_); + Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_); + Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_); + if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_)) + { + maxGroundAngle_ *= M_PI/180.0f; + } + Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_); + UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str()); + Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_); + Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_); + Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_); + Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_); + Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_); + Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_); + Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_); + Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_); + Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_); + + // convert ROI from string to vector + ParametersMap::const_iterator iter; + if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end()) + { + std::list strValues = uSplit(iter->second, ' '); + if(strValues.size() != 4) + { + ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str()); + } + else + { + std::vector tmpValues(4); + unsigned int i=0; + for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) + { + tmpValues[i] = uStr2Float(*jter); + ++i; + } + + if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && + tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && + tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && + tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) + { + roiRatios_ = tmpValues; + } + else + { + ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str()); + } + } + } + + if(maxGroundHeight_ == 0.0f && !normalsSegmentation_) + { + UWARN("\"%s\" should be not equal to 0 if not using normals " + "segmentation approach. Setting it to cell size (%f).", + Parameters::kGridMaxGroundHeight().c_str(), cellSize_); + maxGroundHeight_ = cellSize_; + } + if(maxGroundHeight_ != 0.0f && + maxObstacleHeight_ != 0.0f && + maxObstacleHeight_ < maxGroundHeight_) + { + UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", + Parameters::kGridMaxGroundHeight().c_str(), + Parameters::kGridMaxObstacleHeight().c_str(), + Parameters::kGridMaxObstacleHeight().c_str()); + maxObstacleHeight_ = 0; + } + if(maxGroundHeight_ != 0.0f && + minGroundHeight_ != 0.0f && + maxGroundHeight_ < minGroundHeight_) + { + UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", + Parameters::kGridMinGroundHeight().c_str(), + Parameters::kGridMaxGroundHeight().c_str(), + Parameters::kGridMinGroundHeight().c_str()); + minGroundHeight_ = 0; + } +} + +void LocalGridMaker::createLocalMap( + const Signature & node, + cv::Mat & groundCells, + cv::Mat & obstacleCells, + cv::Mat & emptyCells, + cv::Point3f & viewPoint) +{ + UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d", + node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0); + + if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0) + { + UDEBUG("2D laser scan"); + //2D + viewPoint = cv::Point3f( + node.sensorData().laserScanRaw().localTransform().x(), + node.sensorData().laserScanRaw().localTransform().y(), + node.sensorData().laserScanRaw().localTransform().z()); + + LaserScan scan = node.sensorData().laserScanRaw(); + if(rangeMin_ > 0.0f) + { + scan = util3d::rangeFiltering(scan, rangeMin_, 0.0f); + } + + float maxRange = rangeMax_; + if(rangeMax_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f) + { + maxRange = rangeMax_ < node.sensorData().laserScanRaw().rangeMax()?rangeMax_:node.sensorData().laserScanRaw().rangeMax(); + } + else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f) + { + maxRange = node.sensorData().laserScanRaw().rangeMax(); + } + util3d::occupancy2DFromLaserScan( + util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(), + cv::Mat(), + viewPoint, + emptyCells, + obstacleCells, + cellSize_, + scan2dUnknownSpaceFilled_, + maxRange); + + UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels()); + } + else + { + // 3D + if(occupancySensor_ == 0 || occupancySensor_ == 2) + { + if(!node.sensorData().laserScanRaw().isEmpty()) + { + UDEBUG("3D laser scan"); + const Transform & t = node.sensorData().laserScanRaw().localTransform(); + LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_); +#ifdef RTABMAP_OCTOMAP + // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan() + float maxRange = rayTracing_?0.0f:rangeMax_; +#else + // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan() + float maxRange = !grid3D_ && rayTracing_?0.0f:rangeMax_; +#endif + if(rangeMin_ > 0.0f || maxRange > 0.0f) + { + scan = util3d::rangeFiltering(scan, rangeMin_, maxRange); + } + + // update viewpoint + viewPoint = cv::Point3f(t.x(), t.y(), t.z()); + + UDEBUG("scan format=%d", scan.format()); + + bool normalSegmentationTmp = normalsSegmentation_; + float minGroundHeightTmp = minGroundHeight_; + float maxGroundHeightTmp = maxGroundHeight_; + if(scan.is2d()) + { + // if 2D, assume the whole scan is obstacle + normalsSegmentation_ = false; + minGroundHeight_ = std::numeric_limits::min(); + maxGroundHeight_ = std::numeric_limits::min()+100; + } + + createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint); + + if(scan.is2d()) + { + // restore + normalsSegmentation_ = normalSegmentationTmp; + minGroundHeight_ = minGroundHeightTmp; + maxGroundHeight_ = maxGroundHeightTmp; + } + } + else + { + UWARN("Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.id(), Parameters::kGridSensor().c_str(), occupancySensor_); + } + } + + if(occupancySensor_ >= 1) + { + pcl::IndicesPtr indices(new std::vector); + pcl::PointCloud::Ptr cloud; + UDEBUG("Depth image : decimation=%d max=%f min=%f", + cloudDecimation_, + rangeMax_, + rangeMin_); + cloud = util3d::cloudRGBFromSensorData( + node.sensorData(), + cloudDecimation_, +#ifdef RTABMAP_OCTOMAP + // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan() + rayTracing_?0.0f:rangeMax_, +#else + // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan() + !grid3D_&&rayTracing_?0.0f:rangeMax_, +#endif + rangeMin_, + indices.get(), + parameters_, + roiRatios_); + + // update viewpoint + viewPoint = cv::Point3f(0,0,0); + if(node.sensorData().cameraModels().size()) + { + // average of all local transforms + float sum = 0; + for(unsigned int i=0; i 0.0f) + { + viewPoint.x /= sum; + viewPoint.y /= sum; + viewPoint.z /= sum; + } + } + else + { + // average of all local transforms + float sum = 0; + for(unsigned int i=0; i 0.0f) + { + viewPoint.x /= sum; + viewPoint.y /= sum; + viewPoint.z /= sum; + } + } + + cv::Mat scanGroundCells; + cv::Mat scanObstacleCells; + cv::Mat scanEmptyCells; + if(occupancySensor_ == 2) + { + // backup + scanGroundCells = groundCells; + scanObstacleCells = obstacleCells; + scanEmptyCells = emptyCells; + groundCells = cv::Mat(); + obstacleCells = cv::Mat(); + emptyCells = cv::Mat(); + } + + createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint); + + if(occupancySensor_ == 2) + { + if(grid3D_) + { + // We should convert scans to 4 channels (XYZRGB) to be compatible + scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data(); + scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data(); + scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data(); + } + + UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels()); + UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels()); + UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels()); + + if(!groundCells.empty() && !scanGroundCells.empty()) + cv::hconcat(groundCells, scanGroundCells, groundCells); + else if(!scanGroundCells.empty()) + groundCells = scanGroundCells; + + if(!obstacleCells.empty() && !scanObstacleCells.empty()) + cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells); + else if(!scanObstacleCells.empty()) + obstacleCells = scanObstacleCells; + + if(!emptyCells.empty() && !scanEmptyCells.empty()) + cv::hconcat(emptyCells, scanEmptyCells, emptyCells); + else if(!scanEmptyCells.empty()) + emptyCells = scanEmptyCells; + } + } + } +} + +void LocalGridMaker::createLocalMap( + const LaserScan & scan, + const Transform & pose, + cv::Mat & groundCells, + cv::Mat & obstacleCells, + cv::Mat & emptyCells, + cv::Point3f & viewPointInOut) const +{ + if(projMapFrame_) + { + //we should rotate viewPoint in /map frame + float roll, pitch, yaw; + pose.getEulerAngles(roll, pitch, yaw); + Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0); + viewPointInOut.x = viewpointRotated.x(); + viewPointInOut.y = viewpointRotated.y(); + viewPointInOut.z = viewpointRotated.z(); + } + + if(scan.size()) + { + pcl::IndicesPtr groundIndices(new std::vector); + pcl::IndicesPtr obstaclesIndices(new std::vector); + cv::Mat groundCloud; + cv::Mat obstaclesCloud; + + if(scan.hasRGB() && scan.hasNormals()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform()); + pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); + UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); + if(grid3D_) + { + groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); + obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); + } + else + { + util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); + } + } + else if(scan.hasRGB()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); + pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); + UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); + if(grid3D_) + { + groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); + obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); + } + else + { + util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); + } + } + else if(scan.hasNormals()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform()); + pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); + UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); + if(grid3D_) + { + groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); + obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); + } + else + { + util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); + } + } + else + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform()); + pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); + UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); + if(grid3D_) + { + groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); + obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); + } + else + { + util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); + } + } + + if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty())) + { + UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols); + if(groundIsObstacle_ && !groundCloud.empty()) + { + if(obstaclesCloud.empty()) + { + obstaclesCloud = groundCloud; + groundCloud = cv::Mat(); + } + else + { + UASSERT(obstaclesCloud.type() == groundCloud.type()); + cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type()); + obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols))); + groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols))); + } + } + + // transform back in base frame + float roll, pitch, yaw; + pose.getEulerAngles(roll, pitch, yaw); + Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse(); + + if(rayTracing_) + { +#ifdef RTABMAP_OCTOMAP + if(!groundCloud.empty() || !obstaclesCloud.empty()) + { + //create local octomap + ParametersMap params; + params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_))); + params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(rangeMax_))); + params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_))); + LocalGridCache cache; + OctoMap octomap(&cache, params); + cache.add(1, groundCloud, obstaclesCloud, cv::Mat(), cellSize_, cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z)); + std::map poses; + poses.insert(std::make_pair(1, Transform::getIdentity())); + octomap.update(poses); + + pcl::IndicesPtr groundIndices(new std::vector); + pcl::IndicesPtr obstaclesIndices(new std::vector); + pcl::IndicesPtr emptyIndices(new std::vector); + pcl::PointCloud::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get()); + UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size()); + if(scan.hasRGB()) + { + groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data(); + obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data(); + emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data(); + } + else + { + pcl::PointCloud::Ptr cloudWithRayTracing2(new pcl::PointCloud); + pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2); + groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data(); + obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data(); + emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data(); + } + } + } + else +#else + UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str()); + } +#endif + { + groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data(); + obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data(); + } + + } + else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty())) + { + cv::Mat laserScan = obstacleCells; + cv::Mat laserScanNoHit = groundCells; + obstacleCells = cv::Mat(); + groundCells = cv::Mat(); + util3d::occupancy2DFromLaserScan( + laserScan, + laserScanNoHit, + viewPointInOut, + emptyCells, + obstacleCells, + cellSize_, + false, // don't fill unknown space + rangeMax_); + } + } + UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels()); +} + +} // namespace rtabmap diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 9db104186b..308748fe0b 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/VisualWord.h" #include "rtabmap/core/Features2d.h" +#include "rtabmap/core/GlobalDescriptorExtractor.h" #include "rtabmap/core/RegistrationIcp.h" #include "rtabmap/core/Registration.h" #include "rtabmap/core/RegistrationVis.h" @@ -60,9 +61,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/optimizer/OptimizerG2O.h" #include #include -#include #include #include +#include namespace rtabmap { @@ -107,6 +108,7 @@ Memory::Memory(const ParametersMap & parameters) : _rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()), _useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()), _useOdometryGravity(Parameters::defaultMemUseOdomGravity()), + _rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()), _createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()), _visMaxFeatures(Parameters::defaultVisMaxFeatures()), _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()), @@ -131,6 +133,7 @@ Memory::Memory(const ParametersMap & parameters) : _feature2D = Feature2D::create(parameters); _vwd = new VWDictionary(parameters); _registrationPipeline = Registration::create(parameters); + _globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters); if(!_registrationPipeline->isImageRequired()) { // make sure feature matching is used instead of optical flow to compute the guess @@ -153,7 +156,7 @@ Memory::Memory(const ParametersMap & parameters) : } _registrationIcpMulti = new RegistrationIcp(paramsMulti); - _occupancy = new OccupancyGrid(parameters); + _localMapMaker = new LocalGridMaker(parameters); _markerDetector = new MarkerDetector(parameters); this->parseParameters(parameters); } @@ -283,6 +286,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) -landmarkId, inserted.first->second, landmarkSize.at(0,0)); } } + else + { + UDEBUG("Caching landmark size %f for %d", landmarkSize.at(0,0), -landmarkId); + } } std::map >::iterator nter = _landmarksIndex.find(landmarkId); @@ -542,7 +549,7 @@ Memory::~Memory() delete _registrationPipeline; delete _registrationIcpMulti; delete _registrationVis; - delete _occupancy; + delete _localMapMaker; } void Memory::parseParameters(const ParametersMap & parameters) @@ -594,6 +601,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving); Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures); Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity); + Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp); Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid); Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures); Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified); @@ -746,9 +754,9 @@ void Memory::parseParameters(const ParametersMap & parameters) } } - if(_occupancy) + if(_localMapMaker) { - _occupancy->parseParameters(params); + _localMapMaker->parseParameters(params); } if(_markerDetector) @@ -756,6 +764,22 @@ void Memory::parseParameters(const ParametersMap & parameters) _markerDetector->parseParameters(params); } + int globalDescriptorStrategy = -1; + Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy); + if(globalDescriptorStrategy != -1 && + (_globalDescriptorExtractor==0 || (int)_globalDescriptorExtractor->getType() != globalDescriptorStrategy)) + { + if(_globalDescriptorExtractor) + { + delete _globalDescriptorExtractor; + } + _globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters_); + } + else if(_globalDescriptorExtractor) + { + _globalDescriptorExtractor->parseParameters(params); + } + // do this after all params are parsed // SLAM mode vs Localization mode iter = params.find(Parameters::kMemIncrementalMemory()); @@ -1192,7 +1216,10 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo) } } - this->moveToTrash(s, false); + // Setting true to make sure we save all visual + // words that could be referenced in a previously + // transferred node in LTM (#979) + this->moveToTrash(s, true); s = 0; } } @@ -2765,6 +2792,19 @@ void Memory::removeLink(int oldId, int newId) UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id()); } } + else if(this->_getSignature(newId<0?oldId:newId)) + { + int landmarkId = newId<0?newId:oldId; + Signature * s = this->_getSignature(newId<0?oldId:newId); + s->removeLandmark(newId<0?newId:oldId); + _linksChanged = true; + // Update landmark index + std::map >::iterator nter = _landmarksIndex.find(landmarkId); + if(nter!=_landmarksIndex.end()) + { + nter->second.erase(s->id()); + } + } else { if(!newS) @@ -3367,7 +3407,7 @@ bool Memory::addLink(const Link & link, bool addInDatabase) { UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef); - ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance()); + ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance(false)); Signature * toS = _getSignature(link.to()); Signature * fromS = _getSignature(link.from()); if(toS && fromS) @@ -3703,7 +3743,7 @@ unsigned long Memory::getMemoryUsed() const memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap); memoryUsage += sizeof(Registration); memoryUsage += sizeof(RegistrationIcp); - memoryUsage += _occupancy->getMemoryUsed(); + memoryUsage += sizeof(LocalGridMaker); memoryUsage += sizeof(MarkerDetector); memoryUsage += sizeof(DBDriver); @@ -4664,6 +4704,96 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor preUpdateThread.start(); } + if(_rotateImagesUpsideUp && !data.imageRaw().empty() && !data.cameraModels().empty()) + { + // Currently stereo is not supported + UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols); + int subInputImageWidth = data.imageRaw().cols/data.cameraModels().size(); + int subInputDepthWidth = data.depthRaw().cols/data.cameraModels().size(); + int subOutputImageWidth = 0; + int subOutputDepthWidth = 0; + cv::Mat rotatedColorImages; + cv::Mat rotatedDepthImages; + std::vector rotatedCameraModels; + bool allOutputSizesAreOkay = true; + for(size_t i=0; i(), std::vector(), cv::Mat()); + } + } + } + else if(_rotateImagesUpsideUp) + { + static bool warned = false; + if(!warned) + { + UWARN("Parameter %s can only be used with RGB-only or RGB-D cameras. " + "Ignoring upside up rotation. This message is only printed once.", + Parameters::kMemRotateImagesUpsideUp().c_str()); + warned = true; + } + } + unsigned int preDecimation = 1; std::vector keypoints3D; SensorData decimatedData; @@ -4967,6 +5097,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t); } + if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)) + { + _feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth()); + } } } else if(data.imageRaw().empty()) @@ -5214,9 +5348,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor { UASSERT((int)keypoints.size() == descriptors.rows); int inliersCount = 0; - if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) - { - Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), decimatedData.imageRaw().size(), _feature2D->getGridRows(), _feature2D->getGridCols()); + if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) && + (decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 || + data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)) + { + Feature2D::limitKeypoints(keypoints, + inliers, + _feature2D->getMaxFeatures(), + decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize(): + decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize(): + data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), + _feature2D->getGridRows(), _feature2D->getGridCols()); for(size_t i=0; igetGridRows() > 1 || _feature2D->getGridCols() > 1) + { + UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.", + Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str()); + } Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures()); inliersCount = _feature2D->getMaxFeatures(); } @@ -5822,7 +5969,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor s->sensorData().setGroundTruth(data.groundTruth()); s->sensorData().setGPS(data.gps()); s->sensorData().setEnvSensors(data.envSensors()); - s->sensorData().setGlobalDescriptors(data.globalDescriptors()); + + std::vector globalDescriptors = data.globalDescriptors(); + if(_globalDescriptorExtractor) + { + GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData); + if(!gdescriptor.data().empty()) + { + globalDescriptors.push_back(gdescriptor); + } + } + s->sensorData().setGlobalDescriptors(globalDescriptors); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); @@ -5835,14 +5992,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor // Occupancy grid map stuff if(_createOccupancyGrid && !isIntermediateNode) { - if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) || - (!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty())) + if( (_localMapMaker->isGridFromDepth() && !data.depthOrRightRaw().empty()) || + (!_localMapMaker->isGridFromDepth() && !data.laserScanRaw().empty())) { cv::Mat ground, obstacles, empty; float cellSize = 0.0f; cv::Point3f viewPoint(0,0,0); - _occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint); - cellSize = _occupancy->getCellSize(); + _localMapMaker->createLocalMap(*s, ground, obstacles, empty, viewPoint); + cellSize = _localMapMaker->getCellSize(); s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint); t = timer.ticks(); diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp deleted file mode 100644 index 5305700a07..0000000000 --- a/corelib/src/OccupancyGrid.cpp +++ /dev/null @@ -1,1667 +0,0 @@ -/* -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. -*/ - -#include -#include -#include -#include -#include -#include - -#ifdef RTABMAP_OCTOMAP -#include -#endif - -#include - -namespace rtabmap { - -OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) : - parameters_(parameters), - cloudDecimation_(Parameters::defaultGridDepthDecimation()), - cloudMaxDepth_(Parameters::defaultGridRangeMax()), - cloudMinDepth_(Parameters::defaultGridRangeMin()), - //roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters() - footprintLength_(Parameters::defaultGridFootprintLength()), - footprintWidth_(Parameters::defaultGridFootprintWidth()), - footprintHeight_(Parameters::defaultGridFootprintHeight()), - scanDecimation_(Parameters::defaultGridScanDecimation()), - cellSize_(Parameters::defaultGridCellSize()), - preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()), - occupancySensor_(Parameters::defaultGridSensor()), - projMapFrame_(Parameters::defaultGridMapFrameProjection()), - maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()), - normalKSearch_(Parameters::defaultGridNormalK()), - groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()), - maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f), - clusterRadius_(Parameters::defaultGridClusterRadius()), - minClusterSize_(Parameters::defaultGridMinClusterSize()), - flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()), - minGroundHeight_(Parameters::defaultGridMinGroundHeight()), - maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()), - normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()), - grid3D_(Parameters::defaultGrid3D()), - groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()), - noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()), - noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()), - scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()), - rayTracing_(Parameters::defaultGridRayTracing()), - fullUpdate_(Parameters::defaultGridGlobalFullUpdate()), - minMapSize_(Parameters::defaultGridGlobalMinSize()), - erode_(Parameters::defaultGridGlobalEroded()), - footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()), - updateError_(Parameters::defaultGridGlobalUpdateError()), - occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()), - probHit_(logodds(Parameters::defaultGridGlobalProbHit())), - probMiss_(logodds(Parameters::defaultGridGlobalProbMiss())), - probClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())), - probClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())), - xMin_(0.0f), - yMin_(0.0f), - cloudAssembling_(false), - assembledGround_(new pcl::PointCloud), - assembledObstacles_(new pcl::PointCloud), - assembledEmptyCells_(new pcl::PointCloud) -{ - this->parseParameters(parameters); -} - -void OccupancyGrid::parseParameters(const ParametersMap & parameters) -{ - Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_); - Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_); - if(cloudDecimation_ == 0) - { - cloudDecimation_ = 1; - } - Parameters::parse(parameters, Parameters::kGridRangeMin(), cloudMinDepth_); - Parameters::parse(parameters, Parameters::kGridRangeMax(), cloudMaxDepth_); - Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_); - Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_); - Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_); - Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_); - float cellSize = cellSize_; - if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize)) - { - this->setCellSize(cellSize); - } - - Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_); - Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_); - Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_); - Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_); - Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_); - Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_); - Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_); - if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_)) - { - maxGroundAngle_ *= M_PI/180.0f; - } - Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_); - UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str()); - Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_); - Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_); - Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_); - Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_); - Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_); - Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_); - Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_); - Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_); - Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_); - Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_); - Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_); - Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_); - Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_); - Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_); - - Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_); - if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_)) - { - probHit_ = logodds(probHit_); - UASSERT_MSG(probHit_ >= 0.0f, uFormat("probHit_=%f",probHit_).c_str()); - } - if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_)) - { - probMiss_ = logodds(probMiss_); - UASSERT_MSG(probMiss_ <= 0.0f, uFormat("probMiss_=%f",probMiss_).c_str()); - } - if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_)) - { - probClampingMin_ = logodds(probClampingMin_); - } - if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_)) - { - probClampingMax_ = logodds(probClampingMax_); - } - UASSERT(probClampingMax_ > probClampingMin_); - - UASSERT(minMapSize_ >= 0.0f); - - // convert ROI from string to vector - ParametersMap::const_iterator iter; - if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end()) - { - std::list strValues = uSplit(iter->second, ' '); - if(strValues.size() != 4) - { - ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str()); - } - else - { - std::vector tmpValues(4); - unsigned int i=0; - for(std::list::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter) - { - tmpValues[i] = uStr2Float(*jter); - ++i; - } - - if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] && - tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] && - tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] && - tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2]) - { - roiRatios_ = tmpValues; - } - else - { - ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str()); - } - } - } - - if(maxGroundHeight_ == 0.0f && !normalsSegmentation_) - { - UWARN("\"%s\" should be not equal to 0 if not using normals " - "segmentation approach. Setting it to cell size (%f).", - Parameters::kGridMaxGroundHeight().c_str(), cellSize_); - maxGroundHeight_ = cellSize_; - } - if(maxGroundHeight_ != 0.0f && - maxObstacleHeight_ != 0.0f && - maxObstacleHeight_ < maxGroundHeight_) - { - UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", - Parameters::kGridMaxGroundHeight().c_str(), - Parameters::kGridMaxObstacleHeight().c_str(), - Parameters::kGridMaxObstacleHeight().c_str()); - maxObstacleHeight_ = 0; - } - if(maxGroundHeight_ != 0.0f && - minGroundHeight_ != 0.0f && - maxGroundHeight_ < minGroundHeight_) - { - UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", - Parameters::kGridMinGroundHeight().c_str(), - Parameters::kGridMaxGroundHeight().c_str(), - Parameters::kGridMinGroundHeight().c_str()); - minGroundHeight_ = 0; - } -} - -void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map & poses) -{ - UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d", - map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size()); - this->clear(); - if(!poses.empty() && !map.empty()) - { - UASSERT(cellSize > 0.0f); - UASSERT(map.type() == CV_8SC1); - map_ = map.clone(); - mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4); - for(int i=0; i(i,j); - float * info = mapInfo_.ptr(i,j); - if(value == 0) - { - info[3] = probClampingMin_; - } - else if(value == 100) - { - info[3] = probClampingMax_; - } - } - } - xMin_ = xMin; - yMin_ = yMin; - cellSize_ = cellSize; - addedNodes_.insert(poses.lower_bound(1), poses.end()); - } -} - -void OccupancyGrid::setCellSize(float cellSize) -{ - UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str()); - if(cellSize_ != cellSize) - { - if(!map_.empty()) - { - UWARN("Grid cell size has changed, the map is cleared!"); - } - this->clear(); - cellSize_ = cellSize; - } -} - -void OccupancyGrid::setCloudAssembling(bool enabled) -{ - cloudAssembling_ = enabled; - if(!cloudAssembling_) - { - assembledGround_->clear(); - assembledObstacles_->clear(); - } -} - -void OccupancyGrid::createLocalMap( - const Signature & node, - cv::Mat & groundCells, - cv::Mat & obstacleCells, - cv::Mat & emptyCells, - cv::Point3f & viewPoint) -{ - UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d", - node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0); - - if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0) - { - UDEBUG("2D laser scan"); - //2D - viewPoint = cv::Point3f( - node.sensorData().laserScanRaw().localTransform().x(), - node.sensorData().laserScanRaw().localTransform().y(), - node.sensorData().laserScanRaw().localTransform().z()); - - LaserScan scan = node.sensorData().laserScanRaw(); - if(cloudMinDepth_ > 0.0f) - { - scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f); - } - - float maxRange = cloudMaxDepth_; - if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f) - { - maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().rangeMax()?cloudMaxDepth_:node.sensorData().laserScanRaw().rangeMax(); - } - else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f) - { - maxRange = node.sensorData().laserScanRaw().rangeMax(); - } - util3d::occupancy2DFromLaserScan( - util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(), - cv::Mat(), - viewPoint, - emptyCells, - obstacleCells, - cellSize_, - scan2dUnknownSpaceFilled_, - maxRange); - - UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels()); - } - else - { - // 3D - if(occupancySensor_ == 0 || occupancySensor_ == 2) - { - if(!node.sensorData().laserScanRaw().isEmpty()) - { - UDEBUG("3D laser scan"); - const Transform & t = node.sensorData().laserScanRaw().localTransform(); - LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_); -#ifdef RTABMAP_OCTOMAP - // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan() - float maxRange = rayTracing_?0.0f:cloudMaxDepth_; -#else - // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan() - float maxRange = !grid3D_ && rayTracing_?0.0f:cloudMaxDepth_; -#endif - if(cloudMinDepth_ > 0.0f || maxRange > 0.0f) - { - scan = util3d::rangeFiltering(scan, cloudMinDepth_, maxRange); - } - - // update viewpoint - viewPoint = cv::Point3f(t.x(), t.y(), t.z()); - - UDEBUG("scan format=%d", scan.format()); - - bool normalSegmentationTmp = normalsSegmentation_; - float minGroundHeightTmp = minGroundHeight_; - float maxGroundHeightTmp = maxGroundHeight_; - if(scan.is2d()) - { - // if 2D, assume the whole scan is obstacle - normalsSegmentation_ = false; - minGroundHeight_ = std::numeric_limits::min(); - maxGroundHeight_ = std::numeric_limits::min()+100; - } - - createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint); - - if(scan.is2d()) - { - // restore - normalsSegmentation_ = normalSegmentationTmp; - minGroundHeight_ = minGroundHeightTmp; - maxGroundHeight_ = maxGroundHeightTmp; - } - } - else - { - UWARN("Cannot create local map, scan is empty (node=%d, %s=0).", node.id(), Parameters::kGridSensor().c_str()); - } - } - - if(occupancySensor_ >= 1) - { - pcl::IndicesPtr indices(new std::vector); - pcl::PointCloud::Ptr cloud; - UDEBUG("Depth image : decimation=%d max=%f min=%f", - cloudDecimation_, - cloudMaxDepth_, - cloudMinDepth_); - cloud = util3d::cloudRGBFromSensorData( - node.sensorData(), - cloudDecimation_, -#ifdef RTABMAP_OCTOMAP - // If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan() - rayTracing_?0.0f:cloudMaxDepth_, -#else - // If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan() - !grid3D_&&rayTracing_?0.0f:cloudMaxDepth_, -#endif - cloudMinDepth_, - indices.get(), - parameters_, - roiRatios_); - - // update viewpoint - viewPoint = cv::Point3f(0,0,0); - if(node.sensorData().cameraModels().size()) - { - // average of all local transforms - float sum = 0; - for(unsigned int i=0; i 0.0f) - { - viewPoint.x /= sum; - viewPoint.y /= sum; - viewPoint.z /= sum; - } - } - else - { - // average of all local transforms - float sum = 0; - for(unsigned int i=0; i 0.0f) - { - viewPoint.x /= sum; - viewPoint.y /= sum; - viewPoint.z /= sum; - } - } - - cv::Mat scanGroundCells; - cv::Mat scanObstacleCells; - cv::Mat scanEmptyCells; - if(occupancySensor_ == 2) - { - // backup - scanGroundCells = groundCells.clone(); - scanObstacleCells = obstacleCells.clone(); - scanEmptyCells = emptyCells.clone(); - } - - createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint); - - if(occupancySensor_ == 2) - { - if(grid3D_) - { - // We should convert scans to 4 channels (XYZRGB) to be compatible - scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data(); - scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data(); - scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data(); - } - - UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels()); - UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels()); - UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels()); - - if(!groundCells.empty() && !scanGroundCells.empty()) - cv::hconcat(groundCells, scanGroundCells, groundCells); - else if(!scanGroundCells.empty()) - groundCells = scanGroundCells; - - if(!obstacleCells.empty() && !scanObstacleCells.empty()) - cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells); - else if(!scanObstacleCells.empty()) - obstacleCells = scanObstacleCells; - - if(!emptyCells.empty() && !scanEmptyCells.empty()) - cv::hconcat(emptyCells, scanEmptyCells, emptyCells); - else if(!scanEmptyCells.empty()) - emptyCells = scanEmptyCells; - } - } - } -} - -void OccupancyGrid::createLocalMap( - const LaserScan & scan, - const Transform & pose, - cv::Mat & groundCells, - cv::Mat & obstacleCells, - cv::Mat & emptyCells, - cv::Point3f & viewPointInOut) const -{ - if(projMapFrame_) - { - //we should rotate viewPoint in /map frame - float roll, pitch, yaw; - pose.getEulerAngles(roll, pitch, yaw); - Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0); - viewPointInOut.x = viewpointRotated.x(); - viewPointInOut.y = viewpointRotated.y(); - viewPointInOut.z = viewpointRotated.z(); - } - - if(scan.size()) - { - pcl::IndicesPtr groundIndices(new std::vector); - pcl::IndicesPtr obstaclesIndices(new std::vector); - cv::Mat groundCloud; - cv::Mat obstaclesCloud; - - if(scan.hasRGB() && scan.hasNormals()) - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform()); - pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); - UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); - if(grid3D_) - { - groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); - obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); - } - else - { - util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); - } - } - else if(scan.hasRGB()) - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform()); - pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); - UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); - if(grid3D_) - { - groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); - obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); - } - else - { - util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); - } - } - else if(scan.hasNormals()) - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform()); - pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); - UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); - if(grid3D_) - { - groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); - obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); - } - else - { - util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); - } - } - else - { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform()); - pcl::PointCloud::Ptr cloudSegmented = segmentCloud(cloud, pcl::IndicesPtr(new std::vector), pose, viewPointInOut, groundIndices, obstaclesIndices); - UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size()); - if(grid3D_) - { - groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data(); - obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data(); - } - else - { - util3d::occupancy2DFromGroundObstacles(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_); - } - } - - if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty())) - { - UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols); - if(groundIsObstacle_ && !groundCloud.empty()) - { - if(obstaclesCloud.empty()) - { - obstaclesCloud = groundCloud; - groundCloud = cv::Mat(); - } - else - { - UASSERT(obstaclesCloud.type() == groundCloud.type()); - cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type()); - obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols))); - groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols))); - } - } - - // transform back in base frame - float roll, pitch, yaw; - pose.getEulerAngles(roll, pitch, yaw); - Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse(); - - if(rayTracing_) - { -#ifdef RTABMAP_OCTOMAP - if(!groundCloud.empty() || !obstaclesCloud.empty()) - { - //create local octomap - ParametersMap params; - params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_))); - params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(cloudMaxDepth_))); - params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_))); - OctoMap octomap(params); - octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z)); - std::map poses; - poses.insert(std::make_pair(1, Transform::getIdentity())); - octomap.update(poses); - - pcl::IndicesPtr groundIndices(new std::vector); - pcl::IndicesPtr obstaclesIndices(new std::vector); - pcl::IndicesPtr emptyIndices(new std::vector); - pcl::PointCloud::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get()); - UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size()); - if(scan.hasRGB()) - { - groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data(); - obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data(); - emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data(); - } - else - { - pcl::PointCloud::Ptr cloudWithRayTracing2(new pcl::PointCloud); - pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2); - groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data(); - obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data(); - emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data(); - } - } - } - else -#else - UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str()); - } -#endif - { - groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data(); - obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data(); - } - - } - else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty())) - { - cv::Mat laserScan = obstacleCells; - cv::Mat laserScanNoHit = groundCells; - obstacleCells = cv::Mat(); - groundCells = cv::Mat(); - util3d::occupancy2DFromLaserScan( - laserScan, - laserScanNoHit, - viewPointInOut, - emptyCells, - obstacleCells, - cellSize_, - false, // don't fill unknown space - cloudMaxDepth_); - } - } - UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels()); -} - -void OccupancyGrid::clear() -{ - cache_.clear(); - map_ = cv::Mat(); - mapInfo_ = cv::Mat(); - cellCount_.clear(); - xMin_ = 0.0f; - yMin_ = 0.0f; - addedNodes_.clear(); - assembledGround_->clear(); - assembledObstacles_->clear(); -} - -cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const -{ - xMin = xMin_; - yMin = yMin_; - - cv::Mat map = map_; - - UTimer t; - if(occupancyThr_ != 0.0f && !map.empty()) - { - float occThr = logodds(occupancyThr_); - map = cv::Mat(map.size(), map.type()); - UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows); - for(int i=0; i(i, j); - if(info[3] == 0.0f) - { - map.at(i, j) = -1; // unknown - } - else if(info[3] >= occThr) - { - map.at(i, j) = 100; // unknown - } - else - { - map.at(i, j) = 0; // empty - } - } - } - UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks()); - } - - if(erode_ && !map.empty()) - { - map = util3d::erodeMap(map); - UDEBUG("Eroding map = %fs", t.ticks()); - } - return map; -} - -cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const -{ - xMin = xMin_; - yMin = yMin_; - - cv::Mat map; - if(!mapInfo_.empty()) - { - map = cv::Mat(mapInfo_.size(), map_.type()); - for(int i=0; i(i, j); - if(info[3] == 0.0f) - { - map.at(i, j) = -1; // unknown - } - else - { - map.at(i, j) = char(probability(info[3])*100.0f); // empty - } - } - } - } - else - { - UWARN("Map info is empty, cannot generate probabilistic occupancy grid"); - } - return map; -} - -void OccupancyGrid::addToCache( - int nodeId, - const cv::Mat & ground, - const cv::Mat & obstacles, - const cv::Mat & empty) -{ - UDEBUG("nodeId=%d (ground=%d obstacles=%d empty=%d)", nodeId, ground.cols, obstacles.cols, empty.cols); - if(nodeId < 0) - { - UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId); - return; - } - uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty))); -} - -bool OccupancyGrid::update(const std::map & posesIn) -{ - UTimer timer; - UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size()); - - float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_; - - float minX=-minMapSize_/2.0f; - float minY=-minMapSize_/2.0f; - float maxX=minMapSize_/2.0f; - float maxY=minMapSize_/2.0f; - bool undefinedSize = minMapSize_ == 0.0f; - std::map emptyLocalMaps; - std::map occupiedLocalMaps; - - // First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false). - bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified) - bool graphChanged = !addedNodes_.empty(); // If the new map doesn't have any node from the previous map - std::map transforms; - float updateErrorSqrd = updateError_*updateError_; - for(std::map::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) - { - std::map::const_iterator jter = posesIn.find(iter->first); - if(jter != posesIn.end()) - { - graphChanged = false; - - UASSERT(!iter->second.isNull() && !jter->second.isNull()); - Transform t = Transform::getIdentity(); - if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd) - { - t = jter->second * iter->second.inverse(); - graphOptimized = true; - } - transforms.insert(std::make_pair(jter->first, t)); - - float x = jter->second.x(); - float y =jter->second.y(); - if(undefinedSize) - { - minX = maxX = x; - minY = maxY = y; - undefinedSize = false; - } - else - { - if(minX > x) - minX = x; - else if(maxX < x) - maxX = x; - - if(minY > y) - minY = y; - else if(maxY < y) - maxY = y; - } - } - else - { - UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first); - } - } - - bool assembledGroundUpdated = false; - bool assembledObstaclesUpdated = false; - bool assembledEmptyCellsUpdated = false; - - if(graphOptimized || graphChanged) - { - if(graphChanged) - { - UWARN("Graph has changed! The whole map should be rebuilt."); - } - else - { - UINFO("Graph optimized!"); - } - - if(cloudAssembling_) - { - assembledGround_->clear(); - assembledObstacles_->clear(); - } - - if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells - { - // 1) recreate all local maps - UASSERT(map_.cols == mapInfo_.cols && - map_.rows == mapInfo_.rows); - std::map > tmpIndices; - for(std::map >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter) - { - if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end()) - { - if(iter->second.first) - { - emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2))); - } - if(iter->second.second) - { - occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2))); - } - tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0))); - } - } - for(int y=0; y(y,x); - int nodeId = (int)info[0]; - if(nodeId > 0 && map_.at(y,x) >= 0) - { - if(tmpIndices.find(nodeId)!=tmpIndices.end()) - { - std::map::iterator tter = transforms.find(nodeId); - UASSERT(tter != transforms.end()); - - cv::Point3f pt(info[1], info[2], 0.0f); - pt = util3d::transformPoint(pt, tter->second); - - if(minX > pt.x) - minX = pt.x; - else if(maxX < pt.x) - maxX = pt.x; - - if(minY > pt.y) - minY = pt.y; - else if(maxY < pt.y) - maxY = pt.y; - - std::map >::iterator jter = tmpIndices.find(nodeId); - if(map_.at(y, x) == 0) - { - // ground - std::map::iterator iter = emptyLocalMaps.find(nodeId); - UASSERT(iter != emptyLocalMaps.end()); - UASSERT(jter->second.first < iter->second.cols); - float * ptf = iter->second.ptr(0,jter->second.first++); - ptf[0] = pt.x; - ptf[1] = pt.y; - } - else - { - // obstacle - std::map::iterator iter = occupiedLocalMaps.find(nodeId); - UASSERT(iter != occupiedLocalMaps.end()); - UASSERT(iter!=occupiedLocalMaps.end()); - UASSERT(jter->second.second < iter->second.cols); - float * ptf = iter->second.ptr(0,jter->second.second++); - ptf[0] = pt.x; - ptf[1] = pt.y; - } - } - } - else if(nodeId > 0) - { - UERROR("Cell referred b node %d is unknown!?", nodeId); - } - } - } - - //verify if all cells were added - for(std::map >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter) - { - std::map::iterator jter = emptyLocalMaps.find(iter->first); - UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) || - (iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first), - uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str()); - jter = occupiedLocalMaps.find(iter->first); - UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) || - (iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second), - uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str()); - } - - UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY); - } - - addedNodes_.clear(); - map_ = cv::Mat(); - mapInfo_ = cv::Mat(); - cellCount_.clear(); - xMin_ = 0.0f; - yMin_ = 0.0f; - } - else if(!map_.empty()) - { - // update - minX=xMin_+margin+cellSize_/2.0f; - minY=yMin_+margin+cellSize_/2.0f; - maxX=xMin_+float(map_.cols)*cellSize_ - margin; - maxY=yMin_+float(map_.rows)*cellSize_ - margin; - undefinedSize = false; - } - - bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged; - - std::list > poses; - int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0; - UDEBUG("Last id = %d", lastId); - - // add old poses that were not in the current map (they were just retrieved from LTM) - for(std::map::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter) - { - if(addedNodes_.find(iter->first) == addedNodes_.end()) - { - UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first); - poses.push_back(*iter); - } - } - - // insert zero after - if(posesIn.find(0) != posesIn.end()) - { - poses.push_back(std::make_pair(-1, posesIn.at(0))); - } - - if(!poses.empty()) - { - for(std::list >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - UASSERT(!iter->second.isNull()); - - float x = iter->second.x(); - float y =iter->second.y(); - if(undefinedSize) - { - minX = maxX = x; - minY = maxY = y; - undefinedSize = false; - } - else - { - if(minX > x) - minX = x; - else if(maxX < x) - maxX = x; - - if(minY > y) - minY = y; - else if(maxY < y) - maxY = y; - } - } - - if(!cache_.empty()) - { - UDEBUG("Updating from cache"); - for(std::list >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) - { - if(uContains(cache_, iter->first)) - { - const std::pair, cv::Mat> & pair = cache_.at(iter->first); - - UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols); - - //ground - cv::Mat ground; - if(pair.first.first.cols || pair.second.cols) - { - ground = cv::Mat(1, pair.first.first.cols+pair.second.cols, CV_32FC2); - } - if(pair.first.first.cols) - { - if(pair.first.first.rows > 1 && pair.first.first.cols == 1) - { - UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols); - } - for(int i=0; i(0,i); - float * vo = ground.ptr(0,i); - cv::Point3f vt; - if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5) - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); - } - else - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); - } - vo[0] = vt.x; - vo[1] = vt.y; - if(minX > vo[0]) - minX = vo[0]; - else if(maxX < vo[0]) - maxX = vo[0]; - - if(minY > vo[1]) - minY = vo[1]; - else if(maxY < vo[1]) - maxY = vo[1]; - } - - if(cloudAssembling_) - { - *assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.first), iter->second, 0, 255, 0); - assembledGroundUpdated = true; - } - } - - //empty - if(pair.second.cols) - { - if(pair.second.rows > 1 && pair.second.cols == 1) - { - UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols); - } - for(int i=0; i(0,i); - float * vo = ground.ptr(0,i+pair.first.first.cols); - cv::Point3f vt; - if(pair.second.channels() != 2 && pair.second.channels() != 5) - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); - } - else - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); - } - vo[0] = vt.x; - vo[1] = vt.y; - if(minX > vo[0]) - minX = vo[0]; - else if(maxX < vo[0]) - maxX = vo[0]; - - if(minY > vo[1]) - minY = vo[1]; - else if(maxY < vo[1]) - maxY = vo[1]; - } - - if(cloudAssembling_) - { - *assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.second), iter->second, 0, 255, 0); - assembledEmptyCellsUpdated = true; - } - } - uInsert(emptyLocalMaps, std::make_pair(iter->first, ground)); - - //obstacles - if(pair.first.second.cols) - { - if(pair.first.second.rows > 1 && pair.first.second.cols == 1) - { - UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols); - } - cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2); - for(int i=0; i(0,i); - float * vo = obstacles.ptr(0,i); - cv::Point3f vt; - if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5) - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); - } - else - { - vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); - } - vo[0] = vt.x; - vo[1] = vt.y; - if(minX > vo[0]) - minX = vo[0]; - else if(maxX < vo[0]) - maxX = vo[0]; - - if(minY > vo[1]) - minY = vo[1]; - else if(maxY < vo[1]) - maxY = vo[1]; - } - uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles)); - - if(cloudAssembling_) - { - *assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.second), iter->second, 255, 0, 0); - assembledObstaclesUpdated = true; - } - } - } - } - } - - cv::Mat map; - cv::Mat mapInfo; - if(minX != maxX && minY != maxY) - { - //Get map size - float xMin = minX-margin; - xMin -= cellSize_/2.0f; - float yMin = minY-margin; - yMin -= cellSize_/2.0f; - float xMax = maxX+margin; - float yMax = maxY+margin; - - if(fabs((yMax - yMin) / cellSize_) > 99999 || - fabs((xMax - xMin) / cellSize_) > 99999) - { - UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). " - "There's maybe an error with the poses provided! The map will not be created!", - xMin, yMin, xMax, yMax); - } - else - { - UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax); - cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f); - if(map_.empty()) - { - UDEBUG("Map empty!"); - map = cv::Mat::ones(newMapSize, CV_8S)*-1; - mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4); - } - else - { - if(xMin == xMin_ && yMin == yMin_ && - newMapSize.width == map_.cols && - newMapSize.height == map_.rows) - { - // same map size and origin, don't do anything - UDEBUG("Map same size!"); - map = map_; - mapInfo = mapInfo_; - } - else - { - UASSERT_MSG(xMin <= xMin_+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, xMin_, cellSize_).c_str()); - UASSERT_MSG(yMin <= yMin_+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, yMin_, cellSize_).c_str()); - UASSERT_MSG(xMax >= xMin_+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, xMin_, map_.cols, cellSize_).c_str()); - UASSERT_MSG(yMax >= yMin_+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, yMin_, map_.rows, cellSize_).c_str()); - - UDEBUG("Copy map"); - // copy the old map in the new map - // make sure the translation is cellSize - int deltaX = 0; - if(xMin < xMin_) - { - deltaX = (xMin_ - xMin) / cellSize_ + 1.0f; - xMin = xMin_-float(deltaX)*cellSize_; - } - int deltaY = 0; - if(yMin < yMin_) - { - deltaY = (yMin_ - yMin) / cellSize_ + 1.0f; - yMin = yMin_-float(deltaY)*cellSize_; - } - UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY); - newMapSize.width = (xMax - xMin) / cellSize_+0.5f; - newMapSize.height = (yMax - yMin) / cellSize_+0.5f; - UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height); - UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows); - UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY); - UASSERT(deltaX>=0 && deltaY>=0); - map = cv::Mat::ones(newMapSize, CV_8S)*-1; - mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type()); - map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows))); - mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows))); - } - } - UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows); - UDEBUG("map %d %d", map.cols, map.rows); - if(poses.size()) - { - UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first); - } - for(std::list >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter) - { - std::map::iterator iter = emptyLocalMaps.find(kter->first); - std::map::iterator jter = occupiedLocalMaps.find(kter->first); - if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end()) - { - if(kter->first > 0) - { - uInsert(addedNodes_, *kter); - } - std::map >::iterator cter = cellCount_.find(kter->first); - if(cter == cellCount_.end() && kter->first > 0) - { - cter = cellCount_.insert(std::make_pair(kter->first, std::pair(0,0))).first; - } - if(iter!=emptyLocalMaps.end()) - { - for(int i=0; isecond.cols; ++i) - { - float * ptf = iter->second.ptr(0,i); - cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_); - UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols, - uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)", - kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str()); - char & value = map.at(pt.y, pt.x); - if(value != -2 && (!incrementalGraphUpdate || value==-1)) - { - float * info = mapInfo.ptr(pt.y, pt.x); - int nodeId = (int)info[0]; - if(value != -1) - { - if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) - { - // cannot rewrite on cells referred by more recent nodes - continue; - } - if(nodeId > 0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.first += 1; - } - } - } - if(kter->first > 0) - { - info[0] = (float)kter->first; - info[1] = ptf[0]; - info[2] = ptf[1]; - cter->second.first+=1; - } - value = 0; // free space - - // update odds - if(nodeId != kter->first) - { - info[3] += probMiss_; - if (info[3] < probClampingMin_) - { - info[3] = probClampingMin_; - } - if (info[3] > probClampingMax_) - { - info[3] = probClampingMax_; - } - } - } - } - } - - if(footprintRadius_ >= cellSize_*1.5f) - { - // place free space under the footprint of the robot - cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_); - cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_); - if(ptBegin.x < 0) - ptBegin.x = 0; - if(ptEnd.x >= map.cols) - ptEnd.x = map.cols-1; - - if(ptBegin.y < 0) - ptBegin.y = 0; - if(ptEnd.y >= map.rows) - ptEnd.y = map.rows-1; - - for(int i=ptBegin.x; i(j, i); - float * info = mapInfo.ptr(j, i); - int nodeId = (int)info[0]; - if(value != -1) - { - if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) - { - // cannot rewrite on cells referred by more recent nodes - continue; - } - if(nodeId>0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.first += 1; - } - } - } - if(kter->first > 0) - { - info[0] = (float)kter->first; - info[1] = float(i) * cellSize_ + xMin; - info[2] = float(j) * cellSize_ + yMin; - info[3] = probClampingMin_; - cter->second.first+=1; - } - value = -2; // free space (footprint) - } - } - } - - if(jter!=occupiedLocalMaps.end()) - { - for(int i=0; isecond.cols; ++i) - { - float * ptf = jter->second.ptr(0,i); - cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_); - UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols, - uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)", - kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str()); - char & value = map.at(pt.y, pt.x); - if(value != -2) - { - float * info = mapInfo.ptr(pt.y, pt.x); - int nodeId = (int)info[0]; - if(value != -1) - { - if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) - { - // cannot rewrite on cells referred by more recent nodes - continue; - } - if(nodeId>0) - { - std::map >::iterator eter = cellCount_.find(nodeId); - UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); - if(value == 0) - { - eter->second.first -= 1; - } - else if(value == 100) - { - eter->second.second -= 1; - } - if(kter->first < 0) - { - eter->second.second += 1; - } - } - } - if(kter->first > 0) - { - info[0] = (float)kter->first; - info[1] = ptf[0]; - info[2] = ptf[1]; - cter->second.second+=1; - } - - // update odds - if(nodeId != kter->first || value!=100) - { - info[3] += probHit_; - if (info[3] < probClampingMin_) - { - info[3] = probClampingMin_; - } - if (info[3] > probClampingMax_) - { - info[3] = probClampingMax_; - } - } - - value = 100; // obstacles - } - } - } - } - } - - if(footprintRadius_ >= cellSize_*1.5f || incrementalGraphUpdate) - { - for(int i=1; i(i, j); - if(value == -2) - { - value = 0; - } - - if(incrementalGraphUpdate && value == -1) - { - float * info = mapInfo.ptr(i, j); - - // fill obstacle - if(map.at(i+1, j) == 100 && map.at(i-1, j) == 100) - { - value = 100; - // associate with the nearest pose - if(mapInfo.ptr(i+1, j)[0]>0.0f) - { - info[0] = mapInfo.ptr(i+1, j)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.second+=1; - } - else if(mapInfo.ptr(i-1, j)[0]>0.0f) - { - info[0] = mapInfo.ptr(i-1, j)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.second+=1; - } - } - else if(map.at(i, j+1) == 100 && map.at(i, j-1) == 100) - { - value = 100; - // associate with the nearest pose - if(mapInfo.ptr(i, j+1)[0]>0.0f) - { - info[0] = mapInfo.ptr(i, j+1)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.second+=1; - } - else if(mapInfo.ptr(i, j-1)[0]>0.0f) - { - info[0] = mapInfo.ptr(i, j-1)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.second+=1; - } - } - else - { - // fill empty - char sum = (map.at(i+1, j) == 0?1:0) + - (map.at(i-1, j) == 0?1:0) + - (map.at(i, j+1) == 0?1:0) + - (map.at(i, j-1) == 0?1:0); - if(sum >=3) - { - value = 0; - // associate with the nearest pose, only check two cases (as 3 are required) - if(map.at(i+1, j) != -1 && mapInfo.ptr(i+1, j)[0]>0.0f) - { - info[0] = mapInfo.ptr(i+1, j)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.first+=1; - } - else if(map.at(i-1, j) != -1 && mapInfo.ptr(i-1, j)[0]>0.0f) - { - info[0] = mapInfo.ptr(i-1, j)[0]; - info[1] = float(j) * cellSize_ + xMin; - info[2] = float(i) * cellSize_ + yMin; - std::map >::iterator cter = cellCount_.find(int(info[0])); - UASSERT(cter!=cellCount_.end()); - cter->second.first+=1; - } - } - } - } - - //float * info = mapInfo.ptr(i,j); - //if(info[0] > 0) - //{ - // cloud->at(oi).x = info[1]; - // cloud->at(oi).y = info[2]; - // oi++; - //} - } - } - } - //if(graphChanged) - //{ - // cloud->resize(oi); - // pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud); - // UWARN("Saved mapInfo.pcd"); - //} - - map_ = map; - mapInfo_ = mapInfo; - xMin_ = xMin; - yMin_ = yMin; - - // clean cellCount_ - for(std::map >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();) - { - UASSERT(iter->second.first >= 0 && iter->second.second >= 0); - if(iter->second.first == 0 && iter->second.second == 0) - { - cellCount_.erase(iter++); - } - else - { - ++iter; - } - } - } - } - - if(cloudAssembling_) - { - if(assembledGroundUpdated && assembledGround_->size() > 1) - { - assembledGround_ = util3d::voxelize(assembledGround_, cellSize_); - } - if(assembledObstaclesUpdated && assembledGround_->size() > 1) - { - assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_); - } - if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1) - { - assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_); - } - } - } - - if(!fullUpdate_ && !cloudAssembling_) - { - cache_.clear(); - } - else - { - //clear only negative ids - for(std::map, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();) - { - if(iter->first < 0) - { - cache_.erase(iter++); - } - else - { - break; - } - } - } - - bool updated = !poses.empty() || graphOptimized || graphChanged; - UDEBUG("Occupancy Grid update time = %f s (updated=%s)", timer.ticks(), updated?"true":"false"); - return updated; -} - -unsigned long OccupancyGrid::getMemoryUsed() const -{ - unsigned long memoryUsage = sizeof(OccupancyGrid); - memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap); - - memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair, cv::Mat>) + sizeof(std::map, cv::Mat> >::iterator)) + sizeof(std::map, cv::Mat> >); - for(std::map, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter) - { - memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize(); - memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize(); - memoryUsage += iter->second.second.total() * iter->second.second.elemSize(); - } - memoryUsage += map_.total() * map_.elemSize(); - memoryUsage += mapInfo_.total() * mapInfo_.elemSize(); - memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair) + sizeof(std::map >::iterator)) + sizeof(std::map >); - memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map::iterator)) + sizeof(std::map); - - if(assembledGround_.get()) - { - memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB); - } - if(assembledObstacles_.get()) - { - memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB); - } - if(assembledEmptyCells_.get()) - { - memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB); - } - return memoryUsage; -} - -} diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 40a1907e48..23eb7a8463 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryViso2.h" #include "rtabmap/core/odometry/OdometryDVO.h" #include "rtabmap/core/odometry/OdometryOkvis.h" -#include "rtabmap/core/odometry/OdometryORBSLAM.h" +#include "rtabmap/core/odometry/OdometryORBSLAM3.h" #include "rtabmap/core/odometry/OdometryLOAM.h" #include "rtabmap/core/odometry/OdometryFLOAM.h" #include "rtabmap/core/odometry/OdometryMSCKF.h" @@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util2d.h" #include +#include namespace rtabmap { @@ -84,7 +85,11 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet odometry = new OdometryDVO(parameters); break; case Odometry::kTypeORBSLAM: - odometry = new OdometryORBSLAM(parameters); +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 + odometry = new OdometryORBSLAM2(parameters); +#else + odometry = new OdometryORBSLAM3(parameters); +#endif break; case Odometry::kTypeOkvis: odometry = new OdometryOkvis(parameters); @@ -135,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _alignWithGround(Parameters::defaultOdomAlignWithGround()), _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()), _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()), + _deskewing(Parameters::defaultOdomDeskewing()), _pose(Transform::getIdentity()), _resetCurrentCount(0), previousStamp_(0), @@ -164,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround); Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage); Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified); + Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing); if(_imageDecimation == 0) { @@ -324,6 +331,19 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet imus_.erase(imus_.begin()); } } + else + { + UWARN("Received IMU doesn't have orientation set! It is ignored."); + } + } + + if(!data.imageRaw().empty()) + { + UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld", + data.imageRaw().cols, + data.imageRaw().rows, + data.cameraModels().size(), + data.stereoCameraModels().size()); } @@ -602,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } UTimer time; + + // Deskewing lidar + if( _deskewing && + !data.laserScanRaw().empty() && + data.laserScanRaw().hasTime() && + dt > 0 && + !guess.isNull()) + { + UDEBUG("Deskewing begin"); + // Recompute velocity + float vx,vy,vz, vroll,vpitch,vyaw; + guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); + + // transform to velocity + vx /= dt; + vy /= dt; + vz /= dt; + vroll /= dt; + vpitch /= dt; + vyaw /= dt; + + if(!imus_.empty()) + { + float scanTime = + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] - + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + + // replace orientation velocity based on IMU (if available) + Transform imuFirstScan = Transform::getTransform(imus_, + data.stamp() + + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]); + Transform imuLastScan = Transform::getTransform(imus_, + data.stamp() + + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]); + if(!imuFirstScan.isNull() && !imuLastScan.isNull()) + { + Transform orientation = imuFirstScan.inverse() * imuLastScan; + orientation.getEulerAngles(vroll, vpitch, vyaw); + if(_force3DoF) + { + vroll=0; + vpitch=0; + vyaw /= scanTime; + } + else + { + vroll /= scanTime; + vpitch /= scanTime; + vyaw /= scanTime; + } + } + } + + Transform velocity(vx,vy,vz,vroll,vpitch,vyaw); + LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity); + if(!scanDeskewed.isEmpty()) + { + data.setLaserScan(scanDeskewed); + } + info->timeDeskewing = time.ticks(); + UDEBUG("Deskewing end"); + } + if(data.laserScanRaw().isOrganized()) + { + // Laser scans should be dense passing this point + data.setLaserScan(data.laserScanRaw().densify()); + } + + Transform t; if(_imageDecimation > 1 && !data.imageRaw().empty()) { @@ -671,12 +760,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty()); for(unsigned int i=0; inewCorners.size(); ++i) { - info->refCorners[i].x *= _imageDecimation; - info->refCorners[i].y *= _imageDecimation; + info->newCorners[i].x *= _imageDecimation; + info->newCorners[i].y *= _imageDecimation; if(!info->refCorners.empty()) { - info->newCorners[i].x *= _imageDecimation; - info->newCorners[i].y *= _imageDecimation; + info->refCorners[i].x *= _imageDecimation; + info->refCorners[i].y *= _imageDecimation; } } for(std::multimap::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter) @@ -897,6 +986,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet { UWARN("Odometry automatically reset to latest pose!"); this->reset(_pose); + _resetCurrentCount = _resetCountdown; if(info) { *info = OdometryInfo(); diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 1256118291..ccf064a979 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/Odometry.h" #include "rtabmap/core/odometry/OdometryMono.h" #include "rtabmap/core/OdometryInfo.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/utilite/ULogger.h" @@ -58,12 +58,12 @@ bool OdometryThread::handleEvent(UEvent * event) { if(this->isRunning()) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * cameraEvent = (CameraEvent*)event; - if(cameraEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * sensorEvent = (SensorEvent*)event; + if(sensorEvent->getCode() == SensorEvent::kCodeData) { - this->addData(cameraEvent->data()); + this->addData(sensorEvent->data()); } } else if(event->getClassName().compare("IMUEvent") == 0) diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index cf1d4e10e3..9b3b6704e2 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -190,8 +190,7 @@ void Optimizer::getConnectedGraph( const std::map & posesIn, const std::multimap & linksIn, std::map & posesOut, - std::multimap & linksOut, - bool adjustPosesWithConstraints) const + std::multimap & linksOut) const { UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0); UASSERT(fromId>0); @@ -202,15 +201,15 @@ void Optimizer::getConnectedGraph( std::set nextPoses; nextPoses.insert(fromId); - std::multimap biLinks; + std::multimap > biLinks; for(std::multimap::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter) { if(iter->second.from() != iter->second.to()) { - if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end()) + if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end()) { - biLinks.insert(std::make_pair(iter->second.from(), iter->second.to())); - biLinks.insert(std::make_pair(iter->second.to(), iter->second.from())); + biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type()))); + biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type()))); } } } @@ -234,41 +233,35 @@ void Optimizer::getConnectedGraph( } } - for(std::multimap::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) + for(std::multimap >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter) { - int toId = iter->second; + int toId = iter->second.first; + Link::Type type = iter->second.second; if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0)) { - std::multimap::const_iterator kter = graph::findLink(linksIn, currentId, toId); + std::multimap::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type); if(nextPoses.find(toId) == nextPoses.end()) { if(!uContains(posesOut, toId)) { - if(adjustPosesWithConstraints) + const Transform & poseToIn = posesIn.at(toId); + Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse(); + if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF())) { - if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0) + if(poseToIn.is3DoF()) { - Transform t; - if(kter->second.from()==currentId) - { - t = kter->second.transform(); - } - else - { - t = kter->second.transform().inverse(); - } posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF())); } else { - Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse()); - posesOut.insert(std::make_pair(toId, t)); + posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF())); } } else { - posesOut.insert(*posesIn.find(toId)); + posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t)); } + // add prior links for(std::multimap::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter) { @@ -282,7 +275,7 @@ void Optimizer::getConnectedGraph( } // only add unique links - if(graph::findLink(linksOut, currentId, toId) == linksOut.end()) + if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end()) { if(kter->second.to() < 0) { diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index e1026cc3e4..8c3dabdc72 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -214,14 +214,14 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn) return parameters; } -ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove) +ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn, bool remove) { ParametersMap output; for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) { UASSERT(uSplit(iter->first, '/').size() == 2); std::string group = uSplit(iter->first, '/').front(); - bool sameGroup = group.compare(group) == 0; + bool sameGroup = group.compare(groupIn) == 0; if((!remove && sameGroup) || (remove && !sameGroup)) { output.insert(*iter); @@ -236,6 +236,9 @@ const std::map > & Parameters::getRemo { // removed parameters + // 0.21.3 + removedParameters_.insert(std::make_pair("GridGlobal/FullUpdate", std::make_pair(false, ""))); + // 0.20.15 removedParameters_.insert(std::make_pair("Grid/FromDepth", std::make_pair(true, Parameters::kGridSensor()))); @@ -301,7 +304,7 @@ const std::map > & Parameters::getRemo removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy", std::make_pair(true, Parameters::kVhEpEnabled()))); // 0.12.5 - removedParameters_.insert(std::make_pair("Grid/FullUpdate", std::make_pair(true, Parameters::kGridGlobalFullUpdate()))); + removedParameters_.insert(std::make_pair("Grid/FullUpdate", std::make_pair(false, ""))); // 0.12.1 removedParameters_.insert(std::make_pair("Grid/3DGroundIsObstacle", std::make_pair(true, Parameters::kGridGroundIsObstacle()))); @@ -812,11 +815,23 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif - str = "With octomap:"; + str = "With Open3D:"; +#ifdef RTABMAP_OPEN3D + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With OctoMap:"; #ifdef RTABMAP_OCTOMAP std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With GridMap:"; +#ifdef RTABMAP_GRIDMAP + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With cpu-tsdf:"; #ifdef RTABMAP_CPUTSDF @@ -1157,11 +1172,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam return out; } - -void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly) +void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, ParametersMap & parameters, bool modifiedOnly) { - CSimpleIniA ini; - ini.LoadFile(configFile.c_str()); const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core"); if(keyValMap) { @@ -1176,12 +1188,12 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet { if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str()))) { - if(configFile.find(".rtabmap") != std::string::npos) + if(configFilePath.find(".rtabmap") != std::string::npos) { UWARN("Version in the config file \"%s\" is more recent (\"%s\") than " "current RTAB-Map version used (\"%s\"). The config file will be upgraded " "to new version.", - configFile.c_str(), + configFilePath.c_str(), (*iter).second, RTABMAP_VERSION); } @@ -1190,7 +1202,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet UERROR("Version in the config file \"%s\" is more recent (\"%s\") than " "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will " "be ignored.", - configFile.c_str(), + configFilePath.c_str(), (*iter).second, RTABMAP_VERSION); } @@ -1240,11 +1252,26 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet else { ULOGGER_WARN("Section \"Core\" in %s doesn't exist... " - "Ignore this warning if the ini file does not exist yet. " - "The ini file will be automatically created when rtabmap will close.", configFile.c_str()); + "Ignore this warning if the ini file does not exist yet. " + "The ini file will be automatically created when rtabmap will close.", configFilePath.c_str()); } } + +void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly) +{ + CSimpleIniA ini; + ini.LoadFile(configFile.c_str()); + readINIImpl(ini, configFile, parameters, modifiedOnly); +} + +void Parameters::readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly) +{ + CSimpleIniA ini; + ini.LoadData(configContent); + readINIImpl(ini, "", parameters, modifiedOnly); +} + void Parameters::writeINI(const std::string & configFile, const ParametersMap & parameters) { CSimpleIniA ini; diff --git a/corelib/src/Recovery.cpp b/corelib/src/Recovery.cpp index 5ad0a5e9d5..72546b031f 100644 --- a/corelib/src/Recovery.cpp +++ b/corelib/src/Recovery.cpp @@ -160,7 +160,7 @@ bool databaseRecovery( DBReader dbReader(databasePath, 0, odometryIgnored); dbReader.init(); - CameraInfo info; + SensorCaptureInfo info; SensorData data = dbReader.takeImage(&info); int processed = 0; if (progressState) diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 9520cf0dee..11611ae11d 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -64,10 +64,12 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _rangeMin(Parameters::defaultIcpRangeMin()), _rangeMax(Parameters::defaultIcpRangeMax()), _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()), + _reciprocalCorrespondences(Parameters::defaultIcpReciprocalCorrespondences()), _maxIterations(Parameters::defaultIcpIterations()), _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _force4DoF(Parameters::defaultIcpForce4DoF()), + _filtersEnabled(Parameters::defaultIcpFiltersEnabled()), _pointToPlane(Parameters::defaultIcpPointToPlane()), _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()), _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()), @@ -109,10 +111,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin); Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax); Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance); + Parameters::parse(parameters, Parameters::kIcpReciprocalCorrespondences(), _reciprocalCorrespondences); Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations); Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio); Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF); + Parameters::parse(parameters, Parameters::kIcpFiltersEnabled(), _filtersEnabled); Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio); Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK); @@ -335,6 +339,7 @@ Transform RegistrationIcp::computeTransformationImpl( UDEBUG("Downsampling step=%d", _downsamplingStep); UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false"); UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false"); + UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false"); UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity); UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio); UDEBUG("Strategy=%d", _strategy); @@ -358,7 +363,7 @@ Transform RegistrationIcp::computeTransformationImpl( int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size(); int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size(); - if(!dataFrom.laserScanRaw().empty()) + if(!dataFrom.laserScanRaw().empty() && (_filtersEnabled & 1)) { int pointsBeforeFiltering = dataFrom.laserScanRaw().size(); LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(), @@ -399,7 +404,7 @@ Transform RegistrationIcp::computeTransformationImpl( float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering); maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio); } - if(!dataTo.laserScanRaw().empty()) + if(!dataTo.laserScanRaw().empty() && (_filtersEnabled & 2)) { int pointsBeforeFiltering = dataTo.laserScanRaw().size(); LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(), @@ -649,7 +654,8 @@ Transform RegistrationIcp::computeTransformationImpl( _maxCorrespondenceDistance, _maxRotation, variance, - correspondences); + correspondences, + _reciprocalCorrespondences); } } //////////////////// @@ -840,7 +846,8 @@ Transform RegistrationIcp::computeTransformationImpl( toCloud, _maxCorrespondenceDistance, variance, - correspondences); + correspondences, + _reciprocalCorrespondences); } } // END Registration PointToPLane to PointToPoint UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks()); diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index 4f39149ea3..56cbf1195e 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -69,7 +70,10 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration _PnPReprojError(Parameters::defaultVisPnPReprojError()), _PnPFlags(Parameters::defaultVisPnPFlags()), _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()), + _PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()), _PnPMaxVar(Parameters::defaultVisPnPMaxVariance()), + _PnPSplitLinearCovarianceComponents(Parameters::defaultVisPnPSplitLinearCovComponents()), + _multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()), _correspondencesApproach(Parameters::defaultVisCorType()), _flowWinSize(Parameters::defaultVisCorFlowWinSize()), _flowIterations(Parameters::defaultVisCorFlowIterations()), @@ -125,7 +129,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError); Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags); Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations); + Parameters::parse(parameters, Parameters::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio); Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar); + Parameters::parse(parameters, Parameters::kVisPnPSplitLinearCovComponents(), _PnPSplitLinearCovarianceComponents); + Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy); Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach); Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize); Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations); @@ -290,6 +297,7 @@ Transform RegistrationVis::computeTransformationImpl( UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError); UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags); UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar); + UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents); UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach); UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize); UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations); @@ -484,6 +492,7 @@ Transform RegistrationVis::computeTransformationImpl( if(!imageFrom.empty() && !imageTo.empty()) { + UASSERT(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty()); std::vector cornersFrom; cv::KeyPoint::convert(kptsFrom, cornersFrom); std::vector cornersTo; @@ -506,7 +515,48 @@ Transform RegistrationVis::computeTransformationImpl( } else { - UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored..."); + UTimer t; + int nCameras = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels().size():toSignature.sensorData().stereoCameraModels().size(); + cornersTo = cornersFrom; + // compute inverse transforms one time + std::vector inverseTransforms(nCameras); + for(int c=0; c %s", c, inverseTransforms[c].prettyPrint().c_str()); + } + // Project 3D points in each camera + int inFrame = 0; + UASSERT(kptsFrom3D.size() == cornersTo.size()); + int subImageWidth = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].imageWidth():toSignature.sensorData().stereoCameraModels()[0].left().imageWidth(); + UASSERT(subImageWidth>0); + for(size_t i=0; i 0) + { + float u,v; + model.reproject(ptsInCamFrame.x, ptsInCamFrame.y, ptsInCamFrame.z, u, v); + if(model.inFrame(u,v)) + { + cornersTo[i].x = u+model.imageWidth()*c; + cornersTo[i].y = v; + ++inFrame; + break; + } + } + } + } + + UDEBUG("Projected %d/%ld points inside %d cameras (time=%fs)", + inFrame, cornersTo.size(), nCameras, t.ticks()); } } @@ -1048,7 +1098,7 @@ Transform RegistrationVis::computeTransformationImpl( std::vector > dists; float radius = (float)_guessWinSize; // pixels rtflann::Matrix cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2); - index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams()); + index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false)); UASSERT(indices.size() == cornersProjectedMat.rows); UASSERT(descriptorsFrom.cols == descriptorsTo.cols); @@ -1578,17 +1628,20 @@ Transform RegistrationVis::computeTransformationImpl( words3A, wordsB, models, + _multiSamplingPolicy, _minInliers, _iterations, _PnPReprojError, _PnPFlags, _PnPRefineIterations, + _PnPVarMedianRatio, _PnPMaxVar, dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()), words3B, &covariances[dir], &matchesV, - &inliersV); + &inliersV, + _PnPSplitLinearCovarianceComponents); inliers[dir] = inliersV; matches[dir] = matchesV; } @@ -1605,12 +1658,14 @@ Transform RegistrationVis::computeTransformationImpl( _PnPReprojError, _PnPFlags, _PnPRefineIterations, + _PnPVarMedianRatio, _PnPMaxVar, dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()), words3B, &covariances[dir], &matchesV, - &inliersV); + &inliersV, + _PnPSplitLinearCovarianceComponents); inliers[dir] = inliersV; matches[dir] = matchesV; } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 8a3c9182a3..14cf4639e8 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -143,10 +143,13 @@ Rtabmap::Rtabmap() : _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()), _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()), _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()), + _forceOdom3doF(Parameters::defaultRGBDForceOdom3DoF()), _restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()), _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()), _loopGPS(Parameters::defaultRtabmapLoopGPS()), _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()), + _localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()), + _localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())), _createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()), _markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()), _markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()), @@ -614,10 +617,16 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations); Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity); Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity); + Parameters::parse(parameters, Parameters::kRGBDForceOdom3DoF(), _forceOdom3doF); Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin); Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited); Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS); Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize); + Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing); + double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError(); + Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError); + UASSERT(localizationPriorError>0.0); + _localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError); Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap); Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance); @@ -849,7 +858,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose) if(!_memory->isIncremental()) { _lastLocalizationPose = initialPose; - _localizationCovariance = 0; + _localizationCovariance = cv::Mat(); _lastLocalizationNodeId = 0; _odomCachePoses.clear(); _odomCacheConstraints.clear(); @@ -1087,6 +1096,13 @@ void Rtabmap::resetMemory() { UERROR("RTAB-Map is not initialized. No memory to reset..."); } + + if(_graphOptimizer) + { + delete _graphOptimizer; + _graphOptimizer = Optimizer::create(_parameters); + } + this->setupLogFiles(true); } @@ -1170,6 +1186,7 @@ bool Rtabmap::process( double timeMemoryUpdate = 0; double timeNeighborLinkRefining = 0; double timeProximityByTimeDetection = 0; + double timeProximityBySpaceSearch = 0; double timeProximityBySpaceVisualDetection = 0; double timeProximityBySpaceDetection = 0; double timeCleaningNeighbors = 0; @@ -1237,6 +1254,12 @@ bool Rtabmap::process( { if(!odomPose.isNull()) { + // If we are doing 2D mapping, make sure the pose is 3DoF so that landmark logic works. + if(_forceOdom3doF && _graphOptimizer->isSlam2d() && !odomPose.is3DoF()) + { + odomPose = odomPose.to3DoF(); + } + // this will make sure that all inverse operations will work! if(!odomPose.isInvertible()) { @@ -1331,6 +1354,13 @@ bool Rtabmap::process( { iter->second = mapCorrectionInv * iter->second; } + + std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); + _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose); + UWARN("Transformed map accordingly to last localization pose saved in database (%s=true)! nearest id = %d of last pose = %s", + Parameters::kRGBDOptimizeFromGraphEnd().c_str(), + _lastLocalizationNodeId, + _lastLocalizationPose.prettyPrint().c_str()); } } @@ -1435,8 +1465,10 @@ bool Rtabmap::process( float angleToClosestNodeInTheGraph = 0; if(_rgbdSlamMode) { - statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at(0,0)); - statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at(5,5)); + double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at(0,0), odomCovariance.at(1,1)>=9999?0:odomCovariance.at(1,1), odomCovariance.at(2,2)>=9999?0:odomCovariance.at(2,2)); + double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at(3,3)>=9999?0:odomCovariance.at(3,3), odomCovariance.at(4,4)>=9999?0:odomCovariance.at(4,4), odomCovariance.at(5,5)); + statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar); + statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar); //Verify if there was a rehearsal int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f); @@ -1665,7 +1697,10 @@ bool Rtabmap::process( Link tmp = signature->getLinks().begin()->second.inverse(); - _distanceTravelled += tmp.transform().getNorm(); + if(!smallDisplacement) + { + _distanceTravelled += tmp.transform().getNorm(); + } // if the previous node is an intermediate node, remove it from the local graph if(_constraints.size() && @@ -1690,11 +1725,11 @@ bool Rtabmap::process( odomCovariance.type() == CV_64FC1 && odomCovariance.at(0,0) < 1) { - if(_localizationCovariance.empty() || _lastLocalizationPose.isNull()) + if( _memory->isIncremental() && _localizationCovariance.empty()) { - _localizationCovariance = odomCovariance.clone(); + _localizationCovariance = cv::Mat::zeros(6,6,CV_64FC1); } - else + if(_localizationCovariance.total() == 36) { #ifdef RTABMAP_MRPT // Transform odometry covariance (which in base frame) into global frame @@ -1712,7 +1747,6 @@ bool Rtabmap::process( // build rtabmap with MRPT to use approach above. _localizationCovariance += odomCovariance; #endif - } } _lastLocalizationPose = newPose; // keep in cache the latest corrected pose @@ -1722,7 +1756,10 @@ bool Rtabmap::process( if(!_odomCachePoses.empty()) { float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm(); - _distanceTravelled += odomDistance; + if(!smallDisplacement) + { + _distanceTravelled += odomDistance; + } while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize) { @@ -1854,9 +1891,46 @@ bool Rtabmap::process( //============================================================ // Bayes filter update //============================================================ - int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0; + bool localizationOnPreviousUpdate = false; + if(_memory->isIncremental()) + { + localizationOnPreviousUpdate = + signature->getLinks().size() && + signature->getLinks().begin()->first!=signature->id() && + _memory->getLoopClosureLinks(signature->getLinks().begin()->first, false).size() != 0; + } + else + { + // localization mode + + // Count how many localization links are in the constraints + int localizationLinks = 0; + int previousIdWithLocalizationLink = 0; + for(std::multimap::iterator iter=_odomCacheConstraints.begin(); + iter!=_odomCacheConstraints.end(); ++iter) + { + if(previousIdWithLocalizationLink == iter->first) + { + // ignore links with node already counted + continue; + } + if(iter->second.type() == Link::kGlobalClosure || + iter->second.type() == Link::kLocalSpaceClosure || + iter->second.type() == Link::kLocalTimeClosure || + iter->second.type() == Link::kUserClosure || + iter->second.type() == Link::kNeighborMerged || + iter->second.type() == Link::kLandmark) + { + ++localizationLinks; + previousIdWithLocalizationLink = iter->first; + } + } + + localizationOnPreviousUpdate = localizationLinks > 1; // need two links in case we have delayed localization + } + // Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement - if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement) + if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement) { // If the working memory is empty, don't do the detection. It happens when it // is the first time the detector is started (there needs some images to @@ -2518,7 +2592,8 @@ bool Rtabmap::process( { if(_startNewMapOnLoopClosure && _memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place) - graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session) + _localizationCovariance.empty() && // if we didn't localize yet + graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session { UINFO("Proximity detection by space disabled as if we force to have a global loop " "closure with previous map before doing proximity detections (%s=true).", @@ -2535,7 +2610,7 @@ bool Rtabmap::process( // don't do it if it is a small displacement unless the previous signature didn't have a loop closure // don't do it if there is a too fast movement - if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement) + if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement) { //============================================================ @@ -2546,22 +2621,39 @@ bool Rtabmap::process( // 1) compare visually with nearest locations // UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius); - std::map nearestIds; - if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) - { - nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth); - } - else - { - nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); - } + std::map nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size()); std::map nearestPoses; + std::multimap links; + if(_memory->isIncremental() && _proximityMaxGraphDepth>0) + { + // get bidirectional links + for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter) + { + if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to())) + { + links.insert(std::make_pair(iter->second.from(), iter->second.to())); + links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> + } + } + } for(std::map::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter) { if(_memory->getStMem().find(iter->first) == _memory->getStMem().end()) { - nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) + { + std::list > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first); + UDEBUG("Graph depth to %d = %ld", iter->first, path.size()); + if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth) + { + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + } + } + else + { + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + } } } UDEBUG("nearestPoses=%d", (int)nearestPoses.size()); @@ -2593,6 +2685,9 @@ bool Rtabmap::process( } UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths); + timeProximityBySpaceSearch = timer.ticks(); + ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch); + float proximityFilteringRadius = _proximityFilteringRadius; if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistancehasLink(nearestId) && proximityFilteringRadius>0.0f) + { + UDEBUG("Skipping path %d as most likely ID %d is too far %f > %f (%s)", + iter->first.id, + nearestId, + _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)), + proximityFilteringRadius, + Parameters::kRGBDProximityPathFilteringRadius().c_str()); + } } } @@ -2937,9 +3041,10 @@ bool Rtabmap::process( { // Make the new one the parent of the old one UASSERT(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0); + + loopClosureLinearVariance = uMax3(info.covariance.at(0,0), info.covariance.at(1,1)>=9999?0:info.covariance.at(1,1), info.covariance.at(2,2)>=9999?0:info.covariance.at(2,2)); + loopClosureAngularVariance = uMax3(info.covariance.at(3,3)>=9999?0:info.covariance.at(3,3), info.covariance.at(4,4)>=9999?0:info.covariance.at(4,4), info.covariance.at(5,5)); cv::Mat information = getInformation(info.covariance); - loopClosureLinearVariance = 1.0/information.at(0,0); - loopClosureAngularVariance = 1.0/information.at(5,5); rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information)); if(!rejectedGlobalLoopClosure) { @@ -2965,7 +3070,7 @@ bool Rtabmap::process( // Landmark //============================================================ std::map > landmarksDetected; // - if(!signature->getLandmarks().empty()) + if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored()) { bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0; UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0); @@ -2992,6 +3097,7 @@ bool Rtabmap::process( UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin()); landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second)); rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first)); } } } @@ -3120,6 +3226,7 @@ bool Rtabmap::process( { constraints.insert(std::make_pair(iter->second.from(), iter->second)); } + cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { std::map::iterator iterPose = _optimizedPoses.find(iter->second.to()); @@ -3127,26 +3234,37 @@ bool Rtabmap::process( { poses.insert(*iterPose); // make the poses in the map fixed - constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000))); - UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str()); + constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); + UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf); } - UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); - } - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str()); + UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); } - std::map posesOut; std::multimap edgeConstraintsOut; bool priorsIgnored = _graphOptimizer->priorsIgnored(); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. - _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); + _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); + if(ULogger::level() == ULogger::kDebug) + { + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str()); + } + } cv::Mat locOptCovariance; - std::map optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + std::map optPoses; + if(!posesOut.empty() && + posesOut.begin()->first < _odomCachePoses.begin()->first) + { + optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { @@ -3158,7 +3276,7 @@ bool Rtabmap::process( UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); const Link * maxLinearLink = 0; @@ -3173,10 +3291,10 @@ bool Rtabmap::process( &maxLinearLink, &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0) + if(maxLinearLink == 0 && maxAngularLink==0) { - UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); - optPoses = posesOut; + UWARN("Could not compute graph errors! Rejecting localization!"); + rejectLocalization = true; } if(maxLinearLink) @@ -3188,7 +3306,7 @@ bool Rtabmap::process( maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()), _optimizationMaxError); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3207,6 +3325,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { @@ -3217,7 +3348,7 @@ bool Rtabmap::process( maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()), _optimizationMaxError); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3236,6 +3367,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } @@ -3259,8 +3403,17 @@ bool Rtabmap::process( UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. - _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); - optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); + optPoses.clear(); + if(!posesOut.empty() && + posesOut.begin()->first < _odomCachePoses.begin()->first) + { + optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { @@ -3272,7 +3425,7 @@ bool Rtabmap::process( UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); const Link * maxLinearLink = 0; @@ -3287,10 +3440,10 @@ bool Rtabmap::process( &maxLinearLink, &maxAngularLink, _graphOptimizer->isSlam2d()); - if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0) + if(maxLinearLink == 0 && maxAngularLink==0) { - UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); - optPoses = posesOut; + UWARN("Could not compute graph errors! Rejecting localization!"); + rejectLocalization = true; } if(maxLinearLink) @@ -3302,7 +3455,7 @@ bool Rtabmap::process( maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()), _optimizationMaxError); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3321,6 +3474,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { @@ -3331,7 +3497,7 @@ bool Rtabmap::process( maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()), _optimizationMaxError); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3350,6 +3516,19 @@ bool Rtabmap::process( _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } } @@ -3392,19 +3571,31 @@ bool Rtabmap::process( } // update localization links + UASSERT(uContains(optPoses, signature->id())); Transform newOptPoseInv = optPoses.at(signature->id()).inverse(); for(std::multimap::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter) { - Transform newT = newOptPoseInv * optPoses.at(iter->first); - UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); - UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); - UDEBUG(" to %s", newT.prettyPrint().c_str()); - iter->second.setTransform(newT); - - // Update link in the referred signatures - if(iter->first > 0) - _memory->updateLink(iter->second, false); - + if(!_localizationSmoothing) + { + // Add original link without optimization + UDEBUG("Adding new odom cache constraint %d->%d (%s)", + iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str()); + } + else + { + // Adjust with optimized poses, this will smooth the localization + UASSERT(uContains(optPoses, iter->first)); + Transform newT = newOptPoseInv * optPoses.at(iter->first); + UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to()); + UDEBUG("from %s", iter->second.transform().prettyPrint().c_str()); + UDEBUG(" to %s", newT.prettyPrint().c_str()); + iter->second.setTransform(newT); + + // Update link in the referred signatures + if(iter->first > 0) + _memory->updateLink(iter->second, false); + } + _odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second)); } @@ -3576,7 +3767,6 @@ bool Rtabmap::process( rejectedLandmark = true; } else if(_memory->isIncremental() && - _optimizationMaxError > 0.0f && loopClosureLinksAdded.size() && optimizationIterations > 0 && constraints.size()) @@ -3602,7 +3792,7 @@ bool Rtabmap::process( if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3622,11 +3812,24 @@ bool Rtabmap::process( _optimizationMaxError); reject = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -3646,6 +3849,19 @@ bool Rtabmap::process( _optimizationMaxError); reject = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(reject) @@ -3900,6 +4116,7 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000); + statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000); statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000); statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000); @@ -3977,15 +4194,16 @@ bool Rtabmap::process( ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); } - Signature lastSignatureData(signature->id()); + Signature lastSignatureData = *signature; Transform lastSignatureLocalizedPose; if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) { lastSignatureLocalizedPose = _optimizedPoses.at(signature->id()); } - if(_publishLastSignatureData) + if(!_publishLastSignatureData) { - lastSignatureData = *signature; + lastSignatureData.sensorData().clearCompressedData(); + lastSignatureData.sensorData().clearRawData(); } if(!_rawDataKept) { @@ -4133,6 +4351,7 @@ bool Rtabmap::process( } else { + UDEBUG("Clearing _lastLocalizationNodeId(%d)", _lastLocalizationNodeId); _lastLocalizationNodeId = 0; } } @@ -4268,96 +4487,73 @@ bool Rtabmap::process( poses = _optimizedPoses; constraints = _constraints; } - UDEBUG(""); - if(_publishLastSignatureData) - { - UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1); + UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1); statistics_.addSignatureData(lastSignatureData); - if(_nodesToRepublish.size()) - { - std::multimap missingIds; + if(_nodesToRepublish.size()) + { + std::multimap missingIds; - // priority to loopId - int tmpId = loopId>0?loopId:_highestHypothesis.first; - if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) - { - missingIds.insert(std::make_pair(-1, tmpId)); - } + // priority to loopId + int tmpId = loopId>0?loopId:_highestHypothesis.first; + if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) + { + missingIds.insert(std::make_pair(-1, tmpId)); + } - if(!_lastLocalizationPose.isNull()) + if(!_lastLocalizationPose.isNull()) + { + // Republish data from closest nodes of the current localization + std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); + int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose); + if(id>0) { - // Republish data from closest nodes of the current localization - std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); - int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose); - if(id>0) + std::map ids = _memory->getNeighborsId(id, 0, 0, true, false, true); + for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) { - std::map ids = _memory->getNeighborsId(id, 0, 0, true, false, true); - for(std::map::iterator iter=ids.begin(); iter!=ids.end(); ++iter) + if(iter->first != loopId && + _nodesToRepublish.find(iter->first) != _nodesToRepublish.end()) { - if(iter->first != loopId && - _nodesToRepublish.find(iter->first) != _nodesToRepublish.end()) - { - missingIds.insert(std::make_pair(iter->second, iter->first)); - } + missingIds.insert(std::make_pair(iter->second, iter->first)); } + } - if(_nodesToRepublish.size() != missingIds.size()) + if(_nodesToRepublish.size() != missingIds.size()) + { + // remove requested nodes not anymore in the graph + for(std::set::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();) { - // remove requested nodes not anymore in the graph - for(std::set::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();) + if(ids.find(*iter) == ids.end()) { - if(ids.find(*iter) == ids.end()) - { - iter = _nodesToRepublish.erase(iter); - } - else - { - ++iter; - } + iter = _nodesToRepublish.erase(iter); + } + else + { + ++iter; } } } } + } - int loaded = 0; - std::stringstream stream; - for(std::multimap::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) - { - statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true)); - _nodesToRepublish.erase(iter->second); - ++loaded; - stream << iter->second << " "; - } - if(loaded) - { - UWARN("Republishing data of requested node(s) %s(%s=%d)", - stream.str().c_str(), - Parameters::kRtabmapMaxRepublished().c_str(), - _maxRepublished); - } + int loaded = 0; + std::stringstream stream; + for(std::multimap::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) + { + statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true)); + _nodesToRepublish.erase(iter->second); + ++loaded; + stream << iter->second << " "; } - } - else - { - // only copy node info - Signature nodeInfo( - lastSignatureData.id(), - lastSignatureData.mapId(), - lastSignatureData.getWeight(), - lastSignatureData.getStamp(), - lastSignatureData.getLabel(), - lastSignatureData.getPose(), - lastSignatureData.getGroundTruthPose()); - const std::vector & v = lastSignatureData.getVelocity(); - if(v.size() == 6) + if(loaded) { - nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]); + UWARN("Republishing data of requested node(s) %s(%s=%d)", + stream.str().c_str(), + Parameters::kRtabmapMaxRepublished().c_str(), + _maxRepublished); } - nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps()); - nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors()); - statistics_.addSignatureData(nodeInfo); } + UDEBUG(""); localGraphSize = (int)poses.size(); if(!lastSignatureLocalizedPose.isNull()) @@ -5495,107 +5691,131 @@ int Rtabmap::detectMoreLoopClosures( if(!t.isNull()) { bool updateConstraints = true; - if(_optimizationMaxError > 0.0f) - { - //optimize the graph to see if the new constraint is globally valid - int fromId = from; - int mapId = signatures.at(from).mapId(); - // use first node of the map containing from - for(std::map::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) + //optimize the graph to see if the new constraint is globally valid + + int fromId = from; + int mapId = signatures.at(from).mapId(); + // use first node of the map containing from + for(std::map::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) + { + if(ster->second.mapId() == mapId) { - if(ster->second.mapId() == mapId) - { - fromId = ster->first; - break; - } + fromId = ster->first; + break; } - std::multimap linksIn = links; - linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance)))); - const Link * maxLinearLink = 0; - const Link * maxAngularLink = 0; - float maxLinearError = 0.0f; - float maxAngularError = 0.0f; - float maxLinearErrorRatio = 0.0f; - float maxAngularErrorRatio = 0.0f; - std::map optimizedPoses; - std::multimap links; - UASSERT(poses.find(fromId) != poses.end()); - UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); - UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); - _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links); - UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end()); - UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str()); - UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str()); - UASSERT(graph::findLink(links, from, to) != links.end()); - optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links); - std::string msg; - if(optimizedPoses.size()) + } + std::multimap linksIn = links; + linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance)))); + const Link * maxLinearLink = 0; + const Link * maxAngularLink = 0; + float maxLinearError = 0.0f; + float maxAngularError = 0.0f; + float maxLinearErrorRatio = 0.0f; + float maxAngularErrorRatio = 0.0f; + std::map optimizedPoses; + std::multimap links; + UASSERT(poses.find(fromId) != poses.end()); + UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str()); + UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str()); + _graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links); + UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end()); + UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str()); + UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str()); + UASSERT(graph::findLink(links, from, to) != links.end()); + optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links); + std::string msg; + if(optimizedPoses.size()) + { + graph::computeMaxGraphErrors( + optimizedPoses, + links, + maxLinearErrorRatio, + maxAngularErrorRatio, + maxLinearError, + maxAngularError, + &maxLinearLink, + &maxAngularLink); + if(maxLinearLink) { - graph::computeMaxGraphErrors( - optimizedPoses, - links, - maxLinearErrorRatio, - maxAngularErrorRatio, - maxLinearError, - maxAngularError, - &maxLinearLink, - &maxAngularLink); - if(maxLinearLink) + UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { - UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); - if(maxLinearErrorRatio > _optimizationMaxError) - { - msg = uFormat("Rejecting edge %d->%d because " - "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " - "\"%s\" is %f.", - from, - to, - maxLinearError, - maxLinearLink->from(), - maxLinearLink->to(), - maxLinearErrorRatio, - sqrt(maxLinearLink->transVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - } + msg = uFormat("Rejecting edge %d->%d because " + "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " + "\"%s\" is %f.", + from, + to, + maxLinearError, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearErrorRatio, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); } - else if(maxAngularLink) + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) { - UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); - if(maxAngularErrorRatio > _optimizationMaxError) - { - msg = uFormat("Rejecting edge %d->%d because " - "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " - "\"%s\" is %f m.", - from, - to, - maxAngularError*180.0f/M_PI, - maxAngularLink->from(), - maxAngularLink->to(), - maxAngularErrorRatio, - sqrt(maxAngularLink->rotVariance()), - Parameters::kRGBDOptimizeMaxError().c_str(), - _optimizationMaxError); - } + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); } } - else - { - msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", - from, - to); - } - if(!msg.empty()) + else if(maxAngularLink) { - UWARN("%s", msg.c_str()); - updateConstraints = false; - } - else - { - poses = optimizedPoses; + UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) + { + msg = uFormat("Rejecting edge %d->%d because " + "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " + "\"%s\" is %f m.", + from, + to, + maxAngularError*180.0f/M_PI, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularErrorRatio, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str(), + _optimizationMaxError); + } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } + else + { + msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", + from, + to); + } + if(!msg.empty()) + { + UWARN("%s", msg.c_str()); + updateConstraints = false; + } + else + { + poses = optimizedPoses; + } if(updateConstraints) { @@ -5877,7 +6097,7 @@ bool Rtabmap::addLink(const Link & link) { msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to()); } - else if(_optimizationMaxError > 0.0f) + else { float maxLinearError = 0.0f; float maxLinearErrorRatio = 0.0f; @@ -5898,7 +6118,7 @@ bool Rtabmap::addLink(const Link & link) if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " @@ -5913,11 +6133,24 @@ bool Rtabmap::addLink(const Link & link) Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } else if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { msg = uFormat("Rejecting edge %d->%d because " "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " @@ -5932,6 +6165,19 @@ bool Rtabmap::addLink(const Link & link) Parameters::kRGBDOptimizeMaxError().c_str(), _optimizationMaxError); } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } if(!msg.empty()) @@ -6036,7 +6282,7 @@ bool Rtabmap::addLink(const Link & link) UWARN("Optimization failed, rejecting localization!"); rejectLocalization = true; } - else if(_optimizationMaxError > 0.0f) + else { UINFO("Compute max graph errors..."); float maxLinearError = 0.0f; @@ -6063,7 +6309,7 @@ bool Rtabmap::addLink(const Link & link) if(maxLinearLink) { UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); - if(maxLinearErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6082,11 +6328,24 @@ bool Rtabmap::addLink(const Link & link) _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxLinearErrorRatio, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + maxLinearError, + sqrt(maxLinearLink->transVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } if(maxAngularLink) { UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); - if(maxAngularErrorRatio > _optimizationMaxError) + if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError) { UWARN("Rejecting localization (%d <-> %d) in this " "iteration because a wrong loop closure has been " @@ -6105,6 +6364,19 @@ bool Rtabmap::addLink(const Link & link) _optimizationMaxError); rejectLocalization = true; } + else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100 && !_graphOptimizer->isRobust()) + { + UERROR("Huge optimization error detected!" + "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider " + "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!", + maxAngularErrorRatio, + maxAngularLink->from(), + maxAngularLink->to(), + maxAngularLink->type(), + maxAngularError*180.0f/CV_PI, + sqrt(maxAngularLink->rotVariance()), + Parameters::kRGBDOptimizeMaxError().c_str()); + } } } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 13b0c4fdda..d9f227b59f 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/Camera.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/ParamEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/core/UserDataEvent.h" @@ -371,11 +371,11 @@ bool RtabmapThread::handleEvent(UEvent* event) // IMU events are published at high frequency, early exit return false; } - else if(event->getClassName().compare("CameraEvent") == 0) + else if(event->getClassName().compare("SensorEvent") == 0) { - UDEBUG("CameraEvent"); - CameraEvent * e = (CameraEvent*)event; - if(e->getCode() == CameraEvent::kCodeData) + UDEBUG("SensorEvent"); + SensorEvent * e = (SensorEvent*)event; + if(e->getCode() == SensorEvent::kCodeData) { if (_rtabmap->isRGBDMode()) { diff --git a/corelib/src/SensorCapture.cpp b/corelib/src/SensorCapture.cpp new file mode 100644 index 0000000000..59327878c2 --- /dev/null +++ b/corelib/src/SensorCapture.cpp @@ -0,0 +1,114 @@ +/* +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. +*/ + +#include "rtabmap/core/SensorCapture.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace rtabmap +{ + +SensorCapture::SensorCapture(float frameRate, const Transform & localTransform) : + _frameRate(frameRate), + _localTransform(localTransform), + _frameRateTimer(new UTimer()), + _seq(0) +{ +} + +SensorCapture::~SensorCapture() +{ + delete _frameRateTimer; +} + +void SensorCapture::resetTimer() +{ + _frameRateTimer->start(); +} + +SensorData SensorCapture::takeData(SensorCaptureInfo * info) +{ + bool warnFrameRateTooHigh = false; + float actualFrameRate = 0; + float frameRate = _frameRate; + if(frameRate>0) + { + int sleepTime = (1000.0f/frameRate - 1000.0f*_frameRateTimer->getElapsedTime()); + if(sleepTime > 2) + { + uSleep(sleepTime-2); + } + else if(sleepTime < 0) + { + warnFrameRateTooHigh = true; + actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); + } + + // Add precision at the cost of a small overhead + while(_frameRateTimer->getElapsedTime() < 1.0/double(frameRate)-0.000001) + { + // + } + + double slept = _frameRateTimer->getElapsedTime(); + _frameRateTimer->start(); + UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate)); + } + + UTimer timer; + SensorData data = this->captureData(info); + double captureTime = timer.ticks(); + if(warnFrameRateTooHigh) + { + UWARN("Camera: Cannot reach target frame rate %f Hz, current rate is %f Hz and capture time = %f s.", + frameRate, actualFrameRate, captureTime); + } + else + { + UDEBUG("Time capturing data = %fs", captureTime); + } + if(info) + { + info->id = data.id(); + info->stamp = data.stamp(); + info->timeCapture = captureTime; + } + return data; +} + +} // namespace rtabmap diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp new file mode 100644 index 0000000000..bac9c3aa5f --- /dev/null +++ b/corelib/src/SensorCaptureThread.cpp @@ -0,0 +1,1041 @@ +/* +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. +*/ + +#include "rtabmap/core/SensorCaptureThread.h" +#include "rtabmap/core/Camera.h" +#include "rtabmap/core/Lidar.h" +#include "rtabmap/core/SensorEvent.h" +#include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_surface.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/StereoDense.h" +#include "rtabmap/core/DBReader.h" +#include "rtabmap/core/IMUFilter.h" +#include "rtabmap/core/Features2d.h" +#include "rtabmap/core/clams/discrete_depth_distortion_model.h" +#include +#include +#include +#include +#include + +#include + +namespace rtabmap +{ + +// ownership transferred +SensorCaptureThread::SensorCaptureThread( + Camera * camera, + const ParametersMap & parameters) : + SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters) +{ + UASSERT(camera != 0); +} + +// ownership transferred +SensorCaptureThread::SensorCaptureThread( + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset, + float poseScaleFactor, + double poseWaitTime, + const ParametersMap & parameters) : + SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters) +{ + UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull()); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + const ParametersMap & parameters) : + SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters) +{ + UASSERT(lidar != 0); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + Camera * camera, + const ParametersMap & parameters) : + SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters) +{ + UASSERT(lidar != 0 && camera != 0); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + SensorCapture * odomSensor, + double poseTimeOffset, + float poseScaleFactor, + double poseWaitTime, + const ParametersMap & parameters) : + SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters) +{ + UASSERT(lidar != 0 && odomSensor != 0); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset, + float poseScaleFactor, + double poseWaitTime, + const ParametersMap & parameters) : + _camera(camera), + _odomSensor(odomSensor), + _lidar(lidar), + _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()), + _odomAsGt(false), + _poseTimeOffset(poseTimeOffset), + _poseScaleFactor(poseScaleFactor), + _poseWaitTime(poseWaitTime), + _mirroring(false), + _stereoExposureCompensation(false), + _colorOnly(false), + _imageDecimation(1), + _histogramMethod(0), + _stereoToDepth(false), + _scanDeskewing(false), + _scanFromDepth(false), + _scanDownsampleStep(1), + _scanRangeMin(0.0f), + _scanRangeMax(0.0f), + _scanVoxelSize(0.0f), + _scanNormalsK(0), + _scanNormalsRadius(0.0f), + _scanForceGroundNormalsUp(false), + _stereoDense(StereoDense::create(parameters)), + _distortionModel(0), + _bilateralFiltering(false), + _bilateralSigmaS(10), + _bilateralSigmaR(0.1), + _imuFilter(0), + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) +{ + UASSERT(_camera != 0 || _lidar != 0); + if(_lidar && _camera) + { + _camera->setFrameRate(0); + } + if(_odomSensor) + { + if(_camera) + { + if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull()) + { + _extrinsicsOdomToCamera.setIdentity(); + } + UASSERT(!_extrinsicsOdomToCamera.isNull()); + UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); + } + UDEBUG("_poseTimeOffset =%f", _poseTimeOffset); + UDEBUG("_poseScaleFactor =%f", _poseScaleFactor); + UDEBUG("_poseWaitTime =%f", _poseWaitTime); + } +} + +SensorCaptureThread::~SensorCaptureThread() +{ + join(true); + if(_odomSensor != _camera && _odomSensor != _lidar) + { + delete _odomSensor; + } + delete _camera; + delete _lidar; + delete _distortionModel; + delete _stereoDense; + delete _imuFilter; + delete _featureDetector; +} + +void SensorCaptureThread::setFrameRate(float frameRate) +{ + if(_lidar) + { + _lidar->setFrameRate(frameRate); + } + else if(_camera) + { + _camera->setFrameRate(frameRate); + } +} + +void SensorCaptureThread::setDistortionModel(const std::string & path) +{ + if(_distortionModel) + { + delete _distortionModel; + _distortionModel = 0; + } + if(!path.empty()) + { + _distortionModel = new clams::DiscreteDepthDistortionModel(); + _distortionModel->load(path); + if(!_distortionModel->isValid()) + { + UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str()); + delete _distortionModel; + _distortionModel = 0; + } + } +} + +void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR) +{ + UASSERT(sigmaS > 0.0f && sigmaR > 0.0f); + _bilateralFiltering = true; + _bilateralSigmaS = sigmaS; + _bilateralSigmaR = sigmaR; +} + +void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) +{ + delete _imuFilter; + _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters); + _imuBaseFrameConversion = baseFrameConversion; +} + +void SensorCaptureThread::disableIMUFiltering() +{ + delete _imuFilter; + _imuFilter = 0; +} + +void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters) +{ + delete _featureDetector; + ParametersMap params = parameters; + ParametersMap defaultParams = Parameters::getDefaultParameters("Vis"); + uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType())))); + uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures())))); + uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth())))); + uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth())))); + uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations())))); + uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize())))); + uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows())))); + uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols())))); + _featureDetector = Feature2D::create(params); + _depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask); +} +void SensorCaptureThread::disableFeatureDetection() +{ + delete _featureDetector; + _featureDetector = 0; +} + +void SensorCaptureThread::setScanParameters( + bool fromDepth, + int downsampleStep, + float rangeMin, + float rangeMax, + float voxelSize, + int normalsK, + float normalsRadius, + bool forceGroundNormalsUp, + bool deskewing) +{ + setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing); +} + +void SensorCaptureThread::setScanParameters( + bool fromDepth, + int downsampleStep, // decimation of the depth image in case the scan is from depth image + float rangeMin, + float rangeMax, + float voxelSize, + int normalsK, + float normalsRadius, + float groundNormalsUp, + bool deskewing) +{ + _scanDeskewing = deskewing; + _scanFromDepth = fromDepth; + _scanDownsampleStep=downsampleStep; + _scanRangeMin = rangeMin; + _scanRangeMax = rangeMax; + _scanVoxelSize = voxelSize; + _scanNormalsK = normalsK; + _scanNormalsRadius = normalsRadius; + _scanForceGroundNormalsUp = groundNormalsUp; +} + +bool SensorCaptureThread::odomProvided() const +{ + if(_odomAsGt) + { + return false; + } + return _odomSensor != 0; +} + +void SensorCaptureThread::mainLoopBegin() +{ + ULogger::registerCurrentThread("Camera"); + if(_lidar) + { + _lidar->resetTimer(); + } + else if(_camera) + { + _camera->resetTimer(); + } + if(_imuFilter) + { + // In case we paused the camera and moved somewhere else, restart filtering. + _imuFilter->reset(); + } +} + +void SensorCaptureThread::mainLoop() +{ + UASSERT(_lidar || _camera); + UTimer totalTime; + SensorCaptureInfo info; + SensorData data; + SensorData cameraData; + double lidarStamp = 0.0; + double cameraStamp = 0.0; + if(_lidar) + { + data = _lidar->takeData(&info); + if(data.stamp() == 0.0) + { + UERROR("Could not capture scan! Skipping this frame!"); + return; + } + else + { + lidarStamp = data.stamp(); + if(_camera) + { + cameraData = _camera->takeData(); + if(cameraData.stamp() == 0.0) + { + UERROR("Could not capture image! Skipping this frame!"); + return; + } + else + { + double stampStart = UTimer::now(); + while(cameraData.stamp() < data.stamp() && + !isKilled() && + UTimer::now() - stampStart < _poseWaitTime && + !cameraData.imageRaw().empty()) + { + // Make sure the camera frame is newer than lidar frame so + // that if there are imus published by the cameras, we can get + // them all in odometry before deskewing. + cameraData = _camera->takeData(); + } + + cameraStamp = cameraData.stamp(); + if(cameraData.stamp() < data.stamp()) + { + UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.", + cameraData.stamp(), + data.stamp(), + _poseWaitTime); + } + + if(!cameraData.stereoCameraModels().empty()) + { + data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true); + } + else + { + data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true); + } + } + } + } + } + else if(_camera) + { + data = _camera->takeData(&info); + if(data.stamp() == 0.0) + { + UERROR("Could not capture image! Skipping this frame!"); + return; + } + else + { + cameraStamp = cameraData.stamp(); + } + } + + if(_odomSensor && data.stamp() != 0.0) + { + if(lidarStamp!=0.0 && _scanDeskewing) + { + UDEBUG("Deskewing begin"); + if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime()) + { + float scanTime = + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] - + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + + Transform poseFirstScan; + Transform poseLastScan; + cv::Mat cov; + double firstStamp = data.stamp() + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + double lastStamp = data.stamp() + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]; + if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) && + _odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0)) + { + if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f) + { + poseFirstScan.x() *= _poseScaleFactor; + poseFirstScan.y() *= _poseScaleFactor; + poseFirstScan.z() *= _poseScaleFactor; + poseLastScan.x() *= _poseScaleFactor; + poseLastScan.y() *= _poseScaleFactor; + poseLastScan.z() *= _poseScaleFactor; + } + + UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull()); + + Transform transform = poseFirstScan.inverse() * poseLastScan; + + // convert to velocity + float x,y,z,roll,pitch,yaw; + transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); + x/=scanTime; + y/=scanTime; + z/=scanTime; + roll /= scanTime; + pitch /= scanTime; + yaw /= scanTime; + + Transform velocity(x,y,z,roll,pitch,yaw); + UTimer timeDeskewing; + LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity); + info.timeDeskewing = timeDeskewing.ticks(); + if(!scanDeskewed.isEmpty()) + { + data.setLaserScan(scanDeskewed); + } + } + else + { + UWARN("Failed to get poses for stamps %f and %f! Skipping this frame!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset); + return; + } + } + else if(!data.laserScanRaw().empty()) + { + UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str()); + } + UDEBUG("Deskewing end"); + } + + Transform pose; + cv::Mat covariance; + if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0)) + { + info.odomPose = pose; + info.odomCovariance = covariance; + if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f) + { + info.odomPose.x() *= _poseScaleFactor; + info.odomPose.y() *= _poseScaleFactor; + info.odomPose.z() *= _poseScaleFactor; + } + + if(cameraStamp != 0.0) + { + Transform cameraCorrection = Transform::getIdentity(); + if(lidarStamp > 0.0 && lidarStamp != cameraStamp) + { + if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0)) + { + cameraCorrection = info.odomPose.inverse() * pose; + } + else + { + UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp); + } + } + + // Adjust local transform of the camera based on the pose frame + if(!data.cameraModels().empty()) + { + UASSERT(data.cameraModels().size()==1); + CameraModel model = data.cameraModels()[0]; + model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); + data.setCameraModel(model); + } + else if(!data.stereoCameraModels().empty()) + { + UASSERT(data.stereoCameraModels().size()==1); + StereoCameraModel model = data.stereoCameraModels()[0]; + model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); + data.setStereoCameraModel(model); + } + } + + // Fake IMU to intialize gravity (assuming pose is aligned with gravity!) + Eigen::Quaterniond q = info.odomPose.getQuaterniond(); + data.setIMU(IMU( + cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), + cv::Vec3d(), cv::Mat(), + cv::Vec3d(), cv::Mat(), + Transform::getIdentity())); + this->disableIMUFiltering(); + } + else + { + UWARN("Could not get pose at stamp %f", data.stamp()); + } + } + + if(_odomAsGt && !info.odomPose.isNull()) + { + data.setGroundTruth(info.odomPose); + info.odomPose.setNull(); + } + + if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set + { + postUpdate(&data, &info); + info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial(); + info.timeTotal = totalTime.ticks(); + this->post(new SensorEvent(data, info)); + } + else if(!this->isKilled()) + { + UWARN("no more data..."); + this->kill(); + this->post(new SensorEvent()); + } +} + +void SensorCaptureThread::mainLoopKill() +{ + if(dynamic_cast(_camera) != 0) + { + int i=20; + while(i-->0) + { + uSleep(100); + if(!this->isKilled()) + { + break; + } + } + if(this->isKilled()) + { + //still in killed state, maybe a deadlock + UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked " + "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. " + "Note that rtabmap should link on libusb of libfreenect2. " + "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\""); + } + + } +} + +void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const +{ + UASSERT(dataPtr!=0); + SensorData & data = *dataPtr; + if(_colorOnly) + { + if(!data.depthRaw().empty()) + { + data.setRGBDImage(data.imageRaw(), cv::Mat(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + std::vector models; + for(size_t i=0; igetWidth() >= data.depthRaw().cols && + _distortionModel->getHeight() >= data.depthRaw().rows && + _distortionModel->getWidth() % data.depthRaw().cols == 0 && + _distortionModel->getHeight() % data.depthRaw().rows == 0) + { + cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures. + _distortionModel->undistort(depth); + data.setRGBDImage(data.imageRaw(), depth, data.cameraModels()); + } + else + { + UERROR("Distortion model size is %dx%d but depth image is %dx%d!", + _distortionModel->getWidth(), _distortionModel->getHeight(), + data.depthRaw().cols, data.depthRaw().rows); + } + if(info) info->timeUndistortDepth = timer.ticks(); + } + + if(_bilateralFiltering && !data.depthRaw().empty()) + { + UTimer timer; + data.setRGBDImage(data.imageRaw(), util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR), data.cameraModels()); + if(info) info->timeBilateralFiltering = timer.ticks(); + } + + if(_imageDecimation>1 && !data.imageRaw().empty()) + { + UDEBUG(""); + UTimer timer; + if(!data.depthRaw().empty() && + !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0)) + { + UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! " + "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows); + } + else + { + cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation); + + int depthDecimation = _imageDecimation; + if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols) + { + depthDecimation = 1; + } + else + { + depthDecimation = 2; + while(data.depthOrRightRaw().rows / depthDecimation > image.rows || + data.depthOrRightRaw().cols / depthDecimation > image.cols || + data.depthOrRightRaw().rows % depthDecimation != 0 || + data.depthOrRightRaw().cols % depthDecimation != 0) + { + ++depthDecimation; + } + UDEBUG("depthDecimation=%d", depthDecimation); + } + cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation); + + std::vector models = data.cameraModels(); + for(unsigned int i=0; i stereoModels = data.stereoCameraModels(); + for(unsigned int i=0; i kpts = data.keypoints(); + double log2value = log(double(_imageDecimation))/log(2.0); + for(unsigned int i=0; itimeImageDecimation = timer.ticks(); + } + + if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1) + { + if(data.cameraModels().size() == 1) + { + UDEBUG(""); + UTimer timer; + cv::Mat tmpRgb; + cv::flip(data.imageRaw(), tmpRgb, 1); + + CameraModel tmpModel = data.cameraModels()[0]; + if(data.cameraModels()[0].cx()) + { + tmpModel = CameraModel( + data.cameraModels()[0].fx(), + data.cameraModels()[0].fy(), + float(data.imageRaw().cols) - data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), + data.cameraModels()[0].localTransform(), + data.cameraModels()[0].Tx(), + data.cameraModels()[0].imageSize()); + } + cv::Mat tmpDepth = data.depthOrRightRaw(); + if(!data.depthRaw().empty()) + { + cv::flip(data.depthRaw(), tmpDepth, 1); + } + data.setRGBDImage(tmpRgb, tmpDepth, tmpModel); + if(info) info->timeMirroring = timer.ticks(); + } + else + { + UWARN("Mirroring is not implemented for multiple cameras or stereo..."); + } + } + + if(_histogramMethod && !data.imageRaw().empty()) + { + UDEBUG(""); + UTimer timer; + cv::Mat image; + if(_histogramMethod == 1) + { + if(data.imageRaw().type() == CV_8UC1) + { + cv::equalizeHist(data.imageRaw(), image); + } + else if(data.imageRaw().type() == CV_8UC3) + { + cv::Mat channels[3]; + cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb); + cv::split(image, channels); + cv::equalizeHist(channels[0], channels[0]); + cv::merge(channels, 3, image); + cv::cvtColor(image, image, CV_YCrCb2BGR); + } + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + if(data.rightRaw().type() == CV_8UC1) + { + cv::equalizeHist(data.rightRaw(), right); + } + else if(data.rightRaw().type() == CV_8UC3) + { + cv::Mat channels[3]; + cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb); + cv::split(right, channels); + cv::equalizeHist(channels[0], channels[0]); + cv::merge(channels, 3, right); + cv::cvtColor(right, right, CV_YCrCb2BGR); + } + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + else if(_histogramMethod == 2) + { + cv::Ptr clahe = cv::createCLAHE(3.0); + if(data.imageRaw().type() == CV_8UC1) + { + clahe->apply(data.imageRaw(), image); + } + else if(data.imageRaw().type() == CV_8UC3) + { + cv::Mat channels[3]; + cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb); + cv::split(image, channels); + clahe->apply(channels[0], channels[0]); + cv::merge(channels, 3, image); + cv::cvtColor(image, image, CV_YCrCb2BGR); + } + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + if(data.rightRaw().type() == CV_8UC1) + { + clahe->apply(data.rightRaw(), right); + } + else if(data.rightRaw().type() == CV_8UC3) + { + cv::Mat channels[3]; + cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb); + cv::split(right, channels); + clahe->apply(channels[0], channels[0]); + cv::merge(channels, 3, right); + cv::cvtColor(right, right, CV_YCrCb2BGR); + } + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + if(info) info->timeHistogramEqualization = timer.ticks(); + } + + if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty()) + { + if(data.stereoCameraModels().size()==1) + { +#if CV_MAJOR_VERSION < 3 + UWARN("Stereo exposure compensation not implemented for OpenCV version under 3."); +#else + UDEBUG(""); + UTimer timer; + cv::Ptr compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN); + std::vector topLeftCorners(2, cv::Point(0,0)); + std::vector images; + std::vector masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255))); + images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ)); + images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ)); + compensator->feed(topLeftCorners, images, masks); + cv::Mat imgLeft = data.imageRaw().clone(); + compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]); + cv::Mat imgRight = data.rightRaw().clone(); + compensator->apply(1, cv::Point(0,0), imgRight, masks[1]); + data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]); + cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get(); + UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]); + if(info) info->timeStereoExposureCompensation = timer.ticks(); +#endif + } + else + { + UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras..."); + } + } + + if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty()) + { + if(data.stereoCameraModels().size()==1) + { + UDEBUG(""); + UTimer timer; + cv::Mat depth = util2d::depthFromDisparity( + _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()), + data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].baseline()); + // set Tx for stereo bundle adjustment (when used) + CameraModel model = CameraModel( + data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].left().fy(), + data.stereoCameraModels()[0].left().cx(), + data.stereoCameraModels()[0].left().cy(), + data.stereoCameraModels()[0].localTransform(), + -data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].left().imageSize()); + data.setRGBDImage(data.imageRaw(), depth, model); + if(info) info->timeDisparity = timer.ticks(); + } + else + { + UWARN("Stereo to depth is not implemented for multiple stereo cameras..."); + } + } + if(_scanFromDepth && + data.cameraModels().size() && + data.cameraModels().at(0).isValidForProjection() && + !data.depthRaw().empty()) + { + UDEBUG(""); + if(data.laserScanRaw().isEmpty()) + { + UASSERT(_scanDownsampleStep >= 1); + UTimer timer; + pcl::IndicesPtr validIndices(new std::vector); + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + data, + _scanDownsampleStep, + _scanRangeMax, + _scanRangeMin, + validIndices.get()); + float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep); + LaserScan scan; + const Transform & baseToScan = data.cameraModels()[0].localTransform(); + if(validIndices->size()) + { + if(_scanVoxelSize>0.0f) + { + cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize); + float ratio = float(cloud->size()) / float(validIndices->size()); + maxPoints = ratio * maxPoints; + } + else if(!cloud->is_dense) + { + pcl::PointCloud::Ptr denseCloud(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *validIndices, *denseCloud); + cloud = denseCloud; + } + + if(cloud->size()) + { + if(_scanNormalsK>0 || _scanNormalsRadius>0.0f) + { + Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint); + pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); + pcl::concatenateFields(*cloud, *normals, *cloudNormals); + scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); + } + else + { + scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse()); + } + } + } + data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, baseToScan)); + if(info) info->timeScanFromDepth = timer.ticks(); + } + else + { + UWARN("Option to create laser scan from depth image is enabled, but " + "there is already a laser scan in the captured sensor data. Scan from " + "depth will not be created."); + } + } + else if(!data.laserScanRaw().isEmpty()) + { + UDEBUG(""); + // filter the scan after registration + data.setLaserScan(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp)); + } + + // IMU filtering + if(_imuFilter && !data.imu().empty()) + { + if(data.imu().angularVelocity()[0] == 0 && + data.imu().angularVelocity()[1] == 0 && + data.imu().angularVelocity()[2] == 0 && + data.imu().linearAcceleration()[0] == 0 && + data.imu().linearAcceleration()[1] == 0 && + data.imu().linearAcceleration()[2] == 0) + { + UWARN("IMU's acc and gyr values are null! Please disable IMU filtering."); + } + else + { + // Transform IMU data in base_link to correctly initialize yaw + IMU imu = data.imu(); + if(_imuBaseFrameConversion) + { + UASSERT(!data.imu().localTransform().isNull()); + imu.convertToBaseFrame(); + + } + _imuFilter->update( + imu.angularVelocity()[0], + imu.angularVelocity()[1], + imu.angularVelocity()[2], + imu.linearAcceleration()[0], + imu.linearAcceleration()[1], + imu.linearAcceleration()[2], + data.stamp()); + double qx,qy,qz,qw; + _imuFilter->getOrientation(qx,qy,qz,qw); + + data.setIMU(IMU( + cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1), + imu.angularVelocity(), imu.angularVelocityCovariance(), + imu.linearAcceleration(), imu.linearAccelerationCovariance(), + imu.localTransform())); + + UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)", + data.imu().orientation()[0], + data.imu().orientation()[1], + data.imu().orientation()[2], + data.imu().orientation()[3], + data.imu().angularVelocity()[0], + data.imu().angularVelocity()[1], + data.imu().angularVelocity()[2], + data.imu().linearAcceleration()[0], + data.imu().linearAcceleration()[1], + data.imu().linearAcceleration()[2], + data.stamp()); + } + } + + if(_featureDetector && !data.imageRaw().empty()) + { + UDEBUG("Detecting features"); + cv::Mat grayScaleImg = data.imageRaw(); + if(data.imageRaw().channels() > 1) + { + cv::Mat tmp; + cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY); + grayScaleImg = tmp; + } + + cv::Mat depthMask; + if(!data.depthRaw().empty() && _depthAsMask) + { + if( data.imageRaw().rows % data.depthRaw().rows == 0 && + data.imageRaw().cols % data.depthRaw().cols == 0 && + data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols) + { + depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f); + } + else + { + UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.", + Parameters::kVisDepthAsMask().c_str(), + data.imageRaw().rows, data.imageRaw().cols, + data.depthRaw().rows, data.depthRaw().cols); + } + } + + std::vector keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask); + cv::Mat descriptors; + std::vector keypoints3D; + if(!keypoints.empty()) + { + descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints); + if(!keypoints.empty()) + { + keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints); + } + } + + data.setFeatures(keypoints, keypoints3D, descriptors); + } +} + +} // namespace rtabmap diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 0c0fddbbc9..147e854d69 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector & keypoints, const unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes { return sizeof(SensorData) + - _imageCompressed.total()*_imageCompressed.elemSize() + - _imageRaw.total()*_imageRaw.elemSize() + - _depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() + - _depthOrRightRaw.total()*_depthOrRightRaw.elemSize() + - _userDataCompressed.total()*_userDataCompressed.elemSize() + - _userDataRaw.total()*_userDataRaw.elemSize() + - _laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() + - _laserScanRaw.data().total()*_laserScanRaw.data().elemSize() + - _groundCellsCompressed.total()*_groundCellsCompressed.elemSize() + - _groundCellsRaw.total()*_groundCellsRaw.elemSize() + - _obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() + - _obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+ - _emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() + - _emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+ + (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) + + (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) + + (_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) + + (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) + + (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) + + (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) + + (_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) + + (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) + + (_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) + + (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) + + (_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) + + (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+ + (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) + + (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+ _keypoints.size() * sizeof(cv::KeyPoint) + _keypoints3D.size() * sizeof(cv::Point3f) + - _descriptors.total()*_descriptors.elemSize(); + (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize()); } void SensorData::clearCompressedData(bool images, bool scan, bool userData) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 53f843b2b8..32fca300c2 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -219,19 +219,70 @@ void Signature::removeVirtualLinks() } } +void Signature::addLandmark(const Link & landmark) +{ + UDEBUG("Add landmark %d to %d (type=%d/%s var=%f,%f)", landmark.to(), this->id(), (int)landmark.type(), landmark.typeName().c_str(), landmark.transVariance(), landmark.rotVariance()); + UASSERT_MSG(landmark.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str()); + UASSERT_MSG(landmark.to() < 0, uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str()); + UASSERT_MSG(_landmarks.find(landmark.to()) == _landmarks.end(), uFormat("Landmark %d (type=%d) already added to signature %d!", landmark.to(), landmark.type(), this->id()).c_str()); + _landmarks.insert(std::make_pair(landmark.to(), landmark)); + _linksModified = true; +} + +void Signature::removeLandmarks() +{ + size_t sizeBefore = _landmarks.size(); + _landmarks.clear(); + if(_landmarks.size() != sizeBefore) + _linksModified = true; +} + +void Signature::removeLandmark(int landmarkId) +{ + int count = (int)_landmarks.erase(landmarkId); + if(count) + { + UDEBUG("Removed landmark %d from %d", landmarkId, this->id()); + _linksModified = true; + } +} + float Signature::compareTo(const Signature & s) const { + UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size()); + float similarity = 0.0f; - const std::multimap & words = s.getWords(); + int totalDescs = 0; - if(!s.isBadSignature() && !this->isBadSignature()) + for(size_t i=0; isensorData().globalDescriptors().size(); ++i) { - std::list > > pairs; - int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount()); - UASSERT(totalWords > 0); - EpipolarGeometry::findPairs(words, _words, pairs); + if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1) + { + // rescale dot product from -1<->1 to 0<->1 (we assume normalized vectors!) + float dotProd = (this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()) + 1.0f) / 2.0f; + UASSERT_MSG(dotProd>=0, "Global descriptors should be normalized!"); + similarity += dotProd; + totalDescs += 1; + } + } - similarity = float(pairs.size()) / float(totalWords); + if(totalDescs) + { + similarity /= totalDescs; + } + else + { + const std::multimap & words = s.getWords(); + + if(!s.isBadSignature() && !this->isBadSignature()) + { + std::list > > pairs; + int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount()); + UASSERT(totalWords > 0); + EpipolarGeometry::findPairs(words, _words, pairs); + + similarity = float(pairs.size()) / float(totalWords); + } } return similarity; } @@ -348,7 +399,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo total += _words.size() * (sizeof(int)*2+sizeof(std::multimap::iterator)) + sizeof(std::multimap); total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector); total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector); - total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); + total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map::iterator)) + sizeof(std::map); if(withSensorData) { diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index 6fc2755e7a..1157b62746 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -211,14 +211,24 @@ Transform Transform::to3DoF() const { float x,y,z,roll,pitch,yaw; this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - return Transform(x,y,0, 0,0,yaw); + float A = std::cos(yaw); + float B = std::sin(yaw); + return Transform( + A,-B, 0, x, + B, A, 0, y, + 0, 0, 1, 0); } Transform Transform::to4DoF() const { float x,y,z,roll,pitch,yaw; this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - return Transform(x,y,z, 0,0,yaw); + float A = std::cos(yaw); + float B = std::sin(yaw); + return Transform( + A,-B, 0, x, + B, A, 0, y, + 0, 0, 1, z); } bool Transform::is3DoF() const @@ -232,7 +242,7 @@ bool Transform::is4DoF() const r23() == 0.0 && r31() == 0.0 && r32() == 0.0 && - r33() == 0.0; + r33() == 1.0; } cv::Mat Transform::rotationMatrix() const diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 25e1f46c4e..ae0f781ccb 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include #include @@ -45,19 +46,34 @@ bool CameraDepthAI::available() } CameraDepthAI::CameraDepthAI( - const std::string & deviceSerial, + const std::string & mxidOrName, int resolution, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_DEPTHAI , - deviceSerial_(deviceSerial), - outputDepth_(false), - depthConfidence_(200), + mxidOrName_(mxidOrName), + outputMode_(0), + confThreshold_(200), + lrcThreshold_(5), resolution_(resolution), - imuFirmwareUpdate_(false), - imuPublished_(true) + extendedDisparity_(false), + subpixelFractionalBits_(0), + compandingWidth_(0), + useSpecTranslation_(false), + alphaScaling_(0.0), + imuPublished_(true), + publishInterIMU_(false), + dotIntensity_(0.0), + floodIntensity_(0.0), + detectFeatures_(0), + useHarrisDetector_(false), + minDistance_(7.0), + numTargetFeatures_(1000), + threshold_(0.01), + nms_(true), + nmsRadius_(4) #endif { #ifdef RTABMAP_DEPTHAI @@ -75,32 +91,142 @@ CameraDepthAI::~CameraDepthAI() #endif } -void CameraDepthAI::setOutputDepth(bool enabled, int confidence) +void CameraDepthAI::setOutputMode(int outputMode) { #ifdef RTABMAP_DEPTHAI - outputDepth_ = enabled; - if(outputDepth_) + outputMode_ = outputMode; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold) +{ +#ifdef RTABMAP_DEPTHAI + confThreshold_ = confThreshold; + lrcThreshold_ = lrcThreshold; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setExtendedDisparity(bool extendedDisparity) +{ +#ifdef RTABMAP_DEPTHAI + extendedDisparity_ = extendedDisparity; + if(extendedDisparity_) + { + if(subpixelFractionalBits_>0) + { + UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel..."); + subpixelFractionalBits_ = 0; + } + if(compandingWidth_>0) + { + UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding..."); + compandingWidth_ = 0; + } + } +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits) +{ +#ifdef RTABMAP_DEPTHAI + UASSERT(fractionalBits>=3 && fractionalBits<=5); + subpixelFractionalBits_ = enabled?fractionalBits:0; + if(subpixelFractionalBits_ != 0 && extendedDisparity_) { - depthConfidence_ = confidence; + UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity..."); + extendedDisparity_ = false; } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif } -void CameraDepthAI::setIMUFirmwareUpdate(bool enabled) +void CameraDepthAI::setCompanding(bool enabled, int width) { #ifdef RTABMAP_DEPTHAI - imuFirmwareUpdate_ = enabled; + UASSERT(width == 64 || width == 96); + compandingWidth_ = enabled?width:0; + if(compandingWidth_ != 0 && extendedDisparity_) + { + UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity..."); + extendedDisparity_ = false; + } #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif } -void CameraDepthAI::setIMUPublished(bool published) +void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling) { #ifdef RTABMAP_DEPTHAI - imuPublished_ = published; + useSpecTranslation_ = useSpecTranslation; + alphaScaling_ = alphaScaling; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU) +{ +#ifdef RTABMAP_DEPTHAI + imuPublished_ = imuPublished; + publishInterIMU_ = publishInterIMU; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity) +{ +#ifdef RTABMAP_DEPTHAI + dotIntensity_ = dotIntensity; + floodIntensity_ = floodIntensity; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setDetectFeatures(int detectFeatures) +{ +#ifdef RTABMAP_DEPTHAI + detectFeatures_ = detectFeatures; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setBlobPath(const std::string & blobPath) +{ +#ifdef RTABMAP_DEPTHAI + blobPath_ = blobPath; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, float minDistance, int numTargetFeatures) +{ +#ifdef RTABMAP_DEPTHAI + useHarrisDetector_ = useHarrisDetector; + minDistance_ = minDistance; + numTargetFeatures_ = numTargetFeatures; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setSuperPointDetector(float threshold, bool nms, int nmsRadius) +{ +#ifdef RTABMAP_DEPTHAI + threshold_ = threshold; + nms_ = nms; + nmsRadius_ = nmsRadius; #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif @@ -112,103 +238,203 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin #ifdef RTABMAP_DEPTHAI std::vector devices = dai::Device::getAllAvailableDevices(); - if(devices.empty()) + if(devices.empty() && mxidOrName_.empty()) { + UERROR("No DepthAI device found or specified"); return false; } if(device_.get()) - { device_->close(); - } + accBuffer_.clear(); gyroBuffer_.clear(); - dai::DeviceInfo deviceToUse; - if(deviceSerial_.empty()) - deviceToUse = devices[0]; - for(size_t i=0; i(); auto monoRight = p.create(); auto stereo = p.create(); + std::shared_ptr colorCam; + if(outputMode_==2) + { + colorCam = p.create(); + if(detectFeatures_) + { + UWARN("On-device feature detectors cannot be enabled on color camera input!"); + detectFeatures_ = 0; + } + } std::shared_ptr imu; if(imuPublished_) imu = p.create(); + std::shared_ptr gfttDetector; + std::shared_ptr manip; + std::shared_ptr neuralNetwork; + if(detectFeatures_ == 1) + { + gfttDetector = p.create(); + } + else if(detectFeatures_ >= 2) + { + if(!blobPath_.empty()) + { + manip = p.create(); + neuralNetwork = p.create(); + } + else + { + UWARN("Missing MyriadX blob file!"); + detectFeatures_ = 0; + } + } - auto xoutLeft = p.create(); - auto xoutDepthOrRight = p.create(); + auto sync = p.create(); + auto xoutCamera = p.create(); std::shared_ptr xoutIMU; if(imuPublished_) xoutIMU = p.create(); // XLinkOut - xoutLeft->setStreamName("rectified_left"); - xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right"); + xoutCamera->setStreamName("camera"); if(imuPublished_) xoutIMU->setStreamName("imu"); - // MonoCamera monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); - monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); - if(this->getImageRate()>0) + monoLeft->setCamera("left"); + monoRight->setCamera("right"); + if(detectFeatures_ >= 2) + { + if(this->getImageRate() <= 0 || this->getImageRate() > 15) + { + UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!"); + monoLeft->setFps(15); + monoRight->setFps(15); + } + } + else if(this->getImageRate() > 0) { monoLeft->setFps(this->getImageRate()); monoRight->setFps(this->getImageRate()); } // StereoDepth - stereo->initialConfig.setConfidenceThreshold(depthConfidence_); - stereo->initialConfig.setLeftRightCheckThreshold(5); + if(outputMode_ == 2) + stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A); + else + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); + stereo->setExtendedDisparity(extendedDisparity_); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - stereo->setLeftRightCheck(true); - stereo->setSubpixel(false); - stereo->setExtendedDisparity(false); + stereo->enableDistortionCorrection(true); + stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_); + stereo->setDepthAlignmentUseSpecTranslation(useSpecTranslation_); + if(alphaScaling_ > -1.0f) + stereo->setAlphaScaling(alphaScaling_); + stereo->initialConfig.setConfidenceThreshold(confThreshold_); + stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0); + if(lrcThreshold_>=0) + stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_); + stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7); + auto config = stereo->initialConfig.get(); + config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9; + config.censusTransform.kernelMask = 0X2AA00AA805540155; + config.postProcessing.brightnessFilter.maxBrightness = 255; + stereo->initialConfig.set(config); // Link plugins CAM -> STEREO -> XLINK monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - if(outputDepth_) + if(outputMode_ == 2) { - // Depth is registered to right image by default, so subscribe to right image when depth is used - if(outputDepth_) - stereo->rectifiedRight.link(xoutLeft->input); + colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A); + colorCam->setSize(targetSize_.width, targetSize_.height); + if(this->getImageRate() > 0) + colorCam->setFps(this->getImageRate()); + if(alphaScaling_ > -1.0f) + colorCam->setCalibrationAlpha(alphaScaling_); + } + this->setImageRate(0); + + // Using VideoEncoder on PoE devices, Subpixel is not supported + if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) + { + auto leftOrColorEnc = p.create(); + auto depthOrRightEnc = p.create(); + leftOrColorEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + if(outputMode_ < 2) + { + stereo->rectifiedLeft.link(leftOrColorEnc->input); + leftOrColorEnc->bitstream.link(sync->inputs["left"]); + } else - stereo->rectifiedLeft.link(xoutLeft->input); - stereo->depth.link(xoutDepthOrRight->input); + { + colorCam->video.link(leftOrColorEnc->input); + leftOrColorEnc->bitstream.link(sync->inputs["color"]); + } + if(outputMode_) + { + depthOrRightEnc->setQuality(100); + stereo->disparity.link(depthOrRightEnc->input); + depthOrRightEnc->bitstream.link(sync->inputs["depth"]); + } + else + { + stereo->rectifiedRight.link(depthOrRightEnc->input); + depthOrRightEnc->bitstream.link(sync->inputs["right"]); + } } else { - stereo->rectifiedLeft.link(xoutLeft->input); - stereo->rectifiedRight.link(xoutDepthOrRight->input); + stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5); + if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5) + stereo->setSubpixelFractionalBits(subpixelFractionalBits_); + config = stereo->initialConfig.get(); + config.costMatching.enableCompanding = compandingWidth_>0; + if(compandingWidth_>0) + config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96; + stereo->initialConfig.set(config); + if(outputMode_ < 2) + { + stereo->rectifiedLeft.link(sync->inputs["left"]); + } + else + { + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + colorCam->video.link(sync->inputs["color"]); + } + if(outputMode_) + stereo->depth.link(sync->inputs["depth"]); + else + stereo->rectifiedRight.link(sync->inputs["right"]); } + sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps()))); + sync->out.link(xoutCamera->input); + if(imuPublished_) { // enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate @@ -222,51 +448,169 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // Link plugins IMU -> XLINK imu->out.link(xoutIMU->input); + } - imu->enableFirmwareUpdate(imuFirmwareUpdate_); + if(detectFeatures_ == 1) + { + gfttDetector->setHardwareResources(1, 2); + gfttDetector->initialConfig.setCornerDetector( + useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI); + gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_); + gfttDetector->initialConfig.setMotionEstimator(false); + auto cfg = gfttDetector->initialConfig.get(); + cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_; + gfttDetector->initialConfig.set(cfg); + stereo->rectifiedLeft.link(gfttDetector->inputImage); + gfttDetector->outputFeatures.link(sync->inputs["feat"]); + } + else if(detectFeatures_ >= 2) + { + manip->setKeepAspectRatio(false); + manip->setMaxOutputFrameSize(320 * 200); + manip->initialConfig.setResize(320, 200); + neuralNetwork->setBlobPath(blobPath_); + neuralNetwork->setNumInferenceThreads(2); + neuralNetwork->setNumNCEPerInferenceThread(1); + neuralNetwork->input.setBlocking(false); + stereo->rectifiedLeft.link(manip->inputImage); + manip->out.link(neuralNetwork->input); + neuralNetwork->out.link(sync->inputs["feat"]); } device_.reset(new dai::Device(p, deviceToUse)); + UINFO("Available camera sensors: "); + for(auto& sensor : device_->getCameraSensorNames()) { + UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str()); + } + UINFO("Loading eeprom calibration data"); dai::CalibrationHandler calibHandler = device_->readCalibration(); - std::vector > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height)); - double fx = matrix[0][0]; - double fy = matrix[1][1]; - double cx = matrix[0][2]; - double cy = matrix[1][2]; - matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT); - double baseline = matrix[0][3]/100.0; - UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); - stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize); + + auto eeprom = calibHandler.getEepromData(); + UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str()); + + auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A; + cv::Mat cameraMatrix, distCoeffs, newCameraMatrix; + + std::vector > matrix = calibHandler.getCameraIntrinsics(cameraId, targetSize_.width, targetSize_.height); + cameraMatrix = (cv::Mat_(3,3) << + matrix[0][0], matrix[0][1], matrix[0][2], + matrix[1][0], matrix[1][1], matrix[1][2], + matrix[2][0], matrix[2][1], matrix[2][2]); + + std::vector coeffs = calibHandler.getDistortionCoefficients(cameraId); + if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) + distCoeffs = (cv::Mat_(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]); + + if(alphaScaling_>-1.0f) + newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_); + else + newCameraMatrix = cameraMatrix; + + double fx = newCameraMatrix.at(0, 0); + double fy = newCameraMatrix.at(1, 1); + double cx = newCameraMatrix.at(0, 2); + double cy = newCameraMatrix.at(1, 2); + double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, useSpecTranslation_)/100.0; + UINFO("fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); + if(outputMode_ == 2) + stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); + else + stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform()*Transform(-calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_A)/100.0, 0, 0), targetSize_); if(imuPublished_) { // Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera // Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)" - //matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT); + //matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B); //imuLocalTransform_ = 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]); - // Hard-coded: x->down, y->left, z->forward - imuLocalTransform_ = Transform( - 0, 0, 1, 0, - 0, 1, 0, 0, - -1 ,0, 0, 0); - UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); + if(eeprom.boardName == "OAK-D" || + eeprom.boardName == "BW1098OBC") + { + imuLocalTransform_ = Transform( + 0, -1, 0, 0.0525, + 1, 0, 0, 0.013662, + 0, 0, 1, 0); + } + else if(eeprom.boardName == "DM9098") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.037945, + 1, 0, 0, 0.00079, + 0, 0, -1, 0); + } + else if(eeprom.boardName == "NG2094") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.0374, + 1, 0, 0, 0.00176, + 0, 0, -1, 0); + } + else if(eeprom.boardName == "NG9097") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.04, + 1, 0, 0, 0.020265, + 0, 0, -1, 0); + } + else + { + UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str()); + imuPublished_ = false; + } } else { UINFO("IMU disabled"); } + cameraQueue_ = device_->getOutputQueue("camera", 8, false); if(imuPublished_) { - imuQueue_ = device_->getOutputQueue("imu", 50, false); + imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_; + UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); + device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr data) { + auto imuData = std::dynamic_pointer_cast(data); + auto imuPackets = imuData->packets; + + for(auto& imuPacket : imuPackets) + { + auto& acceleroValues = imuPacket.acceleroMeter; + auto& gyroValues = imuPacket.gyroscope; + double accStamp = std::chrono::duration(acceleroValues.getTimestampDevice().time_since_epoch()).count(); + double gyroStamp = std::chrono::duration(gyroValues.getTimestampDevice().time_since_epoch()).count(); + + if(publishInterIMU_) + { + IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1), + imuLocalTransform_); + UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2)); + } + else + { + UScopeMutex lock(imuMutex_); + accBuffer_.emplace_hint(accBuffer_.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)); + gyroBuffer_.emplace_hint(gyroBuffer_.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)); + } + } + }); + } + + if(!device_->getIrDrivers().empty()) + { + UINFO("Setting IR intensity"); + device_->setIrLaserDotProjectorIntensity(dotIntensity_); + device_->setIrFloodLightIntensity(floodIntensity_); + } + else if(dotIntensity_ > 0 || floodIntensity_ > 0) + { + UWARN("No IR drivers were detected! IR intensity cannot be set."); } - leftQueue_ = device_->getOutputQueue("rectified_left", 1, false); - rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false); uSleep(2000); // avoid bad frames on start @@ -289,187 +633,174 @@ bool CameraDepthAI::isCalibrated() const std::string CameraDepthAI::getSerial() const { #ifdef RTABMAP_DEPTHAI - return deviceSerial_; + return device_->getMxId(); #endif return ""; } -SensorData CameraDepthAI::captureImage(CameraInfo * info) +SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_DEPTHAI - cv::Mat left, depthOrRight; - auto rectifL = leftQueue_->get(); - auto rectifRightOrDepth = rightOrDepthQueue_->get(); + auto messageGroup = cameraQueue_->get(); + auto rectifLeftOrColor = messageGroup->get(outputMode_<2?"left":"color"); + auto rectifRightOrDepth = messageGroup->get(outputMode_?"depth":"right"); - if(rectifL.get() && rectifRightOrDepth.get()) + cv::Mat leftOrColor, depthOrRight; + if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos) { - auto stampLeft = rectifL->getTimestamp().time_since_epoch().count(); - auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count(); - double stamp = double(stampLeft)/10e8; - left = rectifL->getCvFrame(); + leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR); + depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); + if(outputMode_) + { + cv::Mat disp; + depthOrRight.convertTo(disp, CV_16UC1); + cv::divide(-stereoModel_.right().Tx() * 1000, disp, depthOrRight); + } + } + else + { + leftOrColor = rectifLeftOrColor->getCvFrame(); depthOrRight = rectifRightOrDepth->getCvFrame(); + } + + double stamp = std::chrono::duration(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); + if(outputMode_) + data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); + else + data = SensorData(leftOrColor, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); - if(!left.empty() && !depthOrRight.empty()) + if(imuPublished_ && !publishInterIMU_) + { + cv::Vec3d acc, gyro; + std::map::const_iterator iterA, iterB; + + imuMutex_.lock(); + while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp) { - if(depthOrRight.type() == CV_8UC1) - { - if(stereoModel_.isValidForRectification()) - { - left = stereoModel_.left().rectifyImage(left); - depthOrRight = stereoModel_.right().rectifyImage(depthOrRight); - } - data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); - } - else - { - data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); - } + imuMutex_.unlock(); + uSleep(1); + imuMutex_.lock(); + } - if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms - { - UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8); - } + //acc + iterB = accBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != accBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + acc = iterB->second; + } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + acc = iterA->second + t*(iterB->second - iterA->second); + } + accBuffer_.erase(accBuffer_.begin(), iterB); + + //gyro + iterB = gyroBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != gyroBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + gyro = iterB->second; + } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + gyro = iterA->second + t*(iterB->second - iterA->second); + } + gyroBuffer_.erase(gyroBuffer_.begin(), iterB); - //get imu - double stampStart = UTimer::now(); - while(imuPublished_ && imuQueue_.get()) - { - if(imuQueue_->has()) - { - auto imuData = imuQueue_->get(); - - auto imuPackets = imuData->packets; - double accStamp = 0.0; - double gyroStamp = 0.0; - for(auto& imuPacket : imuPackets) { - auto& acceleroValues = imuPacket.acceleroMeter; - auto& gyroValues = imuPacket.gyroscope; - - accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8; - gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8; - accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z))); - gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z))); - if(accBuffer_.size() > 1000) - { - accBuffer_.erase(accBuffer_.begin()); - } - if(gyroBuffer_.size() > 1000) - { - gyroBuffer_.erase(gyroBuffer_.begin()); - } - } - if(accStamp >= stamp && gyroStamp >= stamp) - { - break; - } - } - if((UTimer::now() - stampStart) > 0.01) - { - UWARN("Could not received IMU after 10 ms! Disabling IMU!"); - imuPublished_ = false; - } - } + imuMutex_.unlock(); + data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); + } - cv::Vec3d acc, gyro; - bool valid = !accBuffer_.empty() && !gyroBuffer_.empty(); - //acc - if(!accBuffer_.empty()) - { - std::map::const_iterator iterB = accBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != accBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == accBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - acc[0] = iterA->second[0]; - acc[1] = iterA->second[1]; - acc[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } - //gyro - if(!gyroBuffer_.empty()) - { - std::map::const_iterator iterB = gyroBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != gyroBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == gyroBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - gyro[0] = iterA->second[0]; - gyro[1] = iterA->second[1]; - gyro[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } + if(detectFeatures_ == 1) + { + auto features = messageGroup->get("feat")->trackedFeatures; + std::vector keypoints; + for(auto& feature : features) + keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3)); + data.setFeatures(keypoints, std::vector(), cv::Mat()); + } + else if(detectFeatures_ >= 2) + { + auto features = messageGroup->get("feat"); + std::vector scores_dense, local_descriptor_map, global_descriptor; + if(detectFeatures_ == 2) + { + scores_dense = features->getLayerFp16("heatmap"); + local_descriptor_map = features->getLayerFp16("desc"); + } + else if(detectFeatures_ == 3) + { + scores_dense = features->getLayerFp16("pred/local_head/detector/Squeeze"); + local_descriptor_map = features->getLayerFp16("pred/local_head/descriptor/transpose"); + global_descriptor = features->getLayerFp16("pred/global_head/l2_normalize_1"); + } - if(valid) + cv::Mat scores(200, 320, CV_32FC1, scores_dense.data()); + cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC); + if(nms_) + { + cv::Mat dilated_scores(targetSize_, CV_32FC1); + cv::dilate(scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1))); + cv::Mat max_mask = scores == dilated_scores; + cv::dilate(scores, dilated_scores, cv::Mat()); + cv::Mat max_mask_r1 = scores == dilated_scores; + cv::Mat supp_mask(targetSize_, CV_8UC1); + for(size_t i=0; i<2; i++) { - data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); + cv::dilate(max_mask, supp_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1))); + cv::Mat supp_scores = scores.clone(); + supp_scores.setTo(0, supp_mask); + cv::dilate(supp_scores, dilated_scores, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(nmsRadius_*2+1, nmsRadius_*2+1))); + cv::Mat new_max_mask = cv::Mat::zeros(targetSize_, CV_8UC1); + cv::bitwise_not(supp_mask, supp_mask); + cv::bitwise_and(supp_scores == dilated_scores, supp_mask, new_max_mask, max_mask_r1); + cv::bitwise_or(max_mask, new_max_mask, max_mask); } + cv::bitwise_not(max_mask, supp_mask); + scores.setTo(0, supp_mask); } - } - else - { - UWARN("Null images received!?"); + + std::vector kpts; + cv::findNonZero(scores > threshold_, kpts); + std::vector keypoints; + for(auto& kpt : kpts) + { + float response = scores.at(kpt); + keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response)); + } + + cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data()); + if(detectFeatures_ == 2) + coarse_desc.forEach>([&](cv::Vec& descriptor, const int position[]) -> void { + cv::normalize(descriptor, descriptor); + }); + cv::Mat mapX(keypoints.size(), 1, CV_32FC1); + cv::Mat mapY(keypoints.size(), 1, CV_32FC1); + for(size_t i=0; i(i) = (keypoints[i].pt.x - (targetSize_.width-1)/2) * 40/targetSize_.width + (40-1)/2; + mapY.at(i) = (keypoints[i].pt.y - (targetSize_.height-1)/2) * 25/targetSize_.height + (25-1)/2; + } + cv::Mat map1, map2, descriptors; + cv::convertMaps(mapX, mapY, map1, map2, CV_16SC2); + cv::remap(coarse_desc, descriptors, map1, map2, cv::INTER_LINEAR); + descriptors.forEach>([&](cv::Vec& descriptor, const int position[]) -> void { + cv::normalize(descriptor, descriptor); + }); + descriptors = descriptors.reshape(1); + + data.setFeatures(keypoints, std::vector(), descriptors); + if(detectFeatures_ == 3) + data.addGlobalDescriptor(GlobalDescriptor(1, cv::Mat(1, global_descriptor.size(), CV_32FC1, global_descriptor.data()).clone())); } #else diff --git a/corelib/src/camera/CameraFreenect.cpp b/corelib/src/camera/CameraFreenect.cpp index 7159eb3a2b..9d48215f55 100644 --- a/corelib/src/camera/CameraFreenect.cpp +++ b/corelib/src/camera/CameraFreenect.cpp @@ -418,7 +418,7 @@ std::string CameraFreenect::getSerial() const return ""; } -SensorData CameraFreenect::captureImage(CameraInfo * info) +SensorData CameraFreenect::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FREENECT diff --git a/corelib/src/camera/CameraFreenect2.cpp b/corelib/src/camera/CameraFreenect2.cpp index a1b1db1c6a..85f7bdf05e 100644 --- a/corelib/src/camera/CameraFreenect2.cpp +++ b/corelib/src/camera/CameraFreenect2.cpp @@ -334,7 +334,7 @@ std::string CameraFreenect2::getSerial() const return ""; } -SensorData CameraFreenect2::captureImage(CameraInfo * info) +SensorData CameraFreenect2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FREENECT2 diff --git a/corelib/src/camera/CameraImages.cpp b/corelib/src/camera/CameraImages.cpp index ca6e82071f..0591181aee 100644 --- a/corelib/src/camera/CameraImages.cpp +++ b/corelib/src/camera/CameraImages.cpp @@ -670,7 +670,7 @@ std::vector CameraImages::filenames() const return std::vector(); } -SensorData CameraImages::captureImage(CameraInfo * info) +SensorData CameraImages::captureImage(SensorCaptureInfo * info) { if(_syncImageRateWithStamps && _captureDelay>0.0) { diff --git a/corelib/src/camera/CameraK4A.cpp b/corelib/src/camera/CameraK4A.cpp index 78f72a5a9d..ad5714d744 100644 --- a/corelib/src/camera/CameraK4A.cpp +++ b/corelib/src/camera/CameraK4A.cpp @@ -424,7 +424,7 @@ std::string CameraK4A::getSerial() const #endif } -SensorData CameraK4A::captureImage(CameraInfo * info) +SensorData CameraK4A::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraK4W2.cpp b/corelib/src/camera/CameraK4W2.cpp index 5c3b272502..e289029e8a 100644 --- a/corelib/src/camera/CameraK4W2.cpp +++ b/corelib/src/camera/CameraK4W2.cpp @@ -278,7 +278,7 @@ std::string CameraK4W2::getSerial() const return ""; } -SensorData CameraK4W2::captureImage(CameraInfo * info) +SensorData CameraK4W2::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraMyntEye.cpp b/corelib/src/camera/CameraMyntEye.cpp index 9f1158c08b..01f6d12f1a 100644 --- a/corelib/src/camera/CameraMyntEye.cpp +++ b/corelib/src/camera/CameraMyntEye.cpp @@ -598,7 +598,7 @@ void CameraMyntEye::getPoseAndIMU( } #endif -SensorData CameraMyntEye::captureImage(CameraInfo * info) +SensorData CameraMyntEye::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_MYNTEYE diff --git a/corelib/src/camera/CameraOpenNI2.cpp b/corelib/src/camera/CameraOpenNI2.cpp index 786792ed12..8be86b74ca 100644 --- a/corelib/src/camera/CameraOpenNI2.cpp +++ b/corelib/src/camera/CameraOpenNI2.cpp @@ -473,7 +473,7 @@ std::string CameraOpenNI2::getSerial() const return ""; } -SensorData CameraOpenNI2::captureImage(CameraInfo * info) +SensorData CameraOpenNI2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_OPENNI2 diff --git a/corelib/src/camera/CameraOpenNICV.cpp b/corelib/src/camera/CameraOpenNICV.cpp index 3956a8d2cc..13536d6f76 100644 --- a/corelib/src/camera/CameraOpenNICV.cpp +++ b/corelib/src/camera/CameraOpenNICV.cpp @@ -105,7 +105,7 @@ bool CameraOpenNICV::isCalibrated() const return true; } -SensorData CameraOpenNICV::captureImage(CameraInfo * info) +SensorData CameraOpenNICV::captureImage(SensorCaptureInfo * info) { SensorData data; if(_capture.isOpened()) diff --git a/corelib/src/camera/CameraOpenni.cpp b/corelib/src/camera/CameraOpenni.cpp index 95c876d402..9fb8dc7377 100644 --- a/corelib/src/camera/CameraOpenni.cpp +++ b/corelib/src/camera/CameraOpenni.cpp @@ -182,7 +182,7 @@ std::string CameraOpenni::getSerial() const return ""; } -SensorData CameraOpenni::captureImage(CameraInfo * info) +SensorData CameraOpenni::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_OPENNI diff --git a/corelib/src/camera/CameraRGBDImages.cpp b/corelib/src/camera/CameraRGBDImages.cpp index 0bc5d09510..7bcf83223d 100644 --- a/corelib/src/camera/CameraRGBDImages.cpp +++ b/corelib/src/camera/CameraRGBDImages.cpp @@ -70,7 +70,7 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st return success; } -SensorData CameraRGBDImages::captureImage(CameraInfo * info) +SensorData CameraRGBDImages::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraRealSense.cpp b/corelib/src/camera/CameraRealSense.cpp index 2f0beb8134..3f545bb587 100644 --- a/corelib/src/camera/CameraRealSense.cpp +++ b/corelib/src/camera/CameraRealSense.cpp @@ -856,7 +856,7 @@ Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose) } #endif -SensorData CameraRealSense::captureImage(CameraInfo * info) +SensorData CameraRealSense::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_REALSENSE diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index 7694e05433..9392873d60 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include @@ -78,7 +77,6 @@ CameraRealSense2::CameraRealSense2( cameraDepthHeight_(480), cameraDepthFps_(30), globalTimeSync_(true), - publishInterIMU_(false), dualMode_(false), closing_(false) #endif @@ -138,12 +136,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame) { auto stream = frame.get_profile().stream_type(); cv::Vec3f crnt_reading = *reinterpret_cast(frame.get_data()); - UDEBUG("%s callback! %f (%f %f %f)", - stream == RS2_STREAM_GYRO?"GYRO":"ACC", - frame.get_timestamp(), - crnt_reading[0], - crnt_reading[1], - crnt_reading[2]); + //UDEBUG("%s callback! %f (%f %f %f)", + // stream == RS2_STREAM_GYRO?"GYRO":"ACC", + // frame.get_timestamp(), + // crnt_reading[0], + // crnt_reading[1], + // crnt_reading[2]); UScopeMutex sm(imuMutex_); if(stream == RS2_STREAM_GYRO) { @@ -194,7 +192,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame) void CameraRealSense2::frame_callback(rs2::frame frame) { - UDEBUG("Frame callback! %f", frame.get_timestamp()); + //UDEBUG("Frame callback! %f", frame.get_timestamp()); syncer_(frame); } void CameraRealSense2::multiple_message_callback(rs2::frame frame) @@ -1139,14 +1137,14 @@ bool CameraRealSense2::odomProvided() const #endif } -bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance) +bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) { #ifdef RTABMAP_REALSENSE2 IMU imu; unsigned int confidence = 0; double rsStamp = stamp*1000.0; Transform p; - getPoseAndIMU(rsStamp, p, confidence, imu); + getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000); if(!p.isNull()) { @@ -1202,13 +1200,6 @@ void CameraRealSense2::setGlobalTimeSync(bool enabled) #endif } -void CameraRealSense2::publishInterIMU(bool enabled) -{ -#ifdef RTABMAP_REALSENSE2 - publishInterIMU_ = enabled; -#endif -} - void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics) { #ifdef RTABMAP_REALSENSE2 @@ -1252,7 +1243,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, #endif } -SensorData CameraRealSense2::captureImage(CameraInfo * info) +SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_REALSENSE2 @@ -1466,11 +1457,11 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence); info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence); } - if(!imu.empty() && !publishInterIMU_) + if(!imu.empty() && !isInterIMUPublishing()) { data.setIMU(imu); } - else if(publishInterIMU_ && !gyroBuffer_.empty()) + else if(isInterIMUPublishing() && !gyroBuffer_.empty()) { if(lastImuStamp_ > 0.0) { @@ -1501,7 +1492,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) getPoseAndIMU(stamps[i], tmp, confidence, imuTmp); if(!imuTmp.empty()) { - UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0)); + this->postInterIMU(imuTmp, stamps[i]/1000.0); pub++; } else diff --git a/corelib/src/camera/CameraStereoDC1394.cpp b/corelib/src/camera/CameraStereoDC1394.cpp index b82b591ea1..691516b73c 100644 --- a/corelib/src/camera/CameraStereoDC1394.cpp +++ b/corelib/src/camera/CameraStereoDC1394.cpp @@ -396,7 +396,7 @@ std::string CameraStereoDC1394::getSerial() const return ""; } -SensorData CameraStereoDC1394::captureImage(CameraInfo * info) +SensorData CameraStereoDC1394::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_DC1394 diff --git a/corelib/src/camera/CameraStereoFlyCapture2.cpp b/corelib/src/camera/CameraStereoFlyCapture2.cpp index 8153beb4b4..ca95cc87fb 100644 --- a/corelib/src/camera/CameraStereoFlyCapture2.cpp +++ b/corelib/src/camera/CameraStereoFlyCapture2.cpp @@ -260,7 +260,7 @@ std::string CameraStereoFlyCapture2::getSerial() const return ""; } -SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info) +SensorData CameraStereoFlyCapture2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FLYCAPTURE2 diff --git a/corelib/src/camera/CameraStereoImages.cpp b/corelib/src/camera/CameraStereoImages.cpp index 2ceed75d3e..0a7d3915d2 100644 --- a/corelib/src/camera/CameraStereoImages.cpp +++ b/corelib/src/camera/CameraStereoImages.cpp @@ -156,7 +156,7 @@ std::string CameraStereoImages::getSerial() const return stereoModel_.name(); } -SensorData CameraStereoImages::captureImage(CameraInfo * info) +SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoTara.cpp b/corelib/src/camera/CameraStereoTara.cpp index bde7b89db9..e4ea3cdfd1 100644 --- a/corelib/src/camera/CameraStereoTara.cpp +++ b/corelib/src/camera/CameraStereoTara.cpp @@ -136,7 +136,7 @@ std::string CameraStereoTara::getSerial() const return cameraName_; } -SensorData CameraStereoTara::captureImage(CameraInfo * info) +SensorData CameraStereoTara::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoVideo.cpp b/corelib/src/camera/CameraStereoVideo.cpp index c109136300..392e1c4898 100644 --- a/corelib/src/camera/CameraStereoVideo.cpp +++ b/corelib/src/camera/CameraStereoVideo.cpp @@ -243,7 +243,7 @@ std::string CameraStereoVideo::getSerial() const return cameraName_; } -SensorData CameraStereoVideo::captureImage(CameraInfo * info) +SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index acfbe0ea83..15416afa1e 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #ifdef RTABMAP_ZED @@ -167,12 +166,13 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr class ZedIMUThread: public UThread { public: - ZedIMUThread(float rate, sl::Camera * zed, const Transform & imuLocalTransform, bool accurate) + ZedIMUThread(float rate, sl::Camera * zed, CameraStereoZed * camera, const Transform & imuLocalTransform, bool accurate) { UASSERT(rate > 0.0f); - UASSERT(zed != 0); + UASSERT(zed != 0 && camera != 0); rate_ = rate; zed_= zed; + camera_ = camera; accurate_ = accurate; imuLocalTransform_ = imuLocalTransform; } @@ -212,19 +212,20 @@ class ZedIMUThread: public UThread bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE); if(res == sl::SUCCESS && imudata.valid) { - UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now())); + this->postInterIMU(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()); } #else sl::SensorsData sensordata; - sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE); + sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT); if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available) { - UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now())); + camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp)/10e9); } #endif } float rate_; sl::Camera * zed_; + CameraStereoZed * camera_; bool accurate_; Transform imuLocalTransform_; UTimer frameRateTimer_; @@ -240,6 +241,17 @@ bool CameraStereoZed::available() #endif } + +int CameraStereoZed::sdkVersion() +{ +#ifdef RTABMAP_ZED + return ZED_SDK_MAJOR_VERSION; +#else + return -1; +#endif +} + + CameraStereoZed::CameraStereoZed( int deviceId, int resolution, @@ -268,12 +280,21 @@ CameraStereoZed::CameraStereoZed( computeOdometry_(computeOdometry), lost_(true), force3DoF_(odomForce3DoF), - publishInterIMU_(false), imuPublishingThread_(0) #endif { UDEBUG(""); #ifdef RTABMAP_ZED +#if ZED_SDK_MAJOR_VERSION < 4 + if(resolution_ == 3) + { + resolution_ = 2; + } + else if(resolution_ == 5) + { + resolution_ = 3; + } +#endif #if ZED_SDK_MAJOR_VERSION < 3 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ (resolution_); sl::DEPTH_MODE qual = static_cast(quality_); - sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); +#if ZED_SDK_MAJOR_VERSION < 4 + sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); +#else + UASSERT(sensingMode_ >= 0 && sensingMode_ < 2); +#endif UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); #endif @@ -320,7 +345,6 @@ CameraStereoZed::CameraStereoZed( computeOdometry_(computeOdometry), lost_(true), force3DoF_(odomForce3DoF), - publishInterIMU_(false), imuPublishingThread_(0) #endif { @@ -334,11 +358,15 @@ CameraStereoZed::CameraStereoZed( #else sl::RESOLUTION res = static_cast(resolution_); sl::DEPTH_MODE qual = static_cast(quality_); - sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(res >= sl::RESOLUTION::HD2K && res < sl::RESOLUTION::LAST); UASSERT(qual >= sl::DEPTH_MODE::NONE && qual < sl::DEPTH_MODE::LAST); +#if ZED_SDK_MAJOR_VERSION < 4 + sl::SENSING_MODE sens = static_cast(sensingMode_); UASSERT(sens >= sl::SENSING_MODE::STANDARD && sens < sl::SENSING_MODE::LAST); +#else + UASSERT(sensingMode_ >= 0 && sensingMode_ < 2); +#endif UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100); UASSERT(texturenessConfidenceThr_ >= 0 && texturenessConfidenceThr_ <=100); #endif @@ -357,13 +385,6 @@ CameraStereoZed::~CameraStereoZed() #endif } -void CameraStereoZed::publishInterIMU(bool enabled) -{ -#ifdef RTABMAP_ZED - publishInterIMU_ = enabled; -#endif -} - bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -465,27 +486,54 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str } sl::CameraInformation infos = zed_->getCameraInformation(); +#if ZED_SDK_MAJOR_VERSION < 4 sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters ); +#else + sl::CalibrationParameters *stereoParams = &(infos.camera_configuration.calibration_parameters ); +#endif sl::Resolution res = stereoParams->left_cam.image_size; +#if ZED_SDK_MAJOR_VERSION < 4 + stereoModel_ = StereoCameraModel( + stereoParams->left_cam.fx, + stereoParams->left_cam.fy, + stereoParams->left_cam.cx, + stereoParams->left_cam.cy, + stereoParams->T[0],//baseline + this->getLocalTransform(), + cv::Size(res.width, res.height)); +#else stereoModel_ = StereoCameraModel( stereoParams->left_cam.fx, stereoParams->left_cam.fy, stereoParams->left_cam.cx, stereoParams->left_cam.cy, - stereoParams->T[0],//baseline + stereoParams->getCameraBaseline(), this->getLocalTransform(), cv::Size(res.width, res.height)); +#endif +#if ZED_SDK_MAJOR_VERSION < 4 + UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s", + stereoParams->left_cam.fx, + stereoParams->left_cam.fy, + stereoParams->left_cam.cx, + stereoParams->left_cam.cy, + stereoParams->T[0],//baseline + (int)res.width, + (int)res.height, + this->getLocalTransform().prettyPrint().c_str()); +#else UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s", stereoParams->left_cam.fx, stereoParams->left_cam.fy, stereoParams->left_cam.cx, stereoParams->left_cam.cy, - stereoParams->T[0],//baseline + stereoParams->getCameraBaseline(), (int)res.width, (int)res.height, this->getLocalTransform().prettyPrint().c_str()); +#endif #if ZED_SDK_MAJOR_VERSION < 3 if(infos.camera_model == sl::MODEL_ZED_M) @@ -493,14 +541,24 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str if(infos.camera_model != sl::MODEL::ZED) #endif { +#if ZED_SDK_MAJOR_VERSION < 4 imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse(); - UINFO("IMU local transform: %s (imu2cam=%s))", - imuLocalTransform_.prettyPrint().c_str(), - zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str()); +#else + imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).inverse(); +#endif - if(publishInterIMU_) +#if ZED_SDK_MAJOR_VERSION < 4 + UINFO("IMU local transform: %s (imu2cam=%s))", + imuLocalTransform_.prettyPrint().c_str(), + zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str()); +#else + UINFO("IMU local transform: %s (imu2cam=%s))", + imuLocalTransform_.prettyPrint().c_str(), + zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str()); +#endif + if(isInterIMUPublishing()) { - imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true); + imuPublishingThread_ = new ZedIMUThread(200, zed_, this, imuLocalTransform_, true); imuPublishingThread_->start(); } } @@ -541,7 +599,7 @@ bool CameraStereoZed::odomProvided() const #endif } -bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance) +bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) { #ifdef RTABMAP_ZED @@ -617,14 +675,16 @@ bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covarian return false; } -SensorData CameraStereoZed::captureImage(CameraInfo * info) +SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_ZED #if ZED_SDK_MAJOR_VERSION < 3 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA); -#else +#elif ZED_SDK_MAJOR_VERSION < 4 sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA); +#else + sl::RuntimeParameters rparam(quality_ > 0, sensingMode_ == 1, confidenceThr_, texturenessConfidenceThr_, sl::REFERENCE_FRAME::CAMERA); #endif if(zed_) @@ -643,10 +703,12 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) #else sl::ERROR_CODE res; + sl::Timestamp timestamp; bool imuReceived = true; do { res = zed_->grab(rparam); + timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE); // If the sensor supports IMU, wait IMU to be available before sending data. if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull()) @@ -684,8 +746,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH); #endif slMat2cvMat(tmp).copyTo(depth); - +#if ZED_SDK_MAJOR_VERSION < 3 data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now()); +#else + data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), double(timestamp)/10e9); +#endif } else { @@ -698,8 +763,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) cv::Mat rgbaRight = slMat2cvMat(tmp); cv::Mat right; cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); - +#if ZED_SDK_MAJOR_VERSION < 3 data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now()); +#else + data = SensorData(left, right, stereoModel_, this->getNextSeqID(), double(timestamp)/10e9); +#endif } if(imuPublishingThread_ == 0) @@ -735,6 +803,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) info->odomPose = zedPoseToTransform(pose); if (!info->odomPose.isNull()) { +#if ZED_SDK_MAJOR_VERSION >=3 + if(pose.timestamp != timestamp) + { + UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp); + } +#endif + //transform from: // x->right, y->down, z->forward //to: @@ -790,4 +865,9 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) return data; } +void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp) +{ + postInterIMU(imu, stamp); +} + } // namespace rtabmap diff --git a/corelib/src/camera/CameraStereoZedOC.cpp b/corelib/src/camera/CameraStereoZedOC.cpp index bcceae1e96..5b961fe33f 100644 --- a/corelib/src/camera/CameraStereoZedOC.cpp +++ b/corelib/src/camera/CameraStereoZedOC.cpp @@ -710,7 +710,7 @@ std::string CameraStereoZedOC::getSerial() const return ""; } -SensorData CameraStereoZedOC::captureImage(CameraInfo * info) +SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_ZEDOC diff --git a/corelib/src/camera/CameraVideo.cpp b/corelib/src/camera/CameraVideo.cpp index 53287da1ee..39b09909e0 100644 --- a/corelib/src/camera/CameraVideo.cpp +++ b/corelib/src/camera/CameraVideo.cpp @@ -162,7 +162,7 @@ std::string CameraVideo::getSerial() const return _guid; } -SensorData CameraVideo::captureImage(CameraInfo * info) +SensorData CameraVideo::captureImage(SensorCaptureInfo * info) { cv::Mat img; if(_capture.isOpened()) diff --git a/corelib/src/global_map/CloudMap.cpp b/corelib/src/global_map/CloudMap.cpp new file mode 100644 index 0000000000..bb3ce9fb92 --- /dev/null +++ b/corelib/src/global_map/CloudMap.cpp @@ -0,0 +1,153 @@ +/* +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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rtabmap { + +CloudMap::CloudMap(const LocalGridCache * cache, const ParametersMap & parameters) : + GlobalMap(cache, parameters), + assembledGround_(new pcl::PointCloud), + assembledObstacles_(new pcl::PointCloud), + assembledEmptyCells_(new pcl::PointCloud) +{ +} + +void CloudMap::clear() +{ + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledEmptyCells_->clear(); + GlobalMap::clear(); +} + +void CloudMap::assemble(const std::list > & newPoses) +{ + UTimer timer; + + bool assembledGroundUpdated = false; + bool assembledObstaclesUpdated = false; + bool assembledEmptyCellsUpdated = false; + + if(!cache().empty()) + { + UDEBUG("Updating from cache"); + for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) + { + if(uContains(cache(), iter->first)) + { + const LocalGrid & localGrid = cache().at(iter->first); + + UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols); + + addAssembledNode(iter->first, iter->second); + + //ground + if(localGrid.groundCells.cols) + { + if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols); + } + + *assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.groundCells), iter->second, 0, 255, 0); + assembledGroundUpdated = true; + } + + //empty + if(localGrid.emptyCells.cols) + { + if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols); + } + + *assembledEmptyCells_ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localGrid.emptyCells), iter->second); + assembledEmptyCellsUpdated = true; + } + + //obstacles + if(localGrid.obstacleCells.cols) + { + if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols); + } + + *assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.obstacleCells), iter->second, 255, 0, 0); + assembledObstaclesUpdated = true; + } + } + } + } + + + if(assembledGroundUpdated && assembledGround_->size() > 1) + { + assembledGround_ = util3d::voxelize(assembledGround_, cellSize_); + } + if(assembledObstaclesUpdated && assembledGround_->size() > 1) + { + assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_); + } + if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1) + { + assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_); + } + + UDEBUG("Occupancy Grid update time = %f s", timer.ticks()); +} + +unsigned long CloudMap::getMemoryUsed() const +{ + unsigned long memoryUsage = GlobalMap::getMemoryUsed(); + + if(assembledGround_.get()) + { + memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB); + } + if(assembledObstacles_.get()) + { + memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB); + } + if(assembledEmptyCells_.get()) + { + memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZ); + } + return memoryUsage; +} + +} diff --git a/corelib/src/global_map/GridMap.cpp b/corelib/src/global_map/GridMap.cpp new file mode 100644 index 0000000000..b3cccf2e89 --- /dev/null +++ b/corelib/src/global_map/GridMap.cpp @@ -0,0 +1,485 @@ +/* +Copyright (c) 2010-2023, 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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace rtabmap { + +GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) : + GlobalMap(cache, parameters), + minMapSize_(Parameters::defaultGridGlobalMinSize()) +{ + Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_); +} + +void GridMap::clear() +{ + gridMap_ = grid_map::GridMap(); + GlobalMap::clear(); +} + +cv::Mat GridMap::createHeightMap(float & xMin, float & yMin, float & cellSize) const +{ + return toImage("elevation", xMin, yMin, cellSize); +} + +cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) const +{ + return toImage("colors", xMin, yMin, cellSize); +} + +cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const +{ + if( gridMap_.hasBasicLayers()) + { + const grid_map::Matrix& data = gridMap_[layer]; + + cv::Mat image; + if(layer.compare("elevation") == 0) + { + image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_32FC1); + for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index(*iterator); + const float& value = data(index(0), index(1)); + const grid_map::Index imageIndex(iterator.getUnwrappedIndex()); + if (std::isfinite(value)) + { + image.at(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)) = value; + } + } + } + else if(layer.compare("colors") == 0) + { + image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_8UC3); + for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index(*iterator); + const float& value = data(index(0), index(1)); + const grid_map::Index imageIndex(iterator.getUnwrappedIndex()); + if (std::isfinite(value)) + { + const int * ptr = (const int *)&value; + cv::Vec3b & color = image.at(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)); + color[0] = (unsigned char)(*ptr & 0xFF); // B + color[1] = (unsigned char)((*ptr >> 8) & 0xFF); // G + color[2] = (unsigned char)((*ptr >> 16) & 0xFF); // R + } + } + } + else + { + UFATAL("Unknown layer \"%s\"", layer.c_str()); + } + + xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f; + yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f; + cellSize = gridMap_.getResolution(); + + return image; + + } + return cv::Mat(); +} + +pcl::PointCloud::Ptr GridMap::createTerrainCloud() const +{ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if( gridMap_.hasBasicLayers()) + { + const grid_map::Matrix& dataElevation = gridMap_["elevation"]; + const grid_map::Matrix& dataColors = gridMap_["colors"]; + + cloud->width = gridMap_.getSize()(0); + cloud->height = gridMap_.getSize()(1); + cloud->resize(cloud->width * cloud->height); + cloud->is_dense = false; + + float xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f; + float yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f; + float cellSize = gridMap_.getResolution(); + + for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) + { + const grid_map::Index index(*iterator); + const float& value = dataElevation(index(0), index(1)); + const int* color = (const int*)&dataColors(index(0), index(1)); + const grid_map::Index imageIndex(iterator.getUnwrappedIndex()); + pcl::PointXYZRGB & pt = cloud->at(cloud->width-1-imageIndex(0), imageIndex(1)); + if (std::isfinite(value)) + { + pt.x = xMin + (cloud->width-1-imageIndex(0)) * cellSize; + pt.y = yMin + (cloud->height-1-imageIndex(1)) * cellSize; + pt.z = value; + pt.b = (unsigned char)(*color & 0xFF); + pt.g = (unsigned char)((*color >> 8) & 0xFF); + pt.r = (unsigned char)((*color >> 16) & 0xFF); + } + else + { + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN(); + } + } + } + return cloud; +} + +pcl::PolygonMesh::Ptr GridMap::createTerrainMesh() const +{ + pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); + pcl::PointCloud::Ptr cloud = createTerrainCloud(); + if(!cloud->empty()) + { + mesh->polygons = util3d::organizedFastMesh( + cloud, + M_PI, + true, + 1); + + pcl::toPCLPointCloud2(*cloud, mesh->cloud); + } + + return mesh; +} + +void GridMap::assemble(const std::list > & newPoses) +{ + UTimer timer; + + float margin = cellSize_*10.0f; + + float minX=-minMapSize_/2.0f; + float minY=-minMapSize_/2.0f; + float maxX=minMapSize_/2.0f; + float maxY=minMapSize_/2.0f; + bool undefinedSize = minMapSize_ == 0.0f; + std::map occupiedLocalMaps; + + if(gridMap_.hasBasicLayers()) + { + // update + minX=minValues_[0]+margin+cellSize_/2.0f; + minY=minValues_[1]+margin+cellSize_/2.0f; + maxX=minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - margin; + maxY=minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - margin; + undefinedSize = false; + } + + for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) + { + UASSERT(!iter->second.isNull()); + + float x = iter->second.x(); + float y =iter->second.y(); + if(undefinedSize) + { + minX = maxX = x; + minY = maxY = y; + undefinedSize = false; + } + else + { + if(minX > x) + minX = x; + else if(maxX < x) + maxX = x; + + if(minY > y) + minY = y; + else if(maxY < y) + maxY = y; + } + } + + if(!cache().empty()) + { + UDEBUG("Updating from cache"); + for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) + { + if(uContains(cache(), iter->first)) + { + const LocalGrid & localGrid = cache().at(iter->first); + + if(!localGrid.is3D()) + { + UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)", + localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type()); + continue; + } + + UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols); + + //ground + cv::Mat occupied; + if(localGrid.groundCells.cols || localGrid.obstacleCells.cols) + { + occupied = cv::Mat(1, localGrid.groundCells.cols+localGrid.obstacleCells.cols, CV_32FC4); + } + if(localGrid.groundCells.cols) + { + if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols); + } + for(int i=0; i(0,i); + float * vo = occupied.ptr(0,i); + cv::Point3f vt; + vo[3] = 0xFFFFFFFF; // RGBA + if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5) + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); + if(localGrid.groundCells.channels() == 4) + { + vo[3] = vi[3]; + } + } + else + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); + } + vo[0] = vt.x; + vo[1] = vt.y; + vo[2] = vt.z; + if(minX > vo[0]) + minX = vo[0]; + else if(maxX < vo[0]) + maxX = vo[0]; + + if(minY > vo[1]) + minY = vo[1]; + else if(maxY < vo[1]) + maxY = vo[1]; + } + } + + //obstacles + if(localGrid.obstacleCells.cols) + { + if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols); + } + for(int i=0; i(0,i); + float * vo = occupied.ptr(0,i+localGrid.groundCells.cols); + cv::Point3f vt; + vo[3] = 0xFFFFFFFF; // RGBA + if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5) + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); + if(localGrid.obstacleCells.channels() == 4) + { + vo[3] = vi[3]; + } + } + else + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); + } + vo[0] = vt.x; + vo[1] = vt.y; + vo[2] = vt.z; + if(minX > vo[0]) + minX = vo[0]; + else if(maxX < vo[0]) + maxX = vo[0]; + + if(minY > vo[1]) + minY = vo[1]; + else if(maxY < vo[1]) + maxY = vo[1]; + } + } + uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied)); + } + } + } + + if(minX != maxX && minY != maxY) + { + //Get map size + float xMin = minX-margin; + xMin -= cellSize_/2.0f; + float yMin = minY-margin; + yMin -= cellSize_/2.0f; + float xMax = maxX+margin; + float yMax = maxY+margin; + + if(fabs((yMax - yMin) / cellSize_) > 99999 || + fabs((xMax - xMin) / cellSize_) > 99999) + { + UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). " + "There's maybe an error with the poses provided! The map will not be created!", + xMin, yMin, xMax, yMax); + } + else + { + UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax); + cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f); + if(!gridMap_.hasBasicLayers()) + { + UDEBUG("Map empty!"); + grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin); + grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f); + + UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]); + gridMap_.setGeometry(length, cellSize_, position); + UDEBUG("size: %d, %d", gridMap_.getSize()[0], gridMap_.getSize()[1]); + // Add elevation layer + gridMap_.add("elevation"); + gridMap_.add("node_ids"); + gridMap_.add("colors"); + gridMap_.setBasicLayers({"elevation"}); + } + else + { + if(xMin == minValues_[0] && yMin == minValues_[1] && + newMapSize.width == gridMap_.getSize()[0] && + newMapSize.height == gridMap_.getSize()[1]) + { + // same map size and origin, don't do anything + UDEBUG("Map same size!"); + } + else + { + UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str()); + UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str()); + UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_.getSize()[0], cellSize_).c_str()); + UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_.getSize()[1], cellSize_).c_str()); + + UDEBUG("Copy map"); + // copy the old map in the new map + // make sure the translation is cellSize + int deltaX = 0; + if(xMin < minValues_[0]) + { + deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f; + xMin = minValues_[0]-float(deltaX)*cellSize_; + } + int deltaY = 0; + if(yMin < minValues_[1]) + { + deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f; + yMin = minValues_[1]-float(deltaY)*cellSize_; + } + UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY); + newMapSize.width = (xMax - xMin) / cellSize_+0.5f; + newMapSize.height = (yMax - yMin) / cellSize_+0.5f; + UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], newMapSize.width, newMapSize.height); + UASSERT(newMapSize.width >= gridMap_.getSize()[0] && newMapSize.height >= gridMap_.getSize()[1]); + UASSERT(newMapSize.width >= gridMap_.getSize()[0]+deltaX && newMapSize.height >= gridMap_.getSize()[1]+deltaY); + UASSERT(deltaX>=0 && deltaY>=0); + + grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin); + grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f); + + grid_map::GridMap tmpExtendedMap; + tmpExtendedMap.setGeometry(length, cellSize_, position); + + UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]); + + UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)", + gridMap_.getLength()[0], gridMap_.getLength()[1], + gridMap_.getPosition()[0], gridMap_.getPosition()[1], + tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1], + tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]); + if(!gridMap_.extendToInclude(tmpExtendedMap)) + { + UERROR("Failed to update size of the grid map"); + } + UDEBUG("Updated side: %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]); + } + } + UDEBUG("map %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]); + if(newPoses.size()) + { + UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first); + } + grid_map::Matrix& gridMapData = gridMap_["elevation"]; + grid_map::Matrix& gridMapNodeIds = gridMap_["node_ids"]; + grid_map::Matrix& gridMapColors = gridMap_["colors"]; + for(std::list >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter) + { + std::map::iterator iter = occupiedLocalMaps.find(kter->first); + if(iter!=occupiedLocalMaps.end()) + { + addAssembledNode(kter->first, kter->second); + + for(int i=0; isecond.cols; ++i) + { + float * ptf = iter->second.ptr(0,i); + grid_map::Position position(ptf[0], ptf[1]); + grid_map::Index index; + if(gridMap_.getIndex(position, index)) + { + // If no elevation has been set, use current elevation. + if (!gridMap_.isValid(index)) + { + gridMapData(index(0), index(1)) = ptf[2]; + gridMapNodeIds(index(0), index(1)) = kter->first; + gridMapColors(index(0), index(1)) = ptf[3]; + } + else + { + if ((gridMapData(index(0), index(1)) < ptf[2] && (gridMapNodeIds(index(0), index(1)) <= kter->first || kter->first == -1)) || + gridMapNodeIds(index(0), index(1)) < kter->first) + { + gridMapData(index(0), index(1)) = ptf[2]; + gridMapNodeIds(index(0), index(1)) = kter->first; + gridMapColors(index(0), index(1)) = ptf[3]; + } + } + } + else + { + UERROR("Outside map!? (%d) (%f,%f) -> (%d,%d)", i, ptf[0], ptf[1], index[0], index[1]); + } + } + } + } + + minValues_[0] = xMin; + minValues_[1] = yMin; + } + } + UDEBUG("Occupancy Grid update time = %f s", timer.ticks()); +} + +} diff --git a/corelib/src/global_map/OccupancyGrid.cpp b/corelib/src/global_map/OccupancyGrid.cpp new file mode 100644 index 0000000000..f375564c90 --- /dev/null +++ b/corelib/src/global_map/OccupancyGrid.cpp @@ -0,0 +1,682 @@ +/* +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. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rtabmap { + +OccupancyGrid::OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters) : + GlobalMap(cache, parameters), + minMapSize_(Parameters::defaultGridGlobalMinSize()), + erode_(Parameters::defaultGridGlobalEroded()), + footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()) +{ + Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_); + Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_); + Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_); + + UASSERT(minMapSize_ >= 0.0f); +} + +void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map & poses) +{ + UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d", + map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size()); + this->clear(); + if(!poses.empty() && !map.empty()) + { + UASSERT(cellSize > 0.0f); + UASSERT(map.type() == CV_8SC1); + map_ = map.clone(); + mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4); + for(int i=0; i(i,j); + float * info = mapInfo_.ptr(i,j); + if(value == 0) + { + info[3] = logOddsClampingMin_; + } + else if(value == 100) + { + info[3] = logOddsClampingMax_; + } + } + } + minValues_[0] = xMin; + minValues_[1] = yMin; + cellSize_ = cellSize; + addAssembledNode(poses.lower_bound(1)->first, poses.lower_bound(1)->second); + } +} + +void OccupancyGrid::clear() +{ + map_ = cv::Mat(); + mapInfo_ = cv::Mat(); + cellCount_.clear(); + GlobalMap::clear(); +} + +cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const +{ + xMin = minValues_[0]; + yMin = minValues_[1]; + + cv::Mat map = map_; + + UTimer t; + if(occupancyThr_ != 0.0f && !map.empty()) + { + float occThr = logodds(occupancyThr_); + map = cv::Mat(map.size(), map.type()); + UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows); + for(int i=0; i(i, j); + if(info[3] == 0.0f) + { + map.at(i, j) = -1; // unknown + } + else if(info[3] >= occThr) + { + map.at(i, j) = 100; // unknown + } + else + { + map.at(i, j) = 0; // empty + } + } + } + UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks()); + } + + if(erode_ && !map.empty()) + { + map = util3d::erodeMap(map); + UDEBUG("Eroding map = %fs", t.ticks()); + } + return map; +} + +cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const +{ + xMin = minValues_[0]; + yMin = minValues_[1]; + + cv::Mat map; + if(!mapInfo_.empty()) + { + map = cv::Mat(mapInfo_.size(), map_.type()); + for(int i=0; i(i, j); + if(info[3] == 0.0f) + { + map.at(i, j) = -1; // unknown + } + else + { + map.at(i, j) = char(probability(info[3])*100.0f); // empty + } + } + } + } + else + { + UWARN("Map info is empty, cannot generate probabilistic occupancy grid"); + } + return map; +} + +void OccupancyGrid::assemble(const std::list > & newPoses) +{ + UTimer timer; + + float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_; + + float minX=-minMapSize_/2.0f; + float minY=-minMapSize_/2.0f; + float maxX=minMapSize_/2.0f; + float maxY=minMapSize_/2.0f; + bool undefinedSize = minMapSize_ == 0.0f; + std::map emptyLocalMaps; + std::map occupiedLocalMaps; + + if(!map_.empty()) + { + // update + minX=minValues_[0]+margin+cellSize_/2.0f; + minY=minValues_[1]+margin+cellSize_/2.0f; + maxX=minValues_[0]+float(map_.cols)*cellSize_ - margin; + maxY=minValues_[1]+float(map_.rows)*cellSize_ - margin; + undefinedSize = false; + } + + for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) + { + UASSERT(!iter->second.isNull()); + + float x = iter->second.x(); + float y =iter->second.y(); + if(undefinedSize) + { + minX = maxX = x; + minY = maxY = y; + undefinedSize = false; + } + else + { + if(minX > x) + minX = x; + else if(maxX < x) + maxX = x; + + if(minY > y) + minY = y; + else if(maxY < y) + maxY = y; + } + } + + if(!cache().empty()) + { + UDEBUG("Updating from cache"); + for(std::list >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) + { + if(uContains(cache(), iter->first)) + { + const LocalGrid & localGrid = cache().at(iter->first); + + UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols); + + //ground + cv::Mat ground; + if(localGrid.groundCells.cols || localGrid.emptyCells.cols) + { + ground = cv::Mat(1, localGrid.groundCells.cols+localGrid.emptyCells.cols, CV_32FC2); + } + if(localGrid.groundCells.cols) + { + if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols); + } + for(int i=0; i(0,i); + float * vo = ground.ptr(0,i); + cv::Point3f vt; + if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5) + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); + } + else + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); + } + vo[0] = vt.x; + vo[1] = vt.y; + if(minX > vo[0]) + minX = vo[0]; + else if(maxX < vo[0]) + maxX = vo[0]; + + if(minY > vo[1]) + minY = vo[1]; + else if(maxY < vo[1]) + maxY = vo[1]; + } + } + + //empty + if(localGrid.emptyCells.cols) + { + if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols); + } + for(int i=0; i(0,i); + float * vo = ground.ptr(0,i+localGrid.groundCells.cols); + cv::Point3f vt; + if(localGrid.emptyCells.channels() != 2 && localGrid.emptyCells.channels() != 5) + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); + } + else + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); + } + vo[0] = vt.x; + vo[1] = vt.y; + if(minX > vo[0]) + minX = vo[0]; + else if(maxX < vo[0]) + maxX = vo[0]; + + if(minY > vo[1]) + minY = vo[1]; + else if(maxY < vo[1]) + maxY = vo[1]; + } + } + uInsert(emptyLocalMaps, std::make_pair(iter->first, ground)); + + //obstacles + if(localGrid.obstacleCells.cols) + { + if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1) + { + UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols); + } + cv::Mat obstacles(1, localGrid.obstacleCells.cols, CV_32FC2); + for(int i=0; i(0,i); + float * vo = obstacles.ptr(0,i); + cv::Point3f vt; + if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5) + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second); + } + else + { + vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second); + } + vo[0] = vt.x; + vo[1] = vt.y; + if(minX > vo[0]) + minX = vo[0]; + else if(maxX < vo[0]) + maxX = vo[0]; + + if(minY > vo[1]) + minY = vo[1]; + else if(maxY < vo[1]) + maxY = vo[1]; + } + uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles)); + } + } + } + } + + cv::Mat map; + cv::Mat mapInfo; + if(minX != maxX && minY != maxY) + { + //Get map size + float xMin = minX-margin; + xMin -= cellSize_/2.0f; + float yMin = minY-margin; + yMin -= cellSize_/2.0f; + float xMax = maxX+margin; + float yMax = maxY+margin; + + if(fabs((yMax - yMin) / cellSize_) > 99999 || + fabs((xMax - xMin) / cellSize_) > 99999) + { + UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). " + "There's maybe an error with the poses provided! The map will not be created!", + xMin, yMin, xMax, yMax); + } + else + { + UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax); + cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f); + if(map_.empty()) + { + UDEBUG("Map empty!"); + map = cv::Mat::ones(newMapSize, CV_8S)*-1; + mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4); + } + else + { + if(xMin == minValues_[0] && yMin == minValues_[1] && + newMapSize.width == map_.cols && + newMapSize.height == map_.rows) + { + // same map size and origin, don't do anything + UDEBUG("Map same size!"); + map = map_; + mapInfo = mapInfo_; + } + else + { + UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str()); + UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str()); + UASSERT_MSG(xMax >= minValues_[0]+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], map_.cols, cellSize_).c_str()); + UASSERT_MSG(yMax >= minValues_[1]+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], map_.rows, cellSize_).c_str()); + + UDEBUG("Copy map"); + // copy the old map in the new map + // make sure the translation is cellSize + int deltaX = 0; + if(xMin < minValues_[0]) + { + deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f; + xMin = minValues_[0]-float(deltaX)*cellSize_; + } + int deltaY = 0; + if(yMin < minValues_[1]) + { + deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f; + yMin = minValues_[1]-float(deltaY)*cellSize_; + } + UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY); + newMapSize.width = (xMax - xMin) / cellSize_+0.5f; + newMapSize.height = (yMax - yMin) / cellSize_+0.5f; + UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height); + UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows); + UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY); + UASSERT(deltaX>=0 && deltaY>=0); + map = cv::Mat::ones(newMapSize, CV_8S)*-1; + mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type()); + map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows))); + mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows))); + } + } + UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows); + UDEBUG("map %d %d", map.cols, map.rows); + if(newPoses.size()) + { + UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first); + } + for(std::list >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter) + { + std::map::iterator iter = emptyLocalMaps.find(kter->first); + std::map::iterator jter = occupiedLocalMaps.find(kter->first); + if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end()) + { + addAssembledNode(kter->first, kter->second); + std::map >::iterator cter = cellCount_.find(kter->first); + if(cter == cellCount_.end() && kter->first > 0) + { + cter = cellCount_.insert(std::make_pair(kter->first, std::pair(0,0))).first; + } + if(iter!=emptyLocalMaps.end()) + { + for(int i=0; isecond.cols; ++i) + { + float * ptf = iter->second.ptr(0,i); + cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_); + UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols, + uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d", + kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str()); + char & value = map.at(pt.y, pt.x); + if(value != -2) + { + float * info = mapInfo.ptr(pt.y, pt.x); + int nodeId = (int)info[0]; + if(value != -1) + { + if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) + { + // cannot rewrite on cells referred by more recent nodes + continue; + } + if(nodeId > 0) + { + std::map >::iterator eter = cellCount_.find(nodeId); + UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); + if(value == 0) + { + eter->second.first -= 1; + } + else if(value == 100) + { + eter->second.second -= 1; + } + if(kter->first < 0) + { + eter->second.first += 1; + } + } + } + if(kter->first > 0) + { + info[0] = (float)kter->first; + info[1] = ptf[0]; + info[2] = ptf[1]; + cter->second.first+=1; + } + value = 0; // free space + + // update odds + if(nodeId != kter->first) + { + info[3] += logOddsMiss_; + if (info[3] < logOddsClampingMin_) + { + info[3] = logOddsClampingMin_; + } + if (info[3] > logOddsClampingMax_) + { + info[3] = logOddsClampingMax_; + } + } + } + } + } + + if(footprintRadius_ >= cellSize_*1.5f) + { + // place free space under the footprint of the robot + cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_); + cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_); + if(ptBegin.x < 0) + ptBegin.x = 0; + if(ptEnd.x >= map.cols) + ptEnd.x = map.cols-1; + + if(ptBegin.y < 0) + ptBegin.y = 0; + if(ptEnd.y >= map.rows) + ptEnd.y = map.rows-1; + + for(int i=ptBegin.x; i(j, i); + float * info = mapInfo.ptr(j, i); + int nodeId = (int)info[0]; + if(value != -1) + { + if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) + { + // cannot rewrite on cells referred by more recent nodes + continue; + } + if(nodeId>0) + { + std::map >::iterator eter = cellCount_.find(nodeId); + UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); + if(value == 0) + { + eter->second.first -= 1; + } + else if(value == 100) + { + eter->second.second -= 1; + } + if(kter->first < 0) + { + eter->second.first += 1; + } + } + } + if(kter->first > 0) + { + info[0] = (float)kter->first; + info[1] = float(i) * cellSize_ + xMin; + info[2] = float(j) * cellSize_ + yMin; + info[3] = logOddsClampingMin_; + cter->second.first+=1; + } + value = -2; // free space (footprint) + } + } + } + + if(jter!=occupiedLocalMaps.end()) + { + for(int i=0; isecond.cols; ++i) + { + float * ptf = jter->second.ptr(0,i); + cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_); + UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols, + uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d", + kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str()); + char & value = map.at(pt.y, pt.x); + if(value != -2) + { + float * info = mapInfo.ptr(pt.y, pt.x); + int nodeId = (int)info[0]; + if(value != -1) + { + if(kter->first > 0 && (kter->first < nodeId || nodeId < 0)) + { + // cannot rewrite on cells referred by more recent nodes + continue; + } + if(nodeId>0) + { + std::map >::iterator eter = cellCount_.find(nodeId); + UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str()); + if(value == 0) + { + eter->second.first -= 1; + } + else if(value == 100) + { + eter->second.second -= 1; + } + if(kter->first < 0) + { + eter->second.second += 1; + } + } + } + if(kter->first > 0) + { + info[0] = (float)kter->first; + info[1] = ptf[0]; + info[2] = ptf[1]; + cter->second.second+=1; + } + + // update odds + if(nodeId != kter->first || value!=100) + { + info[3] += logOddsHit_; + if (info[3] < logOddsClampingMin_) + { + info[3] = logOddsClampingMin_; + } + if (info[3] > logOddsClampingMax_) + { + info[3] = logOddsClampingMax_; + } + } + + value = 100; // obstacles + } + } + } + } + } + + if(footprintRadius_ >= cellSize_*1.5f) + { + for(int i=1; i(i, j); + if(value == -2) + { + value = 0; + } + } + } + } + + map_ = map; + mapInfo_ = mapInfo; + minValues_[0] = xMin; + minValues_[1] = yMin; + + // clean cellCount_ + for(std::map >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();) + { + UASSERT(iter->second.first >= 0 && iter->second.second >= 0); + if(iter->second.first == 0 && iter->second.second == 0) + { + cellCount_.erase(iter++); + } + else + { + ++iter; + } + } + } + } + + UDEBUG("Occupancy Grid update time = %f s", timer.ticks()); +} + +unsigned long OccupancyGrid::getMemoryUsed() const +{ + unsigned long memoryUsage = GlobalMap::getMemoryUsed(); + + memoryUsage += map_.total() * map_.elemSize(); + memoryUsage += mapInfo_.total() * mapInfo_.elemSize(); + memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair) + sizeof(std::map >::iterator)) + sizeof(std::map >); + + return memoryUsage; +} + +} diff --git a/corelib/src/OctoMap.cpp b/corelib/src/global_map/OctoMap.cpp similarity index 72% rename from corelib/src/OctoMap.cpp rename to corelib/src/global_map/OctoMap.cpp index 8a1e1bb57e..d4c2249966 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/global_map/OctoMap.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include @@ -288,55 +288,40 @@ RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTr // OctoMap ////////////////////////////////////// -OctoMap::OctoMap(const ParametersMap & parameters) : +OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) : + GlobalMap(cache, parameters), hasColor_(false), - fullUpdate_(Parameters::defaultGridGlobalFullUpdate()), - updateError_(Parameters::defaultGridGlobalUpdateError()), rangeMax_(Parameters::defaultGridRangeMax()), rayTracing_(Parameters::defaultGridRayTracing()), emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth()) { - float cellSize = Parameters::defaultGridCellSize(); - Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize); - UASSERT(cellSize>0.0f); - - minValues_[0] = minValues_[1] = minValues_[2] = 0.0; - maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; - - float occupancyThr = Parameters::defaultGridGlobalOccupancyThr(); - float probHit = Parameters::defaultGridGlobalProbHit(); - float probMiss = Parameters::defaultGridGlobalProbMiss(); - float clampingMin = Parameters::defaultGridGlobalProbClampingMin(); - float clampingMax = Parameters::defaultGridGlobalProbClampingMax(); - Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr); - Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit); - Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss); - Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin); - Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax); - - octree_ = new RtabmapColorOcTree(cellSize); - if(occupancyThr <= 0.0f) + octree_ = new RtabmapColorOcTree(cellSize_); + if(occupancyThr_ <= 0.0f) { UWARN("Cannot set %s to null for OctoMap, using default value %f instead.", Parameters::kGridGlobalOccupancyThr().c_str(), Parameters::defaultGridGlobalOccupancyThr()); - occupancyThr = Parameters::defaultGridGlobalOccupancyThr(); + occupancyThr_ = Parameters::defaultGridGlobalOccupancyThr(); } - octree_->setOccupancyThres(occupancyThr); - octree_->setProbHit(probHit); - octree_->setProbMiss(probMiss); - octree_->setClampingThresMin(clampingMin); - octree_->setClampingThresMax(clampingMax); - Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_); - - Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_); + + UDEBUG("occupancyThr_=%f", occupancyThr_); + UDEBUG("probHit_=%f", probability(logOddsHit_)); + UDEBUG("probMiss_=%f", probability(logOddsMiss_)); + UDEBUG("probClampingMin_=%f", probability(logOddsClampingMin_)); + UDEBUG("probClampingMax_=%f", probability(logOddsClampingMax_)); + + octree_->setOccupancyThres(occupancyThr_); + octree_->setProbHit(probability(logOddsHit_)); + octree_->setProbMiss(probability(logOddsMiss_)); + octree_->setClampingThresMin(probability(logOddsClampingMin_)); + octree_->setClampingThresMax(probability(logOddsClampingMax_)); + Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_); Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_); + Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_); UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16); - UDEBUG("fullUpdate_ =%s", fullUpdate_?"true":"false"); - UDEBUG("updateError_ =%f", updateError_); UDEBUG("rangeMax_ =%f", rangeMax_); UDEBUG("rayTracing_ =%s", rayTracing_?"true":"false"); UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_); @@ -351,47 +336,17 @@ OctoMap::~OctoMap() void OctoMap::clear() { octree_->clear(); - cache_.clear(); - cacheClouds_.clear(); - cacheViewPoints_.clear(); - addedNodes_.clear(); hasColor_ = false; - minValues_[0] = minValues_[1] = minValues_[2] = 0.0; - maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; + GlobalMap::clear(); } -void OctoMap::addToCache(int nodeId, - const pcl::PointCloud::Ptr & ground, - const pcl::PointCloud::Ptr & obstacles, - const pcl::PointXYZ & viewPoint) +unsigned long OctoMap::getMemoryUsed() const { - UDEBUG("nodeId=%d", nodeId); - if(nodeId < 0) - { - UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId); - return; - } - cacheClouds_.erase(nodeId==0?-1:nodeId); - cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles))); - uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z))); -} -void OctoMap::addToCache(int nodeId, - const cv::Mat & ground, - const cv::Mat & obstacles, - const cv::Mat & empty, - const cv::Point3f & viewPoint) -{ - UDEBUG("nodeId=%d", nodeId); - if(nodeId < 0) - { - UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId); - return; - } - UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str()); - UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str()); - UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str()); - uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty))); - uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint)); + unsigned long memoryUsage = GlobalMap::getMemoryUsed(); + + // Note: size of OctoMap object is missing. + + return memoryUsage; } bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition) @@ -509,227 +464,67 @@ std::unordered_set OctoMap::fin } -bool OctoMap::update(const std::map & poses) +void OctoMap::assemble(const std::list > & newPoses) { - UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); - - // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes. - bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified) - bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map - std::map transforms; - std::map updatedAddedNodes; - float updateErrorSqrd = updateError_*updateError_; - for(std::map::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) - { - std::map::const_iterator jter = poses.find(iter->first); - if(jter != poses.end()) - { - graphChanged = false; - UASSERT(!iter->second.isNull() && !jter->second.isNull()); - Transform t = Transform::getIdentity(); - if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd) - { - t = jter->second * iter->second.inverse(); - graphOptimized = true; - } - transforms.insert(std::make_pair(jter->first, t)); - updatedAddedNodes.insert(std::make_pair(jter->first, jter->second)); - } - else - { - UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first); - } - } - - if(graphOptimized || graphChanged) - { - if(graphChanged) - { - UWARN("Graph has changed! The whole map should be rebuilt."); - } - else - { - UINFO("Graph optimized!"); - } - - minValues_[0] = minValues_[1] = minValues_[2] = 0.0; - maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0; - - if(fullUpdate_ || graphChanged) - { - // clear all but keep cache - octree_->clear(); - addedNodes_.clear(); - hasColor_ = false; - } - else - { - RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution()); - int copied=0; - int count=0; - UTimer t; - for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count) - { - RtabmapColorOcTreeNode & nOld = *it; - if(nOld.getNodeRefId() > 0) - { - std::map::iterator jter = transforms.find(nOld.getNodeRefId()); - if(jter != transforms.end()) - { - octomap::point3d pt; - std::map::iterator pter = addedNodes_.find(nOld.getNodeRefId()); - UASSERT(pter != addedNodes_.end()); - - if(nOld.getOccupancyType() > 0) - { - pt = nOld.getPointRef(); - } - else - { - pt = octree_->keyToCoord(it.getKey()); - } - - cv::Point3f cvPt(pt.x(), pt.y(), pt.z()); - cvPt = util3d::transformPoint(cvPt, jter->second); - octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z); - - octomap::OcTreeKey key; - if(newOcTree->coordToKeyChecked(ptTransformed, key)) - { - RtabmapColorOcTreeNode * n = newOcTree->search(key); - if(n) - { - if(n->getNodeRefId() > nOld.getNodeRefId()) - { - // The cell has been updated from more recent node, don't update the cell - continue; - } - else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0) - { - // empty cells cannot overwrite ground/obstacle cells - continue; - } - } - - RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds()); - if(nNew) - { - ++copied; - updateMinMax(ptTransformed); - nNew->setNodeRefId(nOld.getNodeRefId()); - if(nOld.getOccupancyType() > 0) - { - nNew->setPointRef(pt); - } - nNew->setOccupancyType(nOld.getOccupancyType()); - nNew->setColor(nOld.getColor()); - } - else - { - UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z); - } - } - else - { - UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z); - } - } - else if(jter == transforms.end()) - { - // Note: normal if old nodes were transfered to LTM - //UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size()); - } - } - } - UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks()); - delete octree_; - octree_ = newOcTree; - - //update added poses - addedNodes_ = updatedAddedNodes; - } - } - // Original version from A. Hornung: // https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356 // - std::list > orderedPoses; - int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0; + int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0; UDEBUG("Last id = %d", lastId); - // add old poses that were not in the current map (they were just retrieved from LTM) - for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) - { - if(addedNodes_.find(iter->first) == addedNodes_.end()) - { - orderedPoses.push_back(*iter); - } - } + UDEBUG("newPoses = %d", (int)newPoses.size()); - // insert zero after - if(poses.find(0) != poses.end()) - { - orderedPoses.push_back(std::make_pair(-1, poses.at(0))); - } - - UDEBUG("orderedPoses = %d", (int)orderedPoses.size()); - - if(!orderedPoses.empty()) + if(!newPoses.empty()) { float rangeMaxSqrd = rangeMax_*rangeMax_; float cellSize = octree_->getResolution(); - for(std::list >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter) + for(std::list >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter) { - std::map::Ptr, const pcl::PointCloud::Ptr> >::iterator cloudIter; - std::map, cv::Mat> >::iterator occupancyIter; - std::map::iterator viewPointIter; - cloudIter = cacheClouds_.find(iter->first); - occupancyIter = cache_.find(iter->first); - viewPointIter = cacheViewPoints_.find(iter->first); - if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end()) + std::map::const_iterator localGridIter; + localGridIter = cache().find(iter->first); + if(localGridIter != cache().end()) { + cv::Mat ground = localGridIter->second.groundCells; + cv::Mat obstacles = localGridIter->second.obstacleCells; + cv::Mat emptyCells = localGridIter->second.emptyCells; + + if(!localGridIter->second.is3D()) + { + UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)", + ground.type(), obstacles.type(), emptyCells.type()); + continue; + } + UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution()); - UASSERT(viewPointIter != cacheViewPoints_.end()); octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z()); - sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z); + sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z); updateMinMax(sensorOrigin); octomap::OcTreeKey tmpKey; - if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey) - || !octree_->coordToKeyChecked(sensorOrigin, tmpKey)) + if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)) { UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z()); } - bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty()); + bool computeRays = rayTracing_ && emptyCells.empty(); // instead of direct scan insertion, compute update to filter ground: octomap::KeySet free_cells; // insert ground points only as free: - unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size(); + unsigned int maxGroundPts = ground.cols; UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts); Eigen::Affine3f t = iter->second.toEigen3f(); - LaserScan tmpGround; - if(occupancyIter != cache_.end()) - { - tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first); - UASSERT(tmpGround.size() == (int)maxGroundPts); - } + LaserScan tmpGround = LaserScan::backwardCompatibility(ground); + UASSERT(tmpGround.size() == (int)maxGroundPts); for (unsigned int i=0; isecond.first->at(i), t); - } + pt = util3d::laserScanToPointRGB(tmpGround, i); + pt = pcl::transformPoint(pt, t); + octomap::point3d point(pt.x, pt.y, pt.z); bool ignoreOccupiedCell = false; if(rangeMaxSqrd > 0.0f) @@ -793,26 +588,15 @@ bool OctoMap::update(const std::map & poses) UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size()); // all other points: free on ray, occupied on endpoint: - unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size(); + unsigned int maxObstaclePts = obstacles.cols; UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts); - LaserScan tmpObstacle; - if(occupancyIter != cache_.end()) - { - tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second); - UASSERT(tmpObstacle.size() == (int)maxObstaclePts); - } + LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles); + UASSERT(tmpObstacle.size() == (int)maxObstaclePts); for (unsigned int i=0; isecond.second->at(i), t); - } + pt = util3d::laserScanToPointRGB(tmpObstacle, i); + pt = pcl::transformPoint(pt, t); octomap::point3d point(pt.x, pt.y, pt.z); @@ -903,11 +687,11 @@ bool OctoMap::update(const std::map & poses) } // all empty cells - if(occupancyIter != cache_.end() && occupancyIter->second.second.cols) + if(emptyCells.cols) { - unsigned int maxEmptyPts = occupancyIter->second.second.cols; + unsigned int maxEmptyPts = emptyCells.cols; UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts); - LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second); + LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells); UASSERT(tmpEmpty.size() == (int)maxEmptyPts); for (unsigned int i=0; i & poses) } } - if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty()) + if(emptyCells.cols || !free_cells.empty()) { octree_->updateInnerOccupancy(); } // compress map - //if(orderedPoses.size() > 1) + //if(newPoses.size() > 1) //{ // octree_->prune(); //} - // ignore negative ids as they are temporary clouds - if(iter->first > 0) - { - addedNodes_.insert(*iter); - } + addAssembledNode(iter->first, iter->second); UDEBUG("%d: end", iter->first); } else @@ -1005,21 +785,12 @@ bool OctoMap::update(const std::map & poses) } } - for(unsigned int y=0; y < nodeToDelete.size(); y++) { octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_); } UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks()); } - - if(!fullUpdate_) - { - cache_.clear(); - cacheClouds_.clear(); - cacheViewPoints_.clear(); - } - return !orderedPoses.empty() || graphOptimized || graphChanged || emptyFloodFillDepth_>0; } void OctoMap::updateMinMax(const octomap::point3d & point) diff --git a/corelib/src/icp/libpointmatcher.h b/corelib/src/icp/libpointmatcher.h index e2d362a6cd..46a5ef0830 100644 --- a/corelib/src/icp/libpointmatcher.h +++ b/corelib/src/icp/libpointmatcher.h @@ -513,16 +513,28 @@ struct KDTreeMatcherIntensity : public PointMatcher::Matcher for (int i = 0; i < pointsCount; ++i) { float minDistance = std::numeric_limits::max(); + bool minDistFound = false; for(int k=0; k +#include +#include +#include + +#if PCL_VERSION_COMPARE(<, 1, 9, 0) +#define VLP_MAX_NUM_LASERS 16 +#define VLP_DUAL_MODE 0x39 +#endif + +namespace rtabmap { + +/** @brief Function used to check that hour assigned to timestamp in conversion is + * correct. Velodyne only returns time since the top of the hour, so if the computer clock + * and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong + * hour may be associated with the timestamp + * + * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley + * Original license: BSD License 2.0 + * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp + * + * @param stamp timestamp recovered from velodyne + * @param nominal_stamp time coming from computer's clock + * @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments + * disagree by more than a half-hour. + */ +double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) { + const int HALFHOUR_TO_SEC = 1800; + double retval = stamp; + if (nominal_stamp > stamp) { + if (nominal_stamp - stamp > HALFHOUR_TO_SEC) { + retval = retval + 2*HALFHOUR_TO_SEC; + } + } else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) { + retval = retval - 2*HALFHOUR_TO_SEC; + } + return retval; +} + +/* + * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley + * Original license: BSD License 2.0 + * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp + */ +double rosTimeFromGpsTimestamp(const uint32_t data) { + const int HOUR_TO_SEC = 3600; + // time for each packet is a 4 byte uint + // It is the number of microseconds from the top of the hour + double time_nom = UTimer::now(); + uint32_t cur_hour = time_nom / HOUR_TO_SEC; + double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000; + stamp = resolveHourAmbiguity(stamp, time_nom); + return stamp; +} + +LidarVLP16::LidarVLP16( + const std::string& pcapFile, + bool organized, + bool stampLast, + float frameRate, + Transform localTransform) : + Lidar(frameRate, localTransform), + pcl::VLPGrabber(pcapFile), + timingOffsetsDualMode_(false), + startSweepTime_(0), + startSweepTimeHost_(0), + organized_(organized), + useHostTime_(false), + stampLast_(stampLast) +{ + UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str()); +} +LidarVLP16::LidarVLP16( + const boost::asio::ip::address& ipAddress, + const std::uint16_t port, + bool organized, + bool useHostTime, + bool stampLast, + float frameRate, + Transform localTransform) : + Lidar(frameRate, localTransform), + pcl::VLPGrabber(ipAddress, port), + timingOffsetsDualMode_(false), + startSweepTime_(0), + startSweepTimeHost_(0), + organized_(organized), + useHostTime_(useHostTime), + stampLast_(stampLast) +{ + UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port); +} + +LidarVLP16::~LidarVLP16() +{ + UDEBUG("Stopping lidar..."); + stop(); + scanReady_.release(); + UDEBUG("Stopped lidar!"); +} + +void LidarVLP16::setOrganized(bool enable) +{ + organized_ = true; +} + +bool LidarVLP16::init(const std::string &, const std::string &) +{ + UDEBUG("Init lidar"); + if(isRunning()) + { + UDEBUG("Stopping lidar..."); + stop(); + uSleep(2000); // make sure all callbacks are finished + UDEBUG("Stopped lidar!"); + } + startSweepTime_ = 0.0; + startSweepTimeHost_ = 0.0; + accumulatedScans_.clear(); + if(organized_) + { + accumulatedScans_.resize(16); + } + else + { + accumulatedScans_.resize(1); + } + buildTimings(false); + start(); + UDEBUG("Lidar capture started"); + return true; +} + +/** + * Build a timing table for each block/firing. Stores in timing_offsets vector + */ +void LidarVLP16::buildTimings(bool dualMode) +{ + // vlp16 + // timing table calculation, from velodyne user manual + timingOffsets_.resize(12); + for (size_t i=0; i < timingOffsets_.size(); ++i){ + timingOffsets_[i].resize(32); + } + // constants + double full_firing_cycle = 55.296 * 1e-6; // seconds + double single_firing = 2.304 * 1e-6; // seconds + double dataBlockIndex, dataPointIndex; + // compute timing offsets + for (size_t x = 0; x < timingOffsets_.size(); ++x){ + for (size_t y = 0; y < timingOffsets_[x].size(); ++y){ + if (dualMode){ + dataBlockIndex = (x - (x % 2)) + (y / 16); + } + else{ + dataBlockIndex = (x * 2) + (y / 16); + } + dataPointIndex = y % 16; + //timing_offsets[block][firing] + timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex); + } + } + timingOffsetsDualMode_ = dualMode; +} + +void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket) +{ + if (sizeof(HDLLaserReturn) != 3) + return; + + double receivedHostTime = UTimer::now(); + double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp); + if(startSweepTime_==0) + { + startSweepTime_ = packetStamp; + startSweepTimeHost_ = receivedHostTime; + } + + bool dualMode = dataPacket->mode == VLP_DUAL_MODE; + if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode) + { + // reset everything + timingOffsets_.clear(); + buildTimings(dualMode); + startSweepTime_ = packetStamp; + startSweepTimeHost_ = receivedHostTime; + for(size_t i=0; ifiringData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition) + { + interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0; + } + else + { + interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0; + } + + for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i) + { + HDLFiringData firing_data = dataPacket->firingData[i]; + + for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++) + { + double current_azimuth = firing_data.rotationalPosition; + if (j >= VLP_MAX_NUM_LASERS) + { + current_azimuth += interpolated_azimuth_delta; + } + if (current_azimuth > 36000) + { + current_azimuth -= 36000; + } + + double t = 0; + if (timingOffsets_.size()) + t = timingOffsets_[i][j]; + + if (current_azimuth < HDLGrabber::last_azimuth_) + { + if (!accumulatedScans_[0].empty()) + { + UScopeMutex lock(lastScanMutex_); + bool notify = lastScan_.laserScanRaw().empty(); + if(stampLast_) + { + double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t; + double diff = lastStamp - startSweepTime_; + lastScan_.setStamp(useHostTime_?startSweepTimeHost_+diff:lastStamp); + for(size_t r=0; r 1) + { + cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(); + for(size_t k=1; k1) + { + accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit); + } + else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z))) + { + accumulatedScans_[0].push_back (xyzit); + } + last_azimuth_ = current_azimuth; + + if (dualMode) + { + pcl::PointXYZI dual_xyzi; + HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]); + + if(accumulatedScans_.size()>1) + { + xyzit.x = dual_xyzi.y; + xyzit.y = -dual_xyzi.x; + xyzit.z = dual_xyzi.z; + xyzit.i = dual_xyzi.intensity; + xyzit.t = timeSinceStartOfThisScan; + accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit); + } + else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z) + && ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z))) + { + xyzit.x = dual_xyzi.y; + xyzit.y = -dual_xyzi.x; + xyzit.z = dual_xyzi.z; + xyzit.i = dual_xyzi.intensity; + xyzit.t = timeSinceStartOfThisScan; + accumulatedScans_[0].push_back (xyzit); + } + } + } + if (dualMode) + { + i++; + } + } +} + +SensorData LidarVLP16::captureData(SensorCaptureInfo * info) +{ + SensorData data; + if(scanReady_.acquire(1, 5000)) + { + UScopeMutex lock(lastScanMutex_); + if(!lastScan_.laserScanRaw().empty()) + { + data = lastScan_; + lastScan_ = SensorData(); + } + } + else + { + UWARN("Did not receive any scans for the past 5 seconds."); + } + return data; +} + + +} /* namespace rtabmap */ diff --git a/corelib/src/odometry/OdometryF2F.cpp b/corelib/src/odometry/OdometryF2F.cpp index 49c02a87fb..0f335fe9fc 100644 --- a/corelib/src/odometry/OdometryF2F.cpp +++ b/corelib/src/odometry/OdometryF2F.cpp @@ -74,15 +74,15 @@ Transform OdometryF2F::computeTransform( UTimer timer; Transform output; if(!data.rightRaw().empty() && - (data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection())) + (data.stereoCameraModels().empty() || !data.stereoCameraModels()[0].isValidForProjection())) { - UERROR("Calibrated stereo camera required (multi-cameras not supported)"); + UERROR("Calibrated stereo camera required."); return output; } if(!data.depthRaw().empty() && - (data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection())) + (data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection())) { - UERROR("Calibrated camera required (multi-cameras not supported)."); + UERROR("Calibrated camera required."); return output; } diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index fd5183fa3b..6239a8d0aa 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -156,11 +156,14 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : regPipeline_ = Registration::create(bundleParameters); if(bundleAdjustment_>0 && regPipeline_->isScanRequired()) { - UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.", - Parameters::kOdomF2MBundleAdjustment().c_str(), - bundleAdjustment_, - Parameters::kRegStrategy().c_str(), - uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str()); + if(regPipeline_->isImageRequired()) + { + UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.", + Parameters::kOdomF2MBundleAdjustment().c_str(), + bundleAdjustment_, + Parameters::kRegStrategy().c_str(), + uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str()); + } bundleAdjustment_ = 0; } @@ -214,6 +217,11 @@ Transform OdometryF2M::computeTransform( if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty()) { imuT = Transform::getTransform(imus(), data.stamp()); + if(data.imu().empty()) + { + Eigen::Quaternionf q = imuT.getQuaternionf(); + data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat())); + } } RegistrationInfo regInfo; diff --git a/corelib/src/odometry/OdometryMSCKF.cpp b/corelib/src/odometry/OdometryMSCKF.cpp index add7e870af..ac62c25144 100644 --- a/corelib/src/odometry/OdometryMSCKF.cpp +++ b/corelib/src/odometry/OdometryMSCKF.cpp @@ -672,18 +672,17 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose(); T_i_w.translation() = imu_state.position; - Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w * - msckf_vio::IMUState::T_imu_body.inverse(); + Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse(); Eigen::Vector3d body_velocity = msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity; // Publish tf /*if (publish_tf) { - tf::Transform T_b_w_tf; - tf::transformEigenToTF(T_b_w, T_b_w_tf); - tf_pub.sendTransform(tf::StampedTransform( - T_b_w_tf, time, fixed_frame_id, child_frame_id)); - }*/ + tf::Transform T_b_w_tf; + tf::transformEigenToTF(T_b_w, T_b_w_tf); + tf_pub.sendTransform(tf::StampedTransform( + T_b_w_tf, time, fixed_frame_id, child_frame_id)); + }*/ // Publish the odometry nav_msgs::Odometry odom_msg; @@ -725,20 +724,18 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio // Publish the 3D positions of the features that // has been initialized. feature_msg_ptr.reset(new pcl::PointCloud()); - feature_msg_ptr->header.frame_id = fixed_frame_id; - feature_msg_ptr->height = 1; - for (const auto& item : map_server) { - const auto& feature = item.second; - if (feature.is_initialized) { - Eigen::Vector3d feature_position = - msckf_vio::IMUState::T_imu_body.linear() * feature.position; - feature_msg_ptr->points.push_back(pcl::PointXYZ( - feature_position(0), feature_position(1), feature_position(2))); - } - } - feature_msg_ptr->width = feature_msg_ptr->points.size(); - - //feature_pub.publish(feature_msg_ptr); + feature_msg_ptr->header.frame_id = fixed_frame_id; + feature_msg_ptr->height = 1; + for (const auto& item : map_server) { + const auto& feature = item.second; + if (feature.is_initialized) { + feature_msg_ptr->points.push_back(pcl::PointXYZ( + feature.position(0), feature.position(1), feature.position(2))); + } + } + feature_msg_ptr->width = feature_msg_ptr->points.size(); + + // feature_pub.publish(feature_msg_ptr); return odom_msg; } @@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) : imageProcessor_(0), msckf_(0), parameters_(parameters), -fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0), +fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0), previousPose_(Transform::getIdentity()), initGravity_(false) #endif diff --git a/corelib/src/odometry/OdometryORBSLAM.cpp b/corelib/src/odometry/OdometryORBSLAM2.cpp similarity index 87% rename from corelib/src/odometry/OdometryORBSLAM.cpp rename to corelib/src/odometry/OdometryORBSLAM2.cpp index 3212ba62b9..1ae10bc197 100644 --- a/corelib/src/odometry/OdometryORBSLAM.cpp +++ b/corelib/src/odometry/OdometryORBSLAM2.cpp @@ -34,28 +34,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UDirectory.h" #include #include -#include +#include -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 #include #include using namespace std; -#if RTABMAP_ORB_SLAM == 3 -namespace ORB_SLAM3 { -#else namespace ORB_SLAM2 { -#endif + // Override original Tracking object to comment all rendering stuff class Tracker: public Tracking { public: -#if RTABMAP_ORB_SLAM == 3 - Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pMap, -#else Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, -#endif KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) : Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor), maxFeatureMapSize_(maxFeatureMapSize) @@ -67,9 +60,6 @@ class Tracker: public Tracking protected: void Track() { -#if RTABMAP_ORB_SLAM == 3 - Map* mpMap = mpAtlas->GetCurrentMap(); -#endif if(mState==NO_IMAGES_YET) { mState = NOT_INITIALIZED; @@ -91,17 +81,8 @@ class Tracker: public Tracking if(mState!=OK) { -#if RTABMAP_ORB_SLAM == 3 - mLastFrame = Frame(mCurrentFrame); -#endif return; } -#if RTABMAP_ORB_SLAM == 3 - if(mpAtlas->GetAllMaps().size() == 1) - { - mnFirstFrameId = mCurrentFrame.mnId; - } -#endif } else { @@ -384,9 +365,6 @@ class Tracker: public Tracking // Set Frame pose to the origin mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); -#if RTABMAP_ORB_SLAM == 3 - Map* mpMap = mpAtlas->GetCurrentMap(); -#endif // Create KeyFrame KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); @@ -484,11 +462,9 @@ class Tracker: public Tracking cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY); } } -#if RTABMAP_ORB_SLAM == 3 - mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera); -#else + mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); -#endif + Track(); return mCurrentFrame.mTcw.clone(); @@ -516,11 +492,8 @@ class Tracker: public Tracking UASSERT(imDepth.type()==CV_32F); -#if RTABMAP_ORB_SLAM == 3 - mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera); -#else mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); -#endif + Track(); return mCurrentFrame.mTcw.clone(); @@ -531,11 +504,7 @@ class Tracker: public Tracking class LoopCloser: public LoopClosing { public: -#if RTABMAP_ORB_SLAM == 3 - LoopCloser(Atlas* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) : -#else LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) : -#endif LoopClosing(pMap, pDB, pVoc, bFixScale) {} @@ -566,16 +535,12 @@ class LoopCloser: public LoopClosing } // namespace ORB_SLAM -#if RTABMAP_ORB_SLAM == 3 -using namespace ORB_SLAM3; -#else using namespace ORB_SLAM2; -#endif -class ORBSLAMSystem +class ORBSLAM2System { public: - ORBSLAMSystem(const rtabmap::ParametersMap & parameters) : + ORBSLAM2System(const rtabmap::ParametersMap & parameters) : mpVocabulary(0), mpKeyFrameDatabase(0), mpMap(0), @@ -613,7 +578,7 @@ class ORBSLAMSystem } } - bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform) + bool init(const rtabmap::CameraModel & model, bool stereo, double baseline) { if(!mpVocabulary) { @@ -709,31 +674,6 @@ class ORBSLAMSystem ofs << "DepthMapFactor: " << 1000.0 << std::endl; ofs << std::endl; - if(!localIMUTransform.isNull()) - { - //#-------------------------------------------------------------------------------------------- - //# IMU Parameters TODO: hard-coded, not used - //#-------------------------------------------------------------------------------------------- - // Transformation from camera 0 to body-frame (imu) - rtabmap::Transform camImuT = model.localTransform()*localIMUTransform; - ofs << "Tbc: !!opencv-matrix" << std::endl; - ofs << " rows: 4" << std::endl; - ofs << " cols: 4" << std::endl; - ofs << " dt: f" << std::endl; - ofs << " data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2] << ", " << camImuT.data()[3] << ", " << std::endl; - ofs << " " << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6] << ", " << camImuT.data()[7] << ", " << std::endl; - ofs << " " << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl; - ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl; - ofs << std::endl; - - ofs << "IMU.NoiseGyro: " << 1.7e-4 << std::endl; - ofs << "IMU.NoiseAcc: " << 2.0e-3 << std::endl; - ofs << "IMU.GyroWalk: " << 1.9393e-5 << std::endl; - ofs << "IMU.AccWalk: " << 3.e-3 << std::endl; - ofs << "IMU.Frequency: " << 200 << std::endl; - ofs << std::endl; - } - //#-------------------------------------------------------------------------------------------- //# ORB Parameters //#-------------------------------------------------------------------------------------------- @@ -776,22 +716,15 @@ class ORBSLAMSystem mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); //Create the Map -#if RTABMAP_ORB_SLAM == 3 - mpMap = new Atlas(0); -#else mpMap = new ORB_SLAM2::Map(); -#endif //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) mpTracker = new Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize); //Initialize the Local Mapping thread and launch -#if RTABMAP_ORB_SLAM == 3 - mpLocalMapper = new LocalMapping(0, mpMap, false, stereo && !localIMUTransform.isNull()); -#else mpLocalMapper = new LocalMapping(mpMap, false); -#endif + //Initialize the Loop Closing thread and launch mpLoopCloser = new LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true); @@ -812,17 +745,10 @@ class ORBSLAMSystem // Reset all static variables Frame::mbInitialComputations = true; -#if RTABMAP_ORB_SLAM == 3 - if(ULogger::level() > ULogger::kInfo) - Verbose::SetTh(Verbose::VERBOSITY_QUIET); - - mpTracker->Reset(true); -#endif - return true; } - virtual ~ORBSLAMSystem() + virtual ~ORBSLAM2System() { shutdown(); delete mpVocabulary; @@ -869,11 +795,7 @@ class ORBSLAMSystem KeyFrameDatabase* mpKeyFrameDatabase; // Map structure that stores the pointers to all KeyFrames and MapPoints. -#if RTABMAP_ORB_SLAM == 3 - Atlas* mpMap; -#else Map* mpMap; -#endif // Tracker. It receives a frame and computes the associated camera pose. // It also decides when to insert a new keyframe, create some new MapPoints and @@ -898,24 +820,23 @@ class ORBSLAMSystem namespace rtabmap { -OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) : +OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) : Odometry(parameters) -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 , orbslam_(0), firstFrame_(true), - previousPose_(Transform::getIdentity()), - useIMU_(false) // TODO: Not yet supported with ORB_SLAM3 + previousPose_(Transform::getIdentity()) #endif { -#ifdef RTABMAP_ORB_SLAM - orbslam_ = new ORBSLAMSystem(parameters); +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 + orbslam_ = new ORBSLAM2System(parameters); #endif } -OdometryORBSLAM::~OdometryORBSLAM() +OdometryORBSLAM2::~OdometryORBSLAM2() { -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 if(orbslam_) { delete orbslam_; @@ -923,10 +844,10 @@ OdometryORBSLAM::~OdometryORBSLAM() #endif } -void OdometryORBSLAM::reset(const Transform & initialPose) +void OdometryORBSLAM2::reset(const Transform & initialPose) { Odometry::reset(initialPose); -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 if(orbslam_) { orbslam_->shutdown(); @@ -934,60 +855,20 @@ void OdometryORBSLAM::reset(const Transform & initialPose) firstFrame_ = true; originLocalTransform_.setNull(); previousPose_.setIdentity(); - imuLocalTransform_.setNull(); -#endif -} - -bool OdometryORBSLAM::canProcessAsyncIMU() const -{ -#ifdef RTABMAP_ORB_SLAM - return useIMU_; -#else - return false; #endif } // return not null transform if odometry is correctly computed -Transform OdometryORBSLAM::computeTransform( +Transform OdometryORBSLAM2::computeTransform( SensorData & data, const Transform & guess, OdometryInfo * info) { Transform t; -#ifdef RTABMAP_ORB_SLAM +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2 UTimer timer; -#if RTABMAP_ORB_SLAM == 3 - if(useIMU_) - { - if(orbslam_->mpTracker == 0) - { - if(!data.imu().empty()) - { - imuLocalTransform_ = data.imu().localTransform(); - } - } - else if(!data.imu().empty()) - { - ORB_SLAM3::IMU::Point pt( - data.imu().linearAcceleration().val[0], - data.imu().linearAcceleration().val[1], - data.imu().linearAcceleration().val[2], - data.imu().angularVelocity().val[0], - data.imu().angularVelocity().val[1], - data.imu().angularVelocity().val[2], - data.stamp()); - orbslam_->mpTracker->GrabImuData(pt); - } - - if(data.imageRaw().empty() || imuLocalTransform_.isNull()) - { - return Transform(); - } - } -#endif - if(data.imageRaw().empty() || data.imageRaw().rows != data.depthOrRightRaw().rows || data.imageRaw().cols != data.depthOrRightRaw().cols) @@ -1007,18 +888,12 @@ Transform OdometryORBSLAM::computeTransform( } bool stereo = data.cameraModels().size() == 0; - if(!stereo && useIMU_) - { - UWARN("Disabling IMU support (ORB_SLAM3 doesn't support IMU with RGB-D mode)."); - useIMU_ = false; - imuLocalTransform_.setNull(); - } cv::Mat covariance; if(orbslam_->mpTracker == 0) { CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left(); - if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(), imuLocalTransform_)) + if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline())) { return t; } diff --git a/corelib/src/odometry/OdometryORBSLAM3.cpp b/corelib/src/odometry/OdometryORBSLAM3.cpp new file mode 100644 index 0000000000..70f2dc9bbd --- /dev/null +++ b/corelib/src/odometry/OdometryORBSLAM3.cpp @@ -0,0 +1,589 @@ +/* +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. +*/ + +#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UTimer.h" +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UDirectory.h" +#include +#include +#include + +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 +#include +#include + +using namespace std; + +#endif + +namespace rtabmap { + +OdometryORBSLAM3::OdometryORBSLAM3(const ParametersMap & parameters) : + Odometry(parameters) +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + , + orbslam_(0), + firstFrame_(true), + previousPose_(Transform::getIdentity()), + useIMU_(Parameters::defaultOdomORBSLAMInertial()), + parameters_(parameters), + lastImuStamp_(0.0), + lastImageStamp_(0.0) +#endif +{ +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + Parameters::parse(parameters, Parameters::kOdomORBSLAMInertial(), useIMU_); +#endif +} + +OdometryORBSLAM3::~OdometryORBSLAM3() +{ +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + if(orbslam_) + { + orbslam_->Shutdown(); + delete orbslam_; + } +#endif +} + +void OdometryORBSLAM3::reset(const Transform & initialPose) +{ + Odometry::reset(initialPose); +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + if(orbslam_) + { + orbslam_->Shutdown(); + delete orbslam_; + orbslam_=0; + } + firstFrame_ = true; + originLocalTransform_.setNull(); + previousPose_.setIdentity(); + imuLocalTransform_.setNull(); + lastImuStamp_ = 0.0; + lastImageStamp_ = 0.0; +#endif +} + +bool OdometryORBSLAM3::canProcessAsyncIMU() const +{ +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + return useIMU_; +#else + return false; +#endif +} + +bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::CameraModel & model2, double stamp, bool stereo, double baseline) +{ +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + std::string vocabularyPath; + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath); + + if(vocabularyPath.empty()) + { + UERROR("ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str()); + return false; + } + //Load ORB Vocabulary + vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir()); + UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str()); + + // Create configuration file + std::string workingDir; + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir); + if(workingDir.empty()) + { + workingDir = "."; + } + std::string configPath = workingDir+"/rtabmap_orbslam.yaml"; + std::ofstream ofs (configPath, std::ofstream::out); + ofs << "%YAML:1.0" << std::endl; + ofs << std::endl; + + ofs << "File.version: \"1.0\"" << std::endl; + ofs << std::endl; + + ofs << "Camera.type: \"PinHole\"" << std::endl; + ofs << std::endl; + + ofs << fixed << setprecision(13); + + for(int i=1; i<(stereo?3:2); ++i) + { + const CameraModel & model = i==1?model1:model2; + + //# Camera calibration and distortion parameters (OpenCV) + ofs << "Camera" << i << ".fx: " << model.fx() << std::endl; + ofs << "Camera" << i << ".fy: " << model.fy() << std::endl; + ofs << "Camera" << i << ".cx: " << model.cx() << std::endl; + ofs << "Camera" << i << ".cy: " << model.cy() << std::endl; + ofs << std::endl; + + if(model.D().cols < 4) + { + ofs << "Camera" << i << ".k1: " << 0.0 << std::endl; + ofs << "Camera" << i << ".k2: " << 0.0 << std::endl; + ofs << "Camera" << i << ".p1: " << 0.0 << std::endl; + ofs << "Camera" << i << ".p2: " << 0.0 << std::endl; + if(!stereo) + { + ofs << "Camera" << i << ".k3: " << 0.0 << std::endl; + } + } + if(model.D().cols >= 4) + { + ofs << "Camera" << i << ".k1: " << model.D().at(0,0) << std::endl; + ofs << "Camera" << i << ".k2: " << model.D().at(0,1) << std::endl; + ofs << "Camera" << i << ".p1: " << model.D().at(0,2) << std::endl; + ofs << "Camera" << i << ".p2: " << model.D().at(0,3) << std::endl; + } + if(model.D().cols >= 5) + { + ofs << "Camera" << i << ".k3: " << model.D().at(0,4) << std::endl; + } + if(model.D().cols > 5) + { + UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols); + } + ofs << std::endl; + } + + //# IR projector baseline times fx (aprox.) + if(baseline <= 0.0) + { + baseline = rtabmap::Parameters::defaultOdomORBSLAMBf(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline); + } + else + { + // # Transformation matrix from right camera to left camera + ofs << "Stereo.T_c1_c2: !!opencv-matrix" << std::endl; + ofs << " rows: 4" << std::endl; + ofs << " cols: 4" << std::endl; + ofs << " dt: f" << std::endl; + ofs << " data: [" << 1 << ", " << 0 << ", " << 0 << ", " << baseline << ", " << std::endl; + ofs << " " << 0 << ", " << 1 << ", " << 0 << ", " << 0 << ", " << std::endl; + ofs << " " << 0 << ", " << 0 << ", " << 1 << ", " << 0 << ", " << std::endl; + ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl; + ofs << std::endl; + } + ofs << "Camera.bf: " << model1.fx()*baseline << std::endl; + + ofs << "Camera.width: " << model1.imageWidth() << std::endl; + ofs << "Camera.height: " << model1.imageHeight() << std::endl; + ofs << std::endl; + + //# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) + //Camera.RGB: 1 + ofs << "Camera.RGB: 0" << std::endl; + ofs << std::endl; + + float fps = rtabmap::Parameters::defaultOdomORBSLAMFps(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps); + if(fps == 0) + { + UASSERT(stamp > lastImageStamp_); + fps = std::round(1./(stamp - lastImageStamp_)); + UWARN("Camera FPS estimated at %d Hz. If this doesn't look good, " + "set explicitly parameter %s to expected frequency.", + int(fps), Parameters::kOdomORBSLAMFps().c_str()); + } + ofs << "Camera.fps: " << (int)fps << std::endl; + ofs << std::endl; + + //# Close/Far threshold. Baseline times. + double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth); + ofs << "Stereo.ThDepth: " << thDepth << std::endl; + ofs << "Stereo.b: " << baseline << std::endl; + ofs << std::endl; + + //# Deptmap values factor + ofs << "RGBD.DepthMapFactor: " << 1.0 << std::endl; + ofs << std::endl; + + bool withIMU = false; + if(!imuLocalTransform_.isNull()) + { + withIMU = true; + //#-------------------------------------------------------------------------------------------- + //# IMU Parameters TODO: hard-coded, not used + //#-------------------------------------------------------------------------------------------- + // Transformation from camera 0 to body-frame (imu) + rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_; + ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl; + ofs << " rows: 4" << std::endl; + ofs << " cols: 4" << std::endl; + ofs << " dt: f" << std::endl; + ofs << " data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2] << ", " << camImuT.data()[3] << ", " << std::endl; + ofs << " " << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6] << ", " << camImuT.data()[7] << ", " << std::endl; + ofs << " " << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl; + ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl; + ofs << std::endl; + + ofs << "IMU.InsertKFsWhenLost: " << 0 << std::endl; + ofs << std::endl; + + double gyroNoise = rtabmap::Parameters::defaultOdomORBSLAMGyroNoise(); + double accNoise = rtabmap::Parameters::defaultOdomORBSLAMAccNoise(); + double gyroWalk = rtabmap::Parameters::defaultOdomORBSLAMGyroWalk(); + double accWalk = rtabmap::Parameters::defaultOdomORBSLAMAccWalk(); + double samplingRate = rtabmap::Parameters::defaultOdomORBSLAMSamplingRate(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroNoise(), gyroNoise); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccNoise(), accNoise); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroWalk(), gyroWalk); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccWalk(), accWalk); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMSamplingRate(), samplingRate); + + ofs << "IMU.NoiseGyro: " << gyroNoise << std::endl; // 1e-2 + ofs << "IMU.NoiseAcc: " << accNoise << std::endl; // 1e-1 + ofs << "IMU.GyroWalk: " << gyroWalk << std::endl; // 1e-6 + ofs << "IMU.AccWalk: " << accWalk << std::endl; // 1e-4 + if(samplingRate == 0) + { + // estimate rate from imu received. + UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t); + samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t); + samplingRate = std::round(samplingRate); + UWARN("IMU sampling rate estimated at %.0f Hz. If this doesn't look good, " + "set explicitly parameter %s to expected frequency.", + samplingRate, Parameters::kOdomORBSLAMSamplingRate().c_str()); + } + ofs << "IMU.Frequency: " << samplingRate << std::endl; // 200 + ofs << std::endl; + } + + + + //#-------------------------------------------------------------------------------------------- + //# ORB Parameters + //#-------------------------------------------------------------------------------------------- + //# ORB Extractor: Number of features per image + int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features); + ofs << "ORBextractor.nFeatures: " << features << std::endl; + ofs << std::endl; + + //# ORB Extractor: Scale factor between levels in the scale pyramid + double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor); + ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl; + ofs << std::endl; + + //# ORB Extractor: Number of levels in the scale pyramid + int levels = rtabmap::Parameters::defaultORBNLevels(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels); + ofs << "ORBextractor.nLevels: " << levels << std::endl; + ofs << std::endl; + + //# ORB Extractor: Fast threshold + //# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. + //# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST + //# You can lower these values if your images have low contrast + int iniThFAST = rtabmap::Parameters::defaultFASTThreshold(); + int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST); + ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl; + ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl; + ofs << std::endl; + + int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize); + + //# Disable loop closure detection + ofs << "loopClosing: " << 0 << std::endl; + ofs << std::endl; + + //# Set dummy Viewer parameters + ofs << "Viewer.KeyFrameSize: " << 0.05 << std::endl; + ofs << "Viewer.KeyFrameLineWidth: " << 1.0 << std::endl; + ofs << "Viewer.GraphLineWidth: " << 0.9 << std::endl; + ofs << "Viewer.PointSize: " << 2.0 << std::endl; + ofs << "Viewer.CameraSize: " << 0.08 << std::endl; + ofs << "Viewer.CameraLineWidth: " << 3.0 << std::endl; + ofs << "Viewer.ViewpointX: " << 0.0 << std::endl; + ofs << "Viewer.ViewpointY: " << -0.7 << std::endl; + ofs << "Viewer.ViewpointZ: " << -3.5 << std::endl; + ofs << "Viewer.ViewpointF: " << 500.0 << std::endl; + ofs << std::endl; + + ofs.close(); + + orbslam_ = new ORB_SLAM3::System( + vocabularyPath, + configPath, + stereo && withIMU?ORB_SLAM3::System::IMU_STEREO: + stereo?ORB_SLAM3::System::STEREO: + withIMU?ORB_SLAM3::System::IMU_RGBD: + ORB_SLAM3::System::RGBD, + false); + return true; +#else + UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); +#endif + return false; +} + +// return not null transform if odometry is correctly computed +Transform OdometryORBSLAM3::computeTransform( + SensorData & data, + const Transform & guess, + OdometryInfo * info) +{ + Transform t; + +#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3 + UTimer timer; + + if(useIMU_) + { + bool added = false; + if(!data.imu().empty()) + { + if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp()) + { + orbslamImus_.push_back(ORB_SLAM3::IMU::Point( + data.imu().linearAcceleration().val[0], + data.imu().linearAcceleration().val[1], + data.imu().linearAcceleration().val[2], + data.imu().angularVelocity().val[0], + data.imu().angularVelocity().val[1], + data.imu().angularVelocity().val[2], + data.stamp())); + lastImuStamp_ = data.stamp(); + added = true; + } + else + { + UERROR("Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!", data.stamp(), lastImuStamp_); + } + } + + if(orbslam_ == 0) + { + // We need two samples to estimate imu frame rate + if(orbslamImus_.size()>1 && added) + { + imuLocalTransform_ = data.imu().localTransform(); + } + } + + if(data.imageRaw().empty() || imuLocalTransform_.isNull()) + { + return Transform(); + } + } + + if(data.imageRaw().empty() || + data.imageRaw().rows != data.depthOrRightRaw().rows || + data.imageRaw().cols != data.depthOrRightRaw().cols) + { + UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.", + data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows); + return t; + } + + if(!((data.cameraModels().size() == 1 && + data.cameraModels()[0].isValidForReprojection()) || + (data.stereoCameraModels().size() == 1 && + data.stereoCameraModels()[0].isValidForProjection()))) + { + UERROR("Invalid camera model!"); + return t; + } + + bool stereo = data.cameraModels().size() == 0; + + cv::Mat covariance; + if(orbslam_ == 0) + { + // We need two frames to estimate camera frame rate + if(lastImageStamp_ == 0.0) + { + lastImageStamp_ = data.stamp(); + return t; + } + + CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left(); + if(!init(model, !stereo?model:data.stereoCameraModels()[0].right(), data.stamp(), stereo, data.cameraModels().size()==1?0.0:data.stereoCameraModels()[0].baseline())) + { + return t; + } + } + + Sophus::SE3f Tcw; + Transform localTransform; + if(stereo) + { + localTransform = data.stereoCameraModels()[0].localTransform(); + + Tcw = orbslam_->TrackStereo(data.imageRaw(), data.rightRaw(), data.stamp(), orbslamImus_); + orbslamImus_.clear(); + } + else + { + localTransform = data.cameraModels()[0].localTransform(); + cv::Mat depth; + if(data.depthRaw().type() == CV_32FC1) + { + depth = data.depthRaw(); + } + else if(data.depthRaw().type() == CV_16UC1) + { + depth = util2d::cvtDepthToFloat(data.depthRaw()); + } + Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_); + orbslamImus_.clear(); + } + + Transform previousPoseInv = previousPose_.inverse(); + std::vector mapPoints = orbslam_->GetTrackedMapPoints(); + if(orbslam_->isLost() || mapPoints.empty()) + { + covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; + } + else + { + cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).clone(); + UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4); + Transform p = Transform(cv::Mat(TcwMat, cv::Range(0,3), cv::Range(0,4))); + + if(!p.isNull()) + { + if(!localTransform.isNull()) + { + if(originLocalTransform_.isNull()) + { + originLocalTransform_ = localTransform; + } + // transform in base frame + p = originLocalTransform_ * p.inverse() * localTransform.inverse(); + } + t = previousPoseInv*p; + } + previousPose_ = p; + + if(firstFrame_) + { + // just recovered of being lost, set high covariance + covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; + firstFrame_ = false; + } + else + { + float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(); + if(baseline <= 0.0f) + { + baseline = rtabmap::Parameters::defaultOdomORBSLAMBf(); + rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline); + } + double linearVar = 0.0001; + if(baseline > 0.0f) + { + linearVar = baseline/8.0; + linearVar *= linearVar; + } + + covariance = cv::Mat::eye(6,6, CV_64FC1); + covariance.at(0,0) = linearVar; + covariance.at(1,1) = linearVar; + covariance.at(2,2) = linearVar; + covariance.at(3,3) = 0.0001; + covariance.at(4,4) = 0.0001; + covariance.at(5,5) = 0.0001; + } + } + + if(info) + { + info->lost = t.isNull(); + info->type = (int)kTypeORBSLAM; + info->reg.covariance = covariance; + info->localMapSize = mapPoints.size(); + info->localKeyFrames = 0; + + if(this->isInfoDataFilled()) + { + std::vector kpts = orbslam_->GetTrackedKeyPointsUn(); + info->reg.matchesIDs.resize(kpts.size()); + info->reg.inliersIDs.resize(kpts.size()); + int oi = 0; + + UASSERT(mapPoints.size() == kpts.size()); + for (unsigned int i = 0; i < kpts.size(); ++i) + { + int wordId; + if(mapPoints[i] != 0) + { + wordId = mapPoints[i]->mnId; + } + else + { + wordId = -(i+1); + } + info->words.insert(std::make_pair(wordId, kpts[i])); + if(mapPoints[i] != 0) + { + info->reg.matchesIDs[oi] = wordId; + info->reg.inliersIDs[oi] = wordId; + ++oi; + } + } + info->reg.matchesIDs.resize(oi); + info->reg.inliersIDs.resize(oi); + info->reg.inliers = oi; + info->reg.matches = oi; + + Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f(); + for (unsigned int i = 0; i < mapPoints.size(); ++i) + { + if(mapPoints[i]) + { + Eigen::Vector3f pt = mapPoints[i]->GetWorldPos(); + pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt[0], pt[1], pt[2]), fixRot); + info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z))); + } + } + } + } + + UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false"); + +#else + UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); +#endif + return t; +} + +} // namespace rtabmap diff --git a/corelib/src/odometry/OdometryOpenVINS.cpp b/corelib/src/odometry/OdometryOpenVINS.cpp index fda60d38d5..da81f7ee7f 100644 --- a/corelib/src/odometry/OdometryOpenVINS.cpp +++ b/corelib/src/odometry/OdometryOpenVINS.cpp @@ -28,22 +28,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryOpenVINS.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" -#include "rtabmap/utilite/UStl.h" -#include "rtabmap/utilite/UThread.h" -#include "rtabmap/utilite/UDirectory.h" +#include #include #ifdef RTABMAP_OPENVINS #include "core/VioManager.h" -#include "core/VioManagerOptions.h" -#include "core/RosVisualizer.h" -#include "utils/dataset_reader.h" -#include "utils/parse_ros.h" -#include "utils/sensor_data.h" +#include "state/Propagator.h" #include "state/State.h" -#include "types/Type.h" +#include "state/StateHelper.h" #endif namespace rtabmap { @@ -52,17 +47,111 @@ OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) : Odometry(parameters) #ifdef RTABMAP_OPENVINS , - vioManager_(0), initGravity_(false), - previousPose_(Transform::getIdentity()) + previousPoseInv_(Transform::getIdentity()) #endif { -} - -OdometryOpenVINS::~OdometryOpenVINS() -{ #ifdef RTABMAP_OPENVINS - delete vioManager_; + ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1)); + int enum_index; + std::string left_mask_path, right_mask_path; + params_ = std::make_unique(); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt); + Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts); + Parameters::parse(parameters, Parameters::kFASTThreshold(), params_->fast_threshold); + Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x); + Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist); + Parameters::parse(parameters, Parameters::kVisCorNNDR(), params_->knn_ratio); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiTriangulate1d(), params_->featinit_options.triangulate_1d); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiRefineFeatures(), params_->featinit_options.refine_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs); + Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist); + Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist); + if(params_->featinit_options.max_dist == 0) + params_->featinit_options.max_dist = std::numeric_limits::infinity(); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej); + Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index); + params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamExtrinsics(), params_->state_options.do_calib_camera_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamIntrinsics(), params_->state_options.do_calib_camera_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamTimeoffset(), params_->state_options.do_calib_camera_timeoffset); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUIntrinsics(), params_->state_options.do_calib_imu_intrinsics); + Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUGSensitivity(), params_->state_options.do_calib_imu_g_sensitivity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update); + Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index); + params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index); + params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index); + Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag); + Parameters::parse(parameters, Parameters::kVisDepthAsMask(), params_->use_mask); + Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path); + if(!left_mask_path.empty()) + { + if(!UFile::exists(left_mask_path)) + UWARN("OpenVINS: invalid left mask path: %s", left_mask_path.c_str()); + else + params_->masks.emplace(0, cv::imread(left_mask_path, cv::IMREAD_GRAYSCALE)); + } + Parameters::parse(parameters, Parameters::kOdomOpenVINSRightMaskPath(), right_mask_path); + if(!right_mask_path.empty()) + { + if(!UFile::exists(right_mask_path)) + UWARN("OpenVINS: invalid right mask path: %s", right_mask_path.c_str()); + else + params_->masks.emplace(1, cv::imread(right_mask_path, cv::IMREAD_GRAYSCALE)); + } + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynUse(), params_->init_options.init_dyn_use); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEOptCalib(), params_->init_options.init_dyn_mle_opt_calib); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxIter(), params_->init_options.init_dyn_mle_max_iter); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxTime(), params_->init_options.init_dyn_mle_max_time); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxThreads(), params_->init_options.init_dyn_mle_max_threads); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynNumPose(), params_->init_options.init_dyn_num_pose); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinDeg(), params_->init_options.init_dyn_min_deg); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationOri(), params_->init_options.init_dyn_inflation_orientation); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationVel(), params_->init_options.init_dyn_inflation_velocity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBg(), params_->init_options.init_dyn_inflation_bias_gyro); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBa(), params_->init_options.init_dyn_inflation_bias_accel); + Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinRecCond(), params_->init_options.init_dyn_min_rec_cond); + Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity); + Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning); + Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a); + Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w); + Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix); + Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler); + params_->vec_dw << 1, 0, 0, 1, 0, 1; + params_->vec_da << 1, 0, 0, 1, 0, 1; + params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0; + params_->q_ACCtoIMU << 0, 0, 0, 1; + params_->q_GYROtoIMU << 0, 0, 0, 1; + params_->use_aruco = false; + params_->num_opencv_threads = -1; + params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE; + params_->init_options.sigma_a = params_->imu_noises.sigma_a; + params_->init_options.sigma_ab = params_->imu_noises.sigma_ab; + params_->init_options.sigma_w = params_->imu_noises.sigma_w; + params_->init_options.sigma_wb = params_->imu_noises.sigma_wb; + params_->init_options.sigma_pix = params_->slam_options.sigma_pix; + params_->init_options.gravity_mag = params_->gravity_mag; #endif } @@ -72,11 +161,9 @@ void OdometryOpenVINS::reset(const Transform & initialPose) #ifdef RTABMAP_OPENVINS if(!initGravity_) { - delete vioManager_; - vioManager_ = 0; - previousPose_.setIdentity(); - previousLocalTransform_.setNull(); - imuBuffer_.clear(); + vioManager_.reset(); + previousPoseInv_.setIdentity(); + imuLocalTransformInv_.setNull(); } initGravity_ = false; #endif @@ -90,395 +177,306 @@ Transform OdometryOpenVINS::computeTransform( { Transform t; #ifdef RTABMAP_OPENVINS - UTimer timer; - - // Buffer imus; - if(!data.imu().empty()) - { - imuBuffer_.insert(std::make_pair(data.stamp(), data.imu())); - } - // OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp - if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1) + if(!vioManager_) { - if(imuBuffer_.empty()) + if(!data.imu().empty()) { - UWARN("Waiting IMU for initialization..."); - return t; + imuLocalTransformInv_ = data.imu().localTransform().inverse(); + Phi_.setZero(); + Phi_.block(0,0,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); + Phi_.block(3,3,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3); } - if(vioManager_ == 0) - { - UINFO("OpenVINS Initialization"); - - // intialize - ov_msckf::VioManagerOptions params; - - // ESTIMATOR ====================================================================== - // Main EKF parameters - //params.state_options.do_fej = true; - //params.state_options.imu_avg =false; - //params.state_options.use_rk4_integration; - //params.state_options.do_calib_camera_pose = false; - //params.state_options.do_calib_camera_intrinsics = false; - //params.state_options.do_calib_camera_timeoffset = false; - //params.state_options.max_clone_size = 11; - //params.state_options.max_slam_features = 25; - //params.state_options.max_slam_in_update = INT_MAX; - //params.state_options.max_msckf_in_update = INT_MAX; - //params.state_options.max_aruco_features = 1024; - params.state_options.num_cameras = 2; - //params.dt_slam_delay = 2; - - // Set what representation we should be using - //params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - //params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - //params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D - if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) + if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull()) + { + Transform T_imu_left; + Eigen::VectorXd left_calib(8), right_calib(8); + if(!data.rightRaw().empty()) { - printf(RED "VioManager(): invalid feature representation specified:\n" RESET); - printf(RED "\t- GLOBAL_3D\n" RESET); - printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_3D\n" RESET); - printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); - std::exit(EXIT_FAILURE); - } - - // Filter initialization - //params.init_window_time = 1; - //params.init_imu_thresh = 1; - - // Zero velocity update - //params.try_zupt = false; - //params.zupt_options.chi2_multipler = 5; - //params.zupt_max_velocity = 1; - //params.zupt_noise_multiplier = 1; - - // NOISE ====================================================================== - - // Our noise values for inertial sensor - //params.imu_noises.sigma_w = 1.6968e-04; - //params.imu_noises.sigma_a = 2.0000e-3; - //params.imu_noises.sigma_wb = 1.9393e-05; - //params.imu_noises.sigma_ab = 3.0000e-03; - - // Read in update parameters - //params.msckf_options.sigma_pix = 1; - //params.msckf_options.chi2_multipler = 5; - //params.slam_options.sigma_pix = 1; - //params.slam_options.chi2_multipler = 5; - //params.aruco_options.sigma_pix = 1; - //params.aruco_options.chi2_multipler = 5; - - - // STATE ====================================================================== - - // Timeoffset from camera to IMU - //params.calib_camimu_dt = 0.0; - - // Global gravity - //params.gravity[2] = 9.81; - - - // TRACKERS ====================================================================== + params_->state_options.num_cameras = params_->init_options.num_cameras = 2; + T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform(); - // Tracking flags - params.use_stereo = true; - //params.use_klt = true; - params.use_aruco = false; - //params.downsize_aruco = true; - //params.downsample_cameras = false; - //params.use_multi_threading = true; - - // General parameters - //params.num_pts = 200; - //params.fast_threshold = 10; - //params.grid_x = 10; - //params.grid_y = 5; - //params.min_px_dist = 8; - //params.knn_ratio = 0.7; - - // Feature initializer parameters - //nh.param("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d); - //nh.param("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features); - //nh.param("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs); - //nh.param("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda); - //nh.param("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda); - //nh.param("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx); - ///nh.param("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost); - //nh.param("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult); - //nh.param("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist); - //params.featinit_options.max_dist = 75; - //params.featinit_options.max_baseline = 500; - //params.featinit_options.max_cond_number = 5000; - - - // CAMERA ====================================================================== - bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified(); - params.camera_fisheye.insert(std::make_pair(0, fisheye)); - params.camera_fisheye.insert(std::make_pair(1, fisheye)); + bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified(); + if(is_fisheye) + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())); + params_->camera_intrinsics.emplace(1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())); + } + else + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight())); + params_->camera_intrinsics.emplace(1, std::make_shared( + data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight())); + } - Eigen::VectorXd camLeft(8), camRight(8); - if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) - { - camLeft << data.stereoCameraModels()[0].left().fx(), - data.stereoCameraModels()[0].left().fy(), - data.stereoCameraModels()[0].left().cx(), - data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; - camRight << data.stereoCameraModels()[0].right().fx(), - data.stereoCameraModels()[0].right().fy(), - data.stereoCameraModels()[0].right().cx(), - data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; + if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty()) + { + left_calib << data.stereoCameraModels()[0].left().fx(), + data.stereoCameraModels()[0].left().fy(), + data.stereoCameraModels()[0].left().cx(), + data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0; + right_calib << data.stereoCameraModels()[0].right().fx(), + data.stereoCameraModels()[0].right().fy(), + data.stereoCameraModels()[0].right().cx(), + data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0; + } + else + { + UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols); + UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4); + UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4); + left_calib << data.stereoCameraModels()[0].left().K_raw().at(0,0), + data.stereoCameraModels()[0].left().K_raw().at(1,1), + data.stereoCameraModels()[0].left().K_raw().at(0,2), + data.stereoCameraModels()[0].left().K_raw().at(1,2), + data.stereoCameraModels()[0].left().D_raw().at(0,0), + data.stereoCameraModels()[0].left().D_raw().at(0,1), + data.stereoCameraModels()[0].left().D_raw().at(0,is_fisheye?4:2), + data.stereoCameraModels()[0].left().D_raw().at(0,is_fisheye?5:3); + right_calib << data.stereoCameraModels()[0].right().K_raw().at(0,0), + data.stereoCameraModels()[0].right().K_raw().at(1,1), + data.stereoCameraModels()[0].right().K_raw().at(0,2), + data.stereoCameraModels()[0].right().K_raw().at(1,2), + data.stereoCameraModels()[0].right().D_raw().at(0,0), + data.stereoCameraModels()[0].right().D_raw().at(0,1), + data.stereoCameraModels()[0].right().D_raw().at(0,is_fisheye?4:2), + data.stereoCameraModels()[0].right().D_raw().at(0,is_fisheye?5:3); + } } else { - UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols); - UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4); - UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4); + params_->state_options.num_cameras = params_->init_options.num_cameras = 1; + T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform(); - //https://github.com/ethz-asl/kalibr/wiki/supported-models - /// radial-tangential (radtan) - // (distortion_coeffs: [k1 k2 r1 r2]) - /// equidistant (equi) - // (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4) + bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified(); + if(is_fisheye) + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight())); + } + else + { + params_->camera_intrinsics.emplace(0, std::make_shared( + data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight())); + } - camLeft << - data.stereoCameraModels()[0].left().K_raw().at(0,0), - data.stereoCameraModels()[0].left().K_raw().at(1,1), - data.stereoCameraModels()[0].left().K_raw().at(0,2), - data.stereoCameraModels()[0].left().K_raw().at(1,2), - data.stereoCameraModels()[0].left().D_raw().at(0,0), - data.stereoCameraModels()[0].left().D_raw().at(0,1), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].left().D_raw().at(0,fisheye?5:3); - camRight << - data.stereoCameraModels()[0].right().K_raw().at(0,0), - data.stereoCameraModels()[0].right().K_raw().at(1,1), - data.stereoCameraModels()[0].right().K_raw().at(0,2), - data.stereoCameraModels()[0].right().K_raw().at(1,2), - data.stereoCameraModels()[0].right().D_raw().at(0,0), - data.stereoCameraModels()[0].right().D_raw().at(0,1), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?4:2), - data.stereoCameraModels()[0].right().D_raw().at(0,fisheye?5:3); + if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty()) + { + left_calib << data.cameraModels()[0].fx(), + data.cameraModels()[0].fy(), + data.cameraModels()[0].cx(), + data.cameraModels()[0].cy(), 0, 0, 0, 0; + } + else + { + UASSERT(data.cameraModels()[0].D_raw().cols >= 4); + left_calib << data.cameraModels()[0].K_raw().at(0,0), + data.cameraModels()[0].K_raw().at(1,1), + data.cameraModels()[0].K_raw().at(0,2), + data.cameraModels()[0].K_raw().at(1,2), + data.cameraModels()[0].D_raw().at(0,0), + data.cameraModels()[0].D_raw().at(0,1), + data.cameraModels()[0].D_raw().at(0,is_fisheye?4:2), + data.cameraModels()[0].D_raw().at(0,is_fisheye?5:3); + } } - params.camera_intrinsics.insert(std::make_pair(0, camLeft)); - params.camera_intrinsics.insert(std::make_pair(1, camRight)); - const IMU & imu = imuBuffer_.begin()->second; - imuLocalTransform_ = imu.localTransform(); - Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform(); - Transform cam0cam1; - if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull()) + Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d(); + Eigen::Matrix left_eigen; + left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose()); + left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1); + params_->camera_intrinsics.at(0)->set_value(left_calib); + params_->camera_extrinsics.emplace(0, left_eigen); + if(!data.rightRaw().empty()) { - cam0cam1 = Transform( + Transform T_left_right; + if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull()) + { + T_left_right = Transform( 1, 0, 0, data.stereoCameraModels()[0].baseline(), 0, 1, 0, 0, 0, 0, 1, 0); + } + else + { + T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse(); + } + UASSERT(!T_left_right.isNull()); + Transform T_imu_right = T_imu_left * T_left_right; + Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d(); + Eigen::Matrix right_eigen; + right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose()); + right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1); + params_->camera_intrinsics.at(1)->set_value(right_calib); + params_->camera_extrinsics.emplace(1, right_eigen); } - else - { - cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse(); - } - UASSERT(!cam0cam1.isNull()); - Transform imuCam1 = imuCam0 * cam0cam1; - Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d(); - Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d(); - Eigen::Matrix cam_eigen0; - cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose()); - cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1); - Eigen::Matrix cam_eigen1; - cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose()); - cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1); - params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0)); - params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1)); - - params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())}); - params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())}); - - vioManager_ = new ov_msckf::VioManager(params); - } - - cv::Mat left; - cv::Mat right; - if(data.imageRaw().type() == CV_8UC3) - { - cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY); + params_->init_options.camera_intrinsics = params_->camera_intrinsics; + params_->init_options.camera_extrinsics = params_->camera_extrinsics; + vioManager_ = std::make_unique(*params_); } - else if(data.imageRaw().type() == CV_8UC1) - { - left = data.imageRaw().clone(); - } - else - { - UFATAL("Not supported color type!"); - } - if(data.rightRaw().type() == CV_8UC3) - { - cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY); - } - else if(data.rightRaw().type() == CV_8UC1) - { - right = data.rightRaw().clone(); - } - else - { - UFATAL("Not supported color type!"); - } - - // Create the measurement - ov_core::CameraData message; - message.timestamp = data.stamp(); - message.sensor_ids.push_back(0); - message.sensor_ids.push_back(1); - message.images.push_back(left); - message.images.push_back(right); - message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1)); - message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1)); - - // send it to our VIO system - vioManager_->feed_measurement_camera(message); - UDEBUG("Image update stamp=%f", data.stamp()); - - double lastIMUstamp = 0.0; - while(!imuBuffer_.empty()) + } + else + { + if(!data.imu().empty()) { - std::map::iterator iter = imuBuffer_.begin(); - - // Process IMU data until stamp is over image stamp ov_core::ImuData message; - message.timestamp = iter->first; - message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2]; - message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2]; - - UDEBUG("IMU update stamp=%f", message.timestamp); - - // send it to our VIO system + message.timestamp = data.stamp(); + message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2]; + message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2]; vioManager_->feed_measurement_imu(message); - - lastIMUstamp = iter->first; - - imuBuffer_.erase(iter); - - if(lastIMUstamp > data.stamp()) - { - break; - } } - if(vioManager_->initialized()) + if(!data.imageRaw().empty()) { - // Get the current state - std::shared_ptr state = vioManager_->get_state(); - - if(state->_timestamp != data.stamp()) + bool covFilled = false; + Eigen::Matrix state_plus = Eigen::Matrix::Zero(); + Eigen::Matrix cov_plus = Eigen::Matrix::Zero(); + if(vioManager_->initialized()) + covFilled = vioManager_->get_propagator()->fast_state_propagate(vioManager_->get_state(), data.stamp(), state_plus, cov_plus); + + cv::Mat image; + if(data.imageRaw().type() == CV_8UC3) + cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY); + else if(data.imageRaw().type() == CV_8UC1) + image = data.imageRaw().clone(); + else + UFATAL("Not supported color type!"); + ov_core::CameraData message; + message.timestamp = data.stamp(); + message.sensor_ids.emplace_back(0); + message.images.emplace_back(image); + if(params_->masks.find(0) != params_->masks.end()) { - UWARN("OpenVINS: Stamp of the current state %f is not the same " - "than last image processed %f (last IMU stamp=%f). There could be " - "a synchronization issue between camera and IMU. ", - state->_timestamp, - data.stamp(), - lastIMUstamp); + message.masks.emplace_back(params_->masks[0]); } - - Transform p( - (float)state->_imu->pos()(0), - (float)state->_imu->pos()(1), - (float)state->_imu->pos()(2), - (float)state->_imu->quat()(0), - (float)state->_imu->quat()(1), - (float)state->_imu->quat()(2), - (float)state->_imu->quat()(3)); - - - // Finally set the covariance in the message (in the order position then orientation as per ros convention) - std::vector> statevars; - statevars.push_back(state->_imu->pose()->p()); - statevars.push_back(state->_imu->pose()->q()); - - cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1); - if(this->framesProcessed() == 0) + else if(!data.depthRaw().empty() && params_->use_mask) { - covariance *= 9999; + cv::Mat mask; + if(data.depthRaw().type() == CV_32FC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist, mask); + else if(data.depthRaw().type() == CV_16UC1) + cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000, + std::isinf(params_->featinit_options.max_dist)?std::numeric_limits::max():params_->featinit_options.max_dist*1000, mask); + message.masks.emplace_back(255-mask); } else { - Eigen::Matrix covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars); - for(int r=0; r<6; r++) { - for(int c=0; c<6; c++) { - ((double *)covariance.data)[6*r+c] = covariance_posori(r,c); - } - } + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); } + if(!data.rightRaw().empty()) + { + if(data.rightRaw().type() == CV_8UC3) + cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY); + else if(data.rightRaw().type() == CV_8UC1) + image = data.rightRaw().clone(); + else + UFATAL("Not supported color type!"); + message.sensor_ids.emplace_back(1); + message.images.emplace_back(image); + if(params_->masks.find(1) != params_->masks.end()) + message.masks.emplace_back(params_->masks[1]); + else + message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1)); + } + vioManager_->feed_measurement_camera(message); - if(!p.isNull()) + std::shared_ptr state = vioManager_->get_state(); + Transform p((float)state->_imu->pos()(0), + (float)state->_imu->pos()(1), + (float)state->_imu->pos()(2), + (float)state->_imu->quat()(0), + (float)state->_imu->quat()(1), + (float)state->_imu->quat()(2), + (float)state->_imu->quat()(3)); + if(!p.isNull() && !p.isIdentity()) { - p = p * imuLocalTransform_.inverse(); + p = p * imuLocalTransformInv_; if(this->getPose().rotation().isIdentity()) { initGravity_ = true; - this->reset(this->getPose()*p.rotation()); + this->reset(this->getPose() * p.rotation()); } - if(previousPose_.isIdentity()) - { - previousPose_ = p; - } + if(previousPoseInv_.isIdentity()) + previousPoseInv_ = p.inverse(); - // make it incremental - Transform previousPoseInv = previousPose_.inverse(); - t = previousPoseInv*p; - previousPose_ = p; + t = previousPoseInv_ * p; if(info) { + double timestamp; + std::unordered_map feat_posinG, feat_tracks_uvd; + vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd); + auto features_SLAM = vioManager_->get_features_SLAM(); + auto good_features_MSCKF = vioManager_->get_good_features_MSCKF(); + info->type = this->getType(); - info->reg.covariance = covariance; + info->localMapSize = feat_posinG.size(); + info->features = features_SLAM.size() + good_features_MSCKF.size(); + info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1); + if(covFilled) + { + Eigen::Matrix covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose(); + cv::eigen2cv(covariance, info->reg.covariance); + } - // feature map - Transform fixT = this->getPose()*previousPoseInv; - Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse(); - for (auto &it_per_id : vioManager_->get_features_SLAM()) + if(this->isInfoDataFilled()) { - cv::Point3f pt3d; - pt3d.x = it_per_id[0]; - pt3d.y = it_per_id[1]; - pt3d.z = it_per_id[2]; - pt3d = util3d::transformPoint(pt3d, fixT); - info->localMap.insert(std::make_pair(info->localMap.size(), pt3d)); + Transform fixT = this->getPose() * previousPoseInv_; + Transform camT; + if(!data.rightRaw().empty()) + camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + else + camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT; + + for(auto &feature : feat_posinG) + { + cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]); + pt3d = util3d::transformPoint(pt3d, fixT); + info->localMap.emplace(feature.first, pt3d); + } if(this->imagesAlreadyRectified()) { - cv::Point2f pt; - pt3d = util3d::transformPoint(pt3d, camLocalTransformInv); - data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); - info->reg.inliersIDs.push_back(info->newCorners.size()); - info->newCorners.push_back(pt); + for(auto &feature : features_SLAM) + { + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.inliersIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); + } + + for(auto &feature : good_features_MSCKF) + { + cv::Point3f pt3d(feature[0], feature[1], feature[2]); + pt3d = util3d::transformPoint(pt3d, camT); + cv::Point2f pt; + if(!data.rightRaw().empty()) + data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + else + data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y); + info->reg.matchesIDs.emplace_back(info->newCorners.size()); + info->newCorners.emplace_back(pt); + } } } - info->features = info->newCorners.size(); - info->localMapSize = info->localMap.size(); } - UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); + + previousPoseInv_ = p.inverse(); } } } - else if(!data.imageRaw().empty() && !data.depthRaw().empty()) - { - UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!"); - } - else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty()) - { - UERROR("OpenVINS requires stereo images!"); - } - else - { - UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!"); - } #else UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach."); diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index 2da2f3e5e6..c292b4d873 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -720,6 +720,39 @@ std::map OptimizerG2O::optimize( int idTag= id2; id2 = landmarkVertexOffset - id2; +#if defined(RTABMAP_VERTIGO) + VertexSwitchLinear * v = 0; + if(this->isRobust() && isLandmarkWithRotation.at(idTag)) + { + // For landmark links, add switchable edges + // Currently supporting only constraints with rotation + + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + v = new VertexSwitchLinear(); + v->setEstimate(1.0); + v->setId(vertigoVertexId++); + UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str()); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + EdgeSwitchPrior * prior = new EdgeSwitchPrior(); + prior->setMeasurement(1.0); + prior->setVertex(0, v); + UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str()); + } + else if(this->isRobust() && !isLandmarkWithRotation.at(idTag)) + { + UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str()); + } +#endif + if(isSlam2d()) { if(isLandmarkWithRotation.at(idTag)) @@ -737,16 +770,35 @@ std::map OptimizerG2O::optimize( information(2,1) = iter->second.infMatrix().at(5,1); // theta-y information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } - g2o::EdgeSE2 * e = new g2o::EdgeSE2(); - g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); - g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta())); - e->setInformation(information); - edge = e; +#if defined(RTABMAP_VERTIGO) + if(this->isRobust()) + { + EdgeSE2Switchable * e = new EdgeSE2Switchable(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta())); + e->setInformation(information); + edge = e; + } + else +#endif + { + g2o::EdgeSE2 * e = new g2o::EdgeSE2(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta())); + e->setInformation(information); + edge = e; + } } else { @@ -780,16 +832,35 @@ std::map OptimizerG2O::optimize( constraint = a.linear(); constraint.translation() = a.translation(); - g2o::EdgeSE3 * e = new g2o::EdgeSE3(); - g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); - g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(constraint); - e->setInformation(information); - edge = e; +#if defined(RTABMAP_VERTIGO) + if(this->isRobust()) + { + EdgeSE3Switchable * e = new EdgeSE3Switchable(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } + else +#endif + { + g2o::EdgeSE3 * e = new g2o::EdgeSE3(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } } else { @@ -2054,7 +2125,43 @@ bool OptimizerG2O::saveGraph( q.w()); } - int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first+1:0; + // For landmarks, determinate which one has observation with orientation + std::map isLandmarkWithRotation; + for(std::multimap::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + int landmarkId = iter->second.from() < 0?iter->second.from():iter->second.to() < 0?iter->second.to():0; + if(landmarkId != 0 && isLandmarkWithRotation.find(landmarkId) == isLandmarkWithRotation.end()) + { + if(isSlam2d()) + { + if (1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) + { + isLandmarkWithRotation.insert(std::make_pair(landmarkId, false)); + UDEBUG("Tag %d has no orientation", landmarkId); + } + else + { + isLandmarkWithRotation.insert(std::make_pair(landmarkId, true)); + UDEBUG("Tag %d has orientation", landmarkId); + } + } + else if (1 / static_cast(iter->second.infMatrix().at(3,3)) >= 9999.0 || + 1 / static_cast(iter->second.infMatrix().at(4,4)) >= 9999.0 || + 1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) + { + isLandmarkWithRotation.insert(std::make_pair(landmarkId, false)); + UDEBUG("Tag %d has no orientation", landmarkId); + } + else + { + isLandmarkWithRotation.insert(std::make_pair(landmarkId, true)); + UDEBUG("Tag %d has orientation", landmarkId); + } + + } + } + + int landmarkOffset = poses.size()&&poses.rbegin()->first>0?poses.rbegin()->first:0; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { if (isSlam2d()) @@ -2063,18 +2170,30 @@ bool OptimizerG2O::saveGraph( { // VERTEX_SE2 id x y theta fprintf(file, "VERTEX_SE2 %d %f %f %f\n", - landmarkOffset-iter->first, + iter->first, iter->second.x(), iter->second.y(), iter->second.theta()); } else if(!landmarksIgnored()) { - // VERTEX_XY id x y - fprintf(file, "VERTEX_XY %d %f %f\n", - iter->first, - iter->second.x(), - iter->second.y()); + if(uValue(isLandmarkWithRotation, iter->first, false)) + { + // VERTEX_SE2 id x y theta + fprintf(file, "VERTEX_SE2 %d %f %f %f\n", + landmarkOffset-iter->first, + iter->second.x(), + iter->second.y(), + iter->second.theta()); + } + else + { + // VERTEX_XY id x y + fprintf(file, "VERTEX_XY %d %f %f\n", + landmarkOffset-iter->first, + iter->second.x(), + iter->second.y()); + } } } else @@ -2095,12 +2214,29 @@ bool OptimizerG2O::saveGraph( } else if(!landmarksIgnored()) { - // VERTEX_TRACKXYZ id x y z - fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n", - landmarkOffset-iter->first, - iter->second.x(), - iter->second.y(), - iter->second.z()); + if(uValue(isLandmarkWithRotation, iter->first, false)) + { + // VERTEX_SE3 id x y z qw qx qy qz + Eigen::Quaternionf q = iter->second.getQuaternionf(); + fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n", + landmarkOffset-iter->first, + iter->second.x(), + iter->second.y(), + iter->second.z(), + q.x(), + q.y(), + q.z(), + q.w()); + } + else + { + // VERTEX_TRACKXYZ id x y z + fprintf(file, "VERTEX_TRACKXYZ %d %f %f %f\n", + landmarkOffset-iter->first, + iter->second.x(), + iter->second.y(), + iter->second.z()); + } } } } @@ -2116,32 +2252,90 @@ bool OptimizerG2O::saveGraph( } if(isSlam2d()) { - // EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22 - fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n", - iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), - iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), - iter->second.transform().x(), - iter->second.transform().y(), - iter->second.infMatrix().at(0, 0), - iter->second.infMatrix().at(0, 1), - iter->second.infMatrix().at(1, 1)); + if(uValue(isLandmarkWithRotation, iter->first, false)) + { + // EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 + fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n", + iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), + iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().theta(), + iter->second.infMatrix().at(0, 0), + iter->second.infMatrix().at(0, 1), + iter->second.infMatrix().at(0, 5), + iter->second.infMatrix().at(1, 1), + iter->second.infMatrix().at(1, 5), + iter->second.infMatrix().at(5, 5)); + } + else + { + // EDGE_SE2_XY observed_vertex_id observing_vertex_id x y inf_11 inf_12 inf_22 + fprintf(file, "EDGE_SE2_XY %d %d %f %f %f %f %f\n", + iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), + iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.infMatrix().at(0, 0), + iter->second.infMatrix().at(0, 1), + iter->second.infMatrix().at(1, 1)); + } } else { - // EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 - fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n", - iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), - iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), - PARAM_OFFSET, - iter->second.transform().x(), - iter->second.transform().y(), - iter->second.transform().z(), - iter->second.infMatrix().at(0, 0), - iter->second.infMatrix().at(0, 1), - iter->second.infMatrix().at(0, 2), - iter->second.infMatrix().at(1, 1), - iter->second.infMatrix().at(1, 2), - iter->second.infMatrix().at(2, 2)); + if(uValue(isLandmarkWithRotation, iter->first, false)) + { + // EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 + Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); + fprintf(file, "EDGE_SE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", + iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), + iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().z(), + q.x(), + q.y(), + q.z(), + q.w(), + iter->second.infMatrix().at(0, 0), + iter->second.infMatrix().at(0, 1), + iter->second.infMatrix().at(0, 2), + iter->second.infMatrix().at(0, 3), + iter->second.infMatrix().at(0, 4), + iter->second.infMatrix().at(0, 5), + iter->second.infMatrix().at(1, 1), + iter->second.infMatrix().at(1, 2), + iter->second.infMatrix().at(1, 3), + iter->second.infMatrix().at(1, 4), + iter->second.infMatrix().at(1, 5), + iter->second.infMatrix().at(2, 2), + iter->second.infMatrix().at(2, 3), + iter->second.infMatrix().at(2, 4), + iter->second.infMatrix().at(2, 5), + iter->second.infMatrix().at(3, 3), + iter->second.infMatrix().at(3, 4), + iter->second.infMatrix().at(3, 5), + iter->second.infMatrix().at(4, 4), + iter->second.infMatrix().at(4, 5), + iter->second.infMatrix().at(5, 5)); + } + else + { + // EDGE_SE3_TRACKXYZ observed_vertex_id observing_vertex_id param_offset x y z inf_11 inf_12 inf_13 inf_22 inf_23 inf_33 + fprintf(file, "EDGE_SE3_TRACKXYZ %d %d %d %f %f %f %f %f %f %f %f %f\n", + iter->second.from()<0?landmarkOffset-iter->second.from():iter->second.from(), + iter->second.to()<0?landmarkOffset-iter->second.to():iter->second.to(), + PARAM_OFFSET, + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().z(), + iter->second.infMatrix().at(0, 0), + iter->second.infMatrix().at(0, 1), + iter->second.infMatrix().at(0, 2), + iter->second.infMatrix().at(1, 1), + iter->second.infMatrix().at(1, 2), + iter->second.infMatrix().at(2, 2)); + } } continue; } diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index ff256493f0..1c540b8f54 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "gtsam/GravityFactor.h" #include #include +#include #ifdef RTABMAP_VERTIGO #include "vertigo/gtsam/betweenFactorSwitchable.h" @@ -62,6 +63,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { +OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) : + Optimizer(parameters), + internalOptimizerType_(Parameters::defaultGTSAMOptimizer()), + isam2_(0), + lastSwitchId_(1000000000) +{ + lastRootFactorIndex_.first = 0; + parseParameters(parameters); +} + +OptimizerGTSAM::~OptimizerGTSAM() +{ +#ifdef RTABMAP_GTSAM + delete isam2_; +#endif +} + bool OptimizerGTSAM::available() { #ifdef RTABMAP_GTSAM @@ -74,7 +92,47 @@ bool OptimizerGTSAM::available() void OptimizerGTSAM::parseParameters(const ParametersMap & parameters) { Optimizer::parseParameters(parameters); - Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_); +#ifdef RTABMAP_GTSAM + Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_); + + bool incremental = isam2_; + double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold(); + int skip = Parameters::defaultGTSAMIncRelinearizeSkip(); + Parameters::parse(parameters, Parameters::kGTSAMIncremental(), incremental); + Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeThreshold(), threshold); + Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeSkip(), skip); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMOptimizer().c_str(), internalOptimizerType_); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncremental().c_str(), incremental?1:0); + UDEBUG("GTSAM %s=%f", Parameters::kGTSAMIncRelinearizeThreshold().c_str(), threshold); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncRelinearizeSkip().c_str(), skip); + if(incremental && !isam2_) + { + gtsam::ISAM2Params::OptimizationParams optParams; + if(internalOptimizerType_==2) + { + optParams = gtsam::ISAM2DoglegParams(); + } + else + { + optParams = gtsam::ISAM2GaussNewtonParams(); + } + gtsam::ISAM2Params params(optParams); + params.relinearizeThreshold = threshold; + params.relinearizeSkip = skip; + params.evaluateNonlinearError = true; + isam2_ = new ISAM2(params); + + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } + else if(!incremental && isam2_) + { + delete isam2_; + isam2_ = 0; + } +#endif } std::map OptimizerGTSAM::optimize( @@ -138,9 +196,13 @@ std::map OptimizerGTSAM::optimize( } } + std::vector addedPrior; + gtsam::FactorIndices removeFactorIndices; + //prior first pose - if(rootId != 0) + if(rootId != 0 && (!isam2_ || lastRootFactorIndex_.first != rootId)) { + UDEBUG("Setting prior for rootId=%d", rootId); UASSERT(uContains(poses, rootId)); const Transform & initialPose = poses.at(rootId); UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false"); @@ -148,6 +210,7 @@ std::map OptimizerGTSAM::optimize( { gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits::min())); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1)); } else { @@ -157,14 +220,95 @@ std::map OptimizerGTSAM::optimize( (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz ).finished()); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1)); + } + if(isam2_ && lastRootFactorIndex_.first!=0) + { + if(uContains(poses, lastRootFactorIndex_.first)) + { + UDEBUG("isam2: switching rootid from %d to %d", lastRootFactorIndex_.first, rootId); + removeFactorIndices.push_back(lastRootFactorIndex_.second); + } + else + { + UDEBUG("isam2: reset iSAM2, disjoint mapping sessions between previous root %d and new root %d", lastRootFactorIndex_.first, rootId); + // reset iSAM2, disjoint mapping session + gtsam::ISAM2Params params = isam2_->params(); + delete isam2_; + isam2_ = new gtsam::ISAM2(params); + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } + lastRootFactorIndex_.first = 0; } } + std::map newPoses; + std::multimap newEdgeConstraints; + + if(isam2_) + { + UDEBUG("Add new poses..."); + // new poses? + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(addedPoses_.find(iter->first) == addedPoses_.end()) + { + newPoses.insert(*iter); + UDEBUG("Adding pose %d to factor graph", iter->first); + } + } + UDEBUG("Add new links..."); + // new links? + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + if(addedPoses_.find(iter->second.from()) == addedPoses_.end() || + addedPoses_.find(iter->second.to()) == addedPoses_.end()) + { + newEdgeConstraints.insert(*iter); + UDEBUG("Adding constraint %d (%d->%d) to factor graph", iter->first, iter->second.from(), iter->second.to()); + } + } + + if(!this->isRobust()) + { + UDEBUG("Remove links..."); + // Remove constraints not there anymore in case the last loop closures were rejected. + // As we don't track "switch" constraints, we don't support this if vertigo is used. + for(size_t i=0; i%d (factor indice=%ld)", + lastAddedConstraints_[i].from, + lastAddedConstraints_[i].to, + lastAddedConstraints_[i].factorIndice); + } + } + } + else if(poses.rbegin()->first >= 1000000000) + { + UERROR("Lastest pose id (%d) is too huge for robust optimization (over switch offset of 1000000000)", poses.rbegin()->first); + return optimizedPoses; + } + + lastAddedConstraints_ = addedPrior; + } + else + { + newPoses = poses; + newEdgeConstraints = edgeConstraints; + } + UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)", rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0); gtsam::Values initialEstimate; std::map isLandmarkWithRotation; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + for(std::map::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) { UASSERT(!iter->second.isNull()); if(isSlam2d()) @@ -172,12 +316,13 @@ std::map OptimizerGTSAM::optimize( if(iter->first > 0) { initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); + addedPoses_.insert(iter->first); } else if(!landmarksIgnored()) { // check if it is SE2 or only PointXY - std::multimap::const_iterator jter=edgeConstraints.find(iter->first); - UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); + std::multimap::const_iterator jter=newEdgeConstraints.find(iter->first); + UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); if (1 / static_cast(jter->second.infMatrix().at(5,5)) >= 9999.0) { @@ -189,6 +334,7 @@ std::map OptimizerGTSAM::optimize( initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); } + addedPoses_.insert(iter->first); } } @@ -197,12 +343,13 @@ std::map OptimizerGTSAM::optimize( if(iter->first > 0) { initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); + addedPoses_.insert(iter->first); } else if(!landmarksIgnored()) { // check if it is SE3 or only PointXYZ - std::multimap::const_iterator jter=edgeConstraints.find(iter->first); - UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); + std::multimap::const_iterator jter=newEdgeConstraints.find(iter->first); + UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); if (1 / static_cast(jter->second.infMatrix().at(3,3)) >= 9999.0 || 1 / static_cast(jter->second.infMatrix().at(4,4)) >= 9999.0 || @@ -216,19 +363,23 @@ std::map OptimizerGTSAM::optimize( initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); } + addedPoses_.insert(iter->first); } } } UDEBUG("fill edges to gtsam..."); - int switchCounter = poses.rbegin()->first+1; - for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + if(!isam2_) + { + lastSwitchId_ = newPoses.rbegin()->first+1; + } + for(std::multimap::const_iterator iter=newEdgeConstraints.begin(); iter!=newEdgeConstraints.end(); ++iter) { int id1 = iter->second.from(); int id2 = iter->second.to(); - UASSERT_MSG(initialEstimate.find(id1)!=initialEstimate.end(), uFormat("id1=%d", id1).c_str()); - UASSERT_MSG(initialEstimate.find(id2)!=initialEstimate.end(), uFormat("id2=%d", id2).c_str()); + UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str()); + UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str()); UASSERT(!iter->second.transform().isNull()); if(id1 == id2) @@ -244,6 +395,7 @@ std::map OptimizerGTSAM::optimize( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else if (1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) { @@ -251,6 +403,7 @@ std::map OptimizerGTSAM::optimize( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else { @@ -270,6 +423,7 @@ std::map OptimizerGTSAM::optimize( gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); graph.add(gtsam::PriorFactor(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } } else @@ -281,6 +435,7 @@ std::map OptimizerGTSAM::optimize( iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); graph.add(XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else if (1 / static_cast(iter->second.infMatrix().at(3,3)) >= 9999.0 || 1 / static_cast(iter->second.infMatrix().at(4,4)) >= 9999.0 || @@ -291,6 +446,7 @@ std::map OptimizerGTSAM::optimize( iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); graph.add(XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else { @@ -308,15 +464,17 @@ std::map OptimizerGTSAM::optimize( gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); graph.add(gtsam::PriorFactor(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } } } - else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end()) + else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end()) { Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz(); gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1)); gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10)); graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1))); + lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1)); } } else if(id1<0 || id2 < 0) @@ -335,6 +493,32 @@ std::map OptimizerGTSAM::optimize( t = iter->second.transform().inverse(); std::swap(id1, id2); // should be node -> landmark } + +#ifdef RTABMAP_VERTIGO + if(this->isRobust() && isLandmarkWithRotation.at(id2)) + { + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + double prior = 1.0; + initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior)); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + } + else if(this->isRobust() && !isLandmarkWithRotation.at(id2)) + { + UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str()); + } +#endif + if(isSlam2d()) { if(isLandmarkWithRotation.at(id2)) @@ -353,7 +537,19 @@ std::map OptimizerGTSAM::optimize( information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); - graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model)); + +#ifdef RTABMAP_VERTIGO + if(this->isRobust()) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(t.x(), t.y(), t.theta()), model)); + } + else +#endif + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); + } } else { @@ -368,6 +564,7 @@ std::map OptimizerGTSAM::optimize( gtsam::Point2 landmark(t.x(), t.y()); gtsam::Pose2 p; graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } else @@ -386,7 +583,21 @@ std::map OptimizerGTSAM::optimize( mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); - graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(t.toEigen4d()), model)); + +#ifdef RTABMAP_VERTIGO + if(this->isRobust() && + iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(t.toEigen4d()), model)); + } + else +#endif + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(t.toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); + } } else { @@ -401,6 +612,7 @@ std::map OptimizerGTSAM::optimize( gtsam::Point3 landmark(t.x(), t.y(), t.z()); gtsam::Pose3 p; graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } } @@ -418,7 +630,7 @@ std::map OptimizerGTSAM::optimize( // a proper and convenient initial value for all switch variables would be // sij = 1 when using the linear switch function" double prior = 1.0; - initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior)); // create switch prior factor // "If the front-end is not able to assign sound individual values @@ -426,7 +638,7 @@ std::map OptimizerGTSAM::optimize( // to the individual optimal choice of Ξij for a large range of // outliers." gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); - graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel)); } #endif if(isSlam2d()) @@ -452,12 +664,13 @@ std::map OptimizerGTSAM::optimize( iter->second.type() != Link::kNeighborMerged) { // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); } else #endif { graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } else @@ -481,57 +694,72 @@ std::map OptimizerGTSAM::optimize( iter->second.type() != Link::kNeighborMerged) { // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); } else #endif { graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } } } UDEBUG("create optimizer"); - gtsam::NonlinearOptimizer * optimizer; + gtsam::NonlinearOptimizer * optimizer = 0; - if(optimizer_ == 2) + if(!isam2_) // Batch optimization { - gtsam::DoglegParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters); - } - else if(optimizer_ == 1) - { - gtsam::GaussNewtonParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters); + UDEBUG("Batch optimization..."); + if(internalOptimizerType_ == 2) + { + gtsam::DoglegParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters); + } + else if(internalOptimizerType_ == 1) + { + gtsam::GaussNewtonParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters); + } + else + { + gtsam::LevenbergMarquardtParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); + } } else { - gtsam::LevenbergMarquardtParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); + UDEBUG("iSAM2 optimization..."); } UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); UTimer timer; int it = 0; - double lastError = optimizer->error(); + double initialError = optimizer?graph.error(initialEstimate):0; + double lastError = optimizer?optimizer->error():0; for(int i=0; i 0) { float x,y,z,roll,pitch,yaw; std::map tmpPoses; - for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) + const Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter) +#else + for(gtsam::Values::const_iterator iter=values.begin(); iter!=values.end(); ++iter) +#endif { - if(iter->value.dim() > 1) + int key = (int)iter->key; + if(iter->value.dim() > 1 && uContains(newPoses, key)) { - int key = (int)iter->key; if(isSlam2d()) { if(key > 0) @@ -543,13 +771,13 @@ std::map OptimizerGTSAM::optimize( { if(isLandmarkWithRotation.at(key)) { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Pose2 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta()))); } else { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Point2 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw))); } @@ -571,7 +799,7 @@ std::map OptimizerGTSAM::optimize( } else { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Point3 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw))); } @@ -581,25 +809,89 @@ std::map OptimizerGTSAM::optimize( } intermediateGraphes->push_back(tmpPoses); } + + gtsam::ISAM2Result result; + double error = 0; try { - optimizer->iterate(); + if(optimizer) // Batch optimization + { + optimizer->iterate(); + error = optimizer->error(); + } + else if(i==0) // iSAM2 (add factors) + { + UDEBUG("Update iSAM with the new factors"); + result = isam2_->update(graph, initialEstimate, removeFactorIndices); +#if BOOST_VERSION >= 106800 + UASSERT(result.errorBefore.has_value()); + UASSERT(result.errorAfter.has_value()); +#else + UASSERT(result.errorBefore.is_initialized()); + UASSERT(result.errorAfter.is_initialized()); +#endif + UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value()); + initialError = lastError = result.errorBefore.value(); + error = result.errorAfter.value(); + if(!this->isRobust()) + { + UASSERT_MSG(lastAddedConstraints_.size() == result.newFactorsIndices.size(), + uFormat("%ld versus %ld", lastAddedConstraints_.size(), result.newFactorsIndices.size()).c_str()); + for(size_t j=0; j=1); + lastRootFactorIndex_.first = rootId; + lastRootFactorIndex_.second = result.newFactorsIndices[0]; // first one should be always the root prior + } + } + else // iSAM2 (more iterations) + { + result = isam2_->update(); +#if BOOST_VERSION >= 106800 + UASSERT(result.errorBefore.has_value()); + UASSERT(result.errorAfter.has_value()); +#else + UASSERT(result.errorBefore.is_initialized()); + UASSERT(result.errorAfter.is_initialized()); +#endif + UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value()); + + lastError = result.errorBefore.value(); + error = result.errorAfter.value(); + } ++it; } catch(gtsam::IndeterminantLinearSystemException & e) { UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(), - (int)edgeConstraints.size(), - (int)poses.size()); + (int)newEdgeConstraints.size(), + (int)newPoses.size()); delete optimizer; + if(isam2_) + { + // We are in bad state, cleanup + UDEBUG("Reset iSAM2!"); + gtsam::ISAM2Params params = isam2_->params(); + delete isam2_; + isam2_ = new gtsam::ISAM2(params); + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } return optimizedPoses; } // early stop condition - double error = optimizer->error(); UDEBUG("iteration %d error =%f", i+1, error); double errorDelta = lastError - error; - if(i>0 && errorDelta < this->epsilon()) + if((isam2_ || i>0) && errorDelta < this->epsilon()) { if(errorDelta < 0) { @@ -626,15 +918,20 @@ std::map OptimizerGTSAM::optimize( { *iterationsDone = it; } - UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", - optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); + UDEBUG("GTSAM optimizing end (%d iterations done (error initial=%f final=%f), time=%f s)", + it, initialError, lastError, timer.ticks()); float x,y,z,roll,pitch,yaw; - for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) + const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); +#if GTSAM_VERSION_NUMERIC >= 40200 + for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter) +#else + for(gtsam::Values::const_iterator iter=values.begin(); iter!=values.end(); ++iter) +#endif { - if(iter->value.dim() > 1) + int key = (int)iter->key; + if(iter->value.dim() > 1 && uContains(poses, key)) { - int key = (int)iter->key; if(isSlam2d()) { if(key > 0) @@ -686,10 +983,18 @@ std::map OptimizerGTSAM::optimize( // compute marginals try { - UDEBUG("Computing marginals..."); + UDEBUG("Computing marginals for node %d...", poses.rbegin()->first); UTimer t; - gtsam::Marginals marginals(graph, optimizer->values()); - gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first); + gtsam::Matrix info; + if(optimizer) + { + gtsam::Marginals marginals(graph, optimizer->values()); + info = marginals.marginalCovariance(poses.rbegin()->first); + } + else //iSAM2 + { + info = isam2_->marginalCovariance(poses.rbegin()->first); + } UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first); if(isSlam2d() && info.cols() == 3 && info.cols() == 3) { diff --git a/corelib/src/optimizer/OptimizerTORO.cpp b/corelib/src/optimizer/OptimizerTORO.cpp index 8342bbffdf..175f616d2d 100644 --- a/corelib/src/optimizer/OptimizerTORO.cpp +++ b/corelib/src/optimizer/OptimizerTORO.cpp @@ -36,8 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #ifdef RTABMAP_TORO -#include "toro3d/treeoptimizer3.hh" -#include "toro3d/treeoptimizer2.hh" +#include "toro3d/treeoptimizer3.h" +#include "toro3d/treeoptimizer2.h" #endif namespace rtabmap { diff --git a/corelib/src/optimizer/gtsam/GravityFactor.h b/corelib/src/optimizer/gtsam/GravityFactor.h index f292072f13..0ca1a75a77 100644 --- a/corelib/src/optimizer/gtsam/GravityFactor.h +++ b/corelib/src/optimizer/gtsam/GravityFactor.h @@ -63,15 +63,21 @@ class GravityFactor { /** vector of errors */ Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = boost::none) const; +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalJacobian<2,3> H = {}) const; +#else + OptionalJacobian<2,3> H = boost::none) const; +#endif /** Serialization function */ +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); } +#endif }; /** @@ -85,7 +91,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; + #else typedef boost::shared_ptr shared_ptr; +#endif /// Typedef to this class typedef Rot3GravityFactor This; @@ -111,7 +121,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( +#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -124,7 +138,11 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { /** vector of errors */ virtual Vector evaluateError(const Rot3& nRb, // - boost::optional H = boost::none) const { +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return attitudeError(nRb, H); } Unit3 nZ() const { @@ -135,7 +153,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -145,6 +163,7 @@ class Rot3GravityFactor: public NoiseModelFactor1, public GravityFactor { ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -163,8 +182,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, public: /// shorthand for a smart pointer to a factor +#if GTSAM_VERSION_NUMERIC >= 40300 + typedef std::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /// Typedef to this class typedef Pose3GravityFactor This; @@ -189,7 +211,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { +#if GTSAM_VERSION_NUMERIC >= 40300 + return std::static_pointer_cast( +#else return boost::static_pointer_cast( +#endif gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -202,7 +228,11 @@ class Pose3GravityFactor: public NoiseModelFactor1, /** vector of errors */ virtual Vector evaluateError(const Pose3& nTb, // +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else boost::optional H = boost::none) const { +#endif Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; @@ -219,7 +249,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, } private: - +#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300 /** Serialization function */ friend class boost::serialization::access; template @@ -229,7 +259,7 @@ class Pose3GravityFactor: public NoiseModelFactor1, ar & boost::serialization::make_nvp("GravityFactor", boost::serialization::base_object(*this)); } - +#endif public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/corelib/src/optimizer/gtsam/XYFactor.h b/corelib/src/optimizer/gtsam/XYFactor.h index d13a3bc8a0..8a7c65585c 100644 --- a/corelib/src/optimizer/gtsam/XYFactor.h +++ b/corelib/src/optimizer/gtsam/XYFactor.h @@ -41,7 +41,12 @@ class XYFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose2 // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const VALUE& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const VALUE& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif // note that use boost optional like a pointer // only calculate jacobian matrix when non-null pointer exists diff --git a/corelib/src/optimizer/gtsam/XYZFactor.h b/corelib/src/optimizer/gtsam/XYZFactor.h index d4ed63c01a..42e55d0b83 100644 --- a/corelib/src/optimizer/gtsam/XYZFactor.h +++ b/corelib/src/optimizer/gtsam/XYZFactor.h @@ -41,14 +41,24 @@ class XYZFactor: public gtsam::NoiseModelFactor1 { // error function // @param p the pose in Pose // @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Pose3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif if(H) { p.translation(H); } return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } - gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional H = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Point3& p, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H = OptionalNone) const { +#else + boost::optional H = boost::none) const { +#endif return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished(); } }; diff --git a/corelib/src/optimizer/toro3d/dmatrix.hh b/corelib/src/optimizer/toro3d/dmatrix.h similarity index 100% rename from corelib/src/optimizer/toro3d/dmatrix.hh rename to corelib/src/optimizer/toro3d/dmatrix.h diff --git a/corelib/src/optimizer/toro3d/posegraph.hh b/corelib/src/optimizer/toro3d/posegraph.h similarity index 100% rename from corelib/src/optimizer/toro3d/posegraph.hh rename to corelib/src/optimizer/toro3d/posegraph.h diff --git a/corelib/src/optimizer/toro3d/posegraph2.cpp b/corelib/src/optimizer/toro3d/posegraph2.cpp index 061a9d9359..d0117279b1 100644 --- a/corelib/src/optimizer/toro3d/posegraph2.cpp +++ b/corelib/src/optimizer/toro3d/posegraph2.cpp @@ -40,7 +40,8 @@ * such as loading, saving, merging constraints, and etc. **/ -#include "posegraph2.hh" +#include "posegraph2.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/posegraph2.hh b/corelib/src/optimizer/toro3d/posegraph2.h similarity index 98% rename from corelib/src/optimizer/toro3d/posegraph2.hh rename to corelib/src/optimizer/toro3d/posegraph2.h index e602b81e4e..a999b53187 100644 --- a/corelib/src/optimizer/toro3d/posegraph2.hh +++ b/corelib/src/optimizer/toro3d/posegraph2.h @@ -43,10 +43,10 @@ #ifndef _POSEGRAPH2_HH_ #define _POSEGRAPH2_HH_ -#include "posegraph.hh" -#include "transformation2.hh" #include #include +#include "posegraph.h" +#include "transformation2.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/posegraph3.cpp b/corelib/src/optimizer/toro3d/posegraph3.cpp index 0bc013127e..de853fff01 100644 --- a/corelib/src/optimizer/toro3d/posegraph3.cpp +++ b/corelib/src/optimizer/toro3d/posegraph3.cpp @@ -39,7 +39,8 @@ * such as loading, saving, merging constraints, and etc. **/ -#include "posegraph3.hh" +#include "posegraph3.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/posegraph3.hh b/corelib/src/optimizer/toro3d/posegraph3.h similarity index 98% rename from corelib/src/optimizer/toro3d/posegraph3.hh rename to corelib/src/optimizer/toro3d/posegraph3.h index ada4fc4819..a5f85cd62c 100644 --- a/corelib/src/optimizer/toro3d/posegraph3.hh +++ b/corelib/src/optimizer/toro3d/posegraph3.h @@ -43,10 +43,10 @@ #ifndef _POSEGRAPH3_HH_ #define _POSEGRAPH3_HH_ -#include "posegraph.hh" -#include "transformation3.hh" #include #include +#include "posegraph.h" +#include "transformation3.h" typedef unsigned int uint; #ifndef M_PI diff --git a/corelib/src/optimizer/toro3d/transformation2.hh b/corelib/src/optimizer/toro3d/transformation2.h similarity index 100% rename from corelib/src/optimizer/toro3d/transformation2.hh rename to corelib/src/optimizer/toro3d/transformation2.h diff --git a/corelib/src/optimizer/toro3d/transformation3.hh b/corelib/src/optimizer/toro3d/transformation3.h similarity index 99% rename from corelib/src/optimizer/toro3d/transformation3.hh rename to corelib/src/optimizer/toro3d/transformation3.h index e81966c42d..d5f29d4c9b 100644 --- a/corelib/src/optimizer/toro3d/transformation3.hh +++ b/corelib/src/optimizer/toro3d/transformation3.h @@ -39,7 +39,8 @@ #include #include -#include "dmatrix.hh" + +#include "dmatrix.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer2.cpp b/corelib/src/optimizer/toro3d/treeoptimizer2.cpp index 1fb9bce69b..f715db26ef 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer2.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer2.cpp @@ -41,7 +41,8 @@ * **/ -#include "treeoptimizer2.hh" +#include "treeoptimizer2.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/treeoptimizer2.hh b/corelib/src/optimizer/toro3d/treeoptimizer2.h similarity index 99% rename from corelib/src/optimizer/toro3d/treeoptimizer2.hh rename to corelib/src/optimizer/toro3d/treeoptimizer2.h index 34f1a963b5..358f52d0d0 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer2.hh +++ b/corelib/src/optimizer/toro3d/treeoptimizer2.h @@ -44,7 +44,7 @@ #ifndef _TREEOPTIMIZER2_HH_ #define _TREEOPTIMIZER2_HH_ -#include "posegraph2.hh" +#include "posegraph2.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3.cpp b/corelib/src/optimizer/toro3d/treeoptimizer3.cpp index f704e6e02a..3478c77ff6 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer3.cpp @@ -41,7 +41,8 @@ * **/ -#include "treeoptimizer3.hh" +#include "treeoptimizer3.h" + #include #include #include diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3.hh b/corelib/src/optimizer/toro3d/treeoptimizer3.h similarity index 99% rename from corelib/src/optimizer/toro3d/treeoptimizer3.hh rename to corelib/src/optimizer/toro3d/treeoptimizer3.h index 17f25c36e5..66cb6a5b29 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3.hh +++ b/corelib/src/optimizer/toro3d/treeoptimizer3.h @@ -44,7 +44,7 @@ #ifndef _TREEOPTIMIZER3_HH_ #define _TREEOPTIMIZER3_HH_ -#include "posegraph3.hh" +#include "posegraph3.h" namespace AISNavigation { diff --git a/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp b/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp index 2ee07e9453..e308275797 100644 --- a/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp +++ b/corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp @@ -34,9 +34,9 @@ * PURPOSE. **********************************************************************/ -#include "treeoptimizer3.hh" #include #include +#include "treeoptimizer3.h" using namespace std; diff --git a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h index 66cf6954e9..ea502a4c6a 100644 --- a/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h +++ b/corelib/src/optimizer/vertigo/gtsam/DerivedValue.h @@ -72,9 +72,15 @@ class DerivedValue : public gtsam::Value { /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ +#if GTSAM_VERSION_NUMERIC >= 40300 + virtual std::shared_ptr clone() const { + return std::make_shared(static_cast(*this)); + } + #else virtual boost::shared_ptr clone() const { return boost::make_shared(static_cast(*this)); } +#endif /// equals implementing generic Value interface virtual bool equals_(const gtsam::Value& p, double tol = 1e-9) const { diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h index 0ee996f64f..40538fc0f0 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h @@ -12,7 +12,7 @@ #include #include -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 namespace gtsam { gtsam::Matrix inverse(const gtsam::Matrix & matrix) { @@ -49,7 +49,7 @@ namespace vertigo { double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); double l1 = nu1 * exp(-0.5*m1); -#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1) +#if GTSAM_VERSION_NUMERIC >= 40100 double m2 = nullHypothesisModel->squaredMahalanobisDistance(error); #else double m2 = nullHypothesisModel->distance(error); diff --git a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h index ab5a443840..de1b4d7364 100644 --- a/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h +++ b/corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h @@ -30,9 +30,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const +#endif { // calculate error @@ -64,9 +70,15 @@ namespace vertigo { betweenFactor(key1, key2, measured, model) {}; gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const +#else boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const +#endif { // calculate error diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index 2260ea6077..e95e0b568b 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableLinear between(const SwitchVariableLinear& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableLinear(l2.value() - value()); @@ -116,11 +121,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 674e897ac7..79e1fca99d 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -76,8 +76,13 @@ namespace vertigo { /** between operation */ inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, +#if GTSAM_VERSION_NUMERIC >= 40300 + OptionalMatrixType H1=OptionalNone, + OptionalMatrixType H2=OptionalNone) const { +#else boost::optional H1=boost::none, boost::optional H2=boost::none) const { +#endif if(H1) *H1 = -gtsam::Matrix::Identity(1, 1); if(H2) *H2 = gtsam::Matrix::Identity(1, 1); return SwitchVariableSigmoid(l2.value() - value()); @@ -117,11 +122,19 @@ template<> struct traits { typedef OptionalJacobian<3, 3> ChartJacobian; typedef gtsam::Vector TangentVector; static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { +#else + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { +#endif return origin.localCoordinates(other); } static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, +#if GTSAM_VERSION_NUMERIC >= 40300 + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { +#else ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { +#endif return g.retract(v); } }; diff --git a/corelib/src/pcl18/surface/impl/texture_mapping.hpp b/corelib/src/pcl18/surface/impl/texture_mapping.hpp index bb31f4b99d..98ea32ffc8 100644 --- a/corelib/src/pcl18/surface/impl/texture_mapping.hpp +++ b/corelib/src/pcl18/surface/impl/texture_mapping.hpp @@ -975,10 +975,6 @@ pcl::TextureMapping::textureMeshwithMultipleCameras (pcl::TextureMesh visible_faces.resize (cpt_visible_faces); mesh.tex_polygons[current_cam].clear (); mesh.tex_polygons[current_cam] = visible_faces; - - int nb_faces = 0; - for (int i = 0; i < static_cast (mesh.tex_polygons.size ()); i++) - nb_faces += static_cast (mesh.tex_polygons[i].size ()); } // we have been through all the cameras. diff --git a/corelib/src/python/PyDescriptor.cpp b/corelib/src/python/PyDescriptor.cpp new file mode 100644 index 0000000000..f99b742708 --- /dev/null +++ b/corelib/src/python/PyDescriptor.cpp @@ -0,0 +1,218 @@ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#define NPY_NO_DEPRECATED_API NPY_API_VERSION +#include + +namespace rtabmap +{ + +PyDescriptor::PyDescriptor( + const ParametersMap & parameters) : + GlobalDescriptorExtractor(parameters), + pModule_(0), + pFunc_(0), + dim_(Parameters::defaultPyDescriptorDim()) +{ + UDEBUG(""); + this->parseParameters(parameters); +} + +PyDescriptor::~PyDescriptor() +{ + UDEBUG(""); + pybind11::gil_scoped_acquire acquire; + + if(pFunc_) + { + Py_DECREF(pFunc_); + } + if(pModule_) + { + Py_DECREF(pModule_); + } +} + +void PyDescriptor::parseParameters(const ParametersMap & parameters) +{ + UDEBUG(""); + std::string previousPath = path_; + Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_); + Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_); + path_ = uReplaceChar(path_, '~', UDirectory::homeDir()); + UINFO("path = %s", path_.c_str()); + UINFO("dim = %d", dim_); + UTimer timer; + + pybind11::gil_scoped_acquire acquire; + + if(pModule_) + { + if(!previousPath.empty() && previousPath.compare(path_)!=0) + { + UDEBUG("we changed script (old=%s), we need to reload (new=%s)", + previousPath.c_str(), path_.c_str()); + if(pFunc_) + { + Py_DECREF(pFunc_); + } + pFunc_=0; + Py_DECREF(pModule_); + pModule_ = 0; + } + } + + if(pModule_==0) + { + UASSERT(pFunc_ == 0); + if(path_.empty()) + { + return; + } + std::string matcherPythonDir = UDirectory::getDir(path_); + if(!matcherPythonDir.empty()) + { + PyRun_SimpleString("import sys"); + PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str()); + } + + _import_array(); + + std::string scriptName = uSplit(UFile::getName(path_), '.').front(); + PyObject * pName = PyUnicode_FromString(scriptName.c_str()); + UDEBUG("PyImport_Import() beg"); + pModule_ = PyImport_Import(pName); + UDEBUG("PyImport_Import() end"); + + Py_DECREF(pName); + + if(!pModule_) + { + UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + } + else + { + PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); + if(pFunc) + { + if(PyCallable_Check(pFunc)) + { + PyObject * result = PyObject_CallFunction(pFunc, "i", dim_); + + if(result == NULL) + { + UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + } + Py_DECREF(result); + + pFunc_ = PyObject_GetAttrString(pModule_, "extract"); + if(pFunc_ && PyCallable_Check(pFunc_)) + { + // we are ready! + } + else + { + UERROR("Cannot find method \"extract(...)\" in %s", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + if(pFunc_) + { + Py_DECREF(pFunc_); + pFunc_ = 0; + } + } + } + else + { + UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); + UERROR("%s", getPythonTraceback().c_str()); + } + Py_DECREF(pFunc); + } + else + { + UERROR("Cannot find method \"init(...)\""); + UERROR("%s", getPythonTraceback().c_str()); + } + } + } +} + +GlobalDescriptor PyDescriptor::extract( + const SensorData & data) const +{ + UDEBUG(""); + UTimer timer; + GlobalDescriptor descriptor; + + if(!pModule_) + { + UERROR("Python module not loaded!"); + return descriptor; + } + + if(!pFunc_) + { + UERROR("Python function not loaded!"); + return descriptor; + } + + pybind11::gil_scoped_acquire acquire; + + if(!data.imageRaw().empty()) + { + std::vector descriptorsQueryV(data.imageRaw().total()*data.imageRaw().channels()); + memcpy(descriptorsQueryV.data(), data.imageRaw().data, data.imageRaw().total()*data.imageRaw().channels()*sizeof(char)); + npy_intp dimsFrom[3] = {data.imageRaw().rows, data.imageRaw().cols, data.imageRaw().channels()}; + PyObject* pImageQuery = PyArray_SimpleNewFromData(3, dimsFrom, NPY_BYTE, (void*)data.imageRaw().data); + UASSERT(pImageQuery); + + UDEBUG("Preparing data time = %fs", timer.ticks()); + + PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageQuery, NULL); + if(pReturn == NULL) + { + UERROR("Failed to call extract() function!"); + UERROR("%s", getPythonTraceback().c_str()); + } + else + { + UDEBUG("Python extraction time = %fs", timer.ticks()); + + PyArrayObject *np_ret = reinterpret_cast(pReturn); + + // Convert back to C++ array and print. + int len1 = PyArray_SHAPE(np_ret)[0]; + int dim = PyArray_SHAPE(np_ret)[1]; + int type = PyArray_TYPE(np_ret); + UDEBUG("Descriptor array %dx%d (type=%d)", len1, dim, type); + UASSERT(len1 == 1); + UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned descriptor should type FLOAT=11, received type=%d", type).c_str()); + + float* d_out = reinterpret_cast(PyArray_DATA(np_ret)); + descriptor = GlobalDescriptor(1, cv::Mat(1, dim, CV_32FC1, d_out).clone()); + + //std::cout << descriptor.data() << std::endl; + + Py_DECREF(pReturn); + } + + Py_DECREF(pImageQuery); + } + else + { + UERROR("Invalid inputs! Missing image."); + } + return descriptor; +} + +} diff --git a/corelib/src/python/PyDescriptor.h b/corelib/src/python/PyDescriptor.h new file mode 100644 index 0000000000..1588b60259 --- /dev/null +++ b/corelib/src/python/PyDescriptor.h @@ -0,0 +1,38 @@ +/** + * Python interface for python descriptors like: + * - NetVLAD: https://github.com/uzh-rpg/netvlad_tf_open + */ + +#ifndef PYDESCRIPTOR_H +#define PYDESCRIPTOR_H + +#include +#include "rtabmap/core/PythonInterface.h" +#include + +namespace rtabmap +{ + +class PyDescriptor : public GlobalDescriptorExtractor +{ +public: + PyDescriptor(const ParametersMap & parameters = ParametersMap()); + virtual ~PyDescriptor(); + + const std::string & path() const {return path_;} + float dim() const {return dim_;} + + virtual void parseParameters(const ParametersMap & parameters); + virtual GlobalDescriptor extract(const SensorData & data) const; + virtual GlobalDescriptorExtractor::Type getType() const {return kPyDescriptor;} + +private: + PyObject * pModule_; + PyObject * pFunc_; + std::string path_; + int dim_; +}; + +} + +#endif diff --git a/corelib/src/python/PyDetector.cpp b/corelib/src/python/PyDetector.cpp index a532085467..d5ee6cb5a2 100644 --- a/corelib/src/python/PyDetector.cpp +++ b/corelib/src/python/PyDetector.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -32,7 +34,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) : return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -54,15 +56,13 @@ PyDetector::PyDetector(const ParametersMap & parameters) : if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyDetector::~PyDetector() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { @@ -72,8 +72,6 @@ PyDetector::~PyDetector() { Py_DECREF(pModule_); } - - unlock(); } void PyDetector::parseParameters(const ParametersMap & parameters) @@ -102,7 +100,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag return keypoints; } - lock(); + pybind11::gil_scoped_acquire acquire; if(!pFunc_) { @@ -116,7 +114,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(result); @@ -129,7 +127,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -141,7 +139,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } Py_DECREF(pFunc); @@ -149,7 +147,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return keypoints; } UDEBUG("init time = %fs", timer.ticks()); @@ -167,7 +165,7 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -227,8 +225,6 @@ std::vector PyDetector::generateKeypointsImpl(const cv::Mat & imag Py_DECREF(pImageBuffer); } - unlock(); - return keypoints; } diff --git a/corelib/src/python/PyDetector.h b/corelib/src/python/PyDetector.h index 4512b8eb02..fdbf43ae2b 100644 --- a/corelib/src/python/PyDetector.h +++ b/corelib/src/python/PyDetector.h @@ -1,11 +1,10 @@ /** - * Python interface for python matchers like: - * - SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork - * - OANET https://github.com/zjhthu/OANet + * Python interface for python local feature detectors like: + * - SuperPoint: https://github.com/magicleap/SuperPointPretrainedNetwork */ -#ifndef PYMATCHER_H -#define PYMATCHER_H +#ifndef PYDETECTOR_H +#define PYDETECTOR_H #include #include @@ -18,7 +17,7 @@ namespace rtabmap { -class PyDetector : public Feature2D, public PythonInterface +class PyDetector : public Feature2D { public: PyDetector(const ParametersMap & parameters = ParametersMap()); diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index 446f28c961..aac482efe1 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -10,6 +10,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -39,7 +41,7 @@ PyMatcher::PyMatcher( return; } - lock(); + pybind11::gil_scoped_acquire acquire; std::string matcherPythonDir = UDirectory::getDir(path_); if(!matcherPythonDir.empty()) @@ -59,15 +61,13 @@ PyMatcher::PyMatcher( if(!pModule_) { UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - - unlock(); } PyMatcher::~PyMatcher() { - lock(); + pybind11::gil_scoped_acquire acquire; if(pFunc_) { Py_DECREF(pFunc_); @@ -76,7 +76,6 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } - unlock(); } std::vector PyMatcher::match( @@ -104,7 +103,7 @@ std::vector PyMatcher::match( imageSize.width>0 && imageSize.height>0) { - lock(); + pybind11::gil_scoped_acquire acquire; UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0); @@ -120,7 +119,7 @@ std::vector PyMatcher::match( if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(result); @@ -133,7 +132,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -145,7 +144,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } Py_DECREF(pFunc); @@ -153,7 +152,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); return matches; } UDEBUG("init time = %fs", timer.ticks()); @@ -216,7 +215,7 @@ std::vector PyMatcher::match( if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { @@ -260,7 +259,6 @@ std::vector PyMatcher::match( UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks()); } - unlock(); } else { diff --git a/corelib/src/python/PyMatcher.h b/corelib/src/python/PyMatcher.h index 2af25c75cc..da04cd466f 100644 --- a/corelib/src/python/PyMatcher.h +++ b/corelib/src/python/PyMatcher.h @@ -16,7 +16,7 @@ namespace rtabmap { -class PyMatcher : public PythonInterface +class PyMatcher { public: PyMatcher(const std::string & pythonMatcherPath, diff --git a/corelib/src/python/PythonInterface.cpp b/corelib/src/python/PythonInterface.cpp index 7e9afb48b3..0e2d987980 100644 --- a/corelib/src/python/PythonInterface.cpp +++ b/corelib/src/python/PythonInterface.cpp @@ -8,83 +8,26 @@ #include #include #include +#include namespace rtabmap { -UMutex PythonInterface::mutex_; -int PythonInterface::refCount_ = 0; -PyThreadState * PythonInterface::mainThreadState_ = 0; -unsigned long PythonInterface::mainThreadID_ = 0; - -PythonInterface::PythonInterface() : - threadState_(0) +PythonInterface::PythonInterface() { - UScopeMutex lockM(mutex_); - if(refCount_ == 0) - { - UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId()); - // initialize Python - Py_Initialize(); - - // initialize thread support - PyEval_InitThreads(); - Py_DECREF(PyImport_ImportModule("threading")); - - //release the GIL, store thread state, set the current thread state to NULL - mainThreadState_ = PyEval_SaveThread(); - UASSERT(mainThreadState_); - mainThreadID_ = UThread::currentThreadId(); - } - - ++refCount_; + UINFO("Initialize python interpreter"); + guard_ = new pybind11::scoped_interpreter(); + pybind11::module::import("threading"); + release_ = new pybind11::gil_scoped_release(); } PythonInterface::~PythonInterface() { - UScopeMutex lock(mutex_); - if(refCount_>0 && --refCount_==0) - { - // shut down the interpreter - UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId()); - PyEval_RestoreThread(mainThreadState_); - Py_Finalize(); - } -} - -void PythonInterface::lock() -{ - mutex_.lock(); - - UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - if(UThread::currentThreadId() == mainThreadID_) - { - PyEval_RestoreThread(mainThreadState_); - } - else - { - // create a thread state object for this thread - threadState_ = PyThreadState_New(mainThreadState_->interp); - UASSERT(threadState_); - PyEval_RestoreThread(threadState_); - } -} - -void PythonInterface::unlock() -{ - if(UThread::currentThreadId() == mainThreadID_) - { - mainThreadState_ = PyEval_SaveThread(); - } - else - { - PyThreadState_Clear(threadState_); - PyThreadState_DeleteCurrent(); - } - UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_); - mutex_.unlock(); + UINFO("Finalize python interpreter"); + delete release_; + delete guard_; } -std::string PythonInterface::getTraceback() +std::string getPythonTraceback() { // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated diff --git a/corelib/src/python/rtabmap_netvlad.py b/corelib/src/python/rtabmap_netvlad.py new file mode 100644 index 0000000000..3fa7a131ef --- /dev/null +++ b/corelib/src/python/rtabmap_netvlad.py @@ -0,0 +1,83 @@ +#! /usr/bin/env python3 +# +# Drop this file in the "python" folder of NetVLAD git (tensorflow-v1 used): https://github.com/uzh-rpg/netvlad_tf_open/ +# Updated to work with https://github.com/uzh-rpg/netvlad_tf_open/pull/9 +# To use with rtabmap: +# --Mem/GlobalDescriptorStrategy 1 --Kp/TfIdfLikelihoodUsed false --Mem/RehearsalSimilarity 1 --PyDescriptor/Dim 128 --PyDescriptor/Path ~/netvlad_tf_open/python/rtabmap_netvlad.py +# + +import sys +import os +import numpy as np +import time +sys.path.append(os.path.dirname(os.path.realpath(__file__))) +if not hasattr(sys, 'argv'): + sys.argv = [''] + +#print(os.sys.path) +#print(sys.version) + +import tensorflow as tf +import netvlad_tf.net_from_mat as nfm +import netvlad_tf.nets as nets + +image_batch = None +net_out = None +saver = None +sess = None +dim = 4096 + +def init(descriptorDim): + print("NetVLAD python init()") + global image_batch + global net_out + global saver + global sess + global dim + + dim = descriptorDim + + tf.compat.v1.disable_eager_execution() + tf.compat.v1.reset_default_graph() + + image_batch = tf.compat.v1.placeholder( + dtype=tf.float32, shape=[None, None, None, 3]) + + net_out = nets.vgg16NetvladPca(image_batch) + saver = tf.compat.v1.train.Saver() + + sess = tf.compat.v1.Session() + saver.restore(sess, nets.defaultCheckpoint()) + + +def extract(image): + print(f"NetVLAD python extract{image.shape}") + global image_batch + global net_out + global sess + global dim + + if(image.shape[2] == 1): + image = np.dstack((image, image, image)) + + batch = np.expand_dims(image, axis=0) + result = sess.run(net_out, feed_dict={image_batch: batch}) + + # All that needs to be done (only valid for NetVLAD+whitening networks!) + # to reduce the dimensionality of the NetVLAD representation below 4096 to D + # is to keep the first D dimensions and L2-normalize. + if(result.shape[1] > dim): + v = result[:, :dim] + result = v/np.linalg.norm(v) + + return np.float32(result) + + +if __name__ == '__main__': + #test + img = np.zeros([100,100,3],dtype=np.uint8) + img.fill(255) + init(128) + descriptor = extract(img) + print(descriptor.shape) + print(descriptor) \ No newline at end of file diff --git a/corelib/src/stereo/StereoBM.cpp b/corelib/src/stereo/StereoBM.cpp index 7d47494665..93958293f8 100644 --- a/corelib/src/stereo/StereoBM.cpp +++ b/corelib/src/stereo/StereoBM.cpp @@ -121,6 +121,14 @@ cv::Mat StereoBM::computeDisparity( stereo->setDisp12MaxDiff(disp12MaxDiff_); stereo->compute(leftMono, rightImage, disparity); #endif + + if(minDisparity_>0) + { + cv::Mat dst; + cv::threshold(disparity, dst, minDisparity_*16, 0, cv::THRESH_TOZERO); + disparity = dst; + } + return disparity; } diff --git a/corelib/src/stereo/StereoSGBM.cpp b/corelib/src/stereo/StereoSGBM.cpp index f5388b2cf9..02c2fffe4d 100644 --- a/corelib/src/stereo/StereoSGBM.cpp +++ b/corelib/src/stereo/StereoSGBM.cpp @@ -113,6 +113,14 @@ cv::Mat StereoSGBM::computeDisparity( mode_); stereo->compute(leftMono, rightImage, disparity); #endif + + if(minDisparity_>0) + { + cv::Mat dst; + cv::threshold(disparity, dst, minDisparity_*16, 0, cv::THRESH_TOZERO); + disparity = dst; + } + return disparity; } diff --git a/corelib/src/superpoint_torch/SuperPoint.cc b/corelib/src/superpoint_torch/SuperPoint.cc index 6881c4bde6..e7e7f5b177 100644 --- a/corelib/src/superpoint_torch/SuperPoint.cc +++ b/corelib/src/superpoint_torch/SuperPoint.cc @@ -40,7 +40,7 @@ SuperPoint::SuperPoint() convDa(torch::nn::Conv2dOptions(c4, c5, 3).stride(1).padding(1)), convDb(torch::nn::Conv2dOptions(c5, d1, 1).stride(1).padding(0)) - { +{ register_module("conv1a", conv1a); register_module("conv1b", conv1b); @@ -58,11 +58,10 @@ SuperPoint::SuperPoint() register_module("convDa", convDa); register_module("convDb", convDb); - } - - -std::vector SuperPoint::forward(torch::Tensor x) { +} +std::vector SuperPoint::forward(torch::Tensor x) +{ x = torch::relu(conv1a->forward(x)); x = torch::relu(conv1b->forward(x)); x = torch::max_pool2d(x, 2, 2); @@ -104,14 +103,7 @@ std::vector SuperPoint::forward(torch::Tensor x) { ret.push_back(desc); return ret; - } - -void NMS(const std::vector & ptsIn, - const cv::Mat & conf, - const cv::Mat & descriptorsIn, - std::vector & ptsOut, - cv::Mat & descriptorsOut, - int border, int dist_thresh, int img_width, int img_height); +} SPDetector::SPDetector(const std::string & modelPath, float threshold, bool nms, int minDistance, bool cuda) : threshold_(threshold), @@ -162,9 +154,31 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & x = x.set_requires_grad(false); auto out = model_->forward(x.to(device)); - prob_ = out[0].squeeze(0); // [H, W] + auto scores = out[0]; // [1, H, W] desc_ = out[1]; // [1, 256, H/8, W/8] + if(nms_) + { + auto options = torch::nn::functional::MaxPool2dFuncOptions(minDistance_*2+1).stride(1).padding(minDistance_); + auto options_r1 = torch::nn::functional::MaxPool2dFuncOptions(3).stride(1).padding(1); + + auto zeros = torch::zeros_like(scores); + auto max_mask = scores == torch::nn::functional::max_pool2d(scores, options); + auto max_mask_r1 = scores == torch::nn::functional::max_pool2d(scores, options_r1); + for(size_t i=0; i<2; i++) + { + auto supp_mask = torch::nn::functional::max_pool2d(max_mask.to(torch::kF32), options) > 0; + auto supp_scores = torch::where(supp_mask, zeros, scores); + auto new_max_mask = supp_scores == torch::nn::functional::max_pool2d(supp_scores, options); + max_mask = max_mask | (new_max_mask & (~supp_mask) & max_mask_r1); + } + prob_ = torch::where(max_mask, scores, zeros).squeeze(0); + } + else + { + prob_ = scores.squeeze(0); + } + auto kpts = (prob_ > threshold_); kpts = torch::nonzero(kpts); // [n_keypoints, 2] (y, x) @@ -172,52 +186,24 @@ std::vector SPDetector::detect(const cv::Mat &img, const cv::Mat & auto kpts_cpu = kpts.to(torch::kCPU); auto prob_cpu = prob_.to(torch::kCPU); - std::vector keypoints_no_nms; - for (int i = 0; i < kpts_cpu.size(0); i++) { + std::vector keypoints; + for(int i=0; i(kpts_cpu[i][0].item(), kpts_cpu[i][1].item()) != 0) { float response = prob_cpu[kpts_cpu[i][0]][kpts_cpu[i][1]].item(); - keypoints_no_nms.push_back(cv::KeyPoint(kpts_cpu[i][1].item(), kpts_cpu[i][0].item(), 8, -1, response)); + keypoints.emplace_back(cv::KeyPoint(kpts_cpu[i][1].item(), kpts_cpu[i][0].item(), 8, -1, response)); } } detected_ = true; - if (nms_ && !keypoints_no_nms.empty()) { - cv::Mat conf(keypoints_no_nms.size(), 1, CV_32F); - for (size_t i = 0; i < keypoints_no_nms.size(); i++) { - int x = keypoints_no_nms[i].pt.x; - int y = keypoints_no_nms[i].pt.y; - conf.at(i, 0) = prob_cpu[y][x].item(); - } - - int border = 0; - int dist_thresh = minDistance_; - int height = img.rows; - int width = img.cols; - - std::vector keypoints; - cv::Mat descEmpty; - NMS(keypoints_no_nms, conf, descEmpty, keypoints, descEmpty, border, dist_thresh, width, height); - if(keypoints.size()>1) - { - return keypoints; - } - return std::vector(); - } - else if(keypoints_no_nms.size()>1) - { - return keypoints_no_nms; - } - else - { - return std::vector(); - } + return keypoints; } else { UERROR("No model is loaded!"); return std::vector(); - } + } } cv::Mat SPDetector::compute(const std::vector &keypoints) @@ -274,119 +260,4 @@ cv::Mat SPDetector::compute(const std::vector &keypoints) } } -void NMS(const std::vector & ptsIn, - const cv::Mat & conf, - const cv::Mat & descriptorsIn, - std::vector & ptsOut, - cv::Mat & descriptorsOut, - int border, int dist_thresh, int img_width, int img_height) -{ - - std::vector pts_raw; - - for (size_t i = 0; i < ptsIn.size(); i++) - { - int u = (int) ptsIn[i].pt.x; - int v = (int) ptsIn[i].pt.y; - - pts_raw.push_back(cv::Point2f(u, v)); - } - - //Grid Value Legend: - // 255 : Kept. - // 0 : Empty or suppressed. - // 100 : To be processed (converted to either kept or suppressed). - cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); - cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); - - cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); - - grid.setTo(0); - inds.setTo(0); - confidence.setTo(0); - - for (size_t i = 0; i < pts_raw.size(); i++) - { - int uu = (int) pts_raw[i].x; - int vv = (int) pts_raw[i].y; - - grid.at(vv, uu) = 100; - inds.at(vv, uu) = i; - - confidence.at(vv, uu) = conf.at(i, 0); - } - - // debug - //cv::Mat confidenceVis = confidence.clone() * 255; - //confidenceVis.convertTo(confidenceVis, CV_8UC1); - //cv::imwrite("confidence.bmp", confidenceVis); - //cv::imwrite("grid_in.bmp", grid); - - cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0); - - for (size_t i = 0; i < pts_raw.size(); i++) - { - // account for top left padding - int uu = (int) pts_raw[i].x + dist_thresh; - int vv = (int) pts_raw[i].y + dist_thresh; - float c = confidence.at(vv-dist_thresh, uu-dist_thresh); - - if (grid.at(vv, uu) == 100) // If not yet suppressed. - { - for(int k = -dist_thresh; k < (dist_thresh+1); k++) - { - for(int j = -dist_thresh; j < (dist_thresh+1); j++) - { - if(j==0 && k==0) - continue; - - if ( confidence.at(vv + k - dist_thresh, uu + j - dist_thresh) <= c ) - { - grid.at(vv + k, uu + j) = 0; - } - } - } - grid.at(vv, uu) = 255; - } - } - - size_t valid_cnt = 0; - std::vector select_indice; - - grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height)); - - //debug - //cv::imwrite("grid_nms.bmp", grid); - - for (int v = 0; v < img_height; v++) - { - for (int u = 0; u < img_width; u++) - { - if (grid.at(v,u) == 255) - { - int select_ind = (int) inds.at(v, u); - float response = conf.at(select_ind, 0); - ptsOut.push_back(cv::KeyPoint(pts_raw[select_ind], 8.0f, -1, response)); - - select_indice.push_back(select_ind); - valid_cnt++; - } - } - } - - if(!descriptorsIn.empty()) - { - UASSERT(descriptorsIn.rows == (int)ptsIn.size()); - descriptorsOut.create(select_indice.size(), 256, CV_32F); - - for (size_t i=0; i(i, j) = descriptorsIn.at(select_indice[i], j); - } - } - } -} - } diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index bcbf13f6c6..41d97cf358 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -2093,6 +2093,207 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ) } } +void NMS( + const std::vector & ptsIn, + const cv::Mat & descriptorsIn, + std::vector & ptsOut, + cv::Mat & descriptorsOut, + int border, int dist_thresh, int img_width, int img_height) +{ + std::vector pts_raw; + + for (size_t i = 0; i < ptsIn.size(); i++) + { + int u = (int) ptsIn[i].pt.x; + int v = (int) ptsIn[i].pt.y; + + pts_raw.emplace_back(cv::Point2f(u, v)); + } + + //Grid Value Legend: + // 255 : Kept. + // 0 : Empty or suppressed. + // 100 : To be processed (converted to either kept or suppressed). + cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1); + cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1); + + cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); + cv::Mat dilated_conf = cv::Mat(cv::Size(img_width, img_height), CV_32FC1); + + grid.setTo(0); + inds.setTo(0); + confidence.setTo(0); + + for (size_t i = 0; i < pts_raw.size(); i++) + { + int uu = (int) pts_raw[i].x; + int vv = (int) pts_raw[i].y; + + grid.at(vv, uu) = 100; + inds.at(vv, uu) = i; + + confidence.at(vv, uu) = ptsIn[i].response; + } + + cv::dilate(confidence, dilated_conf, cv::Mat()); + cv::Mat peaks = confidence == dilated_conf; + + cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0); + + for (size_t i = 0; i < pts_raw.size(); i++) + { + // account for top left padding + int uu = (int) pts_raw[i].x + dist_thresh; + int vv = (int) pts_raw[i].y + dist_thresh; + float c = confidence.at(vv-dist_thresh, uu-dist_thresh); + + if (grid.at(vv, uu) == 100) // If not yet suppressed. + { + if (peaks.at(vv-dist_thresh, uu-dist_thresh) == 255) + { + for(int k = -dist_thresh; k < (dist_thresh+1); k++) + { + for(int j = -dist_thresh; j < (dist_thresh+1); j++) + { + if ((j==0 && k==0) || grid.at(vv + k, uu + j) == 0) + continue; + + if (confidence.at(vv + k - dist_thresh, uu + j - dist_thresh) <= c) + grid.at(vv + k, uu + j) = 0; + } + } + grid.at(vv, uu) = 255; + } + else + { + grid.at(vv, uu) = 0; + } + } + } + + size_t valid_cnt = 0; + std::vector select_indice; + + grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height)); + + for (int v = 0; v < img_height; v++) + { + for (int u = 0; u < img_width; u++) + { + if (grid.at(v,u) == 255) + { + int select_ind = (int) inds.at(v, u); + ptsOut.emplace_back(ptsIn[select_ind]); + select_indice.emplace_back(select_ind); + valid_cnt++; + } + } + } + + if(!descriptorsIn.empty()) + { + UASSERT(descriptorsIn.rows == (int)ptsIn.size()); + descriptorsOut.create(select_indice.size(), 256, CV_32F); + + for (size_t i=0; i M_PI/4)) + { + // Return original because of ambiguity for what would be considered up... + UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch); + return; + } + if(roll<0) + { + roll+=2*M_PI; + } + if(roll >= M_PI/4 && roll < 3*M_PI/4) + { + UDEBUG("ROTATION_90 (roll=%f)", roll); + if(!rgb.empty()) + { + cv::flip(rgb,rgb,1); + cv::transpose(rgb,rgb); + } + if(!depth.empty()) + { + cv::flip(depth,depth,1); + cv::transpose(depth,depth); + } + cv::Size sizet(model.imageHeight(), model.imageWidth()); + model = CameraModel( + model.fy(), + model.fx(), + model.cy(), + model.cx()>0?model.imageWidth()-model.cx():0, + model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0)); + model.setImageSize(sizet); + } + else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4) + { + UDEBUG("ROTATION_180 (roll=%f)", roll); + if(!rgb.empty()) + { + cv::flip(rgb,rgb,1); + cv::flip(rgb,rgb,0); + } + if(!depth.empty()) + { + cv::flip(depth,depth,1); + cv::flip(depth,depth,0); + } + cv::Size sizet(model.imageWidth(), model.imageHeight()); + model = CameraModel( + model.fx(), + model.fy(), + model.cx()>0?model.imageWidth()-model.cx():0, + model.cy()>0?model.imageHeight()-model.cy():0, + model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0)); + model.setImageSize(sizet); + } + else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4) + { + UDEBUG("ROTATION_270 (roll=%f)", roll); + if(!rgb.empty()) + { + cv::transpose(rgb,rgb); + cv::flip(rgb,rgb,1); + } + if(!depth.empty()) + { + cv::transpose(depth,depth); + cv::flip(depth,depth,1); + } + cv::Size sizet(model.imageHeight(), model.imageWidth()); + model = CameraModel( + model.fy(), + model.fx(), + model.cy()>0?model.imageHeight()-model.cy():0, + model.cx(), + model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0)); + model.setImageSize(sizet); + } + else + { + UDEBUG("ROTATION_0 (roll=%f)", roll); + } +} + } } diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index ecbed214ce..606dfa6996 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -850,12 +851,12 @@ pcl::PointCloud::Ptr cloudFromStereoImages( validIndices); } -pcl::PointCloud::Ptr cloudFromSensorData( +std::vector::Ptr> cloudsFromSensorData( const SensorData & sensorData, int decimation, float maxDepth, float minDepth, - std::vector * validIndices, + std::vector * validIndices, const ParametersMap & stereoParameters, const std::vector & roiRatios) { @@ -864,7 +865,7 @@ pcl::PointCloud::Ptr cloudFromSensorData( decimation = 1; } - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + std::vector::Ptr> clouds; if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) { @@ -873,6 +874,11 @@ pcl::PointCloud::Ptr cloudFromSensorData( int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size(); for(unsigned int i=0; i::Ptr(new pcl::PointCloud)); + if(validIndices) + { + validIndices->push_back(pcl::IndicesPtr(new std::vector())); + } if(sensorData.cameraModels()[i].isValidForProjection()) { cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)); @@ -928,7 +934,7 @@ pcl::PointCloud::Ptr cloudFromSensorData( decimation, maxDepth, minDepth, - sensorData.cameraModels().size()==1?validIndices:0); + validIndices?validIndices->back().get():0); if(tmp->size()) { @@ -936,16 +942,7 @@ pcl::PointCloud::Ptr cloudFromSensorData( { tmp = util3d::transformPointCloud(tmp, model.localTransform()); } - - if(sensorData.cameraModels().size() > 1) - { - tmp = util3d::removeNaNFromPointCloud(tmp); - *cloud += *tmp; - } - else - { - cloud = tmp; - } + clouds.back() = tmp; } } else @@ -974,6 +971,11 @@ pcl::PointCloud::Ptr cloudFromSensorData( int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size(); for(unsigned int i=0; i::Ptr(new pcl::PointCloud)); + if(validIndices) + { + validIndices->push_back(pcl::IndicesPtr(new std::vector())); + } if(sensorData.stereoCameraModels()[i].isValidForProjection()) { cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows)); @@ -1014,7 +1016,7 @@ pcl::PointCloud::Ptr cloudFromSensorData( decimation, maxDepth, minDepth, - validIndices); + validIndices?validIndices->back().get():0); if(tmp->size()) { @@ -1022,16 +1024,7 @@ pcl::PointCloud::Ptr cloudFromSensorData( { tmp = util3d::transformPointCloud(tmp, model.localTransform()); } - - if(sensorData.stereoCameraModels().size() > 1) - { - tmp = util3d::removeNaNFromPointCloud(tmp); - *cloud += *tmp; - } - else - { - cloud = tmp; - } + clouds.back() = tmp; } } else @@ -1041,24 +1034,68 @@ pcl::PointCloud::Ptr cloudFromSensorData( } } - if(!cloud->empty() && cloud->is_dense && validIndices) + return clouds; +} + +pcl::PointCloud::Ptr cloudFromSensorData( + const SensorData & sensorData, + int decimation, + float maxDepth, + float minDepth, + std::vector * validIndices, + const ParametersMap & stereoParameters, + const std::vector & roiRatios) +{ + std::vector validIndicesV; + std::vector::Ptr> clouds = cloudsFromSensorData( + sensorData, + decimation, + maxDepth, + minDepth, + validIndices?&validIndicesV:0, + stereoParameters, + roiRatios); + + if(validIndices) + { + UASSERT(validIndicesV.size() == clouds.size()); + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + if(clouds.size() == 1) { - //generate indices for all points (they are all valid) - validIndices->resize(cloud->size()); - for(unsigned int i=0; isize(); ++i) + cloud = clouds[0]; + if(validIndices) { - validIndices->at(i) = i; + *validIndices = *validIndicesV[0]; + } + } + else + { + for(size_t i=0; iresize(cloud->size()); + for(size_t i=0; isize(); ++i) + { + validIndices->at(i) = i; + } } } return cloud; } -pcl::PointCloud::Ptr cloudRGBFromSensorData( +std::vector::Ptr> cloudsRGBFromSensorData( const SensorData & sensorData, int decimation, float maxDepth, float minDepth, - std::vector * validIndices, + std::vector * validIndices, const ParametersMap & stereoParameters, const std::vector & roiRatios) { @@ -1067,7 +1104,7 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( decimation = 1; } - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + std::vector::Ptr> clouds; if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size()) { @@ -1082,6 +1119,11 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( for(unsigned int i=0; i::Ptr(new pcl::PointCloud)); + if(validIndices) + { + validIndices->push_back(pcl::IndicesPtr(new std::vector())); + } if(sensorData.cameraModels()[i].isValidForProjection()) { cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)); @@ -1128,7 +1170,7 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( decimation, maxDepth, minDepth, - sensorData.cameraModels().size() == 1?validIndices:0); + validIndices?validIndices->back().get():0); if(tmp->size()) { @@ -1136,16 +1178,7 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( { tmp = util3d::transformPointCloud(tmp, model.localTransform()); } - - if(sensorData.cameraModels().size() > 1) - { - tmp = util3d::removeNaNFromPointCloud(tmp); - *cloud += *tmp; - } - else - { - cloud = tmp; - } + clouds.back() = tmp; } } else @@ -1164,6 +1197,11 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size(); for(unsigned int i=0; i::Ptr(new pcl::PointCloud)); + if(validIndices) + { + validIndices->push_back(pcl::IndicesPtr(new std::vector())); + } if(sensorData.stereoCameraModels()[i].isValidForProjection()) { cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)); @@ -1205,7 +1243,7 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( decimation, maxDepth, minDepth, - validIndices, + validIndices?validIndices->back().get():0, stereoParameters); if(tmp->size()) @@ -1214,16 +1252,7 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( { tmp = util3d::transformPointCloud(tmp, model.localTransform()); } - - if(sensorData.stereoCameraModels().size() > 1) - { - tmp = util3d::removeNaNFromPointCloud(tmp); - *cloud += *tmp; - } - else - { - cloud = tmp; - } + clouds.back() = tmp; } } else @@ -1233,16 +1262,59 @@ pcl::PointCloud::Ptr cloudRGBFromSensorData( } } - if(cloud->is_dense && validIndices) + return clouds; +} + +pcl::PointCloud::Ptr cloudRGBFromSensorData( + const SensorData & sensorData, + int decimation, + float maxDepth, + float minDepth, + std::vector * validIndices, + const ParametersMap & stereoParameters, + const std::vector & roiRatios) +{ + std::vector validIndicesV; + std::vector::Ptr> clouds = cloudsRGBFromSensorData( + sensorData, + decimation, + maxDepth, + minDepth, + validIndices?&validIndicesV:0, + stereoParameters, + roiRatios); + + if(validIndices) + { + UASSERT(validIndicesV.size() == clouds.size()); + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + if(clouds.size() == 1) { - //generate indices for all points (they are all valid) - validIndices->resize(cloud->size()); - for(unsigned int i=0; isize(); ++i) + cloud = clouds[0]; + if(validIndices) { - validIndices->at(i) = i; + *validIndices = *validIndicesV[0]; + } + } + else + { + for(size_t i=0; iresize(cloud->size()); + for(size_t i=0; isize(); ++i) + { + validIndices->at(i) = i; + } } } - return cloud; } @@ -2167,7 +2239,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con { pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud); } - else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI) + else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT) { pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud); } @@ -2197,8 +2269,17 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con pcl::PointCloud::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloud(const LaserScan & lase pcl::PointCloud::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull(); for(int i=0; i::Ptr laserScanToPointCloudNormal(const LaserSc pcl::PointCloud::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudRGB(const LaserScan pcl::PointCloud::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudI(const LaserScan & la pcl::PointCloud::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i::Ptr laserScanToPointCloudRGBNormal(cons pcl::PointCloud::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2317,7 +2444,8 @@ pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index) { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointNormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2338,7 +2466,8 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZRGB output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2377,7 +2506,8 @@ pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float i { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZI output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2402,7 +2532,8 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZRGBNormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2449,7 +2580,8 @@ pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int in { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZINormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2532,7 +2664,8 @@ cv::Point3f projectDisparityTo3D( c = model.right().cx() - model.left().cx(); } float W = model.baseline()/(disparity + c); - return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W); + return cv::Point3f((pt.x - model.left().cx())*W, + (pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W); } float bad_point = std::numeric_limits::quiet_NaN (); return cv::Point3f(bad_point, bad_point, bad_point); @@ -3417,6 +3550,162 @@ pcl::PointCloud::Ptr loadCloud( return util3d::transformPointCloud(cloud, transform); } +LaserScan deskew( + const LaserScan & input, + double inputStamp, + const rtabmap::Transform & velocity) +{ + if(velocity.isNull()) + { + UERROR("velocity should be valid!"); + return LaserScan(); + } + + if(input.format() != LaserScan::kXYZIT) + { + UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str()); + return LaserScan(); + } + + if(input.empty()) + { + UERROR("input scan is empty!"); + return LaserScan(); + } + + int offsetTime = input.getTimeOffset(); + + // Get latest timestamp + double firstStamp; + double lastStamp; + firstStamp = inputStamp + input.data().ptr(0, 0)[offsetTime]; + lastStamp = inputStamp + input.data().ptr(0, input.size()-1)[offsetTime]; + + if(lastStamp <= firstStamp) + { + UERROR("First and last stamps in the scan are the same!"); + return LaserScan(); + } + + rtabmap::Transform firstPose; + rtabmap::Transform lastPose; + + float vx,vy,vz, vroll,vpitch,vyaw; + velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); + + // 1- The pose of base frame in odom frame at first stamp + // 2- The pose of base frame in odom frame at last stamp + double dt1 = firstStamp - inputStamp; + double dt2 = lastStamp - inputStamp; + + firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); + lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2); + + if(firstPose.isNull()) + { + UERROR("Could not get transform between stamps %f and %f!", + firstStamp, + inputStamp); + return LaserScan(); + } + if(lastPose.isNull()) + { + UERROR("Could not get transform between stamps %f and %f!", + lastStamp, + inputStamp); + return LaserScan(); + } + + double stamp; + UTimer processingTime; + double scanTime = lastStamp - firstStamp; + cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense + int offsetIntensity = input.getIntensityOffset(); + bool isLocalTransformIdentity = input.localTransform().isIdentity(); + Transform localTransformInv = input.localTransform().inverse(); + + bool timeOnColumns = input.data().cols > input.data().rows; + int oi = 0; + if(timeOnColumns) + { + // t1 t2 ... + // ring1 ring1 ... + // ring2 ring2 ... + // ring3 ring4 ... + // ring4 ring3 ... + for(int u=0; u(0, u); + stamp = inputStamp + inputPtr[offsetTime]; + rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); + + for(int v=0; v(v, u); + pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]); + if(pcl::isFinite(pt)) + { + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, input.localTransform()); + } + pt = rtabmap::util3d::transformPoint(pt, transform); + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, localTransformInv); + } + float * dataPtr = output.ptr(0, oi++); + dataPtr[0] = pt.x; + dataPtr[1] = pt.y; + dataPtr[2] = pt.z; + dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + } + } + } + } + else // time on rows + { + // t1 ring1 ring2 ring3 ring4 + // t2 ring1 ring2 ring3 ring4 + // t3 ring1 ring2 ring3 ring4 + // t4 ring1 ring2 ring3 ring4 + // ... ... ... ... ... + for(int v=0; v(v, 0); + stamp = inputStamp + inputPtr[offsetTime]; + rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); + + for(int u=0; u(v, u); + pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]); + if(pcl::isFinite(pt)) + { + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, input.localTransform()); + } + pt = rtabmap::util3d::transformPoint(pt, transform); + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, localTransformInv); + } + float * dataPtr = output.ptr(0, oi++); + dataPtr[0] = pt.x; + dataPtr[1] = pt.y; + dataPtr[2] = pt.z; + dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + } + } + } + } + output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi)); + UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); + return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform()); +} + + } } diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index c32ffb304c..134cb6a235 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -82,8 +82,8 @@ LaserScan commonFiltering( float groundNormalsUp) { LaserScan scan = scanIn; - UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f", - scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp); + UDEBUG("scan size=%d format=%d, organized=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f", + scan.size(), (int)scan.format(), scan.isOrganized()?1:0, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp); if(!scan.isEmpty()) { // combined downsampling and range filtering step @@ -99,34 +99,44 @@ LaserScan commonFiltering( int oi = 0; float rangeMinSqrd = rangeMin * rangeMin; float rangeMaxSqrd = rangeMax * rangeMax; - for(int i=0; i scan.data().cols?downsamplingStep:1; + int downsamplingCols = scan.data().cols > scan.data().rows?downsamplingStep:1; + for(int j=0; j(0, i); - - if(rangeMin>0.0f || rangeMax>0.0f) + for(int i=0; i(j, i); - if(rangeMin > 0.0f && r < rangeMinSqrd) + if(rangeMin>0.0f || rangeMax>0.0f) { - continue; - } - if(rangeMax > 0.0f && r > rangeMaxSqrd) - { - continue; + float r; + if(is2d) + { + r = ptr[0]*ptr[0] + ptr[1]*ptr[1]; + } + else + { + r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2]; + } + + if(!uIsFinite(r)) + { + continue; + } + + if(rangeMin > 0.0f && r < rangeMinSqrd) + { + continue; + } + if(rangeMax > 0.0f && r > rangeMaxSqrd) + { + continue; + } } - } - cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1))); - ++oi; + cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1))); + ++oi; + } } int previousSize = scan.size(); int scanMaxPtsTmp = scan.maxPoints(); diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 0e5c3928f6..f46dd6b87d 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -655,7 +655,6 @@ cv::Mat create2DMap(const std::map & poses, map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1; UDEBUG("map size = %dx%d", map.cols, map.rows); - int j=0; float scanMaxRangeSqr = scanMaxRange * scanMaxRange; for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { @@ -741,14 +740,12 @@ cv::Mat create2DMap(const std::map & poses, } } } - ++j; } UDEBUG("Ray trace known space=%fs", timer.ticks()); // now fill unknown spaces if(unknownSpaceFilled && scanMaxRange > 0) { - j=0; float angleIncrement = CV_PI/90.0f; // angle increment for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { @@ -813,7 +810,6 @@ cv::Mat create2DMap(const std::map & poses, } } } - ++j; } UDEBUG("Fill empty space=%fs", timer.ticks()); //cv::imwrite("map.png", util3d::convertMap2Image8U(map)); diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp index 61d924573e..87d11a91f9 100644 --- a/corelib/src/util3d_motion_estimation.cpp +++ b/corelib/src/util3d_motion_estimation.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UMath.h" #include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UTimer.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_correspondences.h" @@ -42,8 +43,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_OPENGV #include #include +#include #include +#include #include +#include #endif namespace rtabmap @@ -61,15 +65,18 @@ Transform estimateMotion3DTo2D( double reprojError, int flagsPnP, int refineIterations, + int varianceMedianRatio, float maxVariance, const Transform & guess, const std::map & words3B, cv::Mat * covariance, std::vector * matchesOut, - std::vector * inliersOut) + std::vector * inliersOut, + bool splitLinearCovarianceComponents) { UASSERT(cameraModel.isValidForProjection()); UASSERT(!guess.isNull()); + UASSERT(varianceMedianRatio>1); Transform transform; std::vector matches, inliers; @@ -150,25 +157,36 @@ Transform estimateMotion3DTo2D( if(covariance && (!words3B.empty() || cameraModel.imageSize() != cv::Size())) { std::vector errorSqrdDists(inliers.size()); + std::vector errorSqrdX; + std::vector errorSqrdY; + std::vector errorSqrdZ; + if(splitLinearCovarianceComponents) + { + errorSqrdX.resize(inliers.size()); + errorSqrdY.resize(inliers.size()); + errorSqrdZ.resize(inliers.size()); + } std::vector errorSqrdAngles(inliers.size()); Transform localTransformInv = cameraModel.localTransform().inverse(); - Transform transformCameraFrameInv = (transform * cameraModel.localTransform()).inverse(); + Transform transformCameraFrame = transform * cameraModel.localTransform(); + Transform transformCameraFrameInv = transformCameraFrame.inverse(); for(unsigned int i=0; i::const_iterator iter = words3B.find(matches[inliers[i]]); cv::Point3f newPt; if(iter!=words3B.end() && util3d::isFinite(iter->second)) { - newPt = util3d::transformPoint(iter->second, localTransformInv); + newPt = util3d::transformPoint(iter->second, transform); } else { + + // Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB) + cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformCameraFrameInv); + //compute from projection Eigen::Vector3f ray = projectDepthTo3DRay( cameraModel.imageSize(), @@ -179,7 +197,20 @@ Transform estimateMotion3DTo2D( cameraModel.fx(), cameraModel.fy()); // transform in camera B frame - newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error + newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error + + //transform back into cameraA base frame + newPt = util3d::transformPoint(newPt, transformCameraFrame); + } + + if(splitLinearCovarianceComponents) + { + double errorX = objPt.x-newPt.x; + double errorY = objPt.y-newPt.y; + double errorZ = objPt.z-newPt.z; + errorSqrdX[i] = errorX * errorX; + errorSqrdY[i] = errorY * errorY; + errorSqrdZ[i] = errorZ * errorZ; } errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); @@ -191,14 +222,33 @@ Transform estimateMotion3DTo2D( std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); //divide by 4 instead of 2 to ignore very very far features (stereo) - double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_lin)); (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin; std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end()); - double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2]; + double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_ang)); (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang; + if(splitLinearCovarianceComponents) + { + std::sort(errorSqrdX.begin(), errorSqrdX.end()); + double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio]; + std::sort(errorSqrdY.begin(), errorSqrdY.end()); + double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio]; + std::sort(errorSqrdZ.begin(), errorSqrdZ.end()); + double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio]; + + UASSERT(uIsFinite(median_error_sqr_x)); + UASSERT(uIsFinite(median_error_sqr_y)); + UASSERT(uIsFinite(median_error_sqr_z)); + covariance->at(0,0) = median_error_sqr_x; + covariance->at(1,1) = median_error_sqr_y; + covariance->at(2,2) = median_error_sqr_z; + + median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z); + } + if(maxVariance > 0 && median_error_sqr_lin > maxVariance) { UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance); @@ -242,17 +292,20 @@ Transform estimateMotion3DTo2D( const std::map & words3A, const std::map & words2B, const std::vector & cameraModels, + unsigned int samplingPolicy, int minInliers, int iterations, double reprojError, int flagsPnP, int refineIterations, + int varianceMedianRatio, float maxVariance, const Transform & guess, const std::map & words3B, cv::Mat * covariance, std::vector * matchesOut, - std::vector * inliersOut) + std::vector * inliersOut, + bool splitLinearCovarianceComponents) { Transform transform; #ifndef RTABMAP_OPENGV @@ -267,6 +320,7 @@ Transform estimateMotion3DTo2D( } UASSERT(!guess.isNull()); + UASSERT(varianceMedianRatio > 1); std::vector matches, inliers; @@ -308,12 +362,50 @@ Transform estimateMotion3DTo2D( cameraIndexes.resize(oi); matches.resize(oi); - UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d", + UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld", (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), - guess.prettyPrint().c_str(), reprojError, iterations); + guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy); if((int)matches.size() >= minInliers) { + if(samplingPolicy == 0 || samplingPolicy == 2) + { + std::vector cc; + cc.resize(cameraModels.size()); + std::fill(cc.begin(), cc.end(),0); + for(size_t i=0; i(0, 0)); } - // convert 3d points - opengv::points_t points; - // convert 2d-3d correspondences into bearing vectors - opengv::bearingVectors_t bearingVectors; - opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences; - for(size_t i=0; i> multiPoints; + multiPoints.resize(cameraModels.size()); + // convert 2d-3d correspondences into bearing vectors + std::vector> multiBearingVectors; + multiBearingVectors.resize(cameraModels.size()); + for(size_t i=0; i(); + multiBearingVectors[i] = std::make_shared(); + } + + for(size_t i=0; ipush_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z)); + cv::Vec3f pt; + cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]); + pt = cv::normalize(pt); + multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2])); + } + + //create a non-central absolute multi adapter + opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter( + multiBearingVectors, + multiPoints, + camOffsets, + camRotations ); + + adapter.setR(guess.toEigen4d().block<3,3>(0, 0)); + adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); + + //Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac + //The method is set to GP3P + opengv::sac::MultiRansac ransac; + std::shared_ptr absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter)); + + ransac.sac_model_ = absposeproblem_ptr; + ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); + ransac.max_iterations_ = iterations; + UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); + + //Run the experiment + ransac.computeModel(); + + pnp = Transform::fromEigen3d(ransac.model_coefficients_); + + UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); + UDEBUG("Ransac iterations done: %d", ransac.iterations_); + for (size_t i=0; i < cameraModels.size(); ++i) + { + inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end()); + } } + else + { + // convert 3d points + opengv::points_t points; + + // convert 2d-3d correspondences into bearing vectors + opengv::bearingVectors_t bearingVectors; + opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences; - //create a non-central absolute adapter - opengv::absolute_pose::NoncentralAbsoluteAdapter adapter( - bearingVectors, - camCorrespondences, - points, - camOffsets, - camRotations ); + for(size_t i=0; i(0, 0)); - adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); + //create a non-central absolute adapter + opengv::absolute_pose::NoncentralAbsoluteAdapter adapter( + bearingVectors, + camCorrespondences, + points, + camOffsets, + camRotations ); - //Create a AbsolutePoseSacProblem and Ransac - //The method is set to GP3P - opengv::sac::Ransac ransac; - std::shared_ptr absposeproblem_ptr( - new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); + adapter.setR(guess.toEigen4d().block<3,3>(0, 0)); + adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z())); - ransac.sac_model_ = absposeproblem_ptr; - ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); - ransac.max_iterations_ = iterations; - UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); + //Create a AbsolutePoseSacProblem and Ransac + //The method is set to GP3P + opengv::sac::Ransac ransac; + std::shared_ptr absposeproblem_ptr( + new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P)); - //Run the experiment - ransac.computeModel(); + ransac.sac_model_ = absposeproblem_ptr; + ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx())); + ransac.max_iterations_ = iterations; + UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_); - Transform pnp = Transform::fromEigen3d(ransac.model_coefficients_); + //Run the experiment + ransac.computeModel(); + + pnp = Transform::fromEigen3d(ransac.model_coefficients_); + + UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); + UDEBUG("Ransac iterations done: %d", ransac.iterations_); + inliers = ransac.inliers_; + } - UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str()); - UDEBUG("Ransac iterations done: %d", ransac.iterations_); - inliers = ransac.inliers_; UDEBUG("Ransac inliers: %ld", inliers.size()); - if((int)inliers.size() >= minInliers) + if((int)inliers.size() >= minInliers && !pnp.isNull()) { transform = pnp; // compute variance (like in PCL computeVariance() method of sac_model.h) if(covariance) { + std::vector errorSqrdX; + std::vector errorSqrdY; + std::vector errorSqrdZ; + if(splitLinearCovarianceComponents) + { + errorSqrdX.resize(inliers.size()); + errorSqrdY.resize(inliers.size()); + errorSqrdZ.resize(inliers.size()); + } std::vector errorSqrdDists(inliers.size()); std::vector errorSqrdAngles(inliers.size()); + + std::vector transformsCameraFrame(cameraModels.size()); + std::vector transformsCameraFrameInv(cameraModels.size()); + for(size_t i=0; i::const_iterator iter = words3B.find(matches[inliers[i]]); cv::Point3f newPt; if(iter!=words3B.end() && util3d::isFinite(iter->second)) { - newPt = util3d::transformPoint(iter->second, cameraModels[cameraIndex].localTransform().inverse()); + newPt = util3d::transformPoint(iter->second, transform); } else { + int cameraIndex = cameraIndexes[inliers[i]]; + + // Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB) + cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformsCameraFrameInv[cameraIndex]); + //compute from projection Eigen::Vector3f ray = projectDepthTo3DRay( cameraModels[cameraIndex].imageSize(), @@ -412,7 +585,20 @@ Transform estimateMotion3DTo2D( cameraModels[cameraIndex].fx(), cameraModels[cameraIndex].fy()); // transform in camera B frame - newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error + newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error + + //transfor back into cameraA base frame + newPt = util3d::transformPoint(newPt, transformsCameraFrame[cameraIndex]); + } + + if(splitLinearCovarianceComponents) + { + double errorX = objPt.x-newPt.x; + double errorY = objPt.y-newPt.y; + double errorZ = objPt.z-newPt.z; + errorSqrdX[i] = errorX * errorX; + errorSqrdY[i] = errorY * errorY; + errorSqrdZ[i] = errorZ * errorZ; } errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); @@ -424,14 +610,33 @@ Transform estimateMotion3DTo2D( std::sort(errorSqrdDists.begin(), errorSqrdDists.end()); //divide by 4 instead of 2 to ignore very very far features (stereo) - double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2]; + double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_lin)); (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin; std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end()); - double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2]; + double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio]; UASSERT(uIsFinite(median_error_sqr_ang)); (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang; + if(splitLinearCovarianceComponents) + { + std::sort(errorSqrdX.begin(), errorSqrdX.end()); + double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio]; + std::sort(errorSqrdY.begin(), errorSqrdY.end()); + double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio]; + std::sort(errorSqrdZ.begin(), errorSqrdZ.end()); + double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio]; + + UASSERT(uIsFinite(median_error_sqr_x)); + UASSERT(uIsFinite(median_error_sqr_y)); + UASSERT(uIsFinite(median_error_sqr_z)); + covariance->at(0,0) = median_error_sqr_x; + covariance->at(1,1) = median_error_sqr_y; + covariance->at(2,2) = median_error_sqr_z; + + median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z); + } + if(maxVariance > 0 && median_error_sqr_lin > maxVariance) { UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance); diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 9b61dd9b2c..a424a7e088 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -244,7 +244,8 @@ void computeVarianceAndCorrespondencesImpl( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { variance = 1; correspondencesOut = 0; @@ -255,7 +256,7 @@ void computeVarianceAndCorrespondencesImpl( est->setInputTarget(target); est->setInputSource(source); pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance); if(correspondences.size()) { @@ -305,9 +306,10 @@ void computeVarianceAndCorrespondences( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal); } void computeVarianceAndCorrespondences( @@ -316,9 +318,10 @@ void computeVarianceAndCorrespondences( double maxCorrespondenceDistance, double maxCorrespondenceAngle, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal); } template @@ -327,7 +330,8 @@ void computeVarianceAndCorrespondencesImpl( const typename pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { variance = 1; correspondencesOut = 0; @@ -336,7 +340,7 @@ void computeVarianceAndCorrespondencesImpl( est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance); if(correspondences.size()>=3) { @@ -360,9 +364,10 @@ void computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal); } void computeVarianceAndCorrespondences( @@ -370,9 +375,10 @@ void computeVarianceAndCorrespondences( const pcl::PointCloud::ConstPtr & cloudB, double maxCorrespondenceDistance, double & variance, - int & correspondencesOut) + int & correspondencesOut, + bool reciprocal) { - computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut); + computeVarianceAndCorrespondencesImpl(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal); } // return transform from source to target (All points must be finite!!!) diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 6bafca8a5b..adfc1248c5 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -2398,7 +2398,7 @@ bool multiBandTexturing( std::string tmpImageDirectory = outputDirectory+"/rtabmap_tmp_textures"; UDirectory::removeDir(tmpImageDirectory); UDirectory::makeDir(tmpImageDirectory); - UINFO("Temporary saving images in directory \"%s\"...", tmpImageDirectory.c_str()); + UINFO("Temporary saving images from %ld nodes in directory \"%s\"...", cameraPoses.size(), tmpImageDirectory.c_str()); int viewId = 0; for(std::map::const_iterator iter = cameraPoses.lower_bound(1); iter!=cameraPoses.end(); ++iter) { @@ -2510,28 +2510,45 @@ bool multiBandTexturing( imageSize.height = image.rows; imageSize.width = image.cols; } + UASSERT(image.cols % imageSize.width == 0); cv::Mat imageRoi = image.colRange(i*imageSize.width, (i+1)*imageSize.width); + if(gains.find(camId) != gains.end() && gains.at(camId).find(i) != gains.at(camId).end()) { const cv::Vec4d & g = gains.at(camId).at(i); - std::vector channels; - cv::split(imageRoi, channels); + if(imageRoi.channels() == 1) + { + cv::multiply(imageRoi, g.val[0], imageRoi); + } + else + { + std::vector channels; + cv::split(imageRoi, channels); - // assuming BGR - cv::multiply(channels[0], g.val[gainRGB?3:0], channels[0]); - cv::multiply(channels[1], g.val[gainRGB?2:0], channels[1]); - cv::multiply(channels[2], g.val[gainRGB?1:0], channels[2]); + // assuming BGR + cv::multiply(channels[0], g.val[gainRGB?3:0], channels[0]); + cv::multiply(channels[1], g.val[gainRGB?2:0], channels[1]); + cv::multiply(channels[2], g.val[gainRGB?1:0], channels[2]); - cv::Mat output; - cv::merge(channels, output); - imageRoi = output; + cv::Mat output; + cv::merge(channels, output); + imageRoi = output; + } } if(blendingGains.find(camId) != blendingGains.end() && blendingGains.at(camId).find(i) != blendingGains.at(camId).end()) { + // Should be color for blending options + if(imageRoi.channels() == 1) + { + cv::Mat imageRoiColor; + cv::cvtColor(imageRoi, imageRoiColor, CV_GRAY2BGR); + imageRoi = imageRoiColor; + } + cv::Mat g = blendingGains.at(camId).at(i); cv::Mat dst; cv::blur(g, dst, cv::Size(3,3)); @@ -2559,9 +2576,7 @@ bool multiBandTexturing( sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr)); std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId); - cv::imwrite(imagePath, imageRoi); - std::shared_ptr viewPtr = std::make_shared( imagePath, (IndexT)viewId, @@ -2572,8 +2587,9 @@ bool multiBandTexturing( sfmData.views.insert(std::make_pair((IndexT)viewId, viewPtr)); ++viewId; } + UDEBUG("camId=%d", camId); } - UINFO("Temporary saving images in directory \"%s\"... done (%d images). %fs", tmpImageDirectory.c_str(), viewId, (int)cameraPoses.size(), timer.ticks()); + UINFO("Temporary saving images in directory \"%s\"... done (%d images of %d nodes). %fs", tmpImageDirectory.c_str(), viewId, (int)cameraPoses.size(), timer.ticks()); mvsUtils::MultiViewParams mp(sfmData); @@ -3502,22 +3518,25 @@ LaserScan adjustNormalsToViewPoint( int nz = ny+1; cv::Mat output = scan.data().clone(); #pragma omp parallel for - for(int i=0; i(0, i); - if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])) + for(int i=0; i0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + float * ptr = output.ptr(j, i); + if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])) { - //reverse normal - ptr[nx] *= -1.0f; - ptr[ny] *= -1.0f; - ptr[nz] *= -1.0f; + Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]); + Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]); + + float result = v.dot(n); + if(result < 0 + || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + { + //reverse normal + ptr[nx] *= -1.0f; + ptr[ny] *= -1.0f; + ptr[nz] *= -1.0f; + } } } } diff --git a/data/presets/camera_tof_icp.ini b/data/presets/camera_tof_icp.ini new file mode 100644 index 0000000000..84a09c1fd0 --- /dev/null +++ b/data/presets/camera_tof_icp.ini @@ -0,0 +1,31 @@ +# Could be used with TOF cameras like Kinect v2, Kinect For Azure or L515 + +[Camera] +Scan\downsampleStep = 4 +Scan\fromDepth = true +Scan\normalsK = 20 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0.05 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +Icp\CorrespondenceRatio = 0.2 +Icp\Epsilon = 0.005 +Icp\OutlierRatio = 0.65 +Icp\PointToPlaneMinComplexity = 0 +Icp\Strategy = 0 +Icp\VoxelSize = 0 +Odom\Deskewing = false +Odom\ScanKeyFrameThr = 0.7 +OdomF2M\ScanMaxSize = 15000 +# match the input voxel size: +OdomF2M\ScanSubtractRadius = 0.05 +RGBD\ProximityPathMaxNeighbors = 1 +# Make sure PM is used: +Reg\Strategy = 1 diff --git a/data/presets/lidar3d_icp.ini b/data/presets/lidar3d_icp.ini new file mode 100644 index 0000000000..57caca42a5 --- /dev/null +++ b/data/presets/lidar3d_icp.ini @@ -0,0 +1,46 @@ +# Could be used with LiDARs like Velodyne, RoboSense, Ouster + +[Camera] +Scan\downsampleStep = 1 +Scan\fromDepth = false +Scan\normalsK = 0 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +# Would be 0.01 for odom and 0.2 for mapping: +Icp\CorrespondenceRatio = 0.1 +Icp\Epsilon = 0.001 +Icp\FiltersEnabled = 2 +Icp\Iterations = 10 +Icp\OutlierRatio = 0.7 +# ~10x the voxel size: +Icp\MaxCorrespondenceDistance = 0.5 +Icp\MaxTranslation = 2 +# Uncomment if lidar can see ground most of the time (on a car or wheeled robot): +#Icp\PointToPlaneGroundNormalsUp = 0.8 +Icp\PointToPlaneK = 20 +Icp\PointToPlaneMinComplexity = 0 +# Make sure PM is used: +Icp\Strategy = 1 +Icp\VoxelSize = 0.05 +Mem\NotLinkedNodesKept = false +Mem\STMSize = 30 +Odom\Deskewing = true +Odom\GuessSmoothingDelay = 0.3 +Odom\ScanKeyFrameThr = 0.6 +OdomF2M\ScanMaxSize = 15000 +# Match voxel size: +OdomF2M\ScanSubtractRadius = 0.05 +RGBD\AngularUpdate = 0.05 +RGBD\LinearUpdate = 0.05 +RGBD\ProximityMaxGraphDepth = 0 +RGBD\ProximityPathMaxNeighbors = 1 +Reg\Strategy = 1 diff --git a/docker/bionic/Dockerfile b/docker/bionic/Dockerfile index fcd928bc8e..9547d7b8f4 100644 --- a/docker/bionic/Dockerfile +++ b/docker/bionic/Dockerfile @@ -5,7 +5,7 @@ FROM ros:melodic-perception # Install build dependencies RUN apt-get update && \ apt-get install -y git software-properties-common ros-melodic-rtabmap-ros && \ - apt-get remove -y ros-melodic-rtabmap && \ + apt-get remove -y ros-melodic-rtabmap* && \ apt-get clean && rm -rf /var/lib/apt/lists/ WORKDIR /root/ @@ -15,24 +15,26 @@ RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ apt-get clean && rm -rf /var/lib/apt/lists/ -# MRPT -RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y -RUN apt-get update && apt install libmrpt-poses-dev -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ +# MRPT # build failed: https://github.com/introlab/rtabmap/actions/runs/7259315306/job/19776230419 +#RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +#RUN apt-get update && apt install libmrpt-poses-dev -y && \ +# apt-get clean && rm -rf /var/lib/apt/lists/ ARG TARGETPLATFORM ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} RUN echo "I am building for $TARGETPLATFORM" -# arm64 -RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then ln -s /usr/bin/cmake ~/cmake; fi - -# cmake >=3.11 required for amd64 dependencies +# cmake >=3.14 required +RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt update && apt install -y wget && apt-get clean && rm -rf /var/lib/apt/lists/ && \ + wget -nv https://github.com/Kitware/CMake/releases/download/v3.20.0/cmake-3.20.0-linux-aarch64.tar.gz && \ + tar -xzf cmake-3.20.0-linux-aarch64.tar.gz && \ + rm cmake-3.20.0-linux-aarch64.tar.gz &&\ + ln -s ~/cmake-3.20.0-linux-aarch64/bin/cmake ~/cmake; fi RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt update && apt install -y wget && apt-get clean && rm -rf /var/lib/apt/lists/ && \ - wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0-Linux-x86_64.tar.gz && \ - tar -xzf cmake-3.17.0-Linux-x86_64.tar.gz && \ - rm cmake-3.17.0-Linux-x86_64.tar.gz &&\ - ln -s ~/cmake-3.17.0-Linux-x86_64/bin/cmake ~/cmake; fi + wget -nv https://github.com/Kitware/CMake/releases/download/v3.20.0/cmake-3.20.0-linux-x86_64.tar.gz && \ + tar -xzf cmake-3.20.0-linux-x86_64.tar.gz && \ + rm cmake-3.20.0-linux-x86_64.tar.gz &&\ + ln -s ~/cmake-3.20.0-linux-x86_64/bin/cmake ~/cmake; fi #commit Aug 6 2020 RUN apt-get update && apt install wget && apt-get clean && rm -rf /var/lib/apt/lists/ @@ -49,6 +51,9 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map + RUN rm /bin/sh && ln -s /bin/bash /bin/sh # Copy current source code diff --git a/docker/bionic/android/rtabmap_apiXX/rtabmap.bash b/docker/bionic/android/rtabmap_apiXX/rtabmap.bash index 3c0f39b8ed..c1c89d72f3 100755 --- a/docker/bionic/android/rtabmap_apiXX/rtabmap.bash +++ b/docker/bionic/android/rtabmap_apiXX/rtabmap.bash @@ -48,7 +48,7 @@ rm -r arengine # resource tool cd rtabmap-tango/build -cmake -DANDROID_PREBUILD=ON .. +$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DANDROID_PREBUILD=ON .. make cd ../.. diff --git a/docker/bionic/nvidia/Dockerfile b/docker/bionic/nvidia/Dockerfile deleted file mode 100644 index 797f33de14..0000000000 --- a/docker/bionic/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:bionic - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra \ No newline at end of file diff --git a/docker/focal-foxy/Dockerfile b/docker/focal-foxy/Dockerfile index 8f188b1954..c75b2ff013 100644 --- a/docker/focal-foxy/Dockerfile +++ b/docker/focal-foxy/Dockerfile @@ -1,151 +1,10 @@ # Image: introlab3it/rtabmap:focal-foxy -FROM osrf/ros:foxy-desktop +FROM introlab3it/rtabmap:focal-foxy-deps -# Install build dependencies -RUN apt-get update && \ - apt-get install -y git software-properties-common ros-foxy-rtabmap-ros && \ - apt-get remove -y ros-foxy-rtabmap && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -WORKDIR /root/ - -# GTSAM -RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y -RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# MRPT -RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y -RUN apt-get update && apt install libmrpt-poses-dev -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# PDAL -RUN apt-get update && apt-get install -y libpdal-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# RealSense2 -RUN apt-get update && apt-get install -y ros-foxy-librealsense2 && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -ARG TARGETPLATFORM -ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} -RUN echo "I am building for $TARGETPLATFORM" - -ENV DEBIAN_FRONTEND=noninteractive - -# Azure Kinect DK -# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 -# K4A binaries on 20.04 not released yet, we should take those from 18.04 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ - apt-get update && apt-get install -y curl && \ - echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ - echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - echo "Download k4a-tools_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ - echo "Accept license..." && \ - echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ - echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ - dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - apt-get install -y libsoundio1 && \ - dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ - rm /tmp/libk4a* /tmp/k4a* && \ - apt-get clean && rm -rf /var/lib/apt/lists/; fi - -# libfreenect2 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ - apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/OpenKinect/libfreenect2 && \ - cd libfreenect2 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libfreenect2; fi - -# zed open capture -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ - apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/stereolabs/zed-open-capture.git && \ - cd zed-open-capture && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r zed-open-capture; fi - -# AliceVision v2.4.0 modified (Sept 13 2021) -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ - apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libsuitesparse-dev \ - libceres-dev \ - xorg-dev \ - libglu1-mesa-dev \ - wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ - cd oiio && \ - git checkout Release-2.0.12 && \ - mkdir build && \ - cd build && \ - cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r oiio; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ - cd assimp && \ - git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r assimp; fi -# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ - cd geogram && \ - git checkout v1.7.6 && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ - git apply geogram_8b2ae61.patch && \ - ./configure.sh && \ - cd build/Linux64-gcc-dynamic-Release && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r geogram; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ - cd AliceVision && \ - git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ - git submodule update -i && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ - git apply alicevision_0f6115b.patch && \ - mkdir build && \ - cd build && \ - cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r AliceVision; fi - +# June 19 2023: moved opengv here so that focal-foxy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. #commit Aug 6 2020 -RUN apt-get update && apt install wget && \ +RUN apt-get update && apt install -y wget && \ apt-get clean && rm -rf /var/lib/apt/lists/ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd opengv && \ @@ -160,7 +19,8 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv -RUN rm /bin/sh && ln -s /bin/bash /bin/sh +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map # Copy current source code COPY . /root/rtabmap diff --git a/docker/focal-foxy/deps/Dockerfile b/docker/focal-foxy/deps/Dockerfile new file mode 100644 index 0000000000..54317f0537 --- /dev/null +++ b/docker/focal-foxy/deps/Dockerfile @@ -0,0 +1,176 @@ +# Image: introlab3it/rtabmap:focal-foxy-deps + +FROM ubuntu:20.04 + +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-foxy-ros-base python3-argcomplete ros-dev-tools ros-foxy-rtabmap-ros && \ + apt-get remove -y ros-foxy-rtabmap && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# GTSAM +RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y +RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +RUN apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-foxy-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# Azure Kinect DK +# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 +# K4A binaries on 20.04 not released yet, we should take those from 18.04 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ + apt-get update && apt-get install -y curl && \ + echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ + echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + echo "Download k4a-tools_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ + echo "Accept license..." && \ + echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ + echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ + dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + apt-get install -y libsoundio1 && \ + dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ + rm /tmp/libk4a* /tmp/k4a* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +# AliceVision v2.4.0 modified (Sept 13 2021) +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ + apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libsuitesparse-dev \ + libceres-dev \ + xorg-dev \ + libglu1-mesa-dev \ + wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ + cd oiio && \ + git checkout Release-2.0.12 && \ + mkdir build && \ + cd build && \ + cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r oiio; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ + cd assimp && \ + git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r assimp; fi +# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ + cd geogram && \ + git checkout v1.7.6 && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ + git apply geogram_8b2ae61.patch && \ + ./configure.sh && \ + cd build/Linux64-gcc-dynamic-Release && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r geogram; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ + cd AliceVision && \ + git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ + git submodule update -i && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ + git apply alicevision_0f6115b.patch && \ + mkdir build && \ + cd build && \ + cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r AliceVision; fi + +RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \ + git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/focal-foxy/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/focal-foxy/deps/ros_entrypoint.sh b/docker/focal-foxy/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..60b286226e --- /dev/null +++ b/docker/focal-foxy/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/foxy/setup.bash" -- +exec "$@" diff --git a/docker/focal-foxy/hooks/build b/docker/focal-foxy/hooks/build deleted file mode 100644 index ca94710cfe..0000000000 --- a/docker/focal-foxy/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:20.04-foxy . diff --git a/docker/focal-foxy/hooks/post_push b/docker/focal-foxy/hooks/post_push deleted file mode 100644 index efce29bf00..0000000000 --- a/docker/focal-foxy/hooks/post_push +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker push $DOCKER_REPO:20.04-foxy diff --git a/docker/focal-foxy/nvidia/Dockerfile b/docker/focal-foxy/nvidia/Dockerfile deleted file mode 100644 index 3c3ef17986..0000000000 --- a/docker/focal-foxy/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:focal-foxy - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/focal/Dockerfile b/docker/focal/Dockerfile index cb90a4e214..277f18c920 100644 --- a/docker/focal/Dockerfile +++ b/docker/focal/Dockerfile @@ -1,149 +1,8 @@ # Image: introlab3it/rtabmap:focal -FROM ros:noetic-perception +FROM introlab3it/rtabmap:focal-deps -# Install build dependencies -RUN apt-get update && \ - apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \ - apt-get remove -y ros-noetic-rtabmap && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -WORKDIR /root/ - -# GTSAM -RUN add-apt-repository ppa:borglab/gtsam-release-4.0 -y -RUN apt-get update && apt install libgtsam-dev libgtsam-unstable-dev -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# MRPT -RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y -RUN apt-get update && apt install libmrpt-poses-dev -y && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# PDAL -RUN apt-get update && apt-get install -y libpdal-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# RealSense2 -RUN apt-get update && apt-get install -y ros-noetic-librealsense2 && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -ARG TARGETPLATFORM -ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} -RUN echo "I am building for $TARGETPLATFORM" - -ENV DEBIAN_FRONTEND=noninteractive - -# Azure Kinect DK -# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 -# K4A binaries on 20.04 not released yet, we should take those from 18.04 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ - apt-get update && apt-get install -y curl && \ - echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ - echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - echo "Download k4a-tools_1.3.0_amd64.deb..." && \ - curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ - echo "Accept license..." && \ - echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ - echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ - dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ - dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ - apt-get install -y libsoundio1 && \ - dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ - rm /tmp/libk4a* /tmp/k4a* && \ - apt-get clean && rm -rf /var/lib/apt/lists/; fi - -# libfreenect2 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ - apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/OpenKinect/libfreenect2 && \ - cd libfreenect2 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libfreenect2; fi - -# zed open capture -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ - apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/stereolabs/zed-open-capture.git && \ - cd zed-open-capture && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r zed-open-capture; fi - -# AliceVision v2.4.0 modified (Sept 13 2021) -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ - apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libsuitesparse-dev \ - libceres-dev \ - xorg-dev \ - libglu1-mesa-dev \ - wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ - cd oiio && \ - git checkout Release-2.0.12 && \ - mkdir build && \ - cd build && \ - cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r oiio; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ - cd assimp && \ - git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r assimp; fi -# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ - cd geogram && \ - git checkout v1.7.6 && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ - git apply geogram_8b2ae61.patch && \ - ./configure.sh && \ - cd build/Linux64-gcc-dynamic-Release && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r geogram; fi -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ - cd AliceVision && \ - git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ - git submodule update -i && \ - wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ - git apply alicevision_0f6115b.patch && \ - mkdir build && \ - cd build && \ - cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r AliceVision; fi - +# June 19 2023: moved opengv here so that focal-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. #commit Aug 6 2020 RUN apt-get update && apt install wget && apt-get clean && rm -rf /var/lib/apt/lists/ RUN git clone https://github.com/laurentkneip/opengv.git && \ @@ -159,7 +18,8 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv -RUN rm /bin/sh && ln -s /bin/bash /bin/sh +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map # Copy current source code COPY . /root/rtabmap @@ -173,4 +33,3 @@ RUN source /ros_entrypoint.sh && \ cd ../.. && \ rm -rf rtabmap && \ ldconfig - diff --git a/docker/focal/deps/Dockerfile b/docker/focal/deps/Dockerfile new file mode 100644 index 0000000000..165c95b6aa --- /dev/null +++ b/docker/focal/deps/Dockerfile @@ -0,0 +1,175 @@ +# Image: introlab3it/rtabmap:focal + +FROM ros:noetic-perception + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +ENV DEBIAN_FRONTEND=noninteractive + +WORKDIR /root/ + +# Install build dependencies +RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \ + apt-get update && \ + apt-get install -y git software-properties-common zlib1g-dev ros-noetic-visualization-msgs ros-noetic-velodyne-pointcloud ros-noetic-turtlebot3-navigation ros-noetic-turtlebot3-bringup ros-noetic-tf-conversions ros-noetic-tf2-ros ros-noetic-tf ros-noetic-stereo-msgs ros-noetic-std-srvs ros-noetic-std-msgs ros-noetic-sensor-msgs ros-noetic-rviz ros-noetic-rospy ros-noetic-roscpp ros-noetic-robot-localization ros-noetic-realsense2-camera qtbase5-dev libproj-dev ros-noetic-pluginlib ros-noetic-pcl-ros ros-noetic-pcl-conversions ros-noetic-octomap-msgs ros-noetic-octomap ros-noetic-nodelet ros-noetic-nav-msgs ros-noetic-move-base-msgs ros-noetic-message-runtime ros-noetic-message-generation ros-noetic-message-filters libsqlite3-dev libqt5widgets5 libqt5core5a libpcl-dev ros-noetic-libg2o ros-noetic-laser-geometry ros-noetic-imu-filter-madgwick ros-noetic-image-transport ros-noetic-image-geometry ros-noetic-husky-navigation ros-noetic-hector-mapping ros-noetic-gtsam ros-noetic-grid-map-ros ros-noetic-grid-map-core ros-noetic-geometry-msgs ros-noetic-genmsg ros-noetic-find-object-2d ros-noetic-eigen-conversions ros-noetic-dynamic-reconfigure ros-noetic-diagnostic-updater ros-noetic-cv-bridge ros-noetic-costmap-2d ros-noetic-catkin ros-noetic-apriltag-ros ros-noetic-actionlib-msgs ros-noetic-actionlib wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ ;fi +RUN if [ "$TARGETPLATFORM" != "linux/arm/v7" ]; then \ + apt-get update && \ + apt-get install -y git software-properties-common ros-noetic-rtabmap-ros && \ + apt-get remove -y ros-noetic-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ ;fi + +#https://gitlab.kitware.com/cmake/cmake/-/issues/20568 +RUN if [ "$TARGETPLATFORM" = "linux/arm/v7" ]; then \ + wget -nv https://github.com/Kitware/CMake/releases/download/v3.20.0/cmake-3.20.0.tar.gz && \ + tar -xzf cmake-3.20.0.tar.gz && \ + cd cmake-3.20.0 && \ + export CFLAGS="-D_FILE_OFFSET_BITS=64" && \ + export CXXFLAGS="-D_FILE_OFFSET_BITS=64" && \ + ./bootstrap && \ + make -j$(nproc) && \ + sudo make install && \ + cd .. && \ + cmake --version && \ + rm -r cmake-3.20.0.tar.gz cmake-3.20.0 ;fi + +# MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y +RUN apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-noetic-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Azure Kinect DK +# Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494 +# K4A binaries on 20.04 not released yet, we should take those from 18.04 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \ + apt-get update && apt-get install -y curl && \ + echo "Download libk4a1.3_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \ + echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3-dev/libk4a1.3-dev_1.3.0_amd64.deb > /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + echo "Download libk4abt1.0_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0/libk4abt1.0_1.0.0_amd64.deb > /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + echo "Download libk4abt1.0-dev_1.0.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4abt1.0-dev/libk4abt1.0-dev_1.0.0_amd64.deb > /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + echo "Download k4a-tools_1.3.0_amd64.deb..." && \ + curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/k/k4a-tools/k4a-tools_1.3.0_amd64.deb > /tmp/k4a-tools_1.3.0_amd64.deb && \ + echo "Accept license..." && \ + echo 'libk4a1.3 libk4a1.3/accepted-eula-hash string 0f5d5c5de396e4fee4c0753a21fee0c1ed726cf0316204edda484f08cb266d76' | debconf-set-selections && \ + echo 'libk4abt1.0 libk4abt1.0/accepted-eula-hash string 03a13b63730639eeb6626d24fd45cf25131ee8e8e0df3f1b63f552269b176e38' | debconf-set-selections && \ + dpkg -i /tmp/libk4a1.3_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4a1.3-dev_1.3.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0_1.0.0_amd64.deb && \ + dpkg -i /tmp/libk4abt1.0-dev_1.0.0_amd64.deb && \ + apt-get install -y libsoundio1 && \ + dpkg -i /tmp/k4a-tools_1.3.0_amd64.deb && \ + rm /tmp/libk4a* /tmp/k4a* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +# AliceVision v2.4.0 modified (Sept 13 2021) +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \ + apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libsuitesparse-dev \ + libceres-dev \ + xorg-dev \ + libglu1-mesa-dev \ + wget && apt-get clean && rm -rf /var/lib/apt/lists/; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/OpenImageIO/oiio.git && \ + cd oiio && \ + git checkout Release-2.0.12 && \ + mkdir build && \ + cd build && \ + cmake -DUSE_PYTHON=OFF -DOIIO_BUILD_TESTS=OFF -DOIIO_BUILD_TOOLS=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r oiio; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/assimp/assimp.git && \ + cd assimp && \ + git checkout 71a87b653cd4b5671104fe49e2e38cf5dd4d8675 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r assimp; fi +# Ignore assimp installed from ros, as it has invalid "/usr/lib/include" in its target, making rtabmap's cmake fails +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then rm -rf /usr/lib/x86_64-linux-gnu/cmake/assimp*; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/geogram.git && \ + cd geogram && \ + git checkout v1.7.6 && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/geogram_8b2ae61.patch && \ + git apply geogram_8b2ae61.patch && \ + ./configure.sh && \ + cd build/Linux64-gcc-dynamic-Release && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r geogram; fi +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then git clone https://github.com/alicevision/AliceVision.git --recursive && \ + cd AliceVision && \ + git checkout 0f6115b6af6183c524aa7fcf26141337c1cf3872 && \ + git submodule update -i && \ + wget https://gist.githubusercontent.com/matlabbe/1df724465106c056ca4cc195c81d8cf0/raw/b3ed4cb8f9b270833a40d57d870a259eabfa4415/alicevision_0f6115b.patch && \ + git apply alicevision_0f6115b.patch && \ + mkdir build && \ + cd build && \ + cmake -DALICEVISION_USE_CUDA=OFF -DALICEVISION_USE_APRILTAG=OFF -DALICEVISION_BUILD_SOFTWARE=OFF .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r AliceVision; fi + +RUN git clone --branch 4.2.0 https://github.com/opencv/opencv.git && \ + git clone --branch 4.2.0 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DWITH_VTK=OFF -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/focal/hooks/build b/docker/focal/hooks/build deleted file mode 100644 index e1c97536d2..0000000000 --- a/docker/focal/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:20.04 . diff --git a/docker/focal/hooks/post_push b/docker/focal/hooks/post_push deleted file mode 100644 index cf10a22bed..0000000000 --- a/docker/focal/hooks/post_push +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker push $DOCKER_REPO:20.04 diff --git a/docker/focal/nvidia/Dockerfile b/docker/focal/nvidia/Dockerfile deleted file mode 100644 index 49b3783784..0000000000 --- a/docker/focal/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:focal - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/jammy-iron/Dockerfile b/docker/jammy-iron/Dockerfile new file mode 100644 index 0000000000..8228d9057f --- /dev/null +++ b/docker/jammy-iron/Dockerfile @@ -0,0 +1,37 @@ +# Image: introlab3it/rtabmap:jammy-iron + +FROM introlab3it/rtabmap:jammy-iron-deps + +# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. +#commit Aug 6 2020 +RUN apt-get update && apt install -y wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r opengv + +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map + +# Copy current source code +COPY . /root/rtabmap + +# Build RTAB-Map project +RUN source /ros_entrypoint.sh && \ + cd rtabmap/build && \ + cmake -DWITH_OPENGV=ON .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf rtabmap && \ + ldconfig + diff --git a/docker/jammy-iron/deps/Dockerfile b/docker/jammy-iron/deps/Dockerfile new file mode 100644 index 0000000000..1f525394d3 --- /dev/null +++ b/docker/jammy-iron/deps/Dockerfile @@ -0,0 +1,88 @@ +# Image: introlab3it/rtabmap:jammy-iron-deps + +FROM ubuntu:22.04 + +# Install ROS2 +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-iron-ros-base ros-dev-tools ros-iron-rtabmap-ros && \ + apt-get remove -y ros-iron-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +WORKDIR /root/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-iron-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \ + git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/jammy-iron/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/iron/lib/x86_64-linux-gnu:/opt/ros/iron/lib/aarch64-linux-gnu + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/jammy-iron/deps/ros_entrypoint.sh b/docker/jammy-iron/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..6285c32365 --- /dev/null +++ b/docker/jammy-iron/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/iron/setup.bash" -- +exec "$@" diff --git a/docker/jammy/Dockerfile b/docker/jammy/Dockerfile index 18823d9aa5..c7dbc07cae 100644 --- a/docker/jammy/Dockerfile +++ b/docker/jammy/Dockerfile @@ -1,59 +1,10 @@ # Image: introlab3it/rtabmap:jammy -FROM osrf/ros:humble-desktop +FROM introlab3it/rtabmap:jammy-deps -# Install build dependencies -RUN apt-get update && \ - apt-get install -y git software-properties-common ros-humble-rtabmap-ros && \ - apt-get remove -y ros-humble-rtabmap && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -WORKDIR /root/ - -# PDAL -RUN apt-get update && apt-get install -y libpdal-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -# RealSense2 -RUN apt-get update && apt-get install -y ros-humble-librealsense2 && \ - apt-get clean && rm -rf /var/lib/apt/lists/ - -ARG TARGETPLATFORM -ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} -RUN echo "I am building for $TARGETPLATFORM" - -ENV DEBIAN_FRONTEND=noninteractive - -# libfreenect2 -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ - apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/OpenKinect/libfreenect2 && \ - cd libfreenect2 && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r libfreenect2; fi - -# zed open capture -RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ - apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ - apt-get clean && rm -rf /var/lib/apt/lists/ && \ - git clone https://github.com/stereolabs/zed-open-capture.git && \ - cd zed-open-capture && \ - mkdir build && \ - cd build && \ - cmake .. && \ - make -j$(nproc) && \ - make install && \ - cd && \ - rm -r zed-open-capture; fi - +# June 19 2023: moved opengv here so that jammy-deps can be built on my computer. Not sure why but on my machine opengv arm64 fails, but not on CI. #commit Aug 6 2020 -RUN apt-get update && apt install wget && \ +RUN apt-get update && apt install -y wget && \ apt-get clean && rm -rf /var/lib/apt/lists/ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd opengv && \ @@ -68,14 +19,12 @@ RUN git clone https://github.com/laurentkneip/opengv.git && \ cd && \ rm -r opengv -RUN rm /bin/sh && ln -s /bin/bash /bin/sh +# Will be used to read/store databases on host +RUN mkdir -p /root/Documents/RTAB-Map && chmod 777 /root/Documents/RTAB-Map # Copy current source code COPY . /root/rtabmap -# ros2 seems not sourcing by default its multi-arch folders -ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib/aarch64-linux-gnu - # Build RTAB-Map project RUN source /ros_entrypoint.sh && \ cd rtabmap/build && \ diff --git a/docker/jammy/deps/Dockerfile b/docker/jammy/deps/Dockerfile new file mode 100644 index 0000000000..2aa00f93eb --- /dev/null +++ b/docker/jammy/deps/Dockerfile @@ -0,0 +1,100 @@ +# Image: introlab3it/rtabmap:jammy-deps + +FROM ubuntu:22.04 + +ARG TARGETPLATFORM +ENV TARGETPLATFORM=${TARGETPLATFORM:-linux/amd64} +RUN echo "I am building for $TARGETPLATFORM" + +# Install ROS2 +RUN apt update && \ + apt install software-properties-common -y && \ + add-apt-repository universe && \ + apt update && \ + apt install curl -y && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y git ros-humble-ros-base ros-dev-tools && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then apt-get update && \ + apt upgrade -y && \ + apt-get install -y ros-humble-rtabmap-ros && \ + apt-get remove -y ros-humble-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +# current issue that ros-humble-rtabmap-ros is not available on arm64 +RUN if [ "$TARGETPLATFORM" = "linux/arm64" ]; then apt-get update && \ + apt upgrade -y && \ + apt-get install -y ros-humble-rtabmap-launch && \ + apt-get remove -y ros-humble-rtabmap* && \ + apt-get clean && rm -rf /var/lib/apt/lists/; fi + +WORKDIR /root/ + +# PDAL +RUN apt-get update && apt-get install -y libpdal-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# RealSense2 +RUN apt-get update && apt-get install -y ros-humble-librealsense2 && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# libfreenect2 +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \ + apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/OpenKinect/libfreenect2 && \ + cd libfreenect2 && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r libfreenect2; fi + +# zed open capture +RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \ + apt-get update && apt install -y libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \ + apt-get clean && rm -rf /var/lib/apt/lists/ && \ + git clone https://github.com/stereolabs/zed-open-capture.git && \ + cd zed-open-capture && \ + mkdir build && \ + cd build && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + cd && \ + rm -r zed-open-capture; fi + +RUN git clone --branch 4.5.4 https://github.com/opencv/opencv.git && \ + git clone --branch 4.5.4 https://github.com/opencv/opencv_contrib.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf opencv opencv_contrib + +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +COPY ./docker/jammy/deps/ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +# ros2 seems not sourcing by default its multi-arch folders +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib/aarch64-linux-gnu + +# for jetson (https://github.com/introlab/rtabmap/issues/776) +ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra + diff --git a/docker/jammy/deps/ros_entrypoint.sh b/docker/jammy/deps/ros_entrypoint.sh new file mode 100644 index 0000000000..9d349493c4 --- /dev/null +++ b/docker/jammy/deps/ros_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# setup ros2 environment +source "/opt/ros/humble/setup.bash" -- +exec "$@" diff --git a/docker/jammy/hooks/build b/docker/jammy/hooks/build deleted file mode 100644 index a23082c28d..0000000000 --- a/docker/jammy/hooks/build +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:22.04 . diff --git a/docker/jammy/hooks/post_push b/docker/jammy/hooks/post_push deleted file mode 100644 index 02c9611518..0000000000 --- a/docker/jammy/hooks/post_push +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker push $DOCKER_REPO:22.04 diff --git a/docker/jammy/nvidia/Dockerfile b/docker/jammy/nvidia/Dockerfile deleted file mode 100644 index 21c47efa44..0000000000 --- a/docker/jammy/nvidia/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM introlab3it/rtabmap:jammy - -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# Will be used to read/store databases on host -RUN mkdir -p /root/Documents/RTAB-Map - -# On Nvidia Jetpack, uncomment the following (https://github.com/introlab/rtabmap/issues/776): -# ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/aarch64-linux-gnu/tegra diff --git a/docker/jfr2018/Dockerfile b/docker/jfr2018/Dockerfile index aa4c1f8522..0eab9008ee 100644 --- a/docker/jfr2018/Dockerfile +++ b/docker/jfr2018/Dockerfile @@ -127,7 +127,7 @@ WORKDIR /root/catkin_ws/src RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace' RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout kinetic-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout kinetic -RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/a6d604e730bcbf3dcd7fd8a27302bed4bb94b799/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch +RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/1b7c96b38e063bacfa77408686e4528f3d27af2e/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout db2fc39451e59317cf8486d92085da1c8e414785 RUN git clone https://github.com/ros-perception/vision_opencv.git && cd vision_opencv && git checkout kinetic RUN git clone https://github.com/laboshinl/loam_velodyne.git && cd loam_velodyne && git checkout a4c364a677647f2a35831439032dc5a58378b3fd diff --git a/docker/jfr2018/latest/Dockerfile b/docker/jfr2018/latest/Dockerfile index 7e89c41374..9dde6f351f 100644 --- a/docker/jfr2018/latest/Dockerfile +++ b/docker/jfr2018/latest/Dockerfile @@ -137,8 +137,8 @@ RUN /bin/bash -c 'cd /root/catkin_ws;. /opt/ros/${ROS_DISTRO}/setup.bash; catkin RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout ${ROS_DISTRO}-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml # VISO2 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO} -# MSCKF-VIO -RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/1b7c96b38e063bacfa77408686e4528f3d27af2e/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch +# MSCKF-VIO: using a patched version to be used outside ros and c++14: +RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout 916c917e5b481741c60732057f0141e8311962c1 # FOVIS RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout 896acc8425e9fd7c5609153b8bad349ae1abbb50 # LOAM @@ -227,21 +227,55 @@ ENV ORB_SLAM2_ROOT_DIR /root/ORB_SLAM2 WORKDIR /root +# To make "source" working +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +# Need cmake >=3.10 +RUN apt update && apt install -y wget && apt-get clean && rm -rf /var/lib/apt/lists/ && \ + wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0-Linux-x86_64.tar.gz && \ + tar -xzf cmake-3.17.0-Linux-x86_64.tar.gz && \ + rm cmake-3.17.0-Linux-x86_64.tar.gz &&\ + ln -s ~/cmake-3.17.0-Linux-x86_64/bin/cmake ~/cmake + # Copy current source code COPY . /root/rtabmap # Patch for OpenCV 3.1 -RUN cd rtabmap && wget https://gist.githubusercontent.com/matlabbe/ab42b8ea5d5902ffc6177539b98d9d51/raw/87520db7a54bf2673be3fc898578cd6348dd66f0/rtabmap_opencv310_backward_compatibility.patch && git apply --ignore-space-change --ignore-whitespace rtabmap_opencv310_backward_compatibility.patch +RUN cd rtabmap && git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch # Build RTAB-Map (using standard g2o, then a version for orbslam2, which uses its own g2o version) RUN cp -r rtabmap rtabmap_os2 && cp -r rtabmap rtabmap_msckf && cp -r rtabmap rtabmap_loam -RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap ; cd build ; cmake -DWITH_FOVIS=ON -DWITH_VISO2=ON -DWITH_DVO=ON -DWITH_OKVIS=ON -DWITH_VINS=ON -DWITH_OPENVINS=ON .. ; make -j3 ; make install ; rm -rf * ; ldconfig' + +RUN source /root/catkin_ws/devel/setup.bash && \ + cd rtabmap && \ + cd build && \ + ~/cmake -DWITH_FOVIS=ON -DWITH_VISO2=ON -DWITH_DVO=ON -DWITH_OKVIS=ON -DWITH_VINS=ON -DWITH_OPENVINS=ON -DWITH_OPENGV=OFF .. && \ + make -j3 && \ + make install && \ + rm -rf * && \ + ldconfig # Version with orb_slam2 (system g2o disabled) -RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_os2 ; cd build ; cmake -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON .. ; make -j3 ; rm -rf *' +RUN source /root/catkin_ws/devel/setup.bash && \ + cd rtabmap_os2 && \ + cd build && \ + ~/cmake -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON -DWITH_OPENGV=OFF .. && \ + make -j3 && \ + rm -rf * # rtabmap is crashing if LOAM and MSCKF dependencies are used at the same time, so split them -RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_loam ;cd build ;cmake -DWITH_LOAM=ON -DWITH_FLOAM=ON .. ;make -j3 ;rm -rf *' -RUN /bin/bash -c '. /root/catkin_ws/devel/setup.bash; cd rtabmap_msckf ;cd build ;cmake -DWITH_MSCKF_VIO=ON .. ;make -j3 ;rm -rf *' +RUN source /root/catkin_ws/devel/setup.bash && \ + cd rtabmap_loam && \ + cd build && \ + ~/cmake -DWITH_LOAM=ON -DWITH_FLOAM=ON -DWITH_OPENGV=OFF .. && \ + make -j3 && \ + rm -rf * +RUN source /root/catkin_ws/devel/setup.bash && \ + cd rtabmap_msckf && \ + git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch && \ + cd build && \ + ~/cmake -DWITH_MSCKF_VIO=ON -DWITH_OPENGV=OFF .. && \ + make -j3 && \ + rm -rf * WORKDIR /root diff --git a/docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch b/docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch new file mode 100644 index 0000000000..a396cde070 --- /dev/null +++ b/docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch @@ -0,0 +1,13 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index c14cb659..2c370200 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -830,6 +830,8 @@ IF(NOT MSVC) + ENDIF() + ENDIF() + ++set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals") ++ + + ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### + IF(APPLE AND BUILD_AS_BUNDLE) diff --git a/docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch b/docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch new file mode 100644 index 0000000000..949906ea10 --- /dev/null +++ b/docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch @@ -0,0 +1,13 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 9f2bfd83..65b03eef 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -228,7 +228,7 @@ ENDIF() + set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.") + set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6) + +-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) ++FIND_PACKAGE(OpenCV QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d) + + IF(WITH_QT) + FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) diff --git a/docker/jfr2018/run_kitti_datasets.sh b/docker/jfr2018/run_kitti_datasets.sh index 18d2cd7fe0..8dd1d636ca 100755 --- a/docker/jfr2018/run_kitti_datasets.sh +++ b/docker/jfr2018/run_kitti_datasets.sh @@ -78,7 +78,7 @@ then fi SCAN="--Icp/PointToPlane false --Icp/Iterations 10 --Odom/ScanKeyFrameThr 0.8 $SCAN" fi - SCAN="--scan --Reg/Strategy $REG --OdomF2M/ScanMaxSize 10000 --OdomF2M/ScanSubtractRadius 0.5 --Odom/LOAMSensor 2 --Icp/MaxTranslation 2 --Icp/Epsilon 0.0001 --Icp/MaxCorrespondenceDistance 1.5 --Icp/CorrespondenceRatio 0.01 --Icp/PM true --Icp/PMOutlierRatio 0.7 --Icp/PMMatcherKnn 3 --Icp/PMMatcherEpsilon 1 $SCAN" + SCAN="--scan --Reg/Strategy $REG --OdomF2M/ScanMaxSize 10000 --OdomF2M/ScanSubtractRadius 0.5 --Odom/LOAMSensor 2 --Icp/MaxTranslation 2 --Icp/Epsilon 0.0001 --Icp/MaxCorrespondenceDistance 1.5 --Icp/CorrespondenceRatio 0.01 --Icp/PM true --Icp/PMOutlierRatio 0.7 --Icp/PMMatcherKnn 3 --Icp/PMMatcherEpsilon 1 --Icp/ReciprocalCorrespondences false $SCAN" fi RTABMAP_KITTI_TOOL="rtabmap-kitti_dataset" diff --git a/docker/latest_deps/Dockerfile b/docker/latest_deps/Dockerfile index 6b7a130297..588c62fe3c 100644 --- a/docker/latest_deps/Dockerfile +++ b/docker/latest_deps/Dockerfile @@ -4,7 +4,7 @@ FROM osrf/ros:humble-desktop # Install build dependencies RUN apt-get update && \ apt-get install -y git software-properties-common ros-humble-rtabmap-ros libqt6* qt6* && \ - apt-get remove -y ros-humble-rtabmap libpcl* libqt5* qt5* libvtk* libopencv* && \ + apt-get remove -y ros-humble-rtabmap* ros-humble-libg2o libpcl* libqt5* qt5* libvtk* libopencv* && \ apt-get clean && rm -rf /var/lib/apt/lists/ WORKDIR /root/ @@ -42,12 +42,34 @@ RUN git clone https://github.com/opencv/opencv.git && \ cd opencv && \ mkdir build && \ cd build && \ - cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + cmake -DBUILD_opencv_python3=OFF -DBUILD_opencv_python_bindings_generator=OFF -DBUILD_opencv_python_tests=OFF -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ make -j$(nproc) && \ make install && \ cd ../.. && \ rm -rf opencv +# Build latest gtsam +RUN git clone https://github.com/borglab/gtsam.git && \ + cd gtsam && \ + mkdir build && \ + cd build && \ + cmake -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DGTSAM_INSTALL_CPPUNILITE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON .. && \ + cmake --build . --config Release --target install && \ + cd ../.. && \ + rm -rf gtsam + +# Build latest g2o +RUN git clone https://github.com/RainerKuemmerle/g2o.git && \ + cd g2o && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. && \ + cmake --build . --config Release --target install && \ + cd ../.. && \ + rm -rf g2o + +RUN mkdir -p /root/Documents/RTAB-Map + # Copy current source code COPY . /root/rtabmap @@ -61,9 +83,4 @@ RUN source /ros_entrypoint.sh && \ rm -rf rtabmap && \ ldconfig -# nvidia-container-runtime -ENV NVIDIA_VISIBLE_DEVICES \ - ${NVIDIA_VISIBLE_DEVICES:-all} -ENV NVIDIA_DRIVER_CAPABILITIES \ - ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e619619cd1..5e4a68a218 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -5,6 +5,7 @@ IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( RGBDMapping ) ADD_SUBDIRECTORY( WifiMapping ) ADD_SUBDIRECTORY( NoEventsExample ) + ADD_SUBDIRECTORY( LidarMapping ) ELSE() MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...") ENDIF() diff --git a/examples/LidarMapping/CMakeLists.txt b/examples/LidarMapping/CMakeLists.txt new file mode 100644 index 0000000000..4f88a87219 --- /dev/null +++ b/examples/LidarMapping/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8) + +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) + +if(NOT internal) + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) + +endif() + +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) +ENDIF() + +ADD_EXECUTABLE(lidar_mapping main.cpp ${moc_srcs}) + +TARGET_LINK_LIBRARIES(lidar_mapping rtabmap::gui) + +SET_TARGET_PROPERTIES( + lidar_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + +if(internal) + SET_TARGET_PROPERTIES( lidar_mapping + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_mapping) +endif(internal) diff --git a/examples/LidarMapping/MapBuilder.h b/examples/LidarMapping/MapBuilder.h new file mode 100644 index 0000000000..b6e6b5ded5 --- /dev/null +++ b/examples/LidarMapping/MapBuilder.h @@ -0,0 +1,340 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +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 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 MAPBUILDER_H_ +#define MAPBUILDER_H_ + +#include +#include +#include + +#ifndef Q_MOC_RUN // Mac OS X issue +#include "rtabmap/gui/CloudViewer.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/RtabmapEvent.h" +#include "rtabmap/core/OccupancyGrid.h" +#endif +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UEventsHandler.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/core/OdometryEvent.h" +#include + +using namespace rtabmap; + +// This class receives RtabmapEvent and construct/update a 3D Map +class MapBuilder : public QWidget, public UEventsHandler +{ + Q_OBJECT +public: + //Camera ownership is not transferred! + MapBuilder(SensorCaptureThread * camera = 0) : + sensorCaptureThread_(camera), + odometryCorrection_(Transform::getIdentity()), + processingStatistics_(false), + lastOdometryProcessed_(true), + visibility_(0) + { + this->setWindowFlags(Qt::Dialog); + this->setWindowTitle(tr("3D Map")); + this->setMinimumWidth(800); + this->setMinimumHeight(600); + + cloudViewer_ = new CloudViewer(this); + cloudViewer_->setCameraTargetLocked(true); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(cloudViewer_); + this->setLayout(layout); + + qRegisterMetaType("rtabmap::OdometryEvent"); + qRegisterMetaType("rtabmap::Statistics"); + + QAction * pause = new QAction(this); + this->addAction(pause); + pause->setShortcut(Qt::Key_Space); + connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection())); + + QAction * visibility = new QAction(this); + this->addAction(visibility); + visibility->setShortcut(Qt::Key_Tab); + connect(visibility, SIGNAL(triggered()), this, SLOT(rotateVisibility())); + } + + virtual ~MapBuilder() + { + this->unregisterFromEventsManager(); + } + +protected Q_SLOTS: + virtual void pauseDetection() + { + UWARN(""); + if(sensorCaptureThread_) + { + if(sensorCaptureThread_->isCapturing()) + { + sensorCaptureThread_->join(true); + } + else + { + sensorCaptureThread_->start(); + } + } + } + + virtual void rotateVisibility() + { + visibility_ = (visibility_+1) % 3; + if(visibility_ == 0) + { + UWARN("Show both Odom and Map"); + } + else if(visibility_ == 1) + { + UWARN("Show only Map"); + } + else if(visibility_ == 2) + { + UWARN("Show only Odom"); + } + } + + virtual void processOdometry(const rtabmap::OdometryEvent & odom) + { + if(!this->isVisible()) + { + return; + } + + Transform pose = odom.pose(); + if(pose.isNull()) + { + //Odometry lost + cloudViewer_->setBackgroundColor(Qt::darkRed); + + pose = lastOdomPose_; + } + else + { + cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor()); + } + if(!pose.isNull()) + { + lastOdomPose_ = pose; + + // 3d cloud + if(!odom.data().laserScanRaw().empty()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), odom.data().laserScanRaw().localTransform()); + if(cloud->size() && (visibility_ == 0 || visibility_ == 2)) + { + if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose, Qt::magenta)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdom", false); + if(cloud->empty()) + UWARN("Empty cloudOdom!"); + } + } + + if(!odom.info().localScanMap.empty()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform()); + if(cloud->size() && (visibility_ == 0 || visibility_ == 2)) + { + if(!cloudViewer_->addCloud("cloudOdomLocalMap", cloud, odometryCorrection_, Qt::blue)) + { + UERROR("Adding cloudOdomLocalMap to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdomLocalMap", false); + if(cloud->empty()) + UWARN("Empty cloudOdomLocalMap!"); + } + } + + if(!odom.pose().isNull()) + { + // update camera position + cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose()); + } + } + cloudViewer_->update(); + + lastOdometryProcessed_ = true; + } + + + virtual void processStatistics(const rtabmap::Statistics & stats) + { + processingStatistics_ = true; + + //============================ + // Add RGB-D clouds + //============================ + const std::map & poses = stats.poses(); + QMap clouds = cloudViewer_->getAddedClouds(); + for(std::map::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter) + { + if(!iter->second.isNull()) + { + std::string cloudName = uFormat("cloud_%d", iter->first); + + // 3d point cloud + if(clouds.contains(cloudName)) + { + // Update only if the pose has changed + Transform tCloud; + cloudViewer_->getPose(cloudName, tCloud); + if(tCloud.isNull() || iter->second != tCloud) + { + if(!cloudViewer_->updateCloudPose(cloudName, iter->second)) + { + UERROR("Updating pose cloud %d failed!", iter->first); + } + } + } + else if(iter->first == stats.getLastSignatureData().id()) + { + Signature s = stats.getLastSignatureData(); + s.sensorData().uncompressData(); // make sure data is uncompressed + // Add the new cloud + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudI( + s.sensorData().laserScanRaw(), + s.sensorData().laserScanRaw().localTransform()); + if(cloud->size()) + { + if(!cloudViewer_->addCloud(cloudName, cloud, iter->second)) + { + UERROR("Adding cloud %d to viewer failed!", iter->first); + } + } + else + { + UWARN("Empty cloud %d!", iter->first); + } + } + + cloudViewer_->setCloudVisibility(cloudName, visibility_ == 0 || visibility_ == 1); + } + else + { + UWARN("Null pose for %d ?!?", iter->first); + } + } + + // cleanup + for(QMap::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) + { + if(uStrContains(iter.key(), "cloud_")) + { + int id = uStr2Int(uSplitNumChar(iter.key()).back()); + if(poses.find(id) == poses.end()) + { + cloudViewer_->removeCloud(iter.key()); + } + } + } + + //============================ + // Add 3D graph (show all poses) + //============================ + cloudViewer_->removeAllGraphs(); + cloudViewer_->removeCloud("graph_nodes"); + if(poses.size()) + { + // Set graph + pcl::PointCloud::Ptr graph(new pcl::PointCloud); + pcl::PointCloud::Ptr graphNodes(new pcl::PointCloud); + for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) + { + graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); + } + *graphNodes = *graph; + + + // add graph + cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray); + cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green); + cloudViewer_->setCloudPointSize("graph_nodes", 5); + } + + odometryCorrection_ = stats.mapCorrection(); + + cloudViewer_->update(); + + processingStatistics_ = false; + } + + virtual bool handleEvent(UEvent * event) + { + if(event->getClassName().compare("RtabmapEvent") == 0) + { + RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event; + const Statistics & stats = rtabmapEvent->getStats(); + // Statistics must be processed in the Qt thread + if(this->isVisible()) + { + QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats)); + } + } + else if(event->getClassName().compare("OdometryEvent") == 0) + { + OdometryEvent * odomEvent = (OdometryEvent *)event; + // Odometry must be processed in the Qt thread + if(this->isVisible() && + lastOdometryProcessed_ && + !processingStatistics_) + { + lastOdometryProcessed_ = false; // if we receive too many odometry events! + QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent)); + } + } + return false; + } + +protected: + CloudViewer * cloudViewer_; + SensorCaptureThread * sensorCaptureThread_; + Transform lastOdomPose_; + Transform odometryCorrection_; + bool processingStatistics_; + bool lastOdometryProcessed_; + int visibility_; +}; + + +#endif /* MAPBUILDER_H_ */ diff --git a/examples/LidarMapping/main.cpp b/examples/LidarMapping/main.cpp new file mode 100644 index 0000000000..3bf0eff5e9 --- /dev/null +++ b/examples/LidarMapping/main.cpp @@ -0,0 +1,240 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +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 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. +*/ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include "rtabmap/core/lidar/LidarVLP16.h" + +#include +#include "rtabmap/core/Rtabmap.h" +#include "rtabmap/core/RtabmapThread.h" +#include "rtabmap/core/OdometryThread.h" +#include "rtabmap/core/Graph.h" +#include "rtabmap/utilite/UEventsManager.h" +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UDirectory.h" +#include +#include +#include +#include +#include +#include + +#include "MapBuilder.h" + +void showUsage() +{ + printf("\nUsage:\n" + "rtabmap-lidar_mapping IP PORT\n" + "rtabmap-lidar_mapping PCAP_FILEPATH\n" + "\n" + "Example:" + " rtabmap-lidar_mapping 192.168.1.201 2368\n\n"); + exit(1); +} + +using namespace rtabmap; +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + + std::string filepath; + std::string ip; + int port = 2368; + if(argc < 2) + { + showUsage(); + } + else if(argc == 2) + { + filepath = argv[1]; + } + else + { + ip = argv[1]; + port = uStr2Int(argv[2]); + } + + // Here is the pipeline that we will use: + // LidarVLP16 -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + + // Create the Lidar sensor, it will send a SensorEvent + LidarVLP16 * lidar; + if(!ip.empty()) + { + printf("Using ip=%s port=%d\n", ip.c_str(), port); + lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + } + else + { + filepath = uReplaceChar(filepath, '~', UDirectory::homeDir()); + printf("Using file=%s\n", filepath.c_str()); + lidar = new LidarVLP16(filepath); + } + lidar->setOrganized(true); //faster deskewing + + if(!lidar->init()) + { + UERROR("Lidar init failed!"); + delete lidar; + return -1; + } + + SensorCaptureThread lidarThread(lidar); + + // GUI stuff, there the handler will receive RtabmapEvent and construct the map + // We give it the lidar so the GUI can pause/resume the lidar + QApplication app(argc, argv); + MapBuilder mapBuilder(&lidarThread); + + ParametersMap params; + + float resolution = 0.05; + + // ICP parameters + params.insert(ParametersPair(Parameters::kRegStrategy(), "1")); + params.insert(ParametersPair(Parameters::kIcpFiltersEnabled(), "2")); + params.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true")); + params.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "20")); + params.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), "0")); + params.insert(ParametersPair(Parameters::kIcpIterations(), "10")); + params.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(resolution))); + params.insert(ParametersPair(Parameters::kIcpEpsilon(), "0.001")); + params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(resolution*10.0f))); + params.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "2")); + params.insert(ParametersPair(Parameters::kIcpStrategy(), "1")); + params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), "0.7")); + params.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.01")); + // Uncomment if lidar never pitch/roll (on a car or wheeled robot), for hand-held mapping, keep it commented + //params.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), "0.8")); + + // Odom parameters + params.insert(ParametersPair(Parameters::kOdomStrategy(), "0")); // F2M + params.insert(ParametersPair(Parameters::kOdomScanKeyFrameThr(), "0.6")); + params.insert(ParametersPair(Parameters::kOdomF2MScanSubtractRadius(), uNumber2Str(resolution))); + params.insert(ParametersPair(Parameters::kOdomF2MScanMaxSize(), "15000")); + params.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.3")); + params.insert(ParametersPair(Parameters::kOdomDeskewing(), "true")); + + // Create an odometry thread to process lidar events, it will send OdometryEvent. + OdometryThread odomThread(Odometry::create(params)); + + // Rtabmap params + params.insert(ParametersPair(Parameters::kRGBDProximityBySpace(), "true")); + params.insert(ParametersPair(Parameters::kRGBDProximityMaxGraphDepth(), "0")); + params.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1")); + params.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0.05")); + params.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0.05")); + params.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false")); + params.insert(ParametersPair(Parameters::kMemSTMSize(), "30")); + uInsert(params, ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.2")); // overwritten + + + // Create RTAB-Map to process OdometryEvent + Rtabmap * rtabmap = new Rtabmap(); + rtabmap->init(params); + RtabmapThread rtabmapThread(rtabmap); // ownership is transfered + + // Setup handlers + odomThread.registerToEventsManager(); + rtabmapThread.registerToEventsManager(); + mapBuilder.registerToEventsManager(); + + // The RTAB-Map is subscribed by default to SensorEvent, but we want + // RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent. + // We can do that by creating a "pipe" between the lidar and odometry, then + // only the odometry will receive SensorEvent from that lidar. RTAB-Map is + // also subscribed to OdometryEvent by default, so no need to create a pipe between + // odometry and RTAB-Map. + UEventsManager::createPipe(&lidarThread, &odomThread, "SensorEvent"); + + // Let's start the threads + rtabmapThread.start(); + odomThread.start(); + lidarThread.start(); + + printf("Press Tab key to switch between map and odom views (or both).\n"); + printf("Press Space key to pause.\n"); + + mapBuilder.show(); + app.exec(); // main loop + + // remove handlers + mapBuilder.unregisterFromEventsManager(); + rtabmapThread.unregisterFromEventsManager(); + odomThread.unregisterFromEventsManager(); + + // Kill all threads + lidarThread.kill(); + odomThread.join(true); + rtabmapThread.join(true); + + // Save 3D map + printf("Saving rtabmap_cloud.pcd...\n"); + std::map nodes; + std::map optimizedPoses; + std::multimap links; + rtabmap->getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + Signature node = nodes.find(iter->first)->second; + + // uncompress data + node.sensorData().uncompressData(); + + pcl::PointCloud::Ptr tmp = util3d::laserScanToPointCloudI(node.sensorData().laserScanRaw(), node.sensorData().laserScanRaw().localTransform()); + *cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose + } + if(cloud->size()) + { + printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size()); + cloud = util3d::voxelize(cloud, 0.01f); + + printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size()); + pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud); + //pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format + } + else + { + printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n"); + } + + // Save trajectory + printf("Saving rtabmap_trajectory.txt ...\n"); + if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links)) + { + printf("Saving rtabmap_trajectory.txt... done!\n"); + } + else + { + printf("Saving rtabmap_trajectory.txt... failed!\n"); + } + + rtabmap->close(false); + + return 0; +} diff --git a/examples/NoEventsExample/CMakeLists.txt b/examples/NoEventsExample/CMakeLists.txt index 5ddb54dfbd..e7e074d34a 100644 --- a/examples/NoEventsExample/CMakeLists.txt +++ b/examples/NoEventsExample/CMakeLists.txt @@ -15,18 +15,22 @@ if(NOT internal) FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) endif() -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h) -ELSEIF(Qt5_FOUND) - QT5_WRAP_CPP(moc_srcs MapBuilder.h) -ELSE() - QT6_WRAP_CPP(moc_srcs MapBuilder.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) ENDIF() ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs}) TARGET_LINK_LIBRARIES(noEventsExample rtabmap::gui) +SET_TARGET_PROPERTIES( + noEventsExample + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + if(internal) SET_TARGET_PROPERTIES( noEventsExample PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample) diff --git a/examples/RGBDMapping/CMakeLists.txt b/examples/RGBDMapping/CMakeLists.txt index 33a30466ee..6f6a53104a 100644 --- a/examples/RGBDMapping/CMakeLists.txt +++ b/examples/RGBDMapping/CMakeLists.txt @@ -15,18 +15,22 @@ if(NOT internal) FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) endif() -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h) -ELSEIF(Qt5_FOUND) - QT5_WRAP_CPP(moc_srcs MapBuilder.h) -ELSE() - QT6_WRAP_CPP(moc_srcs MapBuilder.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) ENDIF() ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs}) TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap::gui) +SET_TARGET_PROPERTIES( + rgbd_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + if(internal) SET_TARGET_PROPERTIES( rgbd_mapping PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping) diff --git a/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index 2e5b14e5f2..9f788a57d6 100644 --- a/examples/RGBDMapping/MapBuilder.h +++ b/examples/RGBDMapping/MapBuilder.h @@ -37,15 +37,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d_mapping.h" #include "rtabmap/core/RtabmapEvent.h" -#include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/core/global_map/OccupancyGrid.h" #endif #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraThread.h" +#include using namespace rtabmap; @@ -55,11 +56,12 @@ class MapBuilder : public QWidget, public UEventsHandler Q_OBJECT public: //Camera ownership is not transferred! - MapBuilder(CameraThread * camera = 0) : + MapBuilder(SensorCaptureThread * camera = 0) : camera_(camera), odometryCorrection_(Transform::getIdentity()), processingStatistics_(false), - lastOdometryProcessed_(true) + lastOdometryProcessed_(true), + grid_(&localGrids_) { this->setWindowFlags(Qt::Dialog); this->setWindowTitle(tr("3D Map")); @@ -252,11 +254,11 @@ protected Q_SLOTS: { cv::Mat groundCells, obstacleCells, emptyCells; stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells); - grid_.addToCache(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells); + localGrids_.add(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells, stats.getLastSignatureData().sensorData().gridCellSize(), stats.getLastSignatureData().sensorData().gridViewPoint()); } } - if(grid_.addedNodes().size() || grid_.cacheSize()) + if(grid_.addedNodes().size() || localGrids_.size()) { grid_.update(stats.poses()); } @@ -308,11 +310,12 @@ protected Q_SLOTS: protected: CloudViewer * cloudViewer_; - CameraThread * camera_; + SensorCaptureThread * camera_; Transform lastOdomPose_; Transform odometryCorrection_; bool processingStatistics_; bool lastOdometryProcessed_; + LocalGridCache localGrids_; OccupancyGrid grid_; }; diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index c39c56be35..28064e3bea 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/utilite/UEventsManager.h" @@ -39,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #ifdef RTABMAP_PYTHON #include "rtabmap/core/PythonInterface.h" @@ -80,9 +80,9 @@ int main(int argc, char * argv[]) } // Here is the pipeline that we will use: - // CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + // CameraOpenni -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" - // Create the OpenNI camera, it will send a CameraEvent at the rate specified. + // Create the OpenNI camera, it will send a SensorEvent at the rate specified. // Set transform to camera so z is up, y is left and x going forward Camera * camera = 0; if(driver == 1) @@ -185,7 +185,7 @@ int main(int argc, char * argv[]) UERROR("Camera init failed!"); } - CameraThread cameraThread(camera); + SensorCaptureThread cameraThread(camera); // GUI stuff, there the handler will receive RtabmapEvent and construct the map @@ -210,19 +210,21 @@ int main(int argc, char * argv[]) rtabmapThread.registerToEventsManager(); mapBuilder.registerToEventsManager(); - // The RTAB-Map is subscribed by default to CameraEvent, but we want - // RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent. + // The RTAB-Map is subscribed by default to SensorEvent, but we want + // RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent. // We can do that by creating a "pipe" between the camera and odometry, then - // only the odometry will receive CameraEvent from that camera. RTAB-Map is + // only the odometry will receive SensorEvent from that camera. RTAB-Map is // also subscribed to OdometryEvent by default, so no need to create a pipe between // odometry and RTAB-Map. - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent"); // Let's start the threads rtabmapThread.start(); odomThread.start(); cameraThread.start(); + printf("Press Space key to pause.\n"); + mapBuilder.show(); app.exec(); // main loop diff --git a/examples/WifiMapping/CMakeLists.txt b/examples/WifiMapping/CMakeLists.txt index 8b029fa1e4..0614c99e1c 100644 --- a/examples/WifiMapping/CMakeLists.txt +++ b/examples/WifiMapping/CMakeLists.txt @@ -15,12 +15,8 @@ if(NOT internal) FIND_PACKAGE(RTABMap REQUIRED) endif() -IF(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) -ELSEIF(Qt5_FOUND) - QT5_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) -ELSE() - QT6_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h MapBuilderWifi.h) ENDIF() SET(srcs @@ -46,6 +42,14 @@ ENDIF(APPLE) ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs}) TARGET_LINK_LIBRARIES(wifi_mapping rtabmap::gui ${LIBRARIES}) +SET_TARGET_PROPERTIES( + wifi_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + if(internal) SET_TARGET_PROPERTIES( wifi_mapping PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping) diff --git a/examples/WifiMapping/MapBuilder.h b/examples/WifiMapping/MapBuilder.h index 22378c9f8f..101e6035fa 100644 --- a/examples/WifiMapping/MapBuilder.h +++ b/examples/WifiMapping/MapBuilder.h @@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraThread.h" +#include using namespace rtabmap; @@ -54,7 +54,7 @@ class MapBuilder : public QWidget, public UEventsHandler Q_OBJECT public: //Camera ownership is not transferred! - MapBuilder(CameraThread * camera = 0) : + MapBuilder(SensorCaptureThread * camera = 0) : camera_(camera), odometryCorrection_(Transform::getIdentity()), processingStatistics_(false), @@ -278,7 +278,7 @@ protected Q_SLOTS: protected: CloudViewer * cloudViewer_; - CameraThread * camera_; + SensorCaptureThread * camera_; Transform lastOdomPose_; Transform odometryCorrection_; bool processingStatistics_; diff --git a/examples/WifiMapping/MapBuilderWifi.h b/examples/WifiMapping/MapBuilderWifi.h index a7250382e0..dc199e5e7c 100644 --- a/examples/WifiMapping/MapBuilderWifi.h +++ b/examples/WifiMapping/MapBuilderWifi.h @@ -56,7 +56,7 @@ class MapBuilderWifi : public MapBuilder Q_OBJECT public: // Camera ownership is not transferred! - MapBuilderWifi(CameraThread * camera = 0) : + MapBuilderWifi(SensorCaptureThread * camera = 0) : MapBuilder(camera) {} diff --git a/examples/WifiMapping/main.cpp b/examples/WifiMapping/main.cpp index 847fcf20a0..82a6ebf0a3 100644 --- a/examples/WifiMapping/main.cpp +++ b/examples/WifiMapping/main.cpp @@ -26,11 +26,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/utilite/UEventsManager.h" #include @@ -114,9 +114,9 @@ int main(int argc, char * argv[]) } // Here is the pipeline that we will use: - // CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + // CameraOpenni -> "" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" - // Create the OpenNI camera, it will send a CameraEvent at the rate specified. + // Create the OpenNI camera, it will send a at the rate specified. // Set transform to camera so z is up, y is left and x going forward Camera * camera = 0; Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); @@ -222,7 +222,7 @@ int main(int argc, char * argv[]) showUsage(); exit(1); } - CameraThread cameraThread(camera); + SensorCaptureThread cameraThread(camera); if(mirroring) { cameraThread.setMirroringEnabled(true); @@ -253,13 +253,13 @@ int main(int argc, char * argv[]) rtabmapThread.registerToEventsManager(); mapBuilderWifi.registerToEventsManager(); - // The RTAB-Map is subscribed by default to CameraEvent, but we want - // RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent. + // The RTAB-Map is subscribed by default to , but we want + // RTAB-Map to process OdometryEvent instead, ignoring the . // We can do that by creating a "pipe" between the camera and odometry, then - // only the odometry will receive CameraEvent from that camera. RTAB-Map is + // only the odometry will receive from that camera. RTAB-Map is // also subscribed to OdometryEvent by default, so no need to create a pipe between // odometry and RTAB-Map. - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, ""); // Let's start the threads rtabmapThread.start(); diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 0f227f7b54..834de1bd77 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -53,6 +53,7 @@ class RTABMAP_GUI_EXPORT CameraViewer : public QDialog, public UEventsHandler QWidget * parent = 0, const ParametersMap & parameters = ParametersMap()); virtual ~CameraViewer(); + void setDecimation(int value); public Q_SLOTS: void showImage(const rtabmap::SensorData & data); diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index a073a2ffe2..7275e658b5 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -186,6 +186,14 @@ class RTABMAP_GUI_EXPORT CloudViewer : public PCLQVTKWidget float opacity); void removeOccupancyGridMap(); + bool addElevationMap( + const cv::Mat & map32FC1, + float resolution, // cell size + float xMin, + float yMin, + float opacity); + void removeElevationMap(); + void updateCameraTargetPosition( const Transform & pose); diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 4622580dd4..d71d8e9299 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -44,11 +44,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include class Ui_DatabaseViewer; class QGraphicsScene; class QGraphicsView; class QLabel; +class QToolButton; class QDialog; namespace rtabmap @@ -61,6 +63,7 @@ class OctoMap; class ExportCloudsDialog; class EditDepthArea; class EditMapArea; +class LinkRefiningDialog; class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow { @@ -69,7 +72,7 @@ class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow public: DatabaseViewer(const QString & ini = QString(), QWidget * parent = 0); virtual ~DatabaseViewer(); - bool openDatabase(const QString & path); + bool openDatabase(const QString & path, const ParametersMap & overridenParameters = ParametersMap()); bool isSavedMaximized() const {return savedMaximized_;} void showCloseButton(bool visible = true); @@ -93,6 +96,7 @@ private Q_SLOTS: void selectObstacleColor(); void selectGroundColor(); void selectEmptyColor(); + void selectFrontierColor(); void editDepthImage(); void generateGraph(); void editSaved2DMap(); @@ -123,9 +127,10 @@ private Q_SLOTS: void updateAllNeighborCovariances(); void updateAllLoopClosureCovariances(); void updateAllLandmarkCovariances(); - void refineAllNeighborLinks(); - void refineAllLoopClosureLinks(); + void refineLinks(); void resetAllChanges(); + void graphNodeSelected(int); + void graphLinkSelected(int, int); void sliderAValueChanged(int); void sliderBValueChanged(int); void sliderAMoved(int); @@ -172,6 +177,8 @@ private Q_SLOTS: QLabel * labelScan, QLabel * labelGravity, QLabel * labelPrior, + QToolButton * editPriorButton, + QToolButton * removePriorButton, QLabel * labelGps, QLabel * labelGt, QLabel * labelSensors, @@ -191,8 +198,8 @@ private Q_SLOTS: std::multimap updateLinksWithModifications( const std::multimap & edgeConstraints); void updateLoopClosuresSlider(int from = 0, int to = 0); - void updateAllCovariances(const QList & links); - void refineAllLinks(const QList & links); + void updateCovariances(const QList & links); + void refineLinks(const QList & links); void refineConstraint(int from, int to, bool silent); bool addConstraint(int from, int to, bool silent); void exportPoses(int format); @@ -226,18 +233,17 @@ private Q_SLOTS: std::multimap linksRefined_; std::multimap linksAdded_; std::multimap linksRemoved_; - std::map, cv::Mat> > localMaps_; // < , empty> - std::map > localMapsInfo_; // - std::map, cv::Mat> > generatedLocalMaps_; // < , empty> - std::map > generatedLocalMapsInfo_; // std::map modifiedLaserScans_; std::vector odomMaxInf_; + LocalGridCache localMaps_; + LocalGridCache generatedLocalMaps_; OctoMap * octomap_; ExportCloudsDialog * exportDialog_; QDialog * editDepthDialog_; EditDepthArea * editDepthArea_; QDialog * editMapDialog_; EditMapArea * editMapArea_; + LinkRefiningDialog * linkRefiningDialog_; bool savedMaximized_; bool firstCall_; diff --git a/guilib/include/rtabmap/gui/ExportCloudsDialog.h b/guilib/include/rtabmap/gui/ExportCloudsDialog.h index fcfd66f76e..71bb674c94 100644 --- a/guilib/include/rtabmap/gui/ExportCloudsDialog.h +++ b/guilib/include/rtabmap/gui/ExportCloudsDialog.h @@ -153,6 +153,10 @@ private Q_SLOTS: GainCompensator * _compensator; const DBDriver * _dbDriver; bool _scansHaveRGB; + + bool saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const; + bool saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const; + }; } diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index fac00a0908..9e8996a582 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -166,12 +166,16 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { Q_SIGNALS: void configChanged(); void mapShownRequested(); + void nodeSelected(int); + void linkSelected(int, int); public Q_SLOTS: void restoreDefaults(); protected: virtual void wheelEvent ( QWheelEvent * event ); + virtual void mouseMoveEvent(QMouseEvent * event); + virtual void mouseDoubleClickEvent(QMouseEvent * event); virtual void contextMenuEvent(QContextMenuEvent * event); private: @@ -228,6 +232,7 @@ public Q_SLOTS: float _loopClosureOutlierThr; float _maxLinkLength; bool _orientationENU; + bool _mouseTracking; ViewPlane _viewPlane; bool _ensureFrameVisible; }; diff --git a/guilib/include/rtabmap/gui/ImageView.h b/guilib/include/rtabmap/gui/ImageView.h index 3ef4eee63f..2c09bacb63 100644 --- a/guilib/include/rtabmap/gui/ImageView.h +++ b/guilib/include/rtabmap/gui/ImageView.h @@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include "rtabmap/utilite/UCv2Qt.h" +#include class QAction; class QMenu; @@ -96,7 +97,7 @@ class RTABMAP_GUI_EXPORT ImageView : public QWidget { void setFeatures(const std::vector & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow); void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color); void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString()); - void setImage(const QImage & image); + void setImage(const QImage & image, const std::vector & models = std::vector(), const Transform & pose = Transform()); void setImageDepth(const cv::Mat & imageDepth); void setImageDepth(const QImage & image); void setFeatureColor(int id, QColor color); @@ -121,6 +122,7 @@ class RTABMAP_GUI_EXPORT ImageView : public QWidget { virtual void paintEvent(QPaintEvent *event); virtual void resizeEvent(QResizeEvent* event); virtual void contextMenuEvent(QContextMenuEvent * e); + virtual void mouseMoveEvent(QMouseEvent * event); private Q_SLOTS: void sceneRectChanged(const QRectF &rect); @@ -164,6 +166,7 @@ private Q_SLOTS: QAction * _colorMapBlueToRed; QAction * _colorMapMinRange; QAction * _colorMapMaxRange; + QAction * _mouseTracking; QMenu * _featureMenu; QMenu * _scaleMenu; @@ -175,6 +178,8 @@ private Q_SLOTS: QPixmap _image; QPixmap _imageDepth; cv::Mat _imageDepthCv; + std::vector _models; + Transform _pose; }; } diff --git a/guilib/include/rtabmap/gui/LinkRefiningDialog.h b/guilib/include/rtabmap/gui/LinkRefiningDialog.h new file mode 100644 index 0000000000..67afc4dcb0 --- /dev/null +++ b/guilib/include/rtabmap/gui/LinkRefiningDialog.h @@ -0,0 +1,79 @@ +/* +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 RTABMAP_LINKREFININGDIALOG_H_ +#define RTABMAP_LINKREFININGDIALOG_H_ + +#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines + +#include + +#include + +class Ui_linkRefiningDialog; + +namespace rtabmap { + +class RTABMAP_GUI_EXPORT LinkRefiningDialog : public QDialog +{ + Q_OBJECT + +public: + LinkRefiningDialog(QWidget * parent = 0); + + virtual ~LinkRefiningDialog(); + + void setMinMax( + int nodeIdMin, + int nodeIdMax, + int mapIdMin, + int mapIdMax); + + Link::Type getLinkType() const; + void getIntraInterSessions(bool & intra, bool & inter) const; + bool isRangeByNodeId() const; + bool isRangeByMapId() const; + void getRangeNodeId(int & from, int & to) const; + void getRangeMapId(int & from, int & to) const; + +private Q_SLOTS: + void restoreDefaults(); + void updateIntraInterState(); + void setRangeToNodeId(); + void setRangeToMapId(); + +private: + Ui_linkRefiningDialog * ui_; + int defaultNodeIdMin_; + int defaultNodeIdMax_; + int defaultMapIdMin_; + int defaultMapIdMax_; +}; + +} + +#endif /* RTABMAP_LINKREFININGDIALOG_H_ */ diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 31639048ed..58d4955ab4 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -37,8 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/SensorData.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraInfo.h" #include "rtabmap/core/Optimizer.h" +#include "rtabmap/core/GlobalMap.h" #include "rtabmap/gui/PreferencesDialog.h" #include @@ -46,9 +46,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { -class CameraThread; +class SensorCaptureThread; class OdometryThread; class IMUThread; class CloudViewer; @@ -75,6 +76,7 @@ class PostProcessingDialog; class DepthCalibrationDialog; class DataRecorder; class OctoMap; +class GridMap; class MultiSessionLocWidget; class RTABMAP_GUI_EXPORT MainWindow : public QMainWindow, public UEventsHandler @@ -168,7 +170,6 @@ protected Q_SLOTS: void depthCalibration(); void openWorkingDirectory(); void updateEditMenu(); - void selectStream(); void selectOpenni(); void selectFreenect(); void selectOpenniCv(); @@ -190,6 +191,8 @@ protected Q_SLOTS: void selectMyntEyeS(); void selectDepthAIOAKD(); void selectDepthAIOAKDLite(); + void selectDepthAIOAKDPro(); + void selectVLP16(); void dumpTheMemory(); void dumpThePrediction(); void sendGoal(); @@ -204,7 +207,7 @@ protected Q_SLOTS: void selectScreenCaptureFormat(bool checked); void takeScreenshot(); void updateElapsedTime(); - void processCameraInfo(const rtabmap::CameraInfo & info); + void processCameraInfo(const rtabmap::SensorCaptureInfo & info); void processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored); void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags); void applyPrefSettings(const rtabmap::ParametersMap & parameters); @@ -239,7 +242,7 @@ protected Q_SLOTS: Q_SIGNALS: void statsReceived(const rtabmap::Statistics &); void statsProcessed(); - void cameraInfoReceived(const rtabmap::CameraInfo &); + void cameraInfoReceived(const rtabmap::SensorCaptureInfo &); void cameraInfoProcessed(); void odometryReceived(const rtabmap::OdometryEvent &, bool); void odometryProcessed(); @@ -314,11 +317,6 @@ protected Q_SLOTS: const QString & newDatabasePathOutput() const { return _newDatabasePathOutput; } virtual ParametersMap getCustomParameters() {return ParametersMap();} - virtual Camera * createCamera( - Camera ** odomSensor, - Transform & odomSensorExtrinsics, - double & odomSensorTimeOffset, - float & odomSensorScaleFactor); void postProcessing( bool refineNeighborLinks, @@ -342,7 +340,7 @@ protected Q_SLOTS: Ui_mainWindow * _ui; State _state; - rtabmap::CameraThread * _camera; + rtabmap::SensorCaptureThread * _sensorCapture; rtabmap::OdometryThread * _odomThread; rtabmap::IMUThread * _imuThread; @@ -388,11 +386,13 @@ protected Q_SLOTS: std::pair::Ptr, pcl::PointCloud::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction std::map _cachedWordsCount; std::map _cachedLocalizationsCount; + rtabmap::LocalGridCache _cachedLocalMaps; std::map _createdScans; rtabmap::OccupancyGrid * _occupancyGrid; rtabmap::OctoMap * _octomap; + rtabmap::GridMap * _elevationMap; std::map::Ptr> _createdFeatures; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 3604c4cada..0078d11dd6 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -60,6 +60,8 @@ namespace rtabmap { class Signature; class LoopClosureViewer; class Camera; +class SensorCapture; +class Lidar; class CalibrationDialog; class CreateSimpleCalibrationDialog; @@ -77,7 +79,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog kPanelAll = 15 }; // TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared... - Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag) + Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag); enum Src { kSrcUndef = -1, @@ -113,7 +115,10 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog kSrcImages = 201, kSrcVideo = 202, - kSrcDatabase = 300 + kSrcDatabase = 300, + + kSrcLidar = 400, + kSrcLidarVLP16 = 400, }; public: @@ -226,7 +231,9 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog double getSubtractFilteringRadius() const; double getSubtractFilteringAngle() const; + double getGridUIResolution() const; bool getGridMapShown() const; + int getElevationMapShown() const; int getGridMapSensor() const; bool projMapFrame() const; double projMaxGroundAngle() const; @@ -251,6 +258,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog PreferencesDialog::Src getSourceDriver() const; QString getSourceDriverStr() const; QString getSourceDevice() const; + PreferencesDialog::Src getLidarSourceDriver() const; PreferencesDialog::Src getOdomSourceDriver() const; bool isSourceDatabaseStampsUsed() const; @@ -264,9 +272,12 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog double getBilateralSigmaS() const; double getBilateralSigmaR() const; int getSourceImageDecimation() const; + int getSourceHistogramMethod() const; + bool isSourceFeatureDetection() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; + bool isSourceScanDeskewing() const; int getSourceScanDownsampleStep() const; double getSourceScanRangeMin() const; double getSourceScanRangeMax() const; @@ -280,7 +291,8 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog QString getIMUPath() const; int getIMURate() const; Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null - Camera * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor); // return camera should be deleted if not null + SensorCapture * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor, double & waitTime); // return odom sensor should be deleted if not null + Lidar * createLidar(); // return lidar should be deleted if not null int getIgnoredDCComponents() const; @@ -320,6 +332,7 @@ public Q_SLOTS: void calibrate(); void calibrateSimple(); void calibrateOdomSensorExtrinsics(); + void testLidar(); private Q_SLOTS: void closeDialog ( QAbstractButton * button ); @@ -328,6 +341,7 @@ private Q_SLOTS: void loadConfigFrom(); bool saveConfigTo(); void resetConfig(); + void loadPreset(); void makeObsoleteGeneralPanel(); void makeObsoleteCloudRenderingPanel(); void makeObsoleteLoggingPanel(); @@ -341,6 +355,7 @@ private Q_SLOTS: void updateKpROI(); void updateStereoDisparityVisibility(); void updateFeatureMatchingVisibility(); + void updateGlobalDescriptorVisibility(); void updateOdometryStackedIndex(int index); void useOdomFeatures(); void changeWorkingDirectory(); @@ -348,10 +363,13 @@ private Q_SLOTS: void changeOdometryORBSLAMVocabulary(); void changeOdometryOKVISConfigPath(); void changeOdometryVINSConfigPath(); + void changeOdometryOpenVINSLeftMask(); + void changeOdometryOpenVINSRightMask(); void changeIcpPMConfigPath(); void changeSuperPointModelPath(); void changePyMatcherPath(); void changePyMatcherModel(); + void changePyDescriptorPath(); void changePyDetectorPath(); void readSettingsEnd(); void setupTreeView(); @@ -380,6 +398,8 @@ private Q_SLOTS: void selectSourceMKVPath(); void selectSourceSvoPath(); void selectSourceRealsense2JsonPath(); + void selectSourceDepthaiBlobPath(); + void selectVlp16PcapPath(); void updateSourceGrpVisibility(); void testOdometry(); void testCamera(); @@ -408,6 +428,7 @@ private Q_SLOTS: void setupKpRoiPanel(); bool parseModel(QList & boxes, QStandardItem * parentItem, int currentLevel, int & absoluteIndex); void resetSettings(QGroupBox * groupBox); + void loadPreset(const std::string & presetHexHeader); void addParameter(const QObject * object, int value); void addParameter(const QObject * object, bool value); void addParameter(const QObject * object, double value); diff --git a/guilib/include/rtabmap/gui/StatsToolBox.h b/guilib/include/rtabmap/gui/StatsToolBox.h index 86ab8c7be9..b08b7cdfd5 100644 --- a/guilib/include/rtabmap/gui/StatsToolBox.h +++ b/guilib/include/rtabmap/gui/StatsToolBox.h @@ -95,8 +95,9 @@ class RTABMAP_GUI_EXPORT StatsToolBox : public QWidget StatsToolBox(QWidget * parent); virtual ~StatsToolBox(); - void getFiguresSetup(QList & curvesPerFigure, QStringList & curveNames); + void getFiguresSetup(QList & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds); void addCurve(const QString & name, bool newFigure = true, bool cacheOn = false); + void addThreshold(const QString & name, qreal value); // Add to latest figure void setWorkingDirectory(const QString & workingDirectory); void setNewFigureMaxItems(int value) {_newFigureMaxItems = value;} void closeFigures(); diff --git a/guilib/include/rtabmap/utilite/UPlot.h b/guilib/include/rtabmap/utilite/UPlot.h index 3d78729211..c3d4cd1250 100644 --- a/guilib/include/rtabmap/utilite/UPlot.h +++ b/guilib/include/rtabmap/utilite/UPlot.h @@ -270,6 +270,7 @@ class RTABMAP_GUI_EXPORT UPlotCurveThreshold : public UPlotCurve */ UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0); virtual ~UPlotCurveThreshold(); + qreal getThreshold() const {return _threshold;} public Q_SLOTS: /** @@ -288,6 +289,7 @@ public Q_SLOTS: private: Qt::Orientation _orientation; + qreal _threshold; }; /** @@ -509,8 +511,10 @@ class RTABMAP_GUI_EXPORT UPlot : public QWidget /** * Get all curve names. */ - QStringList curveNames(); - bool contains(const QString & curveName); + QStringList curveNames() const; + bool contains(const QString & curveName) const; + bool isThreshold(const QString & curveName) const; + double getThresholdValue(const QString & curveName) const; void removeCurves(); QString getAllCurveDataAsText() const; /** diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index a25ac53435..437209ebbe 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_octomap->setText("No"); _ui->label_octomap_license->setEnabled(false); #endif +#ifdef RTABMAP_GRIDMAP + _ui->label_gridmap->setText("Yes"); + _ui->label_gridmap_license->setEnabled(true); +#else + _ui->label_gridmap->setText("No"); + _ui->label_gridmap_license->setEnabled(false); +#endif #ifdef RTABMAP_CPUTSDF _ui->label_cputsdf->setText("Yes"); _ui->label_cputsdf_license->setEnabled(true); @@ -184,6 +191,14 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_cccorelib_license->setEnabled(false); #endif +#ifdef RTABMAP_OPEN3D + _ui->label_open3d->setText("Yes"); + _ui->label_open3d_license->setEnabled(true); +#else + _ui->label_open3d->setText("No"); + _ui->label_open3d_license->setEnabled(false); +#endif + #ifdef RTABMAP_FOVIS _ui->label_fovis->setText("Yes"); _ui->label_fovis_license->setEnabled(true); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 349f6f9e10..8eb534336f 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -36,48 +36,15 @@ SET(headers_ui ../include/${PROJECT_PREFIX}/gui/EditMapArea.h ../include/${PROJECT_PREFIX}/gui/MultiSessionLocWidget.h ../include/${PROJECT_PREFIX}/gui/MultiSessionLocSubView.h -) - -SET(uis - ./ui/mainWindow.ui - ./ui/preferencesDialog.ui - ./ui/aboutDialog.ui - ./ui/consoleWidget.ui - ./ui/DatabaseViewer.ui - ./ui/loopClosureViewer.ui - ./ui/exportDialog.ui - ./ui/postProcessingDialog.ui - ./ui/exportCloudsDialog.ui - ./ui/calibrationDialog.ui - ./ui/createSimpleCalibrationDialog.ui - ./ui/depthCalibrationDialog.ui - ./ui/exportBundlerDialog.ui - ./ui/editConstraintDialog.ui - ./ui/multiSessionLocSubView.ui + ../include/${PROJECT_PREFIX}/gui/LinkRefiningDialog.h ) SET(qrc ./GuiLib.qrc ) -IF(QT4_FOUND) - # generate rules for building source files from the resources - QT4_ADD_RESOURCES(srcs_qrc ${qrc}) - - #Generate .h files from the .ui files - QT4_WRAP_UI(moc_uis ${uis}) - - #This will generate moc_* for Qt - QT4_WRAP_CPP(moc_srcs ${headers_ui}) - ### Qt Gui stuff end### -ELSEIF(Qt5_FOUND) - QT5_ADD_RESOURCES(srcs_qrc ${qrc}) - QT5_WRAP_UI(moc_uis ${uis}) - QT5_WRAP_CPP(moc_srcs ${headers_ui}) -ELSE() - QT6_ADD_RESOURCES(srcs_qrc ${qrc}) - QT6_WRAP_UI(moc_uis ${uis}) - QT6_WRAP_CPP(moc_srcs ${headers_ui}) +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + set(CMAKE_AUTOUIC_SEARCH_PATHS ./ui/) ENDIF() @@ -115,12 +82,11 @@ SET(SRC_FILES ./CreateSimpleCalibrationDialog.cpp ./ParametersToolBox.cpp ./DepthCalibrationDialog.cpp + ./LinkRefiningDialog.cpp ./3rdParty/QMultiComboBox.cpp ./opencv/vtkImageMatSource.cpp - - ${moc_srcs} - ${moc_uis} - ${srcs_qrc} + ${qrc} + ${headers_ui} ) # to get includes in visual studio @@ -211,8 +177,28 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) add_definitions(${PCL_DEFINITIONS}) +# Include presets +SET(RESOURCES + ${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini + ${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini +) + +foreach(arg ${RESOURCES}) + get_filename_component(filename ${arg} NAME) + string(REPLACE "." "_" output ${filename}) + set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h") + set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON) +endforeach(arg ${RESOURCES}) + +ADD_CUSTOM_COMMAND( + OUTPUT ${RESOURCES_HEADERS} + COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES} + COMMENT "[Creating resources]" + DEPENDS ${RESOURCES} +) + # create a library from the source files -ADD_LIBRARY(rtabmap_gui ${SRC_FILES}) +ADD_LIBRARY(rtabmap_gui ${SRC_FILES} ${RESOURCES_HEADERS}) ADD_LIBRARY(rtabmap::gui ALIAS rtabmap_gui) generate_export_header(rtabmap_gui @@ -235,6 +221,9 @@ SET_TARGET_PROPERTIES( VERSION ${RTABMAP_VERSION} SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} EXPORT_NAME "gui" + AUTOUIC ON + AUTOMOC ON + AUTORCC ON ) INSTALL(TARGETS rtabmap_gui EXPORT rtabmap_guiTargets diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 3cec92ef89..2a9058f4e4 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include @@ -307,10 +307,10 @@ bool CalibrationDialog::handleEvent(UEvent * event) { if(!processingData_) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event; - if(e->getCode() == rtabmap::CameraEvent::kCodeData) + rtabmap::SensorEvent * e = (rtabmap::SensorEvent *)event; + if(e->getCode() == rtabmap::SensorEvent::kCodeData) { processingData_ = true; QMetaObject::invokeMethod(this, "processImages", diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 569721ed18..2840cb10f8 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/gui/CameraViewer.h" -#include #include #include #include @@ -58,6 +58,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : imageView_->setImageDepthShown(true); imageView_->setMinimumSize(320, 240); + imageView_->setVisible(false); QHBoxLayout * layout = new QHBoxLayout(); layout->setContentsMargins(0,0,0,0); layout->addWidget(imageView_,1); @@ -108,10 +109,16 @@ CameraViewer::~CameraViewer() this->unregisterFromEventsManager(); } +void CameraViewer::setDecimation(int value) +{ + decimationSpin_->setValue(value); +} + void CameraViewer::showImage(const rtabmap::SensorData & data) { processingImages_ = true; QString sizes; + imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty()); if(!data.imageRaw().empty()) { imageView_->setImage(uCvMat2QImage(data.imageRaw())); @@ -199,10 +206,10 @@ bool CameraViewer::handleEvent(UEvent * event) { if(!pause_->isChecked()) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * camEvent = (SensorEvent*)event; + if(camEvent->getCode() == SensorEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 219c7ecaf6..a67374f3b8 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -53,6 +53,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -67,13 +69,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include +#include #include #include +#include #include #include #include +#include #include #if VTK_MAJOR_VERSION >= 7 @@ -82,13 +88,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif - #if VTK_MAJOR_VERSION >= 8 #include #endif #ifdef RTABMAP_OCTOMAP -#include +#include #endif namespace rtabmap { @@ -140,11 +145,8 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) : _intensityAbsMax(100.0f), _coordinateFrameScale(1.0) { - UDEBUG(""); this->setMinimumSize(200, 200); - vtkObject::GlobalWarningDisplayOff(); - int argc = 0; UASSERT(style!=0); style->setCloudViewer(this); @@ -202,18 +204,23 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) : } _visualizer->getRenderWindow()->SetNumberOfLayers(4); +#ifdef VTK_GLOBAL_WARNING_DISPLAY_OFF + _visualizer->getRenderWindow()->GlobalWarningDisplayOff(); +#endif + #if VTK_MAJOR_VERSION > 8 this->setRenderWindow(_visualizer->getRenderWindow()); #else this->SetRenderWindow(_visualizer->getRenderWindow()); #endif - // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as - // the "Invalid drawable" warning when the view is not visible. - //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); + // Replaced by the second line, to avoid a crash in Mac OS X on close, as well as + // the "Invalid drawable" warning when the view is not visible. #if VTK_MAJOR_VERSION > 8 + //_visualizer->setupInteractor(this->interactor(), this->renderWindow()); this->interactor()->SetInteractorStyle (_visualizer->getInteractorStyle()); #else + //_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow()); this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle()); #endif // setup a simple point picker @@ -261,6 +268,7 @@ void CloudViewer::clear() this->removeAllTexts(); this->removeOccupancyGridMap(); this->removeOctomap(); + this->removeElevationMap(); if(_aShowCameraAxis->isChecked()) { @@ -1108,8 +1116,6 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo #ifdef RTABMAP_OCTOMAP UASSERT(octomap!=0); - pcl::IndicesPtr obstacles(new std::vector); - if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth()) { if(treeDepth>0) @@ -1125,7 +1131,12 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo if(!volumeRepresentation) { - pcl::PointCloud::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false); + pcl::IndicesPtr obstacles(new std::vector); + pcl::IndicesPtr ground(new std::vector); + + pcl::PointCloud::Ptr cloud = octomap->createCloud( + treeDepth, obstacles.get(), 0, ground.get(), false); + obstacles->insert(obstacles->end(), ground->begin(), ground->end()); if(obstacles->size()) { //vtkSmartPointer colors = vtkSmartPointer::New(); @@ -1502,11 +1513,11 @@ bool CloudViewer::addTextureMesh ( // Save the viewpoint transformation matrix to the global actor map (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation; #endif - + #if VTK_MAJOR_VERSION >= 7 - actor->GetProperty()->SetAmbient(0.1); -#else - actor->GetProperty()->SetAmbient(0.5); + actor->GetProperty()->SetAmbient(0.1); +#else + actor->GetProperty()->SetAmbient(0.5); #endif actor->GetProperty()->SetLighting(_aSetLighting->isChecked()); actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); @@ -1596,6 +1607,131 @@ void CloudViewer::removeOccupancyGridMap() #endif } +bool CloudViewer::addElevationMap( + const cv::Mat & map32FC1, + float resolution, // cell size + float xMin, + float yMin, + float opacity) +{ + if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end()) + { + _visualizer->removeShape("elevation_map"); + } + + vtkSmartPointer gridPoints = vtkSmartPointer::New (); + vtkSmartPointer gridCells = vtkSmartPointer::New (); + for (int y = 0; y < map32FC1.rows; ++y) + { + const float * previousRow = y>0?map32FC1.ptr(y-1):0; + const float * rowPtr = map32FC1.ptr(y); + for (int x = 0; x < map32FC1.cols; ++x) + { + gridPoints->InsertNextPoint(xMin + x*resolution, yMin + y*resolution, rowPtr[x]); + if(x>0 && y>0 && + rowPtr[x] != 0 && + rowPtr[x-1] != 0 && + previousRow[x] != 0 && + previousRow[x-1] != 0) + { + gridCells->InsertNextCell(4); + gridCells->InsertCellPoint(x-1+y*map32FC1.cols); + gridCells->InsertCellPoint(x+y*map32FC1.cols); + gridCells->InsertCellPoint(x+(y-1)*map32FC1.cols); + gridCells->InsertCellPoint(x-1+(y-1)*map32FC1.cols); + } + } + } + + double bounds[6]; + gridPoints->GetBounds(bounds); + + vtkSmartPointer polyData = vtkSmartPointer::New (); + polyData->SetPoints(gridPoints); + polyData->SetPolys(gridCells); + + vtkSmartPointer elevationFilter = vtkSmartPointer::New (); + elevationFilter->SetInputData(polyData); + elevationFilter->SetLowPoint(0.0, 0.0, bounds[4]); + elevationFilter->SetHighPoint(0.0, 0.0, bounds[5]); + elevationFilter->Update(); + + vtkSmartPointer output = vtkSmartPointer::New (); + output->ShallowCopy(dynamic_cast(elevationFilter->GetOutput())); + + vtkFloatArray* elevation = dynamic_cast( + output->GetPointData()->GetArray("Elevation")); + + // Create the color map + vtkSmartPointer colorLookupTable = vtkSmartPointer::New (); + colorLookupTable->SetTableRange(bounds[4], bounds[5]); + colorLookupTable->Build(); + + // Generate the colors for each point based on the color map + vtkSmartPointer colors = vtkSmartPointer::New (); + colors->SetNumberOfComponents(3); + colors->SetName("Colors"); + + for (vtkIdType i = 0; i < output->GetNumberOfPoints(); i++) + { + double val = elevation->GetValue(i); + double dcolor[3]; + colorLookupTable->GetColor(val, dcolor); + unsigned char color[3]; + for (unsigned int j = 0; j < 3; j++) + { + color[j] = 255 * dcolor[j] / 1.0; + } + +#if VTK_MAJOR_VERSION > 7 || (VTK_MAJOR_VERSION==7 && VTK_MINOR_VERSION >= 1) + colors->InsertNextTypedTuple(color); +#else + colors->InsertNextTupleValue(color); +#endif + + } + + output->GetPointData()->AddArray(colors); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInputData(output); + + vtkSmartPointer actor = vtkSmartPointer::New (); + actor->SetMapper(mapper); + + // Add it to all renderers + _visualizer->getRendererCollection()->InitTraversal (); + vtkRenderer* renderer = NULL; + int i = 0; + int viewport = 1; + while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL) + { + // Should we add the actor to all renderers? + if (viewport == 0) + { + renderer->AddActor (actor); + } + else if (viewport == i) // add the actor only to the specified viewport + { + renderer->AddActor (actor); + } + ++i; + } + + (*_visualizer->getShapeActorMap())["elevation_map"] = actor; + + setCloudOpacity("elevation_map", opacity); + + return true; +} +void CloudViewer::removeElevationMap() +{ + if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end()) + { + _visualizer->removeShape("elevation_map"); + } +} + void CloudViewer::addOrUpdateCoordinate( const std::string & id, const Transform & transform, @@ -2838,8 +2974,6 @@ void CloudViewer::setCameraPosition( { renderer->ResetCameraClippingRange(boundingBox); } - - _visualizer->getRenderWindow()->Render (); } void CloudViewer::updateCameraTargetPosition(const Transform & pose) @@ -2849,14 +2983,6 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose) Eigen::Affine3f m = pose.toEigen3f(); Eigen::Vector3f pos = m.translation(); - Eigen::Vector3f lastPos(0,0,0); - if(_trajectory->size()) - { - lastPos[0]=_trajectory->back().x; - lastPos[1]=_trajectory->back().y; - lastPos[2]=_trajectory->back().z; - } - _trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2])); if(_maxTrajectorySize>0) { diff --git a/guilib/src/CreateSimpleCalibrationDialog.cpp b/guilib/src/CreateSimpleCalibrationDialog.cpp index ed1086306b..20c4bfc1e6 100644 --- a/guilib/src/CreateSimpleCalibrationDialog.cpp +++ b/guilib/src/CreateSimpleCalibrationDialog.cpp @@ -175,7 +175,7 @@ void CreateSimpleCalibrationDialog::saveCalibration() ui_->doubleSpinBox_fy->value(), ui_->doubleSpinBox_cx->value(), ui_->doubleSpinBox_cy->value(), - Transform::getIdentity(), + CameraModel::opticalRotation(), 0, cv::Size(width, height)); UASSERT(modelLeft.isValidForProjection()); diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 3af6637f36..af5dd0b5aa 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -30,8 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include -#include #include #include #include @@ -172,10 +172,10 @@ bool DataRecorder::handleEvent(UEvent * event) { if(memory_) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * camEvent = (SensorEvent*)event; + if(camEvent->getCode() == SensorEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 51cb2e98e6..0e48a7acbd 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -70,7 +70,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Optimizer.h" #include "rtabmap/core/RegistrationVis.h" #include "rtabmap/core/RegistrationIcp.h" -#include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/core/global_map/OccupancyGrid.h" +#include "rtabmap/core/global_map/CloudMap.h" #include "rtabmap/core/GeodeticCoords.h" #include "rtabmap/core/Recovery.h" #include "rtabmap/gui/DataRecorder.h" @@ -82,6 +83,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/VisualWord.h" #include "rtabmap/gui/ExportDialog.h" #include "rtabmap/gui/EditConstraintDialog.h" +#include "rtabmap/gui/LinkRefiningDialog.h" #include "rtabmap/gui/ProgressDialog.h" #include "rtabmap/gui/ParametersToolBox.h" #include "rtabmap/gui/RecoveryState.h" @@ -92,9 +94,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #ifdef RTABMAP_OCTOMAP -#include "rtabmap/core/OctoMap.h" +#include "rtabmap/core/global_map/OctoMap.h" +#endif + +#ifdef RTABMAP_GRIDMAP +#include "rtabmap/core/global_map/GridMap.h" #endif namespace rtabmap { @@ -106,6 +113,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : exportDialog_(new ExportCloudsDialog(this)), editDepthDialog_(new QDialog(this)), editMapDialog_(new QDialog(this)), + linkRefiningDialog_(new LinkRefiningDialog(this)), savedMaximized_(false), firstCall_(true), iniFilePath_(ini), @@ -196,6 +204,13 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->checkBox_octomap->setChecked(false); #endif +#ifndef RTABMAP_GRIDMAP + ui_->checkBox_showElevation->setEnabled(false); + ui_->checkBox_showElevation->setChecked(false); + ui_->checkBox_grid_elevation->setEnabled(false); + ui_->checkBox_grid_elevation->setChecked(false); +#endif + ParametersMap parameters; uInsert(parameters, Parameters::getDefaultParameters("SURF")); uInsert(parameters, Parameters::getDefaultParameters("SIFT")); @@ -233,9 +248,11 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked()); ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction()); ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction()); @@ -251,6 +268,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics())); connect(ui_->dockWidget_info->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateInfo())); + connect(ui_->graphViewer, SIGNAL(nodeSelected(int)), this , SLOT(graphNodeSelected(int))); + connect(ui_->graphViewer, SIGNAL(linkSelected(int,int)), this , SLOT(graphLinkSelected(int,int))); connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &))); @@ -293,8 +312,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances())); connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances())); connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances())); - connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks())); - connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks())); + connect(ui_->actionRefine_links, SIGNAL(triggered()), this, SLOT(refineLinks())); connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps())); connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps())); connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges())); @@ -333,6 +351,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int))); connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int))); connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int))); + connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); + connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); + connect(ui_->toolButton_remove_priorA, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint())); + connect(ui_->toolButton_remove_priorB, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint())); connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView())); connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView())); @@ -346,6 +368,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView())); connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView())); connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView())); + connect(ui_->checkBox_showElevation, SIGNAL(stateChanged(int)), this, SLOT(update3dView())); connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView())); connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView())); @@ -385,10 +408,13 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); + connect(ui_->checkBox_grid_grid, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); + connect(ui_->checkBox_grid_elevation, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView())); connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView())); connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); + connect(ui_->checkBox_grid_frontiers, SIGNAL(stateChanged(int)), this, SLOT(updateGrid())); connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView())); connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView())); connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView())); @@ -412,6 +438,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified())); connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified())); connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified())); + connect(ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified())); @@ -445,12 +472,15 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified())); connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified())); connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified())); + connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified())); connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid())); connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid())); connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid())); + connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid())); connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor())); connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor())); connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor())); + connect(ui_->toolButton_frontierColor, SIGNAL(clicked(bool)), this, SLOT(selectFrontierColor())); connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); @@ -567,6 +597,7 @@ void DatabaseViewer::readSettings() ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString()); ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString()); ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString()); + ui_->lineEdit_frontierColor->setText(settings.value("colorFrontier", ui_->lineEdit_frontierColor->text()).toString()); ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt()); ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool()); settings.endGroup(); @@ -656,6 +687,7 @@ void DatabaseViewer::writeSettings() settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text()); settings.setValue("colorGround", ui_->lineEdit_groundColor->text()); settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text()); + settings.setValue("colorFrontier", ui_->lineEdit_frontierColor->text()); settings.setValue("cropRadius", ui_->spinBox_cropRadius->value()); settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked()); settings.endGroup(); @@ -717,6 +749,7 @@ void DatabaseViewer::restoreDefaultSettings() { // reset GUI parameters ui_->comboBox_logger_level->setCurrentIndex(1); + ui_->checkBox_alignPosesWithGPS->setChecked(true); ui_->checkBox_alignPosesWithGroundTruth->setChecked(true); ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false); ui_->checkBox_ignoreIntermediateNodes->setChecked(false); @@ -742,10 +775,12 @@ void DatabaseViewer::restoreDefaultSettings() ui_->doubleSpinBox_posefilteringRadius->setValue(0.1); ui_->doubleSpinBox_posefilteringAngle->setValue(30); ui_->checkBox_grid_empty->setChecked(true); + ui_->checkBox_grid_frontiers->setChecked(false); ui_->checkBox_octomap->setChecked(false); ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name()); ui_->lineEdit_groundColor->setText(QColor(Qt::green).name()); ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name()); + ui_->lineEdit_frontierColor->setText(QColor(Qt::cyan).name()); ui_->spinBox_cropRadius->setValue(1); ui_->checkBox_grid_showProbMap->setChecked(false); @@ -778,7 +813,7 @@ void DatabaseViewer::openDatabase() } } -bool DatabaseViewer::openDatabase(const QString & path) +bool DatabaseViewer::openDatabase(const QString & path, const ParametersMap & overriddenParameters) { UDEBUG("Open database \"%s\"", path.toStdString().c_str()); if(QFile::exists(path)) @@ -813,6 +848,9 @@ bool DatabaseViewer::openDatabase(const QString & path) // look if there are saved parameters ParametersMap parameters = dbDriver_->getLastParameters(); + // add overridden parameters + uInsert(parameters, overriddenParameters); + if(parameters.size()) { const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters(); @@ -913,12 +951,14 @@ bool DatabaseViewer::closeDatabase() if(refinedIter != linksRefined_.end()) { dbDriver_->addLink(refinedIter->second); - dbDriver_->addLink(refinedIter->second.inverse()); + if(refinedIter->second.from() != refinedIter->second.to()) + dbDriver_->addLink(refinedIter->second.inverse()); } else { dbDriver_->addLink(iter->second); - dbDriver_->addLink(iter->second.inverse()); + if(iter->second.from() != iter->second.to()) + dbDriver_->addLink(iter->second.inverse()); } } @@ -928,7 +968,8 @@ bool DatabaseViewer::closeDatabase() if(!containsLink(linksAdded_, iter->second.from(), iter->second.to())) { dbDriver_->updateLink(iter->second); - dbDriver_->updateLink(iter->second.inverse()); + if(iter->second.from() != iter->second.to()) + dbDriver_->updateLink(iter->second.inverse()); } } @@ -936,7 +977,8 @@ bool DatabaseViewer::closeDatabase() for(std::multimap::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter) { dbDriver_->removeLink(iter->second.to(), iter->second.from()); - dbDriver_->removeLink(iter->second.from(), iter->second.to()); + if(iter->second.from() != iter->second.to()) + dbDriver_->removeLink(iter->second.from(), iter->second.to()); } linksAdded_.clear(); linksRefined_.clear(); @@ -972,24 +1014,20 @@ bool DatabaseViewer::closeDatabase() if(button == QMessageBox::Yes) { - UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size()); - std::map, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin(); - std::map >::iterator infoIter = generatedLocalMapsInfo_.begin(); - for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter) + for(std::map::const_iterator mapIter = generatedLocalMaps_.localGrids().begin(); + mapIter!=generatedLocalMaps_.localGrids().end(); + ++mapIter) { - UASSERT(mapIter->first == infoIter->first); dbDriver_->updateOccupancyGrid( mapIter->first, - mapIter->second.first.first, - mapIter->second.first.second, - mapIter->second.second, - infoIter->second.first, - infoIter->second.second); + mapIter->second.groundCells, + mapIter->second.obstacleCells, + mapIter->second.emptyCells, + mapIter->second.cellSize, + mapIter->second.viewPoint); } generatedLocalMaps_.clear(); - generatedLocalMapsInfo_.clear(); localMaps_.clear(); - localMapsInfo_.clear(); // This will force rtabmap_ros to regenerate the global occupancy grid if there was one dbDriver_->save2DMap(cv::Mat(), 0, 0, 0); @@ -1047,9 +1085,7 @@ bool DatabaseViewer::closeDatabase() linksRefined_.clear(); linksRemoved_.clear(); localMaps_.clear(); - localMapsInfo_.clear(); generatedLocalMaps_.clear(); - generatedLocalMapsInfo_.clear(); modifiedLaserScans_.clear(); ui_->graphViewer->clearAll(); occupancyGridViewer_->clear(); @@ -1070,14 +1106,19 @@ bool DatabaseViewer::closeDatabase() ui_->checkBox_showOptimized->setEnabled(false); ui_->toolBox_statistics->clear(); databaseFileName_.clear(); + ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); ui_->label_rmse_title->setVisible(false); ui_->checkBox_ignoreIntermediateNodes->setVisible(false); ui_->label_ignoreINtermediateNdoes->setVisible(false); + ui_->label_alignPosesWithGPS->setVisible(false); ui_->label_alignPosesWithGroundTruth->setVisible(false); ui_->label_alignScansCloudsWithGroundTruth->setVisible(false); ui_->label_optimizeFrom->setText(tr("Root")); @@ -1136,6 +1177,10 @@ bool DatabaseViewer::closeDatabase() stereoViewer_->refreshView(); ui_->toolBox_statistics->clear(); + + // This will re-init the dialog + delete linkRefiningDialog_; + linkRefiningDialog_ = new LinkRefiningDialog(this); } ui_->actionClose_database->setEnabled(dbDriver_ != 0); @@ -1554,13 +1599,13 @@ void DatabaseViewer::extractImages() StereoCameraModel model( cameraName, data.imageRaw().size(), - data.stereoCameraModels()[i].left().K(), - data.stereoCameraModels()[i].left().D(), + data.stereoCameraModels()[i].left().K_raw(), + data.stereoCameraModels()[i].left().D_raw(), data.stereoCameraModels()[i].left().R(), data.stereoCameraModels()[i].left().P(), data.rightRaw().size(), - data.stereoCameraModels()[i].right().K(), - data.stereoCameraModels()[i].right().D(), + data.stereoCameraModels()[i].right().K_raw(), + data.stereoCameraModels()[i].right().D_raw(), data.stereoCameraModels()[i].right().R(), data.stereoCameraModels()[i].right().P(), data.stereoCameraModels()[i].R(), @@ -1612,8 +1657,8 @@ void DatabaseViewer::extractImages() } CameraModel model(cameraName, data.imageRaw().size(), - data.cameraModels()[i].K(), - data.cameraModels()[i].D(), + data.cameraModels()[i].K_raw(), + data.cameraModels()[i].D_raw(), data.cameraModels()[i].R(), data.cameraModels()[i].P(), data.cameraModels()[i].localTransform()); @@ -1701,16 +1746,25 @@ void DatabaseViewer::updateIds() gpsValues_.clear(); lastSliderIndexBrowsed_ = 0; ui_->checkBox_wmState->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); ui_->label_rmse_title->setVisible(false); ui_->checkBox_ignoreIntermediateNodes->setVisible(false); ui_->label_ignoreINtermediateNdoes->setVisible(false); + ui_->label_alignPosesWithGPS->setVisible(false); ui_->label_alignPosesWithGroundTruth->setVisible(false); ui_->label_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->toolButton_edit_priorA->setVisible(false); + ui_->toolButton_edit_priorB->setVisible(false); + ui_->toolButton_remove_priorA->setVisible(false); + ui_->toolButton_remove_priorB->setVisible(false); ui_->menuEdit->setEnabled(true); ui_->actionGenerate_3D_map_pcd->setEnabled(true); ui_->actionExport->setEnabled(true); @@ -1891,26 +1945,21 @@ void DatabaseViewer::updateIds() uSleep(100); QApplication::processEvents(); - if(!groundTruthPoses_.empty() || !gpsPoses_.empty()) - { - ui_->checkBox_alignPosesWithGroundTruth->setVisible(true); - ui_->doubleSpinBox_optimizationScale->setVisible(true); - ui_->label_scale_title->setVisible(true); - ui_->label_rmse->setVisible(true); - ui_->label_rmse_title->setVisible(true); - ui_->label_alignPosesWithGroundTruth->setVisible(true); + ui_->doubleSpinBox_optimizationScale->setVisible(!groundTruthPoses_.empty()); + ui_->label_scale_title->setVisible(!groundTruthPoses_.empty()); + ui_->label_rmse->setVisible(!groundTruthPoses_.empty()); + ui_->label_rmse_title->setVisible(!groundTruthPoses_.empty()); + + ui_->checkBox_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); + ui_->checkBox_alignPosesWithGPS->setEnabled(!gpsPoses_.empty()); + ui_->label_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); + ui_->checkBox_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); + ui_->label_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); + ui_->label_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); - if(!groundTruthPoses_.empty()) - { - ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth")); - ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(true); - ui_->label_alignScansCloudsWithGroundTruth->setVisible(true); - } - else - { - ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS")); - } - } if(!gpsValues_.empty()) { ui_->menuExport_GPS->setEnabled(true); @@ -2325,6 +2374,14 @@ void DatabaseViewer::selectEmptyColor() ui_->lineEdit_emptyColor->setText(c.name()); } } +void DatabaseViewer::selectFrontierColor() +{ + QColor c = QColorDialog::getColor(ui_->lineEdit_frontierColor->text(), this); + if(c.isValid()) + { + ui_->lineEdit_frontierColor->setText(c.name()); + } +} void DatabaseViewer::editDepthImage() { if(dbDriver_ && ids_.size()) @@ -2553,10 +2610,12 @@ void DatabaseViewer::exportPoses(int format) optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); } - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked()) + if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) { std::map refPoses = groundTruthPoses_; - if(refPoses.empty()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && + ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -2593,7 +2652,7 @@ void DatabaseViewer::exportPoses(int format) rotational_min, rotational_max); - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity()) + if(!gtToMap.isIdentity()) { for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) { @@ -2711,14 +2770,24 @@ void DatabaseViewer::exportPoses(int format) } } - if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported + if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported { - UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format); - std::map::iterator iter=poses.begin(); - while(iter!=poses.end() && iter->first < 0) + UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format); + for(std::map::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;) { poses.erase(iter++); } + for(std::multimap::iterator iter=links.begin(); iter!=links.end();) + { + if(iter->second.from() < 0 || iter->second.to() < 0) + { + links.erase(iter++); + } + else + { + ++iter; + } + } } std::map stamps; @@ -2726,17 +2795,24 @@ void DatabaseViewer::exportPoses(int format) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - Transform p, g; - int w; - std::string l; - double stamp=0.0; - int mapId; - std::vector v; - GPS gps; - EnvSensors sensors; - if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors)) + if(iter->first<0 && format == 11) // in case of landmarks + { + stamps.insert(std::make_pair(iter->first, 0)); + } + else { - stamps.insert(std::make_pair(iter->first, stamp)); + Transform p, g; + int w; + std::string l; + double stamp=0.0; + int mapId; + std::vector v; + GPS gps; + EnvSensors sensors; + if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors)) + { + stamps.insert(std::make_pair(iter->first, stamp)); + } } } if(stamps.size()!=poses.size()) @@ -2958,9 +3034,9 @@ void DatabaseViewer::editSaved2DMap() LaserScan scan; data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty); - if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end()) + if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end()) { - gridObstacles = generatedLocalMaps_.find(iter->first)->second.first.second; + gridObstacles = generatedLocalMaps_.localGrids().at(iter->first).obstacleCells; } if(!gridObstacles.empty()) { @@ -3008,19 +3084,19 @@ void DatabaseViewer::editSaved2DMap() UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi); // update - std::pair, cv::Mat> value; - if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end()) + cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)); + if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end()) { - value = generatedLocalMaps_.at(iter->first); + LocalGrid value = generatedLocalMaps_.localGrids().at(iter->first); + value.obstacleCells = newObstacles; + generatedLocalMaps_.add(iter->first, value); } else { - value.first.first = gridGround; - value.second = gridEmpty; - uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint()))); + LocalGrid value(gridGround, newObstacles, gridEmpty, data.gridCellSize(), data.gridViewPoint()); + generatedLocalMaps_.add(iter->first, value); } - value.first.second = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)); - uInsert(generatedLocalMaps_, std::make_pair(iter->first, value)); + } } @@ -3163,7 +3239,7 @@ void DatabaseViewer::exportSaved2DMap() file << "free_thresh: 0.196" << std::endl; file << std::endl; file.close(); - + QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml)); } @@ -3281,19 +3357,7 @@ void DatabaseViewer::regenerateSavedMap() if(type.compare("From OctoMap projection") == 0) { //create local octomap - OctoMap octomap(ui_->parameters_toolbox->getParameters()); - for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter) - { - if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2) - { - QMessageBox::warning(this, tr(""), - tr("Some local occupancy grids are 2D, but OctoMap requires 3D local " - "occupancy grids. Select default occupancy grid or generate " - "3D local occupancy grids (\"Grid/3D\" core parameter).")); - return; - } - octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second); - } + OctoMap octomap(&localMaps_, ui_->parameters_toolbox->getParameters()); octomap.update(graphes_.back()); map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0); @@ -3301,11 +3365,7 @@ void DatabaseViewer::regenerateSavedMap() else #endif { - OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); - for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter) - { - grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second); - } + OccupancyGrid grid(&localMaps_, ui_->parameters_toolbox->getParameters()); grid.update(graphes_.back()); map = grid.getMap(xMin, yMin); } @@ -3520,7 +3580,9 @@ void DatabaseViewer::updateOptimizedMesh() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() + && !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -3735,10 +3797,9 @@ void DatabaseViewer::generateLocalGraph() void DatabaseViewer::regenerateLocalMaps() { - OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); + LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters()); generatedLocalMaps_.clear(); - generatedLocalMapsInfo_.clear(); rtabmap::ProgressDialog progressDialog(this); progressDialog.setMaximumSteps(ids_.size()); @@ -3843,17 +3904,16 @@ void DatabaseViewer::regenerateLocalMaps() } } - grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint); + localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint); } } else { - grid.createLocalMap(s, ground, obstacles, empty, viewpoint); + localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint); } gridCreationTime = timer.ticks()*1000.0; - uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty))); - uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint))); + generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint); msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size()); totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols); @@ -3889,7 +3949,7 @@ void DatabaseViewer::regenerateLocalMaps() void DatabaseViewer::regenerateCurrentLocalMaps() { UTimer time; - OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); + LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters()); if(ids_.size() == 0) { @@ -3912,9 +3972,6 @@ void DatabaseViewer::regenerateCurrentLocalMaps() for(int i =0; igetNodeData(ids.at(i), data); data.uncompressData(); @@ -3987,17 +4044,15 @@ void DatabaseViewer::regenerateCurrentLocalMaps() } } - grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint); + localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint); } } else { - grid.createLocalMap(s, ground, obstacles, empty, viewpoint); + localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint); } - - uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty))); - uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint))); + generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint); msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks()); } @@ -4037,7 +4092,9 @@ void DatabaseViewer::view3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4088,7 +4145,9 @@ void DatabaseViewer::generate3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4262,7 +4321,7 @@ void DatabaseViewer::detectMoreLoopClosures() void DatabaseViewer::updateAllNeighborCovariances() { - updateAllCovariances(neighborLinks_); + updateCovariances(neighborLinks_); } void DatabaseViewer::updateAllLoopClosureCovariances() { @@ -4274,7 +4333,7 @@ void DatabaseViewer::updateAllLoopClosureCovariances() links.push_back(loopLinks_.at(i)); } } - updateAllCovariances(links); + updateCovariances(links); } void DatabaseViewer::updateAllLandmarkCovariances() { @@ -4286,10 +4345,10 @@ void DatabaseViewer::updateAllLandmarkCovariances() links.push_back(loopLinks_.at(i)); } } - updateAllCovariances(links); + updateCovariances(links); } -void DatabaseViewer::updateAllCovariances(const QList & links) +void DatabaseViewer::updateCovariances(const QList & links) { if(links.size()) { @@ -4377,15 +4436,79 @@ void DatabaseViewer::updateAllCovariances(const QList & links) } } -void DatabaseViewer::refineAllNeighborLinks() -{ - refineAllLinks(neighborLinks_); -} -void DatabaseViewer::refineAllLoopClosureLinks() +void DatabaseViewer::refineLinks() { - refineAllLinks(loopLinks_); + int minNodeId = 0; + int maxNodeId = 0; + int minMapId = 0; + int maxMapId = 0; + std::multimap allLinks = updateLinksWithModifications(links_); + for(std::multimap::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter) + { + int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from(); + int maxId = iter->second.from()second.to()?iter->second.to():iter->second.from(); + if(minNodeId == 0 || minNodeId > minId) + { + minNodeId = minId; + } + if(maxNodeId == 0 || maxNodeId < maxId) + { + maxNodeId = maxId; + } + } + if(minNodeId > 0) + { + minMapId = uValue(mapIds_, minNodeId, 0); + maxMapId = uValue(mapIds_, maxNodeId, minMapId); + + linkRefiningDialog_->setMinMax( + minNodeId, + maxNodeId, + minMapId, + maxMapId); + + if(linkRefiningDialog_->exec() == QDialog::Accepted) + { + QList links; + Link::Type type = linkRefiningDialog_->getLinkType(); + linkRefiningDialog_->getRangeNodeId(minNodeId, maxNodeId); + linkRefiningDialog_->getRangeNodeId(minMapId, maxMapId); + bool intra, inter; + linkRefiningDialog_->getIntraInterSessions(intra, inter); + for(std::multimap::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter) + { + if(type==Link::kEnd || type == iter->second.type()) + { + int from = iter->second.from(); + int to = iter->second.to(); + int mapFrom = uValue(mapIds_, from, 0); + int mapTo = uValue(mapIds_, to, 0); + if(((linkRefiningDialog_->isRangeByNodeId() && + ((from >= minNodeId && from <= maxNodeId) || + (to >= minNodeId && to <= maxNodeId))) || + (linkRefiningDialog_->isRangeByMapId() && + ((mapFrom >= minMapId && mapFrom <= maxMapId) || + (mapTo >= minMapId && mapTo <= maxMapId)))) && + ((intra && mapTo == mapFrom) || + (inter && mapTo != mapFrom))) + { + links.push_back(iter->second); + } + } + } + if(links.isEmpty()) + { + QMessageBox::warning(this, tr("Refine links"), tr("No links found matching the requested parameters.")); + return; + } + else + { + refineLinks(links); + } + } + } } -void DatabaseViewer::refineAllLinks(const QList & links) +void DatabaseViewer::refineLinks(const QList & links) { if(links.size()) { @@ -4435,13 +4558,33 @@ void DatabaseViewer::resetAllChanges() linksRefined_.clear(); linksRemoved_.clear(); generatedLocalMaps_.clear(); - generatedLocalMapsInfo_.clear(); modifiedLaserScans_.clear(); updateLoopClosuresSlider(); this->updateGraphView(); } } +void DatabaseViewer::graphNodeSelected(int id) +{ + if(id>0 && idToIndex_.contains(id)) + { + static bool updateA = true; + if(updateA) + ui_->horizontalSlider_A->setValue(idToIndex_.value(id)); + else + ui_->horizontalSlider_B->setValue(idToIndex_.value(id)); + updateA = !updateA; + } +} + +void DatabaseViewer::graphLinkSelected(int from, int to) +{ + if(from>0 && idToIndex_.contains(from)) + ui_->horizontalSlider_A->setValue(idToIndex_.value(from)); + if(to>0 && idToIndex_.contains(to)) + ui_->horizontalSlider_B->setValue(idToIndex_.value(to)); +} + void DatabaseViewer::sliderAValueChanged(int value) { this->update(value, @@ -4461,6 +4604,8 @@ void DatabaseViewer::sliderAValueChanged(int value) ui_->label_scanA, ui_->label_gravityA, ui_->label_priorA, + ui_->toolButton_edit_priorA, + ui_->toolButton_remove_priorA, ui_->label_gpsA, ui_->label_gtA, ui_->label_sensorsA, @@ -4486,6 +4631,8 @@ void DatabaseViewer::sliderBValueChanged(int value) ui_->label_scanB, ui_->label_gravityB, ui_->label_priorB, + ui_->toolButton_edit_priorB, + ui_->toolButton_remove_priorB, ui_->label_gpsB, ui_->label_gtB, ui_->label_sensorsB, @@ -4509,6 +4656,8 @@ void DatabaseViewer::update(int value, QLabel * labelScan, QLabel * labelGravity, QLabel * labelPrior, + QToolButton * editPriorButton, + QToolButton * removePriorButton, QLabel * labelGps, QLabel * labelGt, QLabel * labelSensors, @@ -4531,6 +4680,8 @@ void DatabaseViewer::update(int value, labelScan ->clear(); labelGravity->clear(); labelPrior->clear(); + editPriorButton->setVisible(false); + removePriorButton->setVisible(false); labelGps->clear(); labelGt->clear(); labelSensors->clear(); @@ -4580,7 +4731,12 @@ void DatabaseViewer::update(int value, QRectF rect; if(!img.isNull()) { - view->setImage(img); + Transform pose; + if(!graphes_.empty() && graphes_.back().find(data.id())!=graphes_.back().end()) + { + pose = graphes_.back().at(data.id()); + } + view->setImage(img, data.cameraModels(), pose); rect = img.rect(); } else @@ -4668,15 +4824,25 @@ void DatabaseViewer::update(int value, labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw)); } - std::multimap priorLink; - dbDriver_->loadLinks(id, priorLink, Link::kPosePrior); - if(!priorLink.empty()) + std::multimap links = updateLinksWithModifications(links_); + if(graph::findLink(links, id, id, false, Link::kPosePrior)!=links.end()) { - priorLink.begin()->second.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); + Link & priorLink = graph::findLink(links, id, id, false, Link::kPosePrior)->second; + priorLink.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw)); std::stringstream out; - out << priorLink.begin()->second.infMatrix().inv(); + out << priorLink.infMatrix().inv(); labelPrior->setToolTip(QString("%1").arg(out.str().c_str())); + removePriorButton->setVisible(true); + editPriorButton->setToolTip(tr("Edit Prior")); + editPriorButton->setText("..."); + editPriorButton->setVisible(true); + } + else if(!odomPose.isNull()) + { + editPriorButton->setToolTip(tr("Add Prior")); + editPriorButton->setText("+"); + editPriorButton->setVisible(true); } if(gps.stamp()>0.0) @@ -4861,15 +5027,10 @@ void DatabaseViewer::update(int value, { cloudViewer_->removeAllLines(); cloudViewer_->removeAllFrustums(); - cloudViewer_->removeCloud("mesh"); - cloudViewer_->removeCloud("cloud"); - cloudViewer_->removeCloud("scan"); - cloudViewer_->removeCloud("map"); - cloudViewer_->removeCloud("ground"); - cloudViewer_->removeCloud("obstacles"); - cloudViewer_->removeCloud("empty_cells"); - cloudViewer_->removeCloud("words"); + cloudViewer_->removeOccupancyGridMap(); + cloudViewer_->removeAllClouds(); cloudViewer_->removeOctomap(); + cloudViewer_->removeElevationMap(); Transform pose = Transform::getIdentity(); if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked()) @@ -5032,8 +5193,8 @@ void DatabaseViewer::update(int value, { if(!data.imageRaw().empty()) { - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr indices(new std::vector); + std::vector::Ptr> clouds; + std::vector allIndices; if(!data.depthRaw().empty() && data.cameraModels().size()==1) { cv::Mat depth = data.depthRaw(); @@ -5041,96 +5202,110 @@ void DatabaseViewer::update(int value, { depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f); } - cloud = util3d::cloudFromDepthRGB( + pcl::IndicesPtr indices(new std::vector); + pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB( data.imageRaw(), depth, data.cameraModels()[0], ui_->spinBox_decimation->value(),0,0,indices.get()); if(indices->size()) { - cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()); + clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform())); + allIndices.push_back(indices); } - } else { - cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters()); + clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters()); } - if(indices->size()) + UASSERT(clouds.size() == allIndices.size()); + for(size_t i=0; idoubleSpinBox_voxelSize->value() > 0.0) - { - cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value()); - } - - if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense) + if(allIndices[i]->size()) { - Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f); - if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull()) - { - viewpoint[0] = data.cameraModels()[0].localTransform().x(); - viewpoint[1] = data.cameraModels()[0].localTransform().y(); - viewpoint[2] = data.cameraModels()[0].localTransform().z(); - } - else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull()) + pcl::PointCloud::Ptr cloud = clouds[i]; + pcl::IndicesPtr indices = allIndices[i]; + if(ui_->doubleSpinBox_voxelSize->value() > 0.0) { - viewpoint[0] = data.stereoCameraModels()[0].localTransform().x(); - viewpoint[1] = data.stereoCameraModels()[0].localTransform().y(); - viewpoint[2] = data.stereoCameraModels()[0].localTransform().z(); + cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value()); } - std::vector polygons = util3d::organizedFastMesh( - cloud, - float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f, - ui_->checkBox_mesh_quad->isChecked(), - ui_->spinBox_mesh_triangleSize->value(), - viewpoint); - - if(ui_->spinBox_mesh_minClusterSize->value()) + + if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense) { - // filter polygons - std::vector > neighbors; - std::vector > vertexToPolygons; - util3d::createPolygonIndexes(polygons, - cloud->size(), - neighbors, - vertexToPolygons); - std::list > clusters = util3d::clusterPolygons( - neighbors, - ui_->spinBox_mesh_minClusterSize->value()); - std::vector filteredPolygons(polygons.size()); - int oi=0; - for(std::list >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) + Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f); + if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull()) + { + viewpoint[0] = data.cameraModels()[0].localTransform().x(); + viewpoint[1] = data.cameraModels()[0].localTransform().y(); + viewpoint[2] = data.cameraModels()[0].localTransform().z(); + } + else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull()) + { + viewpoint[0] = data.stereoCameraModels()[0].localTransform().x(); + viewpoint[1] = data.stereoCameraModels()[0].localTransform().y(); + viewpoint[2] = data.stereoCameraModels()[0].localTransform().z(); + } + std::vector polygons = util3d::organizedFastMesh( + cloud, + float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f, + ui_->checkBox_mesh_quad->isChecked(), + ui_->spinBox_mesh_triangleSize->value(), + viewpoint); + + if(ui_->spinBox_mesh_minClusterSize->value()) { - for(std::list::iterator jter=iter->begin(); jter!=iter->end(); ++jter) + // filter polygons + std::vector > neighbors; + std::vector > vertexToPolygons; + util3d::createPolygonIndexes(polygons, + cloud->size(), + neighbors, + vertexToPolygons); + std::list > clusters = util3d::clusterPolygons( + neighbors, + ui_->spinBox_mesh_minClusterSize->value()); + std::vector filteredPolygons(polygons.size()); + int oi=0; + for(std::list >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) { - filteredPolygons[oi++] = polygons.at(*jter); + for(std::list::iterator jter=iter->begin(); jter!=iter->end(); ++jter) + { + filteredPolygons[oi++] = polygons.at(*jter); + } } + filteredPolygons.resize(oi); + polygons = filteredPolygons; } - filteredPolygons.resize(oi); - polygons = filteredPolygons; - } - cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose); - } - if(ui_->checkBox_showCloud->isChecked()) - { - cloudViewer_->addCloud("cloud", cloud, pose); + cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose); + } + if(ui_->checkBox_showCloud->isChecked()) + { + cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose); + } } } } else if(ui_->checkBox_showCloud->isChecked()) { - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr indices(new std::vector); - cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters()); - if(indices->size()) + std::vector::Ptr> clouds; + std::vector allIndices; + + clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters()); + UASSERT(clouds.size() == allIndices.size()); + for(size_t i=0; idoubleSpinBox_voxelSize->value() > 0.0) + if(allIndices[i]->size()) { - cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value()); - } + pcl::PointCloud::Ptr cloud = clouds[i]; + pcl::IndicesPtr indices = allIndices[i]; + if(ui_->doubleSpinBox_voxelSize->value() > 0.0) + { + cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value()); + } - cloudViewer_->addCloud("cloud", cloud, pose); + cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose); + } } } } @@ -5177,21 +5352,20 @@ void DatabaseViewer::update(int value, } //add occupancy grid - if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked()) + if(ui_->checkBox_showMap->isChecked() || + ui_->checkBox_showGrid->isChecked() || + ui_->checkBox_showElevation->checkState() != Qt::Unchecked) { - std::map, cv::Mat> > localMaps; - std::map > localMapsInfo; - if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end()) + LocalGridCache combinedLocalMaps; + if(generatedLocalMaps_.shareTo(data.id(), combinedLocalMaps)) { - localMaps.insert(*generatedLocalMaps_.find(data.id())); - localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id())); + // add local grid } else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty()) { - localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw()))); - localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint()))); + combinedLocalMaps.add(data.id(), data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint()); } - if(!localMaps.empty()) + if(!combinedLocalMaps.empty()) { std::map poses; poses.insert(std::make_pair(data.id(), pose)); @@ -5199,29 +5373,26 @@ void DatabaseViewer::update(int value, #ifdef RTABMAP_OCTOMAP OctoMap * octomap = 0; if(ui_->checkBox_octomap->isChecked() && - (!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) && - (localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) && - (localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) && - (localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) && - localMapsInfo.begin()->second.first > 0.0f) + (!combinedLocalMaps.localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.localGrids().begin()->second.obstacleCells.empty()) && + combinedLocalMaps.localGrids().begin()->second.is3D() && + combinedLocalMaps.localGrids().begin()->second.cellSize > 0.0f) { //create local octomap ParametersMap params; - params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(localMapsInfo.begin()->second.first))); - octomap = new OctoMap(params); - octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second); + params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(combinedLocalMaps.localGrids().begin()->second.cellSize))); + octomap = new OctoMap(&combinedLocalMaps, params); octomap->update(poses); } #endif - + ParametersMap parameters = ui_->parameters_toolbox->getParameters(); if(ui_->checkBox_showMap->isChecked()) { float xMin=0.0f, yMin=0.0f; cv::Mat map8S; - ParametersMap parameters = ui_->parameters_toolbox->getParameters(); float gridCellSize = Parameters::defaultGridCellSize(); Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize); parameters = Parameters::filterParameters(parameters, "GridGlobal", true); + #ifdef RTABMAP_OCTOMAP if(octomap) { @@ -5230,9 +5401,7 @@ void DatabaseViewer::update(int value, else #endif { - OccupancyGrid grid(parameters); - grid.setCellSize(gridCellSize); - grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second); + OccupancyGrid grid(&combinedLocalMaps, parameters); grid.update(poses); map8S = grid.getMap(xMin, yMin); } @@ -5297,7 +5466,7 @@ void DatabaseViewer::update(int value, #endif { // occupancy cloud - LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first); + LaserScan scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.groundCells); if(scan.hasRGB()) { cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text())); @@ -5306,7 +5475,7 @@ void DatabaseViewer::update(int value, { cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text())); } - scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.second); + scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.obstacleCells); if(scan.hasRGB()) { cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text())); @@ -5322,7 +5491,7 @@ void DatabaseViewer::update(int value, if(ui_->checkBox_grid_empty->isChecked()) { cloudViewer_->addCloud("empty_cells", - util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)), + util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.emptyCells)), pose, QColor(ui_->lineEdit_emptyColor->text())); cloudViewer_->setCloudPointSize("empty_cells", 5); @@ -5336,6 +5505,34 @@ void DatabaseViewer::update(int value, delete octomap; } #endif + +#ifdef RTABMAP_GRIDMAP + if(ui_->checkBox_showElevation->checkState() != Qt::Unchecked) // Show elevation map? + { + GridMap gridMap(&combinedLocalMaps, parameters); + + if(combinedLocalMaps.localGrids().begin()->second.is3D()) + { + gridMap.update(poses); + if(ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked) + { + float xMin, yMin, gridCellSize; + cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, gridCellSize); + cloudViewer_->addElevationMap(elevationMap, gridCellSize, xMin, yMin, 1.0f); + } + else + { + pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh(); + cloudViewer_->addCloudMesh("elevation_mesh", mesh); + } + cloudViewer_->refreshView(); + } + else + { + UWARN("Local grid is not 3D, cannot generate an elevation map"); + } + } +#endif } } cloudViewer_->updateCameraTargetPosition(pose); @@ -5824,16 +6021,38 @@ void DatabaseViewer::editConstraint() { if(ids_.size()) { - Link link; - if(ui_->label_type->text().toInt() == Link::kLandmark) + Link link(0,0,Link::kUndef, Transform::getIdentity()); + int priorId = sender() == ui_->toolButton_edit_priorA?ids_.at(ui_->horizontalSlider_A->value()): + sender() == ui_->toolButton_edit_priorB?ids_.at(ui_->horizontalSlider_B->value()):0; + if(priorId>0) + { + // Prior + std::multimap links = updateLinksWithModifications(links_); + if(graph::findLink(links, priorId, priorId, false, Link::kPosePrior) != links.end()) + { + link = graph::findLink(links, priorId, priorId, false, Link::kPosePrior)->second; + } + else if(odomPoses_.find(priorId) != odomPoses_.end()) + { + // fallback to odom pose + // set undef to go in "add" branch below + link = Link(priorId, priorId, Link::kUndef, odomPoses_.at(priorId)); + } + else + { + QMessageBox::warning(this, tr(""), tr("Node %1 doesn't have odometry pose, cannot add a prior for it!").arg(priorId)); + return; + } + } + else if(ui_->label_type->text().toInt() == Link::kLandmark) { - int position = ui_->horizontalSlider_loops->value(); - link = loopLinks_.at(position); + link = loopLinks_.at(ui_->horizontalSlider_loops->value()); } else { link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value())); } + bool updated = false; if(link.isValid()) { cv::Mat covBefore = link.infMatrix().inv(); @@ -5842,7 +6061,6 @@ void DatabaseViewer::editConstraint() covBefore.at(5,5)<9999.0?std::sqrt(covBefore.at(5,5)):0.0); if(dialog.exec() == QDialog::Accepted) { - bool updated = false; cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); if(dialog.getLinearVariance()>0) { @@ -5878,7 +6096,7 @@ void DatabaseViewer::editConstraint() linksRefined_.insert(std::make_pair(newLink.from(), newLink)); updated = true; } - if(updated) + if(priorId==0) { this->updateGraphView(); updateConstraintView(); @@ -5887,7 +6105,10 @@ void DatabaseViewer::editConstraint() } else { - EditConstraintDialog dialog(Transform::getIdentity()); + EditConstraintDialog dialog( + link.transform(), + priorId>0?0.001:1, + priorId>0?0.001:1); if(dialog.exec() == QDialog::Accepted) { cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); @@ -5907,12 +6128,12 @@ void DatabaseViewer::editConstraint() { covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9; } - int from = ids_.at(ui_->horizontalSlider_A->value()); - int to = ids_.at(ui_->horizontalSlider_B->value()); + int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value()); + int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value()); Link newLink( from, to, - Link::kUserClosure, + priorId>0?Link::kPosePrior:Link::kUserClosure, dialog.getTransform(), covariance.inv()); if(newLink.from() < newLink.to()) @@ -5920,9 +6141,45 @@ void DatabaseViewer::editConstraint() newLink = newLink.inverse(); } linksAdded_.insert(std::make_pair(newLink.from(), newLink)); + updated = true; + if(priorId==0) + { + this->updateGraphView(); + updateLoopClosuresSlider(from, to); + } + } + } + + if(updated && priorId>0) + { + bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(); + Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored); + if(priorsIgnored) + { + if(QMessageBox::question(this, + tr("Updating Prior"), + tr("Parameter %1 is true, do you want to turn it off to update the graph with the updated prior?").arg(Parameters::kOptimizerPriorsIgnored().c_str()), + QMessageBox::Yes | QMessageBox::No, + QMessageBox::Yes) == QMessageBox::Yes) + { + priorsIgnored = false; + ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false"); + } + } + int indexA = ui_->horizontalSlider_A->value(); + int indexB = ui_->horizontalSlider_B->value(); + if(!priorsIgnored) + { this->updateGraphView(); - updateLoopClosuresSlider(from, to); } + if(ui_->horizontalSlider_A->value() != indexA) + ui_->horizontalSlider_A->setValue(indexA); + else + sliderAValueChanged(indexA); + if(ui_->horizontalSlider_B->value() != indexB) + ui_->horizontalSlider_B->setValue(indexB); + else + sliderBValueChanged(indexB); } } } @@ -6081,6 +6338,8 @@ void DatabaseViewer::updateConstraintView( ui_->label_scanA, ui_->label_gravityA, ui_->label_priorA, + ui_->toolButton_edit_priorA, + ui_->toolButton_remove_priorA, ui_->label_gpsA, ui_->label_gtA, ui_->label_sensorsA, @@ -6104,6 +6363,8 @@ void DatabaseViewer::updateConstraintView( ui_->label_scanB, ui_->label_gravityB, ui_->label_priorB, + ui_->toolButton_edit_priorB, + ui_->toolButton_remove_priorB, ui_->label_gpsB, ui_->label_gtB, ui_->label_sensorsB, @@ -6666,7 +6927,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) std::map graph = uValueAt(graphes_, value); std::map refPoses = groundTruthPoses_; - if(refPoses.empty()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -6730,7 +6991,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) UINFO("rotational_min=%f", rotational_min); UINFO("rotational_max=%f", rotational_max); - if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity()) + if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) && + !gtToMap.isIdentity()) { for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) { @@ -6740,7 +7003,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } std::map graphFiltered; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { graphFiltered = groundTruthPoses_; } @@ -6754,14 +7019,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) ui_->doubleSpinBox_posefilteringRadius->value(), ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0); } - std::map, cv::Mat> > localMaps; - std::map > localMapsInfo; + LocalGridCache combinedLocalMaps; #ifdef RTABMAP_OCTOMAP - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } + delete octomap_; + octomap_ = 0; #endif if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) { @@ -6770,48 +7031,28 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) std::vector ids = uKeys(graphFiltered); for(unsigned int i=0; isecond.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty()) - { - localMaps.insert(*localMaps_.find(ids.at(i))); - localMapsInfo.insert(*localMapsInfo_.find(ids[i])); - } + // Added to combined maps } else if(ids.at(i)>0) { SensorData data; dbDriver_->getNodeData(ids.at(i), data, false, false, false); cv::Mat ground, obstacles, empty; - data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty); - localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty))); - localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint()))); + if(data.gridCellSize()>0.0f) + { + data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty); + } + localMaps_.add(ids.at(i), ground, obstacles, empty, data.gridCellSize()>0.0f?data.gridCellSize():Parameters::defaultGridCellSize(), data.gridViewPoint()); if(!ground.empty() || !obstacles.empty()) { - localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty))); - localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint()))); + localMaps_.shareTo(ids.at(i), combinedLocalMaps); } } } - //cleanup - for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();) - { - if(graphFiltered.find(iter->first) == graphFiltered.end()) - { - localMapsInfo_.erase(iter->first); - localMaps_.erase(iter++); - } - else - { - ++iter; - } - } - UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size()); + UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)combinedLocalMaps.size(), (int)graph.size()); } ParametersMap parameters = ui_->parameters_toolbox->getParameters(); @@ -6844,7 +7085,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) QGraphicsRectItem * rectScaleItem = 0; ui_->graphViewer->clearMap(); occupancyGridViewer_->clear(); - if(graph.size() && localMaps.size() && + if(graph.size() && combinedLocalMaps.size() && (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible())) { QElapsedTimer time; @@ -6853,28 +7094,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) #ifdef RTABMAP_OCTOMAP if(ui_->checkBox_octomap->isChecked()) { - octomap_ = new OctoMap(parameters); - bool updateAborted = false; - for(std::map, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter) - { - if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2) - { - QMessageBox::warning(this, tr(""), - tr("Some local occupancy grids are 2D, but OctoMap requires 3D local " - "occupancy grids. Uncheck OctoMap under GUI parameters or generate " - "3D local occupancy grids (\"Grid/3D\" core parameter).")); - updateAborted = true; - break; - } - octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second); - } - if(!updateAborted) - { - octomap_->update(graphFiltered); - } + octomap_ = new OctoMap(&combinedLocalMaps, parameters); + octomap_->update(graphFiltered); } #endif - // Generate 2d grid map? if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) || (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())) @@ -6896,12 +7119,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) { uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true")); } - OccupancyGrid grid(parameters); - grid.setCellSize(cellSize); - for(std::map, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter) - { - grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second); - } + OccupancyGrid grid(&combinedLocalMaps, parameters); grid.update(graphFiltered); if(ui_->checkBox_grid_showProbMap->isChecked()) { @@ -6995,7 +7213,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } // Generate 3d grid map? - if(ui_->dockWidget_occupancyGridView->isVisible()) + if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_grid->isChecked()) { #ifdef RTABMAP_OCTOMAP if(ui_->checkBox_octomap->isChecked()) @@ -7005,116 +7223,66 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) else #endif { - pcl::PointCloud::Ptr groundXYZ(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesXYZ(new pcl::PointCloud); - pcl::PointCloud::Ptr emptyCellsXYZ(new pcl::PointCloud); - pcl::PointCloud::Ptr groundRGB(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesRGB(new pcl::PointCloud); - pcl::PointCloud::Ptr emptyCellsRGB(new pcl::PointCloud); + CloudMap cloudMap(&combinedLocalMaps, parameters); + + cloudMap.update(graphFiltered); + + const pcl::PointCloud::Ptr & groundCells = cloudMap.getMapGround(); + const pcl::PointCloud::Ptr & obstacleCells = cloudMap.getMapObstacles(); + const pcl::PointCloud::Ptr & emptyCells = cloudMap.getMapEmptyCells(); - for(std::map, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter) - { - Transform pose = graphFiltered.at(iter->first); - float x,y,z,roll,pitch,yaw; - pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - Transform pose2d(x,y, 0, 0, 0, yaw); - if(!iter->second.first.first.empty()) - { - if(iter->second.first.first.channels() == 4) - { - *groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose); - } - else - { - *groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose); - } - } - if(!iter->second.first.second.empty()) - { - if(iter->second.first.second.channels() == 4) - { - *obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose); - } - else - { - *obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose); - } - } - if(ui_->checkBox_grid_empty->isChecked()) - { - if(!iter->second.second.empty()) - { - if(iter->second.second.channels() == 4) - { - *emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose); - } - else - { - *emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose); - } - } - } - } // occupancy cloud - if(groundRGB->size()) - { - groundRGB = util3d::voxelize(groundRGB, cellSize); - occupancyGridViewer_->addCloud("groundRGB", - groundRGB, - Transform::getIdentity(), - QColor(ui_->lineEdit_groundColor->text())); - occupancyGridViewer_->setCloudPointSize("groundRGB", 5); - } - if(groundXYZ->size()) + if(groundCells->size()) { - groundXYZ = util3d::voxelize(groundXYZ, cellSize); - occupancyGridViewer_->addCloud("groundXYZ", - groundXYZ, + occupancyGridViewer_->addCloud("groundCells", + groundCells, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text())); - occupancyGridViewer_->setCloudPointSize("groundXYZ", 5); - } - if(obstaclesRGB->size()) - { - obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize); - occupancyGridViewer_->addCloud("obstaclesRGB", - obstaclesRGB, - Transform::getIdentity(), - QColor(ui_->lineEdit_obstacleColor->text())); - occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5); + occupancyGridViewer_->setCloudPointSize("groundCells", 5); } - if(obstaclesXYZ->size()) + if(obstacleCells->size()) { - obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize); - occupancyGridViewer_->addCloud("obstaclesXYZ", - obstaclesXYZ, + occupancyGridViewer_->addCloud("obstacleCells", + obstacleCells, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text())); - occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5); - } - if(emptyCellsRGB->size()) - { - emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize); - occupancyGridViewer_->addCloud("emptyCellsRGB", - emptyCellsRGB, - Transform::getIdentity(), - QColor(ui_->lineEdit_emptyColor->text())); - occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 5); - occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5); + occupancyGridViewer_->setCloudPointSize("obstacleCells", 5); } - if(emptyCellsXYZ->size()) + if(ui_->checkBox_grid_empty->isChecked() && emptyCells->size()) { - emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize); - occupancyGridViewer_->addCloud("emptyCellsXYZ", - emptyCellsXYZ, + occupancyGridViewer_->addCloud("emptyCells", + emptyCells, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text())); - occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 5); - occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5); + occupancyGridViewer_->setCloudPointSize("emptyCells", 5); + occupancyGridViewer_->setCloudOpacity("emptyCells", 0.5); } occupancyGridViewer_->refreshView(); } } + +#ifdef RTABMAP_GRIDMAP + // Show elevation map ? + if(ui_->dockWidget_occupancyGridView->isVisible() && + ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked) + { + GridMap gridMap(&combinedLocalMaps, parameters); + + gridMap.update(graphFiltered); + if(ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked) + { + float xMin, yMin; + cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, cellSize); + occupancyGridViewer_->addElevationMap(elevationMap, cellSize, xMin, yMin, 1.0f); + } + else + { + pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh(); + occupancyGridViewer_->addCloudMesh("elevation_mesh", mesh); + } + occupancyGridViewer_->refreshView(); + } +#endif } ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio); if(rectScaleItem != 0) @@ -7174,6 +7342,15 @@ void DatabaseViewer::updateGraphView() ui_->label_poses->clear(); ui_->label_rmse->clear(); + if(sender() == ui_->checkBox_alignPosesWithGPS && ui_->checkBox_alignPosesWithGPS->isChecked()) + { + ui_->checkBox_alignPosesWithGroundTruth->setChecked(false); + } + else if(sender() == ui_->checkBox_alignPosesWithGroundTruth && ui_->checkBox_alignPosesWithGroundTruth->isChecked()) + { + ui_->checkBox_alignPosesWithGPS->setChecked(false); + } + if(odomPoses_.size()) { int fromId = ui_->spinBox_optimizationsFrom->value(); @@ -7327,15 +7504,25 @@ void DatabaseViewer::updateGraphView() int totalPriors = 0; int totalLandmarks = 0; int totalGravity = 0; + std::multimap uniqueLinks; for(std::multimap::iterator iter=links.begin(); iter!=links.end();) { + bool isUnique = iter->second.from() == iter->second.to(); // Count all self-reference links + if(graph::findLink(uniqueLinks, iter->second.from(), iter->second.to(), true) == uniqueLinks.end()) + { + uniqueLinks.insert(std::make_pair(iter->second.from(), iter->second.to())); + isUnique = true; + } + if(iter->second.type() == Link::kNeighbor) { - ++totalNeighbor; + if(isUnique) + ++totalNeighbor; } else if(iter->second.type() == Link::kNeighborMerged) { - ++totalNeighborMerged; + if(isUnique) + ++totalNeighborMerged; } else if(iter->second.type() == Link::kGlobalClosure) { @@ -7345,7 +7532,8 @@ void DatabaseViewer::updateGraphView() continue; } loopLinks_.push_back(iter->second); - ++totalGlobal; + if(isUnique) + ++totalGlobal; } else if(iter->second.type() == Link::kLocalSpaceClosure) { @@ -7355,7 +7543,8 @@ void DatabaseViewer::updateGraphView() continue; } loopLinks_.push_back(iter->second); - ++totalLocalSpace; + if(isUnique) + ++totalLocalSpace; } else if(iter->second.type() == Link::kLocalTimeClosure) { @@ -7365,7 +7554,8 @@ void DatabaseViewer::updateGraphView() continue; } loopLinks_.push_back(iter->second); - ++totalLocalTime; + if(isUnique) + ++totalLocalTime; } else if(iter->second.type() == Link::kUserClosure) { @@ -7375,7 +7565,8 @@ void DatabaseViewer::updateGraphView() continue; } loopLinks_.push_back(iter->second); - ++totalUser; + if(isUnique) + ++totalUser; } else if(iter->second.type() == Link::kLandmark) { @@ -7390,7 +7581,8 @@ void DatabaseViewer::updateGraphView() poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform())); } loopLinks_.push_back(iter->second); - ++totalLandmarks; + if(isUnique) + ++totalLandmarks; // add landmark priors if there are some int markerId = iter->second.to(); @@ -7402,16 +7594,19 @@ void DatabaseViewer::updateGraphView() links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix))); UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(), markerPriorsLinearVariance, markerPriorsAngularVariance); - ++totalPriors; + if(isUnique) + ++totalPriors; } } else if(iter->second.type() == Link::kPosePrior) { - ++totalPriors; + if(isUnique) + ++totalPriors; } else if(iter->second.type() == Link::kGravity) { - ++totalGravity; + if(isUnique) + ++totalGravity; } else { @@ -7445,12 +7640,17 @@ void DatabaseViewer::updateGraphView() while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0) { std::multimap::iterator uter = links.find(link.to()); + while(uter != links.end() && + uter->first==link.to() && + uter->second.from()>uter->second.to()) + { + ++uter; + } if(uter != links.end()) { - UASSERT(links.count(link.to()) == 1); poses.erase(link.to()); link = link.merge(uter->second, uter->second.type()); - links.erase(uter); + links.erase(uter->first); } else { @@ -7600,6 +7800,7 @@ void DatabaseViewer::updateGraphView() ui_->spinBox_optimizationsFrom->setEnabled(false); ui_->checkBox_spanAllMaps->setEnabled(false); ui_->checkBox_wmState->setEnabled(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->checkBox_ignoreIntermediateNodes->setEnabled(false); @@ -7612,8 +7813,9 @@ void DatabaseViewer::updateGraphView() ui_->spinBox_optimizationsFrom->setEnabled(true); ui_->checkBox_spanAllMaps->setEnabled(true); ui_->checkBox_wmState->setEnabled(true); - ui_->checkBox_alignPosesWithGroundTruth->setEnabled(true); - ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(true); + ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible()); ui_->checkBox_ignoreIntermediateNodes->setEnabled(true); } updateGraphRotation(); @@ -7661,9 +7863,11 @@ void DatabaseViewer::updateGrid() ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked()); ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); update3dView(); updateGraphView(); @@ -7676,9 +7880,11 @@ void DatabaseViewer::updateOctomapView() ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked()); ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked()); ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0); + ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0); if(ui_->checkBox_octomap->isChecked()) { @@ -7686,7 +7892,12 @@ void DatabaseViewer::updateOctomapView() { occupancyGridViewer_->removeOctomap(); occupancyGridViewer_->removeCloud("octomap_obstacles"); + occupancyGridViewer_->removeCloud("octomap_ground"); occupancyGridViewer_->removeCloud("octomap_empty"); + occupancyGridViewer_->removeCloud("octomap_frontiers"); + occupancyGridViewer_->removeCloud("groundCells"); + occupancyGridViewer_->removeCloud("obstacleCells"); + occupancyGridViewer_->removeCloud("emptyCells"); if(ui_->comboBox_octomap_rendering_type->currentIndex()>0) { occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1); @@ -7696,6 +7907,7 @@ void DatabaseViewer::updateOctomapView() pcl::IndicesPtr obstacles(new std::vector); pcl::IndicesPtr empty(new std::vector); pcl::IndicesPtr ground(new std::vector); + pcl::IndicesPtr frontiers(new std::vector); std::vector prob; pcl::PointCloud::Ptr cloud = octomap_->createCloud( ui_->spinBox_grid_depth->value(), @@ -7703,7 +7915,7 @@ void DatabaseViewer::updateOctomapView() empty.get(), ground.get(), true, - 0, + frontiers.get(), &prob); if(octomap_->hasColor()) @@ -7763,6 +7975,15 @@ void DatabaseViewer::updateOctomapView() occupancyGridViewer_->setCloudPointSize("octomap_empty", 5); } } + + if(ui_->checkBox_grid_frontiers->isChecked()) + { + pcl::PointCloud::Ptr frontiersCloud(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud); + occupancyGridViewer_->addCloud("octomap_frontiers", frontiersCloud, Transform::getIdentity(), QColor(ui_->lineEdit_frontierColor->text())); + occupancyGridViewer_->setCloudOpacity("octomap_frontiers", 0.5); + occupancyGridViewer_->setCloudPointSize("octomap_frontiers", 5); + } } occupancyGridViewer_->refreshView(); } @@ -8814,9 +9035,12 @@ void DatabaseViewer::resetConstraint() void DatabaseViewer::rejectConstraint() { - int from = ids_.at(ui_->horizontalSlider_A->value()); - int to = ids_.at(ui_->horizontalSlider_B->value()); - if(ui_->label_type->text().toInt() == Link::kLandmark) + int priorId = sender() == ui_->toolButton_remove_priorA?ids_.at(ui_->horizontalSlider_A->value()): + sender() == ui_->toolButton_remove_priorB?ids_.at(ui_->horizontalSlider_B->value()):0; + + int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value()); + int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value()); + if(priorId==0 && ui_->label_type->text().toInt() == Link::kLandmark) { int position = ui_->horizontalSlider_loops->value(); const rtabmap::Link & link = loopLinks_.at(position); @@ -8831,7 +9055,7 @@ void DatabaseViewer::rejectConstraint() from = tmp; } - if(from == to) + if(priorId==0 && from == to) { UWARN("Cannot reject link to same node"); return; @@ -8873,9 +9097,31 @@ void DatabaseViewer::rejectConstraint() } if(removed) { - this->updateGraphView(); + if(priorId==0) + { + this->updateGraphView(); + updateLoopClosuresSlider(); + } + else + { + bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(); + Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored); + int indexA = ui_->horizontalSlider_A->value(); + int indexB = ui_->horizontalSlider_B->value(); + if(!priorsIgnored) + { + this->updateGraphView(); + } + if(ui_->horizontalSlider_A->value() != indexA) + ui_->horizontalSlider_A->setValue(indexA); + else + sliderAValueChanged(indexA); + if(ui_->horizontalSlider_B->value() != indexB) + ui_->horizontalSlider_B->setValue(indexB); + else + sliderBValueChanged(indexB); + } } - updateLoopClosuresSlider(); } std::multimap DatabaseViewer::updateLinksWithModifications( diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 378ddbae15..a860c2f2f2 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -129,6 +129,14 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_ceilingHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_floorHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->groupBox_offAxisFiltering, SIGNAL(toggled(bool)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_offAxisFilteringAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringPosX, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringNegX, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringPosY, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringNegY, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringPosZ, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_offAxisFilteringNegZ, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_footprintWidth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_footprintLength, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_footprintHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -370,6 +378,14 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()); settings.setValue("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()); settings.setValue("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()); + settings.setValue("regenerate_offaxis_filtering", _ui->groupBox_offAxisFiltering->isChecked()); + settings.setValue("regenerate_offaxis_filtering_angle", _ui->doubleSpinBox_offAxisFilteringAngle->value()); + settings.setValue("regenerate_offaxis_filtering_pos_x", _ui->checkBox_offAxisFilteringPosX->isChecked()); + settings.setValue("regenerate_offaxis_filtering_neg_x", _ui->checkBox_offAxisFilteringNegX->isChecked()); + settings.setValue("regenerate_offaxis_filtering_pos_y", _ui->checkBox_offAxisFilteringPosY->isChecked()); + settings.setValue("regenerate_offaxis_filtering_neg_y", _ui->checkBox_offAxisFilteringNegY->isChecked()); + settings.setValue("regenerate_offaxis_filtering_pos_z", _ui->checkBox_offAxisFilteringPosZ->isChecked()); + settings.setValue("regenerate_offaxis_filtering_neg_z", _ui->checkBox_offAxisFilteringNegZ->isChecked()); settings.setValue("regenerate_footprint_height", _ui->doubleSpinBox_footprintHeight->value()); settings.setValue("regenerate_footprint_width", _ui->doubleSpinBox_footprintWidth->value()); settings.setValue("regenerate_footprint_length", _ui->doubleSpinBox_footprintLength->value()); @@ -543,6 +559,14 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble()); _ui->doubleSpinBox_ceilingHeight->setValue(settings.value("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()).toDouble()); _ui->doubleSpinBox_floorHeight->setValue(settings.value("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()).toDouble()); + _ui->groupBox_offAxisFiltering->setChecked(settings.value("regenerate_offaxis_filtering", _ui->groupBox_offAxisFiltering->isChecked()).toBool()); + _ui->doubleSpinBox_offAxisFilteringAngle->setValue(settings.value("regenerate_offaxis_filtering_angle", _ui->doubleSpinBox_offAxisFilteringAngle->value()).toDouble()); + _ui->checkBox_offAxisFilteringPosX->setChecked(settings.value("regenerate_offaxis_filtering_pos_x", _ui->checkBox_offAxisFilteringPosX->isChecked()).toBool()); + _ui->checkBox_offAxisFilteringNegX->setChecked(settings.value("regenerate_offaxis_filtering_neg_x", _ui->checkBox_offAxisFilteringNegX->isChecked()).toBool()); + _ui->checkBox_offAxisFilteringPosY->setChecked(settings.value("regenerate_offaxis_filtering_pos_y", _ui->checkBox_offAxisFilteringPosY->isChecked()).toBool()); + _ui->checkBox_offAxisFilteringNegY->setChecked(settings.value("regenerate_offaxis_filtering_neg_y", _ui->checkBox_offAxisFilteringNegY->isChecked()).toBool()); + _ui->checkBox_offAxisFilteringPosZ->setChecked(settings.value("regenerate_offaxis_filtering_pos_z", _ui->checkBox_offAxisFilteringPosZ->isChecked()).toBool()); + _ui->checkBox_offAxisFilteringNegZ->setChecked(settings.value("regenerate_offaxis_filtering_neg_z", _ui->checkBox_offAxisFilteringNegZ->isChecked()).toBool()); _ui->doubleSpinBox_footprintHeight->setValue(settings.value("regenerate_footprint_height", _ui->doubleSpinBox_footprintHeight->value()).toDouble()); _ui->doubleSpinBox_footprintWidth->setValue(settings.value("regenerate_footprint_width", _ui->doubleSpinBox_footprintWidth->value()).toDouble()); _ui->doubleSpinBox_footprintLength->setValue(settings.value("regenerate_footprint_length", _ui->doubleSpinBox_footprintLength->value()).toDouble()); @@ -719,6 +743,14 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_minDepth->setValue(0); _ui->doubleSpinBox_ceilingHeight->setValue(0); _ui->doubleSpinBox_floorHeight->setValue(0); + _ui->groupBox_offAxisFiltering->setChecked(false); + _ui->doubleSpinBox_offAxisFilteringAngle->setValue(10); + _ui->checkBox_offAxisFilteringPosX->setChecked(true); + _ui->checkBox_offAxisFilteringNegX->setChecked(true); + _ui->checkBox_offAxisFilteringPosY->setChecked(true); + _ui->checkBox_offAxisFilteringNegY->setChecked(true); + _ui->checkBox_offAxisFilteringPosZ->setChecked(true); + _ui->checkBox_offAxisFilteringNegZ->setChecked(true); _ui->doubleSpinBox_footprintHeight->setValue(0); _ui->doubleSpinBox_footprintLength->setValue(0); _ui->doubleSpinBox_footprintWidth->setValue(0); @@ -1504,6 +1536,7 @@ void ExportCloudsDialog::viewClouds( } } viewer->refreshView(); + window->activateWindow(); } else { @@ -2844,7 +2877,7 @@ bool ExportCloudsDialog::getExportedClouds( } if(!image.empty()) { - + if(_ui->spinBox_camProjDecimation->value()>1) { image = util2d::decimate(image, _ui->spinBox_camProjDecimation->value()); @@ -3914,6 +3947,36 @@ std::map::Ptr, pcl::Indic UWARN("Point cloud %d doesn't have anymore points (had %d points) after radius filtering.", iter->first, (int)cloud->size()); } } + if( !indices->empty() && _ui->groupBox_offAxisFiltering->isChecked() && + (_ui->checkBox_offAxisFilteringPosX->isChecked() || + _ui->checkBox_offAxisFilteringNegX->isChecked() || + _ui->checkBox_offAxisFilteringPosY->isChecked() || + _ui->checkBox_offAxisFilteringNegY->isChecked() || + _ui->checkBox_offAxisFilteringPosZ->isChecked() || + _ui->checkBox_offAxisFilteringNegZ->isChecked())) + { + pcl::PointCloud::Ptr cloudInMapFrame = util3d::transformPointCloud(cloud, iter->second); + std::vector indicesVector; + double maxDeltaAngle = _ui->doubleSpinBox_offAxisFilteringAngle->value()*M_PI/180.0; + Eigen::Vector4f viewpoint(iter->second.x(), iter->second.y(), iter->second.z(), 0); + if(_ui->checkBox_offAxisFilteringPosX->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(1,0,0,0), 20, viewpoint)); + if(_ui->checkBox_offAxisFilteringPosY->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,1,0,0), 20, viewpoint)); + if(_ui->checkBox_offAxisFilteringPosZ->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,0,1,0), 20, viewpoint)); + if(_ui->checkBox_offAxisFilteringNegX->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(-1,0,0,0), 20, viewpoint)); + if(_ui->checkBox_offAxisFilteringNegY->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,-1,0,0), 20, viewpoint)); + if(_ui->checkBox_offAxisFilteringNegZ->isChecked()) + indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,0,-1,0), 20, viewpoint)); + indices = util3d::concatenate(indicesVector); + if(indices->empty()) + { + UWARN("Point cloud %d doesn't have anymore points (had %d points) after offaxis filtering.", iter->first, (int)cloud->size()); + } + } } if(!indices->empty()) @@ -4369,11 +4432,11 @@ void ExportCloudsDialog::saveMeshes( } else if(QFileInfo(path).suffix() == "obj") { - success = pcl::io::saveOBJFile(path.toStdString(), *meshes.begin()->second) == 0; + success = saveOBJFile(path, *meshes.begin()->second); } else { - UERROR("Extension not recognized! (%s) Should be (*.ply).", QFileInfo(path).suffix().toStdString().c_str()); + UERROR("Extension not recognized! (%s) Should be (*.ply) or (*.obj).", QFileInfo(path).suffix().toStdString().c_str()); } if(success) { @@ -4455,7 +4518,7 @@ void ExportCloudsDialog::saveMeshes( } else if(suffix == "obj") { - success = pcl::io::saveOBJFile(pathFile.toStdString(), mesh) == 0; + success = saveOBJFile(pathFile, mesh); } else { @@ -4613,7 +4676,7 @@ void ExportCloudsDialog::saveTextureMeshes( path.toStdString(), mesh->cloud, mesh->tex_polygons[0], - poses, + filterNodes(poses), textureVertexToPixels, images, calibrations, @@ -4779,8 +4842,7 @@ void ExportCloudsDialog::saveTextureMeshes( } } - success = pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0; - if(success) + if(saveOBJFile(path, mesh)) { _progressDialog->incrementStep(); _progressDialog->appendText(tr("Saving the mesh (with %1 textures)... done.").arg(mesh->tex_materials.size())); @@ -4976,7 +5038,7 @@ void ExportCloudsDialog::saveTextureMeshes( bool success =false; if(suffix == "obj") { - success = pcl::io::saveOBJFile(pathFile.toStdString(), *mesh) == 0; + success = saveOBJFile(pathFile, mesh); } else { @@ -5010,4 +5072,28 @@ void ExportCloudsDialog::saveTextureMeshes( } } + bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const { +#if PCL_VERSION_COMPARE(>=, 1, 13, 0) + mesh->tex_coord_indices = std::vector>(); + auto nr_meshes = static_cast(mesh->tex_polygons.size()); + unsigned f_idx = 0; + for (unsigned m = 0; m < nr_meshes; m++) { + std::vector ci = mesh->tex_polygons[m]; + for(std::size_t i = 0; i < ci.size(); i++) { + for (std::size_t j = 0; j < ci[i].vertices.size(); j++) { + ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j; + } + } + mesh->tex_coord_indices.push_back(ci); + f_idx += static_cast(mesh->tex_polygons[m].size()); + } +#endif + return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0; + } + + bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const { + return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0; + } + + } diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 0953e93d34..bbd6618775 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #include #include +#include #include #include @@ -62,6 +63,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#include + namespace rtabmap { class NodeItem: public QGraphicsEllipseItem @@ -249,10 +252,10 @@ class LinkItem: public QGraphicsLineItem protected: virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ) { - QString str = QString("%1->%2 (%3 m)").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB)); + QString str = QString("%1->%2 (type=%3 length=%4 m)").arg(_from).arg(_to).arg(_link.type()).arg(_poseA.getDistance(_poseB)); if(!_link.transform().isNull()) { - str.append(QString("\n%1\n%2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance())); + str.append(QString("\n%1\nvar= %2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance())); } this->setToolTip(str); QPen pen = this->pen(); @@ -314,6 +317,7 @@ GraphViewer::GraphViewer(QWidget * parent) : _loopClosureOutlierThr(0), _maxLinkLength(0.02f), _orientationENU(false), + _mouseTracking(false), _viewPlane(XY), _ensureFrameVisible(true) { @@ -564,7 +568,7 @@ void GraphViewer::updateGraph(const std::map & poses, itemIter = _linkItems.find(iter->first); while(itemIter.key() == idFrom && itemIter != _linkItems.end()) { - if(itemIter.value()->to() == idTo) + if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type()) { itemIter.value()->setPoses(poseA, poseB, _viewPlane); itemIter.value()->show(); @@ -1787,6 +1791,50 @@ void GraphViewer::wheelEvent ( QWheelEvent * event ) } } +void GraphViewer::mouseMoveEvent(QMouseEvent * event) +{ + QPointF scenePoint = mapToScene(event->pos()); + if(_mouseTracking && _viewPlane==XY && this->sceneRect().contains(scenePoint)) + { +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + QToolTip::showText(event->globalPosition().toPoint(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0)); +#else + QToolTip::showText(event->globalPos(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0)); +#endif + } + else + { + QToolTip::hideText(); + } + QGraphicsView::mouseMoveEvent(event); +} + +void GraphViewer::mouseDoubleClickEvent(QMouseEvent * event) +{ + QGraphicsItem *item = this->scene()->itemAt(mapToScene(event->pos()), QTransform()); + if(item) + { + NodeItem *nodeItem = qgraphicsitem_cast(item); + LinkItem *linkItem = qgraphicsitem_cast(item); + if(nodeItem && nodeItem->parentItem() == _graphRoot && nodeItem->id() != 0) + { + Q_EMIT nodeSelected(nodeItem->id()); + } + else if(linkItem && linkItem->parentItem() == _graphRoot && linkItem->from() != 0 && linkItem->to() != 0) + { + Q_EMIT linkSelected(linkItem->from(), linkItem->to()); + } + else + { + QGraphicsView::mouseDoubleClickEvent(event); + } + } + else + { + QGraphicsView::mouseDoubleClickEvent(event); + } +} + QIcon createIcon(const QColor & color) { QPixmap pixmap(50, 50); @@ -1798,6 +1846,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { QMenu menu; QAction * aScreenShot = menu.addAction(tr("Take a screenshot...")); + QAction * aExportGridMap = menu.addAction(tr("Export grid map...")); + aExportGridMap->setEnabled(!_gridMap->pixmap().isNull()); menu.addSeparator(); QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color...")); @@ -1874,6 +1924,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aShowHideGPSGraph; QAction * aShowHideOdomCacheOverlay; QAction * aOrientationENU; + QAction * aMouseTracking; QAction * aViewPlaneXY; QAction * aViewPlaneXZ; QAction * aViewPlaneYZ; @@ -1972,6 +2023,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aOrientationENU = menu.addAction(tr("ENU Orientation")); aOrientationENU->setCheckable(true); aOrientationENU->setChecked(_orientationENU); + aMouseTracking = menu.addAction(tr("Show mouse cursor position (m)")); + aMouseTracking->setCheckable(true); + aMouseTracking->setChecked(_mouseTracking); + aMouseTracking->setEnabled(_viewPlane == XY); aShowHideGraph->setEnabled(_nodeItems.size() && _viewPlane == XY); aShowHideGraphNodes->setEnabled(_nodeItems.size() && _graphRoot->isVisible()); aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size()); @@ -2095,6 +2150,48 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) } return; // without emitting configChanged } + else if(r == aExportGridMap) + { + float xMin, yMin, cellSize; + + cellSize = _gridCellSize; + xMin = -_gridMap->y()/100.0f; + yMin = -_gridMap->x()/100.0f; + + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + "map.pgm", + tr("Map (*.pgm)")); + + if(!path.isEmpty()) + { + if(QFileInfo(path).suffix() == "") + { + path += ".pgm"; + } + _gridMap->pixmap().save(path); + + QFileInfo info(path); + QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml"; + + float occupancyThr = Parameters::defaultGridGlobalOccupancyThr(); + + std::ofstream file; + file.open (yaml.toStdString()); + file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl; + file << "resolution: " << cellSize << std::endl; + file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl; + file << "negate: 0" << std::endl; + file << "occupied_thresh: " << occupancyThr << std::endl; + file << "free_thresh: 0.196" << std::endl; + file << std::endl; + file.close(); + + + QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml)); + } + } else if(r == aSetIntraInterSessionColors) { setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked()); @@ -2359,6 +2456,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { this->setOrientationENU(!this->isOrientationENU()); } + else if(r == aMouseTracking) + { + _mouseTracking = aMouseTracking->isChecked(); + } else if(r == aViewPlaneXY) { this->setViewPlane(XY); diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 4fca8981e0..dd9dd83d45 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -44,5 +44,6 @@ images/oakd.png images/oakd_lite.png images/astra.png + images/oakdpro.png diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index 118954a8c4..a98a158561 100644 --- a/guilib/src/ImageView.cpp +++ b/guilib/src/ImageView.cpp @@ -40,9 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "rtabmap/utilite/ULogger.h" #include "rtabmap/gui/KeypointItem.h" #include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d_transforms.h" #include #if QT_VERSION >= 0x050000 @@ -268,9 +270,14 @@ ImageView::ImageView(QWidget * parent) : group->addAction(_colorMapRedToBlue); group->addAction(_colorMapBlueToRed); group->addAction(_colorMapMaxRange); + _mouseTracking = _menu->addAction(tr("Show pixel depth")); + _mouseTracking->setCheckable(true); + _mouseTracking->setChecked(false); _saveImage = _menu->addAction(tr("Save picture...")); _saveImage->setEnabled(false); + setMouseTracking(true); + connect(_graphicsView->scene(), SIGNAL(sceneRectChanged(const QRectF &)), this, SLOT(sceneRectChanged(const QRectF &))); } @@ -1047,6 +1054,65 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e) } } +void ImageView::mouseMoveEvent(QMouseEvent * event) +{ + if(_mouseTracking->isChecked() && + !_graphicsView->scene()->sceneRect().isNull() && + !_image.isNull() && + !_imageDepthCv.empty() && + (_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) + { + float scale, offsetX, offsetY; + computeScaleOffsets(this->rect(), scale, offsetX, offsetY); + float u = (event->pos().x() - offsetX) / scale; + float v = (event->pos().y() - offsetY) / scale; + float depthScale = 1; + if(_image.width() > _imageDepth.width()) + { + depthScale = _imageDepth.width() / _image.width(); + } + int ud = int(u*depthScale); + int vd = int(v*depthScale); + if( ud>=0 && vd>=0 && + ud < _imageDepthCv.cols && + vd < _imageDepthCv.rows) + { + float depth = 0; + if(_imageDepthCv.type() == CV_32FC1) + { + depth = _imageDepthCv.at(vd, ud); + } + else + { + depth = float(_imageDepthCv.at(vd, ud)) / 1000.0f; + } + + cv::Point3f pt(0,0,0); + if(depth>0 && !_models.empty() && !_pose.isNull()) + { + int subImageWidth = _imageDepthCv.cols / _models.size(); + int subImageIndex = ud / subImageWidth; + UASSERT(subImageIndex < (int)_models.size()); + float x,y,z; + _models[subImageIndex].project(u, v, depth, x, y, z); + pt = cv::Point3f(x,y,z); + pt = util3d::transformPoint(pt, _pose*_models[subImageIndex].localTransform()); + } + +#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0) + QToolTip::showText(event->globalPosition().toPoint(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA")); +#else + QToolTip::showText(event->globalPos(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA")); +#endif + } + else + { + QToolTip::hideText(); + } + } + QWidget::mouseMoveEvent(event); +} + void ImageView::updateOpacity() { if(_imageItem && _imageDepthItem) @@ -1166,9 +1232,11 @@ void ImageView::addLine(float x1, float y1, float x2, float y2, QColor color, co } } -void ImageView::setImage(const QImage & image) +void ImageView::setImage(const QImage & image, const std::vector & models, const Transform & pose) { _image = QPixmap::fromImage(image); + _models = models; + _pose = pose; if(_graphicsView->isVisible()) { if(_imageItem) @@ -1394,6 +1462,8 @@ void ImageView::clear() _imageItem = 0; } _image = QPixmap(); + _models.clear(); + _pose.setNull(); if(_imageDepthItem) { diff --git a/guilib/src/LinkRefiningDialog.cpp b/guilib/src/LinkRefiningDialog.cpp new file mode 100644 index 0000000000..5763aebf77 --- /dev/null +++ b/guilib/src/LinkRefiningDialog.cpp @@ -0,0 +1,156 @@ +/* +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. +*/ + +#include "rtabmap/gui/LinkRefiningDialog.h" +#include "ui_linkRefiningDialog.h" + +#include + +namespace rtabmap { + +LinkRefiningDialog::LinkRefiningDialog(QWidget * parent) : + QDialog(parent), + defaultNodeIdMin_(0), + defaultNodeIdMax_(0), + defaultMapIdMin_(0), + defaultMapIdMax_(0) +{ + ui_ = new Ui_linkRefiningDialog(); + ui_->setupUi(this); + + ui_->comboBox_link_type->addItem("All"); + for(int i =0; icomboBox_link_type->addItem(Link::typeName((Link::Type)i).c_str()); + if(((Link::Type)i) == Link::kVirtualClosure) + { + ui_->comboBox_link_type->setItemData(i+1, 0, Qt::UserRole - 1); + } + } + + restoreDefaults(); + + connect(ui_->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults())); + connect(ui_->comboBox_link_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateIntraInterState())); + connect(ui_->spinBox_node_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId())); + connect(ui_->spinBox_node_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId())); + connect(ui_->spinBox_map_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId())); + connect(ui_->spinBox_map_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId())); +} + +LinkRefiningDialog::~LinkRefiningDialog() +{ + delete ui_; +} + +void LinkRefiningDialog::setMinMax( + int nodeIdMin, + int nodeIdMax, + int mapIdMin, + int mapIdMax) +{ + bool reset = defaultNodeIdMin_ == 0; + defaultNodeIdMin_ = nodeIdMin; + defaultNodeIdMax_ = nodeIdMax; + defaultMapIdMin_ = mapIdMin; + defaultMapIdMax_ = mapIdMax; + ui_->spinBox_node_from->setMinimum(defaultNodeIdMin_); + ui_->spinBox_node_to->setMaximum(defaultNodeIdMax_); + ui_->spinBox_map_from->setMinimum(defaultMapIdMin_); + ui_->spinBox_map_to->setMaximum(defaultMapIdMax_); + if(reset) + { + restoreDefaults(); + } +} + +Link::Type LinkRefiningDialog::getLinkType() const +{ + if(ui_->comboBox_link_type->currentIndex() == 0) + { + return Link::kEnd; + } + return (Link::Type)(ui_->comboBox_link_type->currentIndex()-1); +} + +void LinkRefiningDialog::getIntraInterSessions(bool & intra, bool & inter) const +{ + intra = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 1 || getLinkType() == Link::kNeighbor; + inter = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 2; +} + +bool LinkRefiningDialog::isRangeByNodeId() const +{ + return ui_->radioButton_nodes->isChecked(); +} + +bool LinkRefiningDialog::isRangeByMapId() const +{ + return ui_->radioButton_maps->isChecked(); +} + +void LinkRefiningDialog::getRangeNodeId(int & from, int & to) const +{ + from = ui_->spinBox_node_from->value(); + to = ui_->spinBox_node_to->value(); +} + +void LinkRefiningDialog::getRangeMapId(int & from, int & to) const +{ + from = ui_->spinBox_map_from->value(); + to = ui_->spinBox_map_to->value(); +} + +void LinkRefiningDialog::restoreDefaults() +{ + ui_->comboBox_link_type->setCurrentIndex(0); + ui_->comboBox_link_inter_intra->setCurrentIndex(0); + ui_->spinBox_node_from->setValue(defaultNodeIdMin_); + ui_->spinBox_node_to->setValue(defaultNodeIdMax_); + ui_->spinBox_map_from->setValue(defaultMapIdMin_); + ui_->spinBox_map_to->setValue(defaultMapIdMax_); + + ui_->radioButton_nodes->setChecked(true); +} + +void LinkRefiningDialog::updateIntraInterState() +{ + ui_->comboBox_link_inter_intra->setEnabled(getLinkType() != Link::kNeighbor); +} + +void LinkRefiningDialog::setRangeToNodeId() +{ + ui_->radioButton_nodes->setChecked(true); +} + +void LinkRefiningDialog::setRangeToMapId() +{ + ui_->radioButton_maps->setChecked(true); +} + + +} diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 8b0811957a..e2c95f1f6a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -31,9 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" +#include "rtabmap/core/Lidar.h" #include "rtabmap/core/IMUThread.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/DBReader.h" #include "rtabmap/core/Parameters.h" #include "rtabmap/core/ParamEvent.h" @@ -41,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Memory.h" #include "rtabmap/core/DBDriver.h" #include "rtabmap/core/RegistrationVis.h" -#include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/core/global_map/OccupancyGrid.h" #include "rtabmap/core/GainCompensator.h" #include "rtabmap/core/Recovery.h" #include "rtabmap/core/util2d.h" @@ -113,9 +112,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #ifdef RTABMAP_OCTOMAP -#include +#include +#endif + +#ifdef RTABMAP_GRIDMAP +#include #endif #ifdef HAVE_OPENCV_ARUCO @@ -138,7 +143,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh QMainWindow(parent), _ui(0), _state(kIdle), - _camera(0), + _sensorCapture(0), _odomThread(0), _imuThread(0), _preferencesDialog(0), @@ -164,6 +169,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _createdCloudsMemoryUsage(0), _occupancyGrid(0), _octomap(0), + _elevationMap(0), _odometryCorrection(Transform::getIdentity()), _processingOdometry(false), _oneSecondTimer(0), @@ -259,9 +265,17 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh setupMainLayout(_preferencesDialog->isVerticalLayoutUsed()); ParametersMap parameters = _preferencesDialog->getAllParameters(); - _occupancyGrid = new OccupancyGrid(parameters); + // Override map resolution for UI + if(_preferencesDialog->getGridUIResolution()>0) + { + uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution()))); + } + _occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters); #ifdef RTABMAP_OCTOMAP - _octomap = new OctoMap(parameters); + _octomap = new OctoMap(&_cachedLocalMaps, parameters); +#endif +#ifdef RTABMAP_GRIDMAP + _elevationMap = new GridMap(&_cachedLocalMaps, parameters); #endif // Timer @@ -427,7 +441,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh //Settings menu connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource())); - connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream())); connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni())); connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni())); connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect())); @@ -455,6 +468,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS())); connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD())); connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite())); + connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro())); + connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16())); _ui->actionFreenect->setEnabled(CameraFreenect::available()); _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available()); _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available()); @@ -479,6 +494,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available()); _ui->actionDepthAI_oakd->setEnabled(CameraDepthAI::available()); _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available()); + _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available()); this->updateSelectSourceMenu(); connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences())); @@ -552,8 +568,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh qRegisterMetaType("rtabmap::Statistics"); connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics))); - qRegisterMetaType("rtabmap::CameraInfo"); - connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo))); + qRegisterMetaType("rtabmap::SensorCaptureInfo"); + connect(this, SIGNAL(cameraInfoReceived(rtabmap::SensorCaptureInfo)), this, SLOT(processCameraInfo(rtabmap::SensorCaptureInfo))); qRegisterMetaType("rtabmap::OdometryEvent"); connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool))); @@ -598,10 +614,16 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Planning/Length/m", false); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false); + _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", false); + _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", false); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", false); + _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", false); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time total/ms", false); _ui->statsToolBox->updateStat("Odometry/ID/", false); _ui->statsToolBox->updateStat("Odometry/Features/", false); @@ -622,6 +644,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", false); _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false); _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false); + _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", false); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false); _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false); _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false); @@ -661,6 +684,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh #ifdef RTABMAP_OCTOMAP _ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false); _ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false); +#endif +#ifdef RTABMAP_GRIDMAP + _ui->statsToolBox->updateStat("GUI/Elevation Update/ms", false); + _ui->statsToolBox->updateStat("GUI/Elevation Rendering/ms", false); #endif _ui->statsToolBox->updateStat("GUI/Grid Update/ms", false); _ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false); @@ -698,6 +725,9 @@ MainWindow::~MainWindow() delete _logEventTime; #ifdef RTABMAP_OCTOMAP delete _octomap; +#endif +#ifdef RTABMAP_GRIDMAP + delete _elevationMap; #endif delete _occupancyGrid; UDEBUG(""); @@ -813,11 +843,11 @@ void MainWindow::closeEvent(QCloseEvent* event) _ui->dockWidget_odometry->close(); _ui->dockWidget_multiSessionLoc->close(); - if(_camera) + if(_sensorCapture) { UERROR("Camera must be already deleted here!"); - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; if(_imuThread) { delete _imuThread; @@ -909,10 +939,10 @@ bool MainWindow::handleEvent(UEvent* anEvent) { Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode()); } - else if(anEvent->getClassName().compare("CameraEvent") == 0) + else if(anEvent->getClassName().compare("SensorEvent") == 0) { - CameraEvent * cameraEvent = (CameraEvent*)anEvent; - if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages) + SensorEvent * sensorEvent = (SensorEvent*)anEvent; + if(sensorEvent->getCode() == SensorEvent::kCodeNoMoreImages) { if(_preferencesDialog->beepOnPause()) { @@ -922,15 +952,15 @@ bool MainWindow::handleEvent(UEvent* anEvent) } else { - Q_EMIT cameraInfoReceived(cameraEvent->info()); - if (_odomThread == 0 && (_camera->odomProvided()) && _preferencesDialog->isRGBDMode()) + Q_EMIT cameraInfoReceived(sensorEvent->info()); + if (_odomThread == 0 && (_sensorCapture->odomProvided()) && _preferencesDialog->isRGBDMode()) { OdometryInfo odomInfo; - odomInfo.reg.covariance = cameraEvent->info().odomCovariance; + odomInfo.reg.covariance = sensorEvent->info().odomCovariance; if (!_processingOdometry && !_processingStatistics) { _processingOdometry = true; // if we receive too many odometry events! - OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo); + OdometryEvent tmp(sensorEvent->data(), sensorEvent->info().odomPose, odomInfo); Q_EMIT odometryReceived(tmp, false); } else @@ -980,7 +1010,7 @@ bool MainWindow::handleEvent(UEvent* anEvent) return false; } -void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) +void MainWindow::processCameraInfo(const rtabmap::SensorCaptureInfo & info) { if(_firstStamp == 0.0) { @@ -988,15 +1018,17 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) } if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { - _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeHistogramEqualization*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); } Q_EMIT(cameraInfoProcessed()); @@ -1012,6 +1044,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI UDEBUG(""); _processingOdometry = true; UTimer time; + UTimer timeTotal; // Process Data // Set color code as tooltip @@ -1707,6 +1740,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI //draw lines UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size()); std::set inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end()); + int subImageWidth = 0; + if(data->cameraModels().size()>1 || data->stereoCameraModels().size()>1) + { + subImageWidth = data->cameraModels().size()?data->cameraModels()[0].imageWidth():data->stereoCameraModels()[0].left().imageWidth(); + } for(unsigned int i=0; iimageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) @@ -1715,12 +1753,16 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI } if(_ui->imageView_odometry->isLinesShown()) { - _ui->imageView_odometry->addLine( - odom.info().newCorners[i].x, - odom.info().newCorners[i].y, - odom.info().refCorners[i].x, - odom.info().refCorners[i].y, - inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); + // just draw lines in same camera + if(subImageWidth==0 || int(odom.info().refCorners[i].x/subImageWidth) == int(odom.info().newCorners[i].x/subImageWidth)) + { + _ui->imageView_odometry->addLine( + odom.info().newCorners[i].x, + odom.info().newCorners[i].y, + odom.info().refCorners[i].x, + odom.info().refCorners[i].y, + inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); + } } } } @@ -1744,6 +1786,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI //Process info if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { + double linVar = uMax3(odom.info().reg.covariance.at(0,0), odom.info().reg.covariance.at(1,1)>=9999?0:odom.info().reg.covariance.at(1,1), odom.info().reg.covariance.at(2,2)>=9999?0:odom.info().reg.covariance.at(2,2)); + double angVar = uMax3(odom.info().reg.covariance.at(3,3)>=9999?0:odom.info().reg.covariance.at(3,3), odom.info().reg.covariance.at(4,4)>=9999?0:odom.info().reg.covariance.at(4,4), odom.info().reg.covariance.at(5,5)); _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures()); @@ -1757,10 +1801,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at(0,0)), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at(0,0), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at(5,5)), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at(5,5), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)linVar), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)linVar, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)angVar), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)angVar, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); if(odom.info().timeParticleFiltering>0.0f) { @@ -1883,7 +1928,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/Distance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().distanceTravelled, _preferencesDialog->isCacheSavedInFigures()); } - _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), time.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), timeTotal.elapsed()*1000.0, _preferencesDialog->isCacheSavedInFigures()); UDEBUG("Time updating Stats toolbox: %fs", time.ticks()); } _processingOdometry = false; @@ -2212,7 +2257,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("time= %d ms (update detection ui)", time.restart()); //update image views - if(!signature.sensorData().imageRaw().empty() || signature.getWords().size()) + if(!signature.sensorData().imageRaw().empty() || + !loopSignature.sensorData().imageRaw().empty() || + signature.getWords().size()) { cv::Mat refImage = signature.sensorData().imageRaw(); cv::Mat loopImage = loopSignature.sensorData().imageRaw(); @@ -2393,7 +2440,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) //====================== // RGB-D Mapping stuff //====================== - _odometryCorrection = stat.mapCorrection(); + if(!stat.mapCorrection().isNull()) + { + _odometryCorrection = stat.mapCorrection(); + } // update clouds if(stat.poses().size()) { @@ -2939,13 +2989,20 @@ void MainWindow::updateMapCloud( (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) && _occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end(); bool updateOctomap = false; + bool updateElevationMap = false; #ifdef RTABMAP_OCTOMAP updateOctomap = _cloudViewer->isVisible() && _preferencesDialog->isOctomapUpdated() && _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end(); #endif - if(updateGridMap || updateOctomap) +#ifdef RTABMAP_GRIDMAP + updateElevationMap = + _cloudViewer->isVisible() && + _preferencesDialog->getElevationMapShown() > 0 && + _elevationMap->addedNodes().find(iter->first) == _elevationMap->addedNodes().end(); +#endif + if(updateGridMap || updateOctomap || updateElevationMap) { QMap::iterator jter = _cachedSignatures.find(iter->first); if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f) @@ -2956,23 +3013,28 @@ void MainWindow::updateMapCloud( jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty); - _occupancyGrid->addToCache(iter->first, ground, obstacles, empty); - -#ifdef RTABMAP_OCTOMAP - if(updateOctomap) + double resolution = jter->sensorData().gridCellSize(); + if(_preferencesDialog->getGridUIResolution() > jter->sensorData().gridCellSize()) { - if((ground.empty() || ground.channels() > 2) && - (obstacles.empty() || obstacles.channels() > 2)) - { - cv::Point3f viewpoint = jter->sensorData().gridViewPoint(); - _octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint); - } - else if(!ground.empty() || !obstacles.empty()) - { - UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first); - } + resolution = _preferencesDialog->getGridUIResolution(); + pcl::PointCloud::Ptr groundCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(ground)), resolution); + pcl::PointCloud::Ptr obstaclesCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(obstacles)), resolution); + pcl::PointCloud::Ptr emptyCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(empty)), resolution); + if(ground.channels() == 2) + ground = util3d::laserScan2dFromPointCloud(*groundCloud).data(); + else + ground = util3d::laserScanFromPointCloud(*groundCloud).data(); + if(obstacles.channels() == 2) + obstacles = util3d::laserScan2dFromPointCloud(*obstaclesCloud).data(); + else + obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud).data(); + if(empty.channels() == 2) + empty = util3d::laserScan2dFromPointCloud(*emptyCloud).data(); + else + empty = util3d::laserScanFromPointCloud(*emptyCloud).data(); } -#endif + + _cachedLocalMaps.add(iter->first, ground, obstacles, empty, resolution, jter->sensorData().gridViewPoint()); } } @@ -3311,6 +3373,50 @@ void MainWindow::updateMapCloud( } #endif +#ifdef RTABMAP_GRIDMAP + _cloudViewer->removeElevationMap(); + _cloudViewer->removeCloud("elevation_mesh"); + if(_preferencesDialog->getElevationMapShown() > 0) + { + UDEBUG(""); + UTimer time; + _elevationMap->update(poses); + UINFO("Elevation map update time = %fs", time.ticks()); + } + if(stats) + { + stats->insert(std::make_pair("GUI/Elevation Update/ms", (float)timer.restart()*1000.0f)); + } + if(_preferencesDialog->getElevationMapShown() > 0) + { + UDEBUG(""); + UTimer time; + if(_preferencesDialog->getElevationMapShown() == 1) + { + float xMin, yMin, cellSize; + cv::Mat map = _elevationMap->createHeightMap(xMin, yMin, cellSize); + if(!map.empty()) + { + _cloudViewer->addElevationMap(map, cellSize, xMin, yMin, 1); + } + } + else // RGB elevation + { + pcl::PolygonMesh::Ptr mesh = _elevationMap->createTerrainMesh(); + if(mesh->cloud.data.size()) + { + _cloudViewer->addCloudMesh("elevation_mesh", mesh); + } + } + UINFO("Show elevation map time = %fs", time.ticks()); + } + UDEBUG(""); + if(stats) + { + stats->insert(std::make_pair("GUI/Elevation Rendering/ms", (float)timer.restart()*1000.0f)); + } +#endif + // Add landmarks to 3D Map view #if PCL_VERSION_COMPARE(>=, 1, 7, 2) _cloudViewer->removeAllCoordinates("landmark_"); @@ -3357,7 +3463,8 @@ void MainWindow::updateMapCloud( } } cv::Mat map8U; - if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())) + if((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) || + (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) { float xMin, yMin; float resolution = _occupancyGrid->getCellSize(); @@ -3366,12 +3473,11 @@ void MainWindow::updateMapCloud( if(_preferencesDialog->isOctomap2dGrid()) { map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth()); - } else #endif { - if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0) + if(_occupancyGrid->addedNodes().size() || _cachedLocalMaps.size()>0) { _occupancyGrid->update(poses); } @@ -3386,12 +3492,12 @@ void MainWindow::updateMapCloud( //convert to gray scaled map map8U = util3d::convertMap2Image8U(map8S); - if(_preferencesDialog->getGridMapShown()) + if(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown()) { float opacity = _preferencesDialog->getGridMapOpacity(); _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity); } - if(_ui->graphicsView_graphView->isVisible()) + if(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) { _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin); } @@ -3399,6 +3505,8 @@ void MainWindow::updateMapCloud( } _ui->graphicsView_graphView->update(); + _cachedLocalMaps.clear(true); + UDEBUG(""); if(stats) { @@ -4761,15 +4869,15 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags) this->updateSelectSourceMenu(); _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr()); - if(_camera) + if(_sensorCapture) { - if(dynamic_cast(_camera->camera()) != 0) + if(dynamic_cast(_sensorCapture->camera()) != 0) { - _camera->setImageRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate()); + _sensorCapture->setFrameRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate()); } else { - _camera->setImageRate(_preferencesDialog->getGeneralInputRate()); + _sensorCapture->setFrameRate(_preferencesDialog->getGeneralInputRate()); } } @@ -4824,7 +4932,6 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters) void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent) { ULOGGER_DEBUG(""); - _occupancyGrid->parseParameters(_preferencesDialog->getAllParameters()); if(parameters.size()) { for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter) @@ -5154,10 +5261,9 @@ bool MainWindow::eventFilter(QObject *obj, QEvent *event) void MainWindow::updateSelectSourceMenu() { - _ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice); - _ui->actionMore_options->setChecked( _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice || _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages || _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo || _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages || @@ -5192,6 +5298,8 @@ void MainWindow::updateSelectSourceMenu() _ui->actionMYNT_EYE_S_SDK->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoMyntEye); _ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); + _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); + _ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16); } void MainWindow::changeImgRateSetting() @@ -5609,29 +5717,6 @@ void MainWindow::editDatabase() } } -Camera * MainWindow::createCamera( - Camera ** odomSensor, - Transform & odomSensorExtrinsics, - double & odomSensorTimeOffset, - float & odomSensorScaleFactor) -{ - Camera * camera = _preferencesDialog->createCamera(); - - if(camera && - _preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef && - _preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() && - !(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 && - _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2)) - { - UINFO("Create Odom Sensor %d (camera = %d)", - _preferencesDialog->getOdomSourceDriver(), - _preferencesDialog->getSourceDriver()); - *odomSensor = _preferencesDialog->createOdomSensor(odomSensorExtrinsics, odomSensorTimeOffset, odomSensorScaleFactor); - } - - return camera; -} - void MainWindow::startDetection() { UDEBUG(""); @@ -5709,18 +5794,19 @@ void MainWindow::startDetection() UDEBUG(""); Q_EMIT stateChanged(kStartingDetection); - if(_camera != 0) + if(_sensorCapture != 0) { QMessageBox::warning(this, tr("RTAB-Map"), tr("A camera is running, stop it first.")); - UWARN("_camera is not null... it must be stopped first"); + UWARN("_sensorCapture is not null... it must be stopped first"); Q_EMIT stateChanged(kInitialized); return; } // Adjust pre-requirements - if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef) + if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef && + _preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcUndef) { QMessageBox::warning(this, tr("RTAB-Map"), @@ -5733,29 +5819,75 @@ void MainWindow::startDetection() double poseTimeOffset = 0.0; float scaleFactor = 0.0f; + double waitTime = 0.1; Transform extrinsics; - Camera * odomSensor = 0; - Camera * camera = this->createCamera(&odomSensor, extrinsics, poseTimeOffset, scaleFactor); - if(!camera) + SensorCapture * odomSensor = 0; + Camera * camera = 0; + Lidar * lidar = 0; + + if(_preferencesDialog->getLidarSourceDriver() != PreferencesDialog::kSrcUndef) { - Q_EMIT stateChanged(kInitialized); - return; + lidar = _preferencesDialog->createLidar(); + if(!lidar) + { + Q_EMIT stateChanged(kInitialized); + return; + } } - if(odomSensor) + if(!lidar || _preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcUndef) { - _camera = new CameraThread(camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, _preferencesDialog->isOdomSensorAsGt(), parameters); + camera = _preferencesDialog->createCamera(); + if(!camera) + { + delete lidar; + Q_EMIT stateChanged(kInitialized); + return; + } } - else + + if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef) { - _camera = new CameraThread(camera, _preferencesDialog->isOdomSensorAsGt(), parameters); + if(camera == 0 || + (_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() && + !(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 && + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2))) + { + UINFO("Create Odom Sensor %d (camera = %d)", + _preferencesDialog->getOdomSourceDriver(), + _preferencesDialog->getSourceDriver()); + odomSensor = _preferencesDialog->createOdomSensor(extrinsics, poseTimeOffset, scaleFactor, waitTime); + if(!odomSensor) + { + delete camera; + delete lidar; + Q_EMIT stateChanged(kInitialized); + return; + } + } + else if(camera->odomProvided()) + { + UINFO("The camera is also the odometry sensor (camera=%d odom=%d).", + _preferencesDialog->getSourceDriver(), + _preferencesDialog->getOdomSourceDriver()); + odomSensor = camera; + } } - _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); - _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); - _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); - _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); - _camera->setScanParameters( + + _sensorCapture = new SensorCaptureThread(lidar, camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, waitTime, parameters); + + _sensorCapture->setOdomAsGroundTruth(_preferencesDialog->isOdomSensorAsGt()); + _sensorCapture->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); + _sensorCapture->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); + _sensorCapture->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); + _sensorCapture->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod()); + if(_preferencesDialog->isSourceFeatureDetection()) + { + _sensorCapture->enableFeatureDetection(parameters); + } + _sensorCapture->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); + _sensorCapture->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); + _sensorCapture->setScanParameters( _preferencesDialog->isSourceScanFromDepth(), _preferencesDialog->getSourceScanDownsampleStep(), _preferencesDialog->getSourceScanRangeMin(), @@ -5763,32 +5895,33 @@ void MainWindow::startDetection() _preferencesDialog->getSourceScanVoxelSize(), _preferencesDialog->getSourceScanNormalsK(), _preferencesDialog->getSourceScanNormalsRadius(), - (float)_preferencesDialog->getSourceScanForceGroundNormalsUp()); + (float)_preferencesDialog->getSourceScanForceGroundNormalsUp(), + _preferencesDialog->isSourceScanDeskewing()); if(_preferencesDialog->getIMUFilteringStrategy()>0 && dynamic_cast(camera) == 0) { - _camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); + _sensorCapture->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); } if(_preferencesDialog->isDepthFilteringAvailable()) { if(_preferencesDialog->isBilateralFiltering()) { - _camera->enableBilateralFiltering( + _sensorCapture->enableBilateralFiltering( _preferencesDialog->getBilateralSigmaS(), _preferencesDialog->getBilateralSigmaR()); } - _camera->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString()); + _sensorCapture->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString()); } //Create odometry thread if rgbd slam if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str())) { // Require calibrated camera - if(!camera->isCalibrated()) + if(camera && !camera->isCalibrated()) { UWARN("Camera is not calibrated!"); Q_EMIT stateChanged(kInitialized); - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; int button = QMessageBox::question(this, tr("Camera is not calibrated!"), @@ -5816,7 +5949,7 @@ void MainWindow::startDetection() _imuThread = 0; } - if((!_camera->odomProvided() || _preferencesDialog->isOdomSensorAsGt()) && !_preferencesDialog->isOdomDisabled()) + if(!_sensorCapture->odomProvided() && !_preferencesDialog->isOdomDisabled()) { ParametersMap odomParameters = parameters; if(_preferencesDialog->getOdomRegistrationApproach() < 3) @@ -5843,35 +5976,28 @@ void MainWindow::startDetection() _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) && !_preferencesDialog->getIMUPath().isEmpty()) { - if( odomStrategy != Odometry::kTypeOkvis && - odomStrategy != Odometry::kTypeMSCKF && - odomStrategy != Odometry::kTypeVINS && - odomStrategy != Odometry::kTypeOpenVINS) + _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform()); + if(_preferencesDialog->getIMUFilteringStrategy()>0) { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok); + _imuThread->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); } - else + if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString())) { - _imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform()); - if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString())) - { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok); - delete _camera; - _camera = 0; - delete _imuThread; - _imuThread = 0; - return; - } + QMessageBox::warning(this, tr("Source IMU Path"), + tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok); + delete _sensorCapture; + _sensorCapture = 0; + delete _imuThread; + _imuThread = 0; + return; } } Odometry * odom = Odometry::create(odomParameters); _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize()); UEventsManager::addHandler(_odomThread); - UEventsManager::createPipe(_camera, _odomThread, "CameraEvent"); - UEventsManager::createPipe(_camera, this, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _odomThread, "SensorEvent"); + UEventsManager::createPipe(_sensorCapture, this, "SensorEvent"); if(_imuThread) { UEventsManager::createPipe(_imuThread, _odomThread, "IMUEvent"); @@ -5881,9 +6007,9 @@ void MainWindow::startDetection() } } - if(_dataRecorder && _camera && _odomThread) + if(_dataRecorder && _sensorCapture && _odomThread) { - UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent"); } _lastOdomPose.setNull(); @@ -5903,13 +6029,25 @@ void MainWindow::startDetection() "progress will not be shown in the GUI.")); } - _occupancyGrid->clear(); - _occupancyGrid->parseParameters(parameters); + // Override map resolution for UI + if(_preferencesDialog->getGridUIResolution()>0) + { + uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution()))); + } + + _cachedLocalMaps.clear(); + + delete _occupancyGrid; + _occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters); #ifdef RTABMAP_OCTOMAP - UASSERT(_octomap != 0); delete _octomap; - _octomap = new OctoMap(parameters); + _octomap = new OctoMap(&_cachedLocalMaps, parameters); +#endif + +#ifdef RTABMAP_GRIDMAP + delete _elevationMap; + _elevationMap = new GridMap(&_cachedLocalMaps, parameters); #endif // clear odometry visual stuff @@ -5925,7 +6063,7 @@ void MainWindow::startDetection() // Could not be in the main thread here! (see handleEvents()) void MainWindow::pauseDetection() { - if(_camera) + if(_sensorCapture) { if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier)) { @@ -5959,13 +6097,13 @@ void MainWindow::pauseDetection() void MainWindow::stopDetection() { - if(!_camera && !_odomThread) + if(!_sensorCapture && !_odomThread) { return; } if(_state == kDetecting && - (_camera && _camera->isRunning()) ) + (_sensorCapture && _sensorCapture->isRunning()) ) { QMessageBox::StandardButton button = QMessageBox::question(this, tr("Stopping process..."), tr("Are you sure you want to stop the process?"), QMessageBox::Yes|QMessageBox::No, QMessageBox::No); @@ -5982,9 +6120,9 @@ void MainWindow::stopDetection() _imuThread->join(true); } - if(_camera) + if(_sensorCapture) { - _camera->join(true); + _sensorCapture->join(true); } if(_odomThread) @@ -5999,10 +6137,10 @@ void MainWindow::stopDetection() delete _imuThread; _imuThread = 0; } - if(_camera) + if(_sensorCapture) { - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; } if(_odomThread) { @@ -6193,8 +6331,8 @@ void MainWindow::exportPoses(int format) std::multimap links; if(localTransforms.empty()) { - poses = std::map(_currentPosesMap.lower_bound(1), _currentPosesMap.end()); - links = std::multimap(_currentLinksMap.lower_bound(1), _currentLinksMap.end()); + poses = std::map(_currentPosesMap.begin(), _currentPosesMap.end()); + links = std::multimap(_currentLinksMap.begin(), _currentLinksMap.end()); } else { @@ -6215,14 +6353,24 @@ void MainWindow::exportPoses(int format) } } - if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported + if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported { - UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format); - std::map::iterator iter=poses.begin(); - while(iter!=poses.end() && iter->first < 0) + UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format); + for(std::map::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;) { poses.erase(iter++); } + for(std::multimap::iterator iter=links.begin(); iter!=links.end();) + { + if(iter->second.from() < 0 || iter->second.to() < 0) + { + links.erase(iter++); + } + else + { + ++iter; + } + } } std::map stamps; @@ -6230,7 +6378,11 @@ void MainWindow::exportPoses(int format) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - if(_cachedSignatures.contains(iter->first)) + if(iter->first < 0 && format == 11) // in case of landmarks + { + stamps.insert(std::make_pair(iter->first, 0)); + } + else if(_cachedSignatures.contains(iter->first)) { stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp())); } @@ -6969,11 +7121,6 @@ void MainWindow::updateEditMenu() } } -void MainWindow::selectStream() -{ - _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice); -} - void MainWindow::selectOpenni() { _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL); @@ -7076,6 +7223,16 @@ void MainWindow::selectDepthAIOAKDLite() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 0); // variant 0=no IMU } +void MainWindow::selectDepthAIOAKDPro() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 2); // variant 2=IMU+color +} + +void MainWindow::selectVLP16() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16); +} + void MainWindow::dumpTheMemory() { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory)); @@ -7212,6 +7369,11 @@ void MainWindow::updateCacheFromDatabase(const QString & path) signatures.insert(std::make_pair((*iter)->id(), *(*iter))); delete *iter; } + if(_currentPosesMap.empty() && _currentLinksMap.empty()) + { + _currentPosesMap = driver->loadOptimizedPoses(); + driver->getAllLinks(_currentLinksMap, true, true); + } RtabmapEvent3DMap event(signatures, _currentPosesMap, _currentLinksMap); processRtabmapEvent3DMap(event); } @@ -7379,11 +7541,11 @@ void MainWindow::clearTheCache() _ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor()); _ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor()); _multiSessionLocWidget->clear(); + _cachedLocalMaps.clear(); #ifdef RTABMAP_OCTOMAP // re-create one if the resolution has changed - UASSERT(_octomap != 0); delete _octomap; - _octomap = new OctoMap(_preferencesDialog->getAllParameters()); + _octomap = new OctoMap(&_cachedLocalMaps, _preferencesDialog->getAllParameters()); #endif _occupancyGrid->clear(); _rectCameraModels.clear(); @@ -7415,7 +7577,8 @@ void MainWindow::saveFigures() { QList curvesPerFigure; QStringList curveNames; - _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames); + QStringList curveThresholds; + _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames, curveThresholds); QStringList curvesPerFigureStr; for(int i=0; isaveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" ")); _preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" ")); + _preferencesDialog->saveCustomConfig("Figures", "thresholds", curveThresholds.join(" ")); } void MainWindow::loadFigures() { QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts"); QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves"); + QString curveThresholds = _preferencesDialog->loadCustomConfig("Figures", "thresholds"); if(!curvesPerFigure.isEmpty()) { QStringList curvesPerFigureList = curvesPerFigure.split(" "); QStringList curvesNamesList = curveNames.split(" "); + QStringList curveThresholdsList = curveThresholds.split(" "); + if(curveThresholdsList[0].isEmpty()) { + curveThresholdsList.clear(); + } + UASSERT(curveThresholdsList.isEmpty() || curveThresholdsList.size() == curvesNamesList.size()); int j=0; for(int i=0; istatsToolBox->addCurve(curvesNamesList[j++].replace('_', ' ')); for(int k=1; kstatsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false); + if(curveThresholdsList.size()) + { + bool ok = false; + double thresholdValue = curveThresholdsList[j].toDouble(&ok); + if(ok) + { + _ui->statsToolBox->addThreshold(curvesNamesList[j++].replace('_', ' '), thresholdValue); + } + else + { + _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false); + } + } + else + { + _ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false); + } } } } @@ -8157,9 +8343,9 @@ void MainWindow::dataRecorder() this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed())); _dataRecorder->show(); _dataRecorder->registerToEventsManager(); - if(_camera) + if(_sensorCapture) { - UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent"); } _ui->actionData_recorder->setEnabled(false); } @@ -8426,9 +8612,9 @@ void MainWindow::changeState(MainWindow::State newState) _databaseUpdated = true; // if a new database is used, it won't be empty anymore... - if(_camera) + if(_sensorCapture) { - _camera->start(); + _sensorCapture->start(); if(_imuThread) { _imuThread->start(); @@ -8462,9 +8648,9 @@ void MainWindow::changeState(MainWindow::State newState) _elapsedTime->start(); _oneSecondTimer->start(); - if(_camera) + if(_sensorCapture) { - _camera->start(); + _sensorCapture->start(); if(_imuThread) { _imuThread->start(); @@ -8500,13 +8686,13 @@ void MainWindow::changeState(MainWindow::State newState) _oneSecondTimer->stop(); // kill sensors - if(_camera) + if(_sensorCapture) { if(_imuThread) { _imuThread->join(true); } - _camera->join(true); + _sensorCapture->join(true); } } break; diff --git a/guilib/src/ParametersToolBox.cpp b/guilib/src/ParametersToolBox.cpp index d23242056a..1bd583d25e 100644 --- a/guilib/src/ParametersToolBox.cpp +++ b/guilib/src/ParametersToolBox.cpp @@ -296,19 +296,14 @@ void ParametersToolBox::addParameter( { addParameter(layout, key.c_str(), QString::fromStdString(value)); } - else if(type.compare("int") == 0) + else if(type.compare("int") == 0 || + type.compare("uint") == 0 || + type.compare("unsigned int") == 0) { addParameter(layout, key.c_str(), uStr2Int(value)); } - else if(type.compare("uint") == 0) - { - addParameter(layout, key.c_str(), uStr2Int(value)); - } - else if(type.compare("double") == 0) - { - addParameter(layout, key.c_str(), uStr2Double(value)); - } - else if(type.compare("float") == 0) + else if(type.compare("double") == 0 || + type.compare("float") == 0) { addParameter(layout, key.c_str(), uStr2Double(value)); } @@ -316,6 +311,10 @@ void ParametersToolBox::addParameter( { addParameter(layout, key.c_str(), uStr2Bool(value)); } + else + { + UWARN("Not implemented type \"%s\" for parameter \"%s\". Parameter is not added to toolbox.", type.c_str(), key.c_str()); + } } void ParametersToolBox::addParameter(QVBoxLayout * layout, diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 4910410e6c..c802459f85 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -13,6 +13,7 @@ modification, are permitted provided that the following conditions are met: 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 @@ -25,6 +26,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Should be first on windows to avoid "WinSock.h has already been included" error +#include "rtabmap/core/lidar/LidarVLP16.h" + #include "rtabmap/gui/PreferencesDialog.h" #include "rtabmap/gui/DatabaseViewer.h" #include "rtabmap/gui/OdometryViewer.h" @@ -55,9 +59,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Odometry.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/CameraRGBD.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraStereo.h" +#include "rtabmap/core/IMUFilter.h" #include "rtabmap/core/IMUThread.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" @@ -83,7 +87,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/utilite/UPlot.h" +// Presets +#include "camera_tof_icp_ini.h" +#include "lidar3d_icp_ini.h" + #include +#include #if CV_MAJOR_VERSION < 3 #ifdef HAVE_OPENCV_GPU #include @@ -157,6 +166,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->groupBox_octomap->setEnabled(false); #endif +#ifndef RTABMAP_GRIDMAP + _ui->checkBox_elevation_shown->setChecked(false); + _ui->checkBox_elevation_shown->setEnabled(false); + _ui->label_show_elevation->setEnabled(false); +#endif + #ifndef RTABMAP_REALSENSE_SLAM _ui->checkbox_realsenseOdom->setChecked(false); _ui->checkbox_realsenseOdom->setEnabled(false); @@ -256,6 +271,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_detector_strategy->setItemData(15, 0, Qt::UserRole - 1); _ui->vis_feature_detector->setItemData(15, 0, Qt::UserRole - 1); _ui->reextract_nn->setItemData(6, 0, Qt::UserRole - 1); + _ui->comboBox_globalDescriptorExtractor->setItemData(1, 0, Qt::UserRole - 1); #endif #if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1)) @@ -380,6 +396,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1); _ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1); } + else if(CameraStereoZed::sdkVersion() < 4) + { + _ui->comboBox_stereoZed_resolution->setItemData(2, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1); + _ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1); + } if (!CameraStereoTara::available()) { _ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1); @@ -430,6 +453,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->comboBox_odom_sensor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(updateSourceGrpVisibility())); this->resetSettings(_ui->groupBox_source0); _ui->predictionPlot->showLegend(false); @@ -444,9 +470,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->pushButton_loadConfig, SIGNAL(clicked()), this, SLOT(loadConfigFrom())); connect(_ui->pushButton_saveConfig, SIGNAL(clicked()), this, SLOT(saveConfigTo())); connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig())); + connect(_ui->pushButton_presets_camera_tof_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); + connect(_ui->pushButton_presets_lidar_3d_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView())); connect(_ui->pushButton_testOdometry, SIGNAL(clicked()), this, SLOT(testOdometry())); connect(_ui->pushButton_test_camera, SIGNAL(clicked()), this, SLOT(testCamera())); + connect(_ui->pushButton_test_lidar, SIGNAL(clicked()), this, SLOT(testLidar())); // General panel connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel())); @@ -611,8 +640,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_subtractFilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_subtractFilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_elevation_shown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -796,23 +827,35 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->spinBox_stereoMyntEye_irControl, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_output_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_depthai_conf_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_depthai_lrc_threshold, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_depthai_extended_disparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_subpixel_fractional_bits, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_disparity_companding, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_depthai_use_spec_translation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_alpha_scaling, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_dot_intensity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_flood_intensity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_depthai_detect_features, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_depthai_blob_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_depthai_blob_path, SIGNAL(clicked()), this, SLOT(selectSourceDepthaiBlobPath())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_source_histogramMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_source_feature_detection, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple())); connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath())); connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path())); - connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_k4a_mkv, SIGNAL(clicked()), this, SLOT(selectSourceMKVPath())); connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel())); connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel())); @@ -833,6 +876,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_odomSourceDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_odom_sensor_time_offset, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_odom_sensor_scale_factor, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_odom_sensor_wait_time, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_odom_sensor_use_as_gt, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -841,6 +885,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_imuFilter_baseFrameConversion, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_publishInterIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_lidar_src, SLOT(setCurrentIndex(int))); + _ui->stackedWidget_lidar_src->setCurrentIndex(_ui->comboBox_lidar_src->currentIndex()); + connect(_ui->checkBox_source_scanDeskewing, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_source_scanRangeMin, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); @@ -850,6 +898,17 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_source_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_source_scanNormalsForceGroundUp, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_lidar_local_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_vlp16_pcap_path, SIGNAL(clicked()), this, SLOT(selectVlp16PcapPath())); + connect(_ui->lineEdit_vlp16_pcap_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip1, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip4, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_port, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_organized, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_hostTime, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_stamp_last, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //Rtabmap basic connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double))); @@ -930,6 +989,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str()); _ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str()); _ui->memCovOffDiagIgnored->setObjectName(Parameters::kMemCovOffDiagIgnored().c_str()); + _ui->general_checkBox_rotateImagesUpsideUp->setObjectName(Parameters::kMemRotateImagesUpsideUp().c_str()); // Database _ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str()); @@ -943,6 +1003,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Create hypotheses _ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str()); _ui->general_doubleSpinBox_loopRatio->setObjectName(Parameters::kRtabmapLoopRatio().c_str()); + _ui->comboBox_globalDescriptorExtractor->setObjectName(Parameters::kMemGlobalDescriptorStrategy().c_str()); + connect(_ui->comboBox_globalDescriptorExtractor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGlobalDescriptorVisibility())); //Bayes _ui->general_doubleSpinBox_vp->setObjectName(Parameters::kBayesVirtualPlacePriorThr().c_str()); @@ -1074,6 +1136,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_gms_withScale->setObjectName(Parameters::kGMSWithScale().c_str()); _ui->gms_thresholdFactor->setObjectName(Parameters::kGMSThresholdFactor().c_str()); + // PyDescriptor + _ui->lineEdit_pydescriptor_path->setObjectName(Parameters::kPyDescriptorPath().c_str()); + connect(_ui->toolButton_pydescriptor_path, SIGNAL(clicked()), this, SLOT(changePyDescriptorPath())); + _ui->pydescriptor_dim->setObjectName(Parameters::kPyDescriptorDim().c_str()); + // verifyHypotheses _ui->groupBox_vh_epipolar2->setObjectName(Parameters::kVhEpEnabled().c_str()); _ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str()); @@ -1086,6 +1153,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str()); _ui->rgdb_linearSpeedUpdate->setObjectName(Parameters::kRGBDLinearSpeedUpdate().c_str()); _ui->rgdb_angularSpeedUpdate->setObjectName(Parameters::kRGBDAngularSpeedUpdate().c_str()); + _ui->rgbd_forceOdom3DoF->setObjectName(Parameters::kRGBDForceOdom3DoF().c_str()); _ui->rgbd_savedLocalizationIgnored->setObjectName(Parameters::kRGBDStartAtOrigin().c_str()); _ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str()); _ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str()); @@ -1112,6 +1180,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str()); _ui->comboBox_gtsam_optimizer->setObjectName(Parameters::kGTSAMOptimizer().c_str()); + _ui->gtsam_incremental->setObjectName(Parameters::kGTSAMIncremental().c_str()); + _ui->gtsam_incremental_threshold->setObjectName(Parameters::kGTSAMIncRelinearizeThreshold().c_str()); + _ui->gtsam_incremental_skip->setObjectName(Parameters::kGTSAMIncRelinearizeSkip().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str()); @@ -1139,6 +1210,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str()); _ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str()); _ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str()); + _ui->checkbox_localizationSmoothing->setObjectName(Parameters::kRGBDLocalizationSmoothing().c_str()); + _ui->doubleSpinBox_localizationPriorError->setObjectName(Parameters::kRGBDLocalizationPriorError().c_str()); // Registration _ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str()); @@ -1160,7 +1233,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str()); _ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str()); _ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str()); + _ui->loopClosure_pnpVarMedianRatio->setObjectName(Parameters::kVisPnPVarianceMedianRatio().c_str()); _ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str()); + _ui->loopClosure_pnpSamplingPolicy->setObjectName(Parameters::kVisPnPSamplingPolicy().c_str()); + _ui->loopClosure_pnpSplitLinearCovComponents->setObjectName(Parameters::kVisPnPSplitLinearCovComponents().c_str()); _ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str()); connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility())); _ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str()); @@ -1193,7 +1269,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str()); _ui->loopClosure_icpRangeMin->setObjectName(Parameters::kIcpRangeMin().c_str()); _ui->loopClosure_icpRangeMax->setObjectName(Parameters::kIcpRangeMax().c_str()); + _ui->loopClosure_icpFiltersEnabled->setObjectName(Parameters::kIcpFiltersEnabled().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str()); + _ui->loopClosure_icpReciprocalCorrespondences->setObjectName(Parameters::kIcpReciprocalCorrespondences().c_str()); _ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str()); _ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str()); _ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str()); @@ -1247,7 +1325,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_grid_unknownSpaceFilled->setObjectName(Parameters::kGridScan2dUnknownSpaceFilled().c_str()); _ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str()); - _ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str()); _ui->doubleSpinBox_grid_updateError->setObjectName(Parameters::kGridGlobalUpdateError().c_str()); _ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str()); _ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str()); @@ -1277,6 +1354,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str()); _ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str()); _ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str()); + _ui->odom_lidar_deskewing->setObjectName(Parameters::kOdomDeskewing().c_str()); //Odometry Frame to Map _ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str()); @@ -1369,6 +1447,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_OdomORBSLAMFps->setObjectName(Parameters::kOdomORBSLAMFps().c_str()); _ui->spinBox_OdomORBSLAMMaxFeatures->setObjectName(Parameters::kOdomORBSLAMMaxFeatures().c_str()); _ui->spinBox_OdomORBSLAMMapSize->setObjectName(Parameters::kOdomORBSLAMMapSize().c_str()); + _ui->groupBox_OdomORBSLAMInertial->setObjectName(Parameters::kOdomORBSLAMInertial().c_str()); + _ui->doubleSpinBox_ORBSLAMGyroNoise->setObjectName(Parameters::kOdomORBSLAMGyroNoise().c_str()); + _ui->doubleSpinBox_ORBSLAMAccNoise->setObjectName(Parameters::kOdomORBSLAMAccNoise().c_str()); + _ui->doubleSpinBox_ORBSLAMGyroWalk->setObjectName(Parameters::kOdomORBSLAMGyroWalk().c_str()); + _ui->doubleSpinBox_ORBSLAMAccWalk->setObjectName(Parameters::kOdomORBSLAMAccWalk().c_str()); + _ui->doubleSpinBox_ORBSLAMSamplingRate->setObjectName(Parameters::kOdomORBSLAMSamplingRate().c_str()); // Odometry Okvis _ui->lineEdit_OdomOkvisPath->setObjectName(Parameters::kOdomOKVISConfigPath().c_str()); @@ -1415,6 +1499,70 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->lineEdit_OdomVinsPath->setObjectName(Parameters::kOdomVINSConfigPath().c_str()); connect(_ui->toolButton_OdomVinsPath, SIGNAL(clicked()), this, SLOT(changeOdometryVINSConfigPath())); + // Odometry OpenVINS + _ui->checkBox_OdomOpenVINSUseStereo->setObjectName(Parameters::kOdomOpenVINSUseStereo().c_str()); + _ui->checkBox_OdomOpenVINSUseKLT->setObjectName(Parameters::kOdomOpenVINSUseKLT().c_str()); + _ui->spinBox_OdomOpenVINSNumPts->setObjectName(Parameters::kOdomOpenVINSNumPts().c_str()); + _ui->spinBox_OdomOpenVINSMinPxDist->setObjectName(Parameters::kOdomOpenVINSMinPxDist().c_str()); + _ui->checkBox_OdomOpenVINSFiTriangulate1d->setObjectName(Parameters::kOdomOpenVINSFiTriangulate1d().c_str()); + _ui->checkBox_OdomOpenVINSFiRefineFeatures->setObjectName(Parameters::kOdomOpenVINSFiRefineFeatures().c_str()); + _ui->spinBox_OdomOpenVINSFiMaxRuns->setObjectName(Parameters::kOdomOpenVINSFiMaxRuns().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxBaseline->setObjectName(Parameters::kOdomOpenVINSFiMaxBaseline().c_str()); + _ui->doubleSpinBox_OdomOpenVINSFiMaxCondNumber->setObjectName(Parameters::kOdomOpenVINSFiMaxCondNumber().c_str()); + + _ui->checkBox_OdomOpenVINSUseFEJ->setObjectName(Parameters::kOdomOpenVINSUseFEJ().c_str()); + _ui->comboBox_OdomOpenVINSIntegration->setObjectName(Parameters::kOdomOpenVINSIntegration().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamExtrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamExtrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibCamIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibCamTimeoffset->setObjectName(Parameters::kOdomOpenVINSCalibCamTimeoffset().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUIntrinsics->setObjectName(Parameters::kOdomOpenVINSCalibIMUIntrinsics().c_str()); + _ui->checkBox_OdomOpenVINSCalibIMUGSensitivity->setObjectName(Parameters::kOdomOpenVINSCalibIMUGSensitivity().c_str()); + _ui->spinBox_OdomOpenVINSMaxClones->setObjectName(Parameters::kOdomOpenVINSMaxClones().c_str()); + _ui->spinBox_OdomOpenVINSMaxSLAM->setObjectName(Parameters::kOdomOpenVINSMaxSLAM().c_str()); + _ui->spinBox_OdomOpenVINSMaxSLAMInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxSLAMInUpdate().c_str()); + _ui->spinBox_OdomOpenVINSMaxMSCKFInUpdate->setObjectName(Parameters::kOdomOpenVINSMaxMSCKFInUpdate().c_str()); + _ui->comboBox_OdomOpenVINSFeatRepMSCKF->setObjectName(Parameters::kOdomOpenVINSFeatRepMSCKF().c_str()); + _ui->comboBox_OdomOpenVINSFeatRepSLAM->setObjectName(Parameters::kOdomOpenVINSFeatRepSLAM().c_str()); + _ui->doubleSpinBox_OdomOpenVINSDtSLAMDelay->setObjectName(Parameters::kOdomOpenVINSDtSLAMDelay().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGravityMag->setObjectName(Parameters::kOdomOpenVINSGravityMag().c_str()); + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setObjectName(Parameters::kOdomOpenVINSLeftMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSLeftMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSLeftMask())); + _ui->lineEdit_OdomOpenVINSRightMaskPath->setObjectName(Parameters::kOdomOpenVINSRightMaskPath().c_str()); + connect(_ui->toolButton_OdomOpenVINSRightMaskPath, SIGNAL(clicked()), this, SLOT(changeOdometryOpenVINSRightMask())); + + _ui->doubleSpinBox_OdomOpenVINSInitWindowTime->setObjectName(Parameters::kOdomOpenVINSInitWindowTime().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitIMUThresh->setObjectName(Parameters::kOdomOpenVINSInitIMUThresh().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitMaxDisparity->setObjectName(Parameters::kOdomOpenVINSInitMaxDisparity().c_str()); + _ui->spinBox_OdomOpenVINSInitMaxFeatures->setObjectName(Parameters::kOdomOpenVINSInitMaxFeatures().c_str()); + _ui->checkBox_OdomOpenVINSInitDynUse->setObjectName(Parameters::kOdomOpenVINSInitDynUse().c_str()); + _ui->checkBox_OdomOpenVINSInitDynMLEOptCalib->setObjectName(Parameters::kOdomOpenVINSInitDynMLEOptCalib().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxIter->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxIter().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMLEMaxTime->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxTime().c_str()); + _ui->spinBox_OdomOpenVINSInitDynMLEMaxThreads->setObjectName(Parameters::kOdomOpenVINSInitDynMLEMaxThreads().c_str()); + _ui->spinBox_OdomOpenVINSInitDynNumPose->setObjectName(Parameters::kOdomOpenVINSInitDynNumPose().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinDeg->setObjectName(Parameters::kOdomOpenVINSInitDynMinDeg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationOri->setObjectName(Parameters::kOdomOpenVINSInitDynInflationOri().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationVel->setObjectName(Parameters::kOdomOpenVINSInitDynInflationVel().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBg->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBg().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynInflationBa->setObjectName(Parameters::kOdomOpenVINSInitDynInflationBa().c_str()); + _ui->doubleSpinBox_OdomOpenVINSInitDynMinRecCond->setObjectName(Parameters::kOdomOpenVINSInitDynMinRecCond().c_str()); + + _ui->checkBox_OdomOpenVINSTryZUPT->setObjectName(Parameters::kOdomOpenVINSTryZUPT().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSZUPTChi2Multiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTMaxVelodicy->setObjectName(Parameters::kOdomOpenVINSZUPTMaxVelodicy().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTNoiseMultiplier->setObjectName(Parameters::kOdomOpenVINSZUPTNoiseMultiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSZUPTMaxDisparity->setObjectName(Parameters::kOdomOpenVINSZUPTMaxDisparity().c_str()); + _ui->checkBox_OdomOpenVINSZUPTOnlyAtBeginning->setObjectName(Parameters::kOdomOpenVINSZUPTOnlyAtBeginning().c_str()); + + _ui->doubleSpinBox_OdomOpenVINSAccelerometerNoiseDensity->setObjectName(Parameters::kOdomOpenVINSAccelerometerNoiseDensity().c_str()); + _ui->doubleSpinBox_OdomOpenVINSAccelerometerRandomWalk->setObjectName(Parameters::kOdomOpenVINSAccelerometerRandomWalk().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGyroscopeNoiseDensity->setObjectName(Parameters::kOdomOpenVINSGyroscopeNoiseDensity().c_str()); + _ui->doubleSpinBox_OdomOpenVINSGyroscopeRandomWalk->setObjectName(Parameters::kOdomOpenVINSGyroscopeRandomWalk().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpMSCKFSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpMSCKFSigmaPx().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpMSCKFChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpSLAMSigmaPx->setObjectName(Parameters::kOdomOpenVINSUpSLAMSigmaPx().c_str()); + _ui->doubleSpinBox_OdomOpenVINSUpSLAMChi2Multiplier->setObjectName(Parameters::kOdomOpenVINSUpSLAMChi2Multiplier().c_str()); + //Stereo _ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str()); _ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str()); @@ -1504,6 +1652,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : this->setupTreeView(); _obsoletePanels = kPanelAll; + + updateFeatureMatchingVisibility(); + updateGlobalDescriptorVisibility(); } PreferencesDialog::~PreferencesDialog() { @@ -1523,6 +1674,7 @@ void PreferencesDialog::init(const QString & iniFilePath) } this->readSettings(iniFilePath); + this->writeSettings(getTmpIniFilePath()); _initialized = true; @@ -1886,8 +2038,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) } else if(groupBox->objectName() == _ui->groupBox_gridMap2->objectName()) { + _ui->doubleSpinBox_map_resolution->setValue(0); _ui->checkBox_map_shown->setChecked(false); _ui->doubleSpinBox_map_opacity->setValue(0.75); + _ui->checkBox_elevation_shown->setCheckState(Qt::Unchecked); _ui->groupBox_octomap->setChecked(false); _ui->spinBox_octomap_treeDepth->setValue(16); @@ -1968,6 +2122,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->spinBox_source_imageDecimation->setValue(1); + _ui->comboBox_source_histogramMethod->setCurrentIndex(0); + _ui->checkbox_source_feature_detection->setChecked(false); _ui->checkbox_stereo_depthGenerated->setChecked(false); _ui->checkBox_stereo_exposureCompensation->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); @@ -2005,9 +2161,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->lineEdit_rs2_jsonFile->clear(); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); - _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0); - _ui->comboBox_k4a_framerate->setCurrentIndex(2); - _ui->comboBox_k4a_depth_resolution->setCurrentIndex(2); + _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(0); + _ui->comboBox_k4a_framerate->setCurrentIndex(2); + _ui->comboBox_k4a_depth_resolution->setCurrentIndex(2); _ui->checkbox_k4a_irDepth->setChecked(false); _ui->lineEdit_k4a_mkv->clear(); _ui->source_checkBox_useMKVStamps->setChecked(true); @@ -2032,7 +2188,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_stereo_right_device->setValue(-1); _ui->spinBox_stereousbcam_streamWidth->setValue(0); _ui->spinBox_stereousbcam_streamHeight->setValue(0); - _ui->comboBox_stereoZed_resolution->setCurrentIndex(2); + _ui->comboBox_stereoZed_resolution->setCurrentIndex(CameraStereoZed::sdkVersion()<4?3:6); _ui->comboBox_stereoZed_quality->setCurrentIndex(1); _ui->checkbox_stereoZed_selfCalibration->setChecked(true); _ui->comboBox_stereoZed_sensingMode->setCurrentIndex(0); @@ -2048,10 +2204,19 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_stereoMyntEye_contrast->setValue(116); _ui->spinBox_stereoMyntEye_irControl->setValue(0); _ui->comboBox_depthai_resolution->setCurrentIndex(1); - _ui->checkBox_depthai_depth->setChecked(false); - _ui->spinBox_depthai_confidence->setValue(200); + _ui->comboBox_depthai_output_mode->setCurrentIndex(0); + _ui->spinBox_depthai_conf_threshold->setValue(200); + _ui->checkBox_depthai_extended_disparity->setChecked(false); + _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(2); + _ui->comboBox_depthai_disparity_companding->setCurrentIndex(1); + _ui->spinBox_depthai_lrc_threshold->setValue(5); + _ui->checkBox_depthai_use_spec_translation->setChecked(false); + _ui->doubleSpinBox_depthai_alpha_scaling->setValue(-1); _ui->checkBox_depthai_imu_published->setChecked(true); - _ui->checkBox_depthai_imu_firmware_update->setChecked(false); + _ui->doubleSpinBox_depthai_dot_intensity->setValue(0.0); + _ui->doubleSpinBox_depthai_flood_intensity->setValue(0.0); + _ui->comboBox_depthai_detect_features->setCurrentIndex(0); + _ui->lineEdit_depthai_blob_path->clear(); _ui->checkBox_cameraImages_configForEachFrame->setChecked(false); _ui->checkBox_cameraImages_timestamps->setChecked(false); @@ -2075,6 +2240,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->lineEdit_odomSourceDevice->setText(""); _ui->doubleSpinBox_odom_sensor_time_offset->setValue(0.0); _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(1); + _ui->doubleSpinBox_odom_sensor_wait_time->setValue(100); _ui->checkBox_odom_sensor_use_as_gt->setChecked(false); _ui->comboBox_imuFilter_strategy->setCurrentIndex(2); @@ -2087,6 +2253,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_imuFilter_baseFrameConversion->setChecked(true); _ui->checkbox_publishInterIMU->setChecked(false); + _ui->comboBox_lidar_src->setCurrentIndex(0); + _ui->checkBox_source_scanDeskewing->setChecked(false); _ui->checkBox_source_scanFromDepth->setChecked(false); _ui->spinBox_source_scanDownsampleStep->setValue(1); _ui->doubleSpinBox_source_scanRangeMin->setValue(0); @@ -2096,6 +2264,17 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_source_scanNormalsRadius->setValue(0.0); _ui->doubleSpinBox_source_scanNormalsForceGroundUp->setValue(0); + _ui->lineEdit_lidar_local_transform->setText("0 0 0 0 0 0"); + _ui->lineEdit_vlp16_pcap_path->clear(); + _ui->spinBox_vlp16_ip1->setValue(192); + _ui->spinBox_vlp16_ip2->setValue(168); + _ui->spinBox_vlp16_ip3->setValue(1); + _ui->spinBox_vlp16_ip4->setValue(201); + _ui->spinBox_vlp16_port->setValue(2368); + _ui->checkBox_vlp16_organized->setChecked(false); + _ui->checkBox_vlp16_hostTime->setChecked(true); + _ui->checkBox_vlp16_stamp_last->setChecked(true); + _ui->groupBox_depthFromScan->setChecked(false); _ui->groupBox_depthFromScan_fillHoles->setChecked(true); _ui->radioButton_depthFromScan_vertical->setChecked(true); @@ -2359,8 +2538,10 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->doubleSpinBox_subtractFilteringRadius->setValue(settings.value("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value()).toDouble()); _ui->doubleSpinBox_subtractFilteringAngle->setValue(settings.value("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value()).toDouble()); + _ui->doubleSpinBox_map_resolution->setValue(settings.value("gridUIResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble()); _ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool()); _ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble()); + _ui->checkBox_elevation_shown->setCheckState((Qt::CheckState)settings.value("elevationMapShown", _ui->checkBox_elevation_shown->checkState()).toInt()); _ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool()); _ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt()); @@ -2397,6 +2578,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString()); _ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString()); _ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt()); + _ui->comboBox_source_histogramMethod->setCurrentIndex(settings.value("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()).toInt()); + _ui->checkbox_source_feature_detection->setChecked(settings.value("featureDetection", _ui->checkbox_source_feature_detection->isChecked()).toBool()); // Backward compatibility if(_ui->lineEdit_sourceLocalTransform->text().compare("0 0 1 -1 0 0 0 -1 0") == 0) { @@ -2456,9 +2639,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup(); // K4W2 settings.beginGroup("K4A"); - _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt()); - _ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt()); - _ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt()); + _ui->comboBox_k4a_rgb_resolution->setCurrentIndex(settings.value("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()).toInt()); + _ui->comboBox_k4a_framerate->setCurrentIndex(settings.value("framerate", _ui->comboBox_k4a_framerate->currentIndex()).toInt()); + _ui->comboBox_k4a_depth_resolution->setCurrentIndex(settings.value("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()).toInt()); _ui->checkbox_k4a_irDepth->setChecked(settings.value("ir", _ui->checkbox_k4a_irDepth->isChecked()).toBool()); _ui->lineEdit_k4a_mkv->setText(settings.value("mkvPath", _ui->lineEdit_k4a_mkv->text()).toString()); _ui->source_checkBox_useMKVStamps->setChecked(settings.value("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()).toBool()); @@ -2532,10 +2715,19 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("DepthAI"); _ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt()); - _ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool()); - _ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt()); + _ui->comboBox_depthai_output_mode->setCurrentIndex(settings.value("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()).toInt()); + _ui->spinBox_depthai_conf_threshold->setValue(settings.value("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()).toInt()); + _ui->spinBox_depthai_lrc_threshold->setValue(settings.value("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()).toInt()); + _ui->checkBox_depthai_extended_disparity->setChecked(settings.value("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()).toBool()); + _ui->comboBox_depthai_subpixel_fractional_bits->setCurrentIndex(settings.value("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()).toInt()); + _ui->comboBox_depthai_disparity_companding->setCurrentIndex(settings.value("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()).toInt()); + _ui->checkBox_depthai_use_spec_translation->setChecked(settings.value("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()).toBool()); + _ui->doubleSpinBox_depthai_alpha_scaling->setValue(settings.value("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()).toDouble()); _ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool()); - _ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool()); + _ui->doubleSpinBox_depthai_dot_intensity->setValue(settings.value("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value()).toDouble()); + _ui->doubleSpinBox_depthai_flood_intensity->setValue(settings.value("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value()).toDouble()); + _ui->comboBox_depthai_detect_features->setCurrentIndex(settings.value("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()).toInt()); + _ui->lineEdit_depthai_blob_path->setText(settings.value("blob_path", _ui->lineEdit_depthai_blob_path->text()).toString()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -2569,6 +2761,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_odomSourceDevice->setText(settings.value("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text()).toString()); _ui->doubleSpinBox_odom_sensor_time_offset->setValue(settings.value("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value()).toDouble()); _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(settings.value("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()).toDouble()); + _ui->doubleSpinBox_odom_sensor_wait_time->setValue(settings.value("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()).toDouble()); _ui->checkBox_odom_sensor_use_as_gt->setChecked(settings.value("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()).toBool()); settings.endGroup(); // OdomSensor @@ -2594,6 +2787,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup();//IMU settings.beginGroup("Scan"); + _ui->comboBox_lidar_src->setCurrentIndex(settings.value("source", _ui->comboBox_lidar_src->currentIndex()).toInt()); + _ui->checkBox_source_scanDeskewing->setChecked(settings.value("deskewing", _ui->checkBox_source_scanDeskewing->isChecked()).toBool()); _ui->checkBox_source_scanFromDepth->setChecked(settings.value("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()).toBool()); _ui->spinBox_source_scanDownsampleStep->setValue(settings.value("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()).toInt()); _ui->doubleSpinBox_source_scanRangeMin->setValue(settings.value("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value()).toDouble()); @@ -2628,6 +2823,23 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup(); // Camera + settings.beginGroup("Lidar"); + + settings.beginGroup("VLP16"); + _ui->lineEdit_lidar_local_transform->setText(settings.value("localTransform",_ui->lineEdit_lidar_local_transform->text()).toString()); + _ui->lineEdit_vlp16_pcap_path->setText(settings.value("pcapPath",_ui->lineEdit_vlp16_pcap_path->text()).toString()); + _ui->spinBox_vlp16_ip1->setValue(settings.value("ip1", _ui->spinBox_vlp16_ip1->value()).toInt()); + _ui->spinBox_vlp16_ip2->setValue(settings.value("ip2", _ui->spinBox_vlp16_ip2->value()).toInt()); + _ui->spinBox_vlp16_ip3->setValue(settings.value("ip3", _ui->spinBox_vlp16_ip3->value()).toInt()); + _ui->spinBox_vlp16_ip4->setValue(settings.value("ip4", _ui->spinBox_vlp16_ip4->value()).toInt()); + _ui->spinBox_vlp16_port->setValue(settings.value("port", _ui->spinBox_vlp16_port->value()).toInt()); + _ui->checkBox_vlp16_organized->setChecked(settings.value("organized", _ui->checkBox_vlp16_organized->isChecked()).toBool()); + _ui->checkBox_vlp16_hostTime->setChecked(settings.value("hostTime", _ui->checkBox_vlp16_hostTime->isChecked()).toBool()); + _ui->checkBox_vlp16_stamp_last->setChecked(settings.value("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()).toBool()); + settings.endGroup(); // VLP16 + + settings.endGroup(); // Lidar + _calibrationDialog->loadSettings(settings, "CalibrationDialog"); } @@ -2752,6 +2964,45 @@ void PreferencesDialog::resetConfig() } } +void PreferencesDialog::loadPreset(const std::string & presetHexHeader) +{ + ParametersMap parameters = Parameters::getDefaultParameters(); + if(!presetHexHeader.empty()) + { + Parameters::readINIStr(uHex2Str(presetHexHeader), parameters); + } + + // Reset 3D rendering panel + this->resetSettings(1); + // Update parameters + parameters.erase(Parameters::kRtabmapWorkingDirectory()); + for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + this->setParameter(iter->first, iter->second); + } +} + +void PreferencesDialog::loadPreset() +{ + if(sender() == _ui->pushButton_presets_camera_tof_icp) + { + loadPreset(CAMERA_TOF_ICP_INI); + } + else if(sender() == _ui->pushButton_presets_lidar_3d_icp) + { + loadPreset(LIDAR3D_ICP_INI); + } + else + { + UERROR("Unknown sender!"); + return; + } + QMessageBox::information(this, + tr("Preset"), + tr("Loaded \"%1\" preset!").arg(((QPushButton*)sender())->text()), + QMessageBox::Ok); +} + void PreferencesDialog::writeSettings(const QString & filePath) { writeGuiSettings(filePath); @@ -2890,8 +3141,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value()); settings.setValue("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value()); + settings.setValue("gridUIResolution", _ui->doubleSpinBox_map_resolution->value()); settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked()); settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()); + settings.setValue("elevationMapShown", _ui->checkBox_elevation_shown->checkState()); + settings.setValue("octomap", _ui->groupBox_octomap->isChecked()); settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value()); @@ -2930,6 +3184,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("device", _ui->lineEdit_sourceDevice->text()); settings.setValue("localTransform", _ui->lineEdit_sourceLocalTransform->text()); settings.setValue("imageDecimation", _ui->spinBox_source_imageDecimation->value()); + settings.setValue("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()); + settings.setValue("featureDetection", _ui->checkbox_source_feature_detection->isChecked()); settings.beginGroup("rgbd"); settings.setValue("driver", _ui->comboBox_cameraRGBD->currentIndex()); @@ -2983,9 +3239,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // K4W2 settings.beginGroup("K4A"); - settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()); - settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex()); - settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()); + settings.setValue("rgb_resolution", _ui->comboBox_k4a_rgb_resolution->currentIndex()); + settings.setValue("framerate", _ui->comboBox_k4a_framerate->currentIndex()); + settings.setValue("depth_resolution", _ui->comboBox_k4a_depth_resolution->currentIndex()); settings.setValue("ir", _ui->checkbox_k4a_irDepth->isChecked()); settings.setValue("mkvPath", _ui->lineEdit_k4a_mkv->text()); settings.setValue("useMkvStamps", _ui->source_checkBox_useMKVStamps->isChecked()); @@ -3058,11 +3314,20 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // MyntEye settings.beginGroup("DepthAI"); - settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); - settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); - settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); - settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); - settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); + settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex()); + settings.setValue("output_mode", _ui->comboBox_depthai_output_mode->currentIndex()); + settings.setValue("conf_threshold", _ui->spinBox_depthai_conf_threshold->value()); + settings.setValue("lrc_threshold", _ui->spinBox_depthai_lrc_threshold->value()); + settings.setValue("extended_disparity", _ui->checkBox_depthai_extended_disparity->isChecked()); + settings.setValue("subpixel_fractional_bits", _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()); + settings.setValue("companding", _ui->comboBox_depthai_disparity_companding->currentIndex()); + settings.setValue("use_spec_translation", _ui->checkBox_depthai_use_spec_translation->isChecked()); + settings.setValue("alpha_scaling", _ui->doubleSpinBox_depthai_alpha_scaling->value()); + settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); + settings.setValue("dot_intensity", _ui->doubleSpinBox_depthai_dot_intensity->value()); + settings.setValue("flood_intensity", _ui->doubleSpinBox_depthai_flood_intensity->value()); + settings.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); + settings.setValue("blob_path", _ui->lineEdit_depthai_blob_path->text()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -3094,6 +3359,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text()); settings.setValue("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value()); settings.setValue("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()); + settings.setValue("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()); settings.setValue("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()); settings.endGroup(); // OdomSensor @@ -3119,6 +3385,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup();//IMU settings.beginGroup("Scan"); + settings.setValue("source", _ui->comboBox_lidar_src->currentIndex()); + settings.setValue("deskewing", _ui->checkBox_source_scanDeskewing->isChecked()); settings.setValue("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()); settings.setValue("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()); settings.setValue("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value()); @@ -3153,6 +3421,23 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // Camera + settings.beginGroup("Lidar"); + + settings.beginGroup("VLP16"); + settings.setValue("localTransform",_ui->lineEdit_lidar_local_transform->text()); + settings.setValue("pcapPath",_ui->lineEdit_vlp16_pcap_path->text()); + settings.setValue("ip1", _ui->spinBox_vlp16_ip1->value()); + settings.setValue("ip2", _ui->spinBox_vlp16_ip2->value()); + settings.setValue("ip3", _ui->spinBox_vlp16_ip3->value()); + settings.setValue("ip4", _ui->spinBox_vlp16_ip4->value()); + settings.setValue("port", _ui->spinBox_vlp16_port->value()); + settings.setValue("organized", _ui->checkBox_vlp16_organized->isChecked()); + settings.setValue("hostTime", _ui->checkBox_vlp16_hostTime->isChecked()); + settings.setValue("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()); + settings.endGroup(); // VLP16 + + settings.endGroup(); // Lidar + _calibrationDialog->saveSettings(settings, "CalibrationDialog"); } @@ -3889,6 +4174,9 @@ void PreferencesDialog::updateParameters(const ParametersMap & parameters, bool void PreferencesDialog::selectSourceDriver(Src src, int variant) { + Src previousCameraSrc = getSourceDriver(); + Src previousLidarSrc = getLidarSourceDriver(); + if(_ui->comboBox_imuFilter_strategy->currentIndex()==0) { _ui->comboBox_imuFilter_strategy->setCurrentIndex(2); @@ -3944,10 +4232,12 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) // disable IMU filtering (zed sends already quaternion) _ui->comboBox_imuFilter_strategy->setCurrentIndex(0); } - else if(src == kSrcStereoDepthAI) // OAK-D (variant==1), OAK-D Lite (variant==0) + else if(src == kSrcStereoDepthAI) // OAK-D-Pro (variant==2), OAK-D (variant==1), OAK-D Lite (variant==0) { - _ui->checkBox_depthai_imu_published->setChecked(variant == 1); - _ui->comboBox_depthai_resolution->setCurrentIndex(variant == 1?1:3); + _ui->checkBox_depthai_imu_published->setChecked(variant >= 1); + _ui->comboBox_depthai_resolution->setCurrentIndex(variant >= 1?1:3); + _ui->comboBox_depthai_output_mode->setCurrentIndex(variant==2?2:0); + _ui->doubleSpinBox_depthai_dot_intensity->setValue(variant==2?1:0); } } else if(src >= kSrcRGB && srccomboBox_sourceType->setCurrentIndex(2); _ui->source_comboBox_image_type->setCurrentIndex(src - kSrcRGB); } - else if(src >= kSrcDatabase) + else if(src == kSrcDatabase) { _ui->comboBox_sourceType->setCurrentIndex(3); } + else if(src >= kSrcLidar) + { + _ui->comboBox_lidar_src->setCurrentIndex(kSrcLidarVLP16 - kSrcLidar + 1); + } + + if(previousCameraSrc == kSrcUndef && src < kSrcDatabase && + QMessageBox::question(this, tr("Camera Source..."), + tr("Do you want to use default camera settings?"), + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + { + loadPreset(""); + _ui->comboBox_lidar_src->setCurrentIndex(0); // Set Lidar to None; + _ui->comboBox_odom_sensor->setCurrentIndex(0); // Set odom sensor to None + } + else if(previousLidarSrc== kSrcUndef && src >= kSrcLidar && + QMessageBox::question(this, tr("LiDAR Source..."), + tr("Do you want to use \"LiDAR 3D ICP\" preset?"), + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + { + loadPreset(LIDAR3D_ICP_INI); + _ui->comboBox_sourceType->setCurrentIndex(4); // Set camera to None; + _ui->comboBox_odom_sensor->setCurrentIndex(0); // Set odom sensor to No + } if(validateForm()) { @@ -4395,6 +4708,34 @@ void PreferencesDialog::selectSourceRealsense2JsonPath() } } +void PreferencesDialog::selectSourceDepthaiBlobPath() +{ + QString dir = _ui->lineEdit_depthai_blob_path->text(); + if(dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("MyriadX blob (*.blob)")); + if(path.size()) + { + _ui->lineEdit_depthai_blob_path->setText(path); + } +} + +void PreferencesDialog::selectVlp16PcapPath() +{ + QString dir = _ui->lineEdit_vlp16_pcap_path->text(); + if (dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Velodyne recording (*.pcap)")); + if (!path.isEmpty()) + { + _ui->lineEdit_vlp16_pcap_path->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -4745,6 +5086,8 @@ void PreferencesDialog::addParameters(const QGroupBox * box) void PreferencesDialog::updateBasicParameter() { + UDEBUG(""); + // This method is used to update basic/advanced referred parameters, see above editingFinished() // basic to advanced (advanced to basic must be done by connecting signal valueChanged()) @@ -4782,19 +5125,29 @@ void PreferencesDialog::updateBasicParameter() } else if(sender() == _ui->general_checkBox_activateRGBD) { + _ui->general_checkBox_activateRGBD_2->blockSignals(true); _ui->general_checkBox_activateRGBD_2->setChecked(_ui->general_checkBox_activateRGBD->isChecked()); + _ui->general_checkBox_activateRGBD_2->blockSignals(false); } else if(sender() == _ui->general_checkBox_activateRGBD_2) { + _ui->general_checkBox_activateRGBD->blockSignals(true); _ui->general_checkBox_activateRGBD->setChecked(_ui->general_checkBox_activateRGBD_2->isChecked()); + addParameter(_ui->general_checkBox_activateRGBD, _ui->general_checkBox_activateRGBD->isChecked()); + _ui->general_checkBox_activateRGBD->blockSignals(false); } else if(sender() == _ui->general_checkBox_SLAM_mode) { + _ui->general_checkBox_SLAM_mode_2->blockSignals(true); _ui->general_checkBox_SLAM_mode_2->setChecked(_ui->general_checkBox_SLAM_mode->isChecked()); + _ui->general_checkBox_SLAM_mode_2->blockSignals(false); } else if(sender() == _ui->general_checkBox_SLAM_mode_2) { + _ui->general_checkBox_SLAM_mode->blockSignals(true); _ui->general_checkBox_SLAM_mode->setChecked(_ui->general_checkBox_SLAM_mode_2->isChecked()); + addParameter(_ui->general_checkBox_SLAM_mode, _ui->general_checkBox_SLAM_mode->isChecked()); + _ui->general_checkBox_SLAM_mode->blockSignals(false); } else { @@ -4976,6 +5329,11 @@ void PreferencesDialog::updateFeatureMatchingVisibility() _ui->groupBox_gms->setVisible(_ui->reextract_nn->currentIndex() == 7); } +void PreferencesDialog::updateGlobalDescriptorVisibility() +{ + _ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1); +} + void PreferencesDialog::updateOdometryStackedIndex(int index) { if(index == 11) // FLOAM -> LOAM @@ -5102,6 +5460,40 @@ void PreferencesDialog::changeOdometryVINSConfigPath() } } +void PreferencesDialog::changeOdometryOpenVINSLeftMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSLeftMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Left Mask"), _ui->lineEdit_OdomOpenVINSLeftMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSLeftMaskPath->setText(path); + } +} + +void PreferencesDialog::changeOdometryOpenVINSRightMask() +{ + QString path; + if(_ui->lineEdit_OdomOpenVINSRightMaskPath->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), this->getWorkingDirectory(), tr("Image mask (*.jpg *.png)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select Right Mask"), _ui->lineEdit_OdomOpenVINSRightMaskPath->text(), tr("Image mask (*.jpg *.png)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_OdomOpenVINSRightMaskPath->setText(path); + } +} + void PreferencesDialog::changeIcpPMConfigPath() { QString path; @@ -5170,6 +5562,22 @@ void PreferencesDialog::changePyMatcherModel() } } +void PreferencesDialog::changePyDescriptorPath() +{ + QString path; + if(_ui->lineEdit_pydescriptor_path->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("Python wrapper (*.py)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pydescriptor_path->text(), tr("Python wrapper (*.py)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_pydescriptor_path->setText(path); + } +} void PreferencesDialog::changePyDetectorPath() { QString path; @@ -5189,14 +5597,14 @@ void PreferencesDialog::changePyDetectorPath() void PreferencesDialog::updateSourceGrpVisibility() { + _ui->stackedWidget_src->setVisible(_ui->comboBox_sourceType->currentIndex() != 4); // Not Camera None + _ui->frame_camera_sensor->setVisible(_ui->comboBox_sourceType->currentIndex() != 4); // Not Camera None + _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); _ui->groupBox_sourceStereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1); _ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2); _ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3); - _ui->checkBox_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); - _ui->label_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); - _ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || @@ -5249,6 +5657,30 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->comboBox_sourceType->currentIndex() == 3); // Database _ui->groupBox_depthImageFiltering->setVisible(_ui->groupBox_depthImageFiltering->isEnabled()); + _ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); + + // Odom Sensor Group + _ui->frame_visual_odometry_sensor->setVisible(getOdomSourceDriver() != kSrcUndef); // Not Lidar None + _ui->groupBox_odom_sensor->setVisible(_ui->comboBox_sourceType->currentIndex() != 3); // Don't show when database is selected + + // Lidar Sensor Group + _ui->comboBox_lidar_src->setEnabled(_ui->comboBox_sourceType->currentIndex() != 3); // Disable if database input + if(!_ui->comboBox_lidar_src->isEnabled() && _ui->comboBox_lidar_src->currentIndex() != 0) + { + _ui->comboBox_lidar_src->setCurrentIndex(0); // Set to none + } + _ui->checkBox_source_scanFromDepth->setEnabled(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); + _ui->label_source_scanFromDepth->setEnabled(_ui->checkBox_source_scanFromDepth->isEnabled()); + if(!_ui->checkBox_source_scanFromDepth->isEnabled()) + { + _ui->checkBox_source_scanFromDepth->setChecked(false); + } + _ui->stackedWidget_lidar_src->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0); + _ui->groupBox_vlp16->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarVLP16-kSrcLidar); + _ui->frame_lidar_sensor->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0 || _ui->checkBox_source_scanFromDepth->isChecked()); // Not Lidar None or database input + _ui->pushButton_test_lidar->setEnabled(_ui->comboBox_lidar_src->currentIndex() > 0); + + // IMU group _ui->groupBox_imuFiltering->setEnabled( (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) || @@ -5261,14 +5693,11 @@ void PreferencesDialog::updateSourceGrpVisibility() (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo)); + _ui->frame_imu_filtering->setVisible(getIMUFilteringStrategy() > 0); // Not None _ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0); _ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1); _ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2); _ui->groupBox_imuFiltering->setVisible(_ui->groupBox_imuFiltering->isEnabled()); - - //_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3); - - _ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); } /*** GETTERS ***/ @@ -5662,10 +6091,18 @@ double PreferencesDialog::getSubtractFilteringAngle() const { return _ui->doubleSpinBox_subtractFilteringAngle->value()*M_PI/180.0; } +double PreferencesDialog::getGridUIResolution() const +{ + return _ui->doubleSpinBox_map_resolution->value(); +} bool PreferencesDialog::getGridMapShown() const { return _ui->checkBox_map_shown->isChecked(); } +int PreferencesDialog::getElevationMapShown() const +{ + return _ui->checkBox_elevation_shown->checkState(); +} int PreferencesDialog::getGridMapSensor() const { return _ui->comboBox_grid_sensor->currentIndex(); @@ -5797,6 +6234,15 @@ PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const return kSrcUndef; } +PreferencesDialog::Src PreferencesDialog::getLidarSourceDriver() const +{ + if(_ui->comboBox_lidar_src->currentIndex() == 0) + { + return kSrcUndef; + } + return (PreferencesDialog::Src)(_ui->comboBox_lidar_src->currentIndex()-1 + kSrcLidar); +} + Transform PreferencesDialog::getSourceLocalTransform() const { Transform t = Transform::fromString(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); @@ -5875,6 +6321,14 @@ int PreferencesDialog::getSourceImageDecimation() const { return _ui->spinBox_source_imageDecimation->value(); } +int PreferencesDialog::getSourceHistogramMethod() const +{ + return _ui->comboBox_source_histogramMethod->currentIndex(); +} +bool PreferencesDialog::isSourceFeatureDetection() const +{ + return _ui->checkbox_source_feature_detection->isChecked(); +} bool PreferencesDialog::isSourceStereoDepthGenerated() const { return _ui->checkbox_stereo_depthGenerated->isChecked(); @@ -5887,6 +6341,10 @@ bool PreferencesDialog::isSourceScanFromDepth() const { return _ui->checkBox_source_scanFromDepth->isChecked(); } +bool PreferencesDialog::isSourceScanDeskewing() const +{ + return _ui->checkBox_source_scanDeskewing->isChecked(); +} int PreferencesDialog::getSourceScanDownsampleStep() const { return _ui->spinBox_source_scanDownsampleStep->value(); @@ -5940,7 +6398,7 @@ Camera * PreferencesDialog::createCamera( if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed)) { QMessageBox::warning(this, tr("Odometry Sensor"), - tr("Driver \%1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok); + tr("Driver %1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok); return 0; } @@ -6038,9 +6496,9 @@ Camera * PreferencesDialog::createCamera( } ((CameraK4A*)camera)->setIRDepthFormat(_ui->checkbox_k4a_irDepth->isChecked()); - ((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(), - _ui->comboBox_k4a_framerate->currentIndex(), - _ui->comboBox_k4a_depth_resolution->currentIndex()); + ((CameraK4A*)camera)->setPreferences(_ui->comboBox_k4a_rgb_resolution->currentIndex(), + _ui->comboBox_k4a_framerate->currentIndex(), + _ui->comboBox_k4a_depth_resolution->currentIndex()); } else if (driver == kSrcRealSense) { @@ -6079,11 +6537,14 @@ Camera * PreferencesDialog::createCamera( device.isEmpty()&&driver == kSrcStereoRealSense2?"T265":device.toStdString(), this->getGeneralInputRate(), this->getSourceLocalTransform()); - ((CameraRealSense2*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); if(driver == kSrcStereoRealSense2) { ((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages); - ((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly, odomSensorExtrinsicsCalib); + ((CameraRealSense2*)camera)->setOdomProvided(getOdomSourceDriver() == kSrcStereoRealSense2 || odomOnly, odomOnly, odomSensorExtrinsicsCalib); } else { @@ -6092,7 +6553,7 @@ Camera * PreferencesDialog::createCamera( ((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value()); ((CameraRealSense2*)camera)->setDepthResolution(_ui->spinBox_rs2_width_depth->value(), _ui->spinBox_rs2_height_depth->value(), _ui->spinBox_rs2_rate_depth->value()); ((CameraRealSense2*)camera)->setGlobalTimeSync(_ui->checkbox_rs2_globalTimeStync->isChecked()); - ((CameraRealSense2*)camera)->setDualMode(_ui->comboBox_odom_sensor->currentIndex()==1, Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().toStdString())); + ((CameraRealSense2*)camera)->setDualMode(getOdomSourceDriver() == kSrcStereoRealSense2, Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().toStdString())); ((CameraRealSense2*)camera)->setJsonConfig(_ui->lineEdit_rs2_jsonFile->text().toStdString()); } } @@ -6265,7 +6726,7 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZed_quality->currentIndex(), _ui->comboBox_stereoZed_sensingMode->currentIndex(), _ui->spinBox_stereoZed_confidenceThr->value(), - _ui->comboBox_odom_sensor->currentIndex() == 2, + getOdomSourceDriver() == kSrcStereoZed, this->getGeneralInputRate(), this->getSourceLocalTransform(), _ui->checkbox_stereoZed_selfCalibration->isChecked(), @@ -6281,14 +6742,17 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZed_quality->currentIndex()==0&&odomOnly?1:_ui->comboBox_stereoZed_quality->currentIndex(), _ui->comboBox_stereoZed_sensingMode->currentIndex(), _ui->spinBox_stereoZed_confidenceThr->value(), - _ui->comboBox_odom_sensor->currentIndex() == 2 || odomOnly, + getOdomSourceDriver() == kSrcStereoZed || odomOnly, this->getGeneralInputRate(), this->getSourceLocalTransform(), _ui->checkbox_stereoZed_selfCalibration->isChecked(), _ui->loopClosure_bowForce2D->isChecked(), _ui->spinBox_stereoZed_texturenessConfidenceThr->value()); } - ((CameraStereoZed*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); } else if (driver == kSrcStereoZedOC) { @@ -6307,9 +6771,24 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_depthai_resolution->currentIndex(), this->getGeneralInputRate(), this->getSourceLocalTransform()); - ((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); - ((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked()); - ((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked()); + ((CameraDepthAI*)camera)->setOutputMode(_ui->comboBox_depthai_output_mode->currentIndex()); + ((CameraDepthAI*)camera)->setDepthProfile(_ui->spinBox_depthai_conf_threshold->value(), _ui->spinBox_depthai_lrc_threshold->value()); + ((CameraDepthAI*)camera)->setExtendedDisparity(_ui->checkBox_depthai_extended_disparity->isChecked()); + ((CameraDepthAI*)camera)->setSubpixelMode(_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()!=0, _ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==2?4:_ui->comboBox_depthai_subpixel_fractional_bits->currentIndex()==3?5:3); + ((CameraDepthAI*)camera)->setCompanding(_ui->comboBox_depthai_disparity_companding->currentIndex()!=0, _ui->comboBox_depthai_disparity_companding->currentIndex()==1?64:96); + ((CameraDepthAI*)camera)->setRectification(_ui->checkBox_depthai_use_spec_translation->isChecked(), _ui->doubleSpinBox_depthai_alpha_scaling->value()); + ((CameraDepthAI*)camera)->setIMU(_ui->checkBox_depthai_imu_published->isChecked(), _ui->checkbox_publishInterIMU->isChecked()); + ((CameraDepthAI*)camera)->setIrIntensity(_ui->doubleSpinBox_depthai_dot_intensity->value(), _ui->doubleSpinBox_depthai_flood_intensity->value()); + ((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex()); + ((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString()); + if(_ui->comboBox_depthai_detect_features->currentIndex() == 1) + { + ((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value()); + } + else if(_ui->comboBox_depthai_detect_features->currentIndex() >= 2) + { + ((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value()); + } } else if(driver == kSrcUsbDevice) { @@ -6439,7 +6918,7 @@ Camera * PreferencesDialog::createCamera( return camera; } -Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor) +SensorCapture * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor, double & waitTime) { Src driver = getOdomSourceDriver(); if(driver != kSrcUndef) @@ -6458,12 +6937,67 @@ Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & ti extrinsics = Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0; scaleFactor = (float)_ui->doubleSpinBox_odom_sensor_scale_factor->value(); + waitTime = _ui->doubleSpinBox_odom_sensor_wait_time->value()/1000.0; return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true, false); } return 0; } +Lidar * PreferencesDialog::createLidar() +{ + Lidar * lidar = 0; + Src driver = getLidarSourceDriver(); + if(driver == kSrcLidarVLP16) + { + Transform localTransform = Transform::fromString(_ui->lineEdit_lidar_local_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); + if(localTransform.isNull()) + { + UWARN("Failed to parse lidar local transfor string \"%s\"!", + _ui->lineEdit_lidar_local_transform->text().toStdString().c_str()); + localTransform = Transform::getIdentity(); + } + if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty()) + { + // PCAP mode + lidar = new LidarVLP16( + _ui->lineEdit_vlp16_pcap_path->text().toStdString(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + } + else + { + // Connect to sensor + + lidar = new LidarVLP16( + boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld", + (size_t)_ui->spinBox_vlp16_ip1->value(), + (size_t)_ui->spinBox_vlp16_ip2->value(), + (size_t)_ui->spinBox_vlp16_ip3->value(), + (size_t)_ui->spinBox_vlp16_ip4->value())), + _ui->spinBox_vlp16_port->value(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_hostTime->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + + } + if(!lidar->init()) + { + UWARN("init lidar failed... "); + QMessageBox::warning(this, + tr("RTAB-Map"), + tr("Lidar initialization failed...")); + delete lidar; + lidar = 0; + } + } + return lidar; +} + bool PreferencesDialog::isStatisticsPublished() const { return _ui->groupBox_publishing->isChecked(); @@ -6608,35 +7142,28 @@ void PreferencesDialog::testOdometry() return; } + ParametersMap parameters = this->getAllParameters(); IMUThread * imuThread = 0; if((this->getSourceDriver() == kSrcStereoImages || this->getSourceDriver() == kSrcRGBDImages || this->getSourceDriver() == kSrcImages) && !_ui->lineEdit_cameraImages_path_imu->text().isEmpty()) { - if(this->getOdomStrategy() != Odometry::kTypeOkvis && - this->getOdomStrategy() != Odometry::kTypeMSCKF && - this->getOdomStrategy() != Odometry::kTypeVINS && - this->getOdomStrategy() != Odometry::kTypeOpenVINS) + imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform()); + if(getIMUFilteringStrategy()>0) { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok); + imuThread->enableIMUFiltering(getIMUFilteringStrategy()-1, parameters, getIMUFilteringBaseFrameConversion()); } - else + if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString())) { - imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform()); - if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString())) - { - QMessageBox::warning(this, tr("Source IMU Path"), - tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok); - delete camera; - delete imuThread; - return; - } + QMessageBox::warning(this, tr("Source IMU Path"), + tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok); + delete camera; + delete imuThread; + return; } } - ParametersMap parameters = this->getAllParameters(); if(getOdomRegistrationApproach() < 3) { uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach()))); @@ -6668,10 +7195,15 @@ void PreferencesDialog::testOdometry() odomViewer->resize(1280, 480+QPushButton().minimumHeight()); odomViewer->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera + SensorCaptureThread cameraThread(camera, this->getAllParameters()); // take ownership of camera cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); + cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); + if(_ui->checkbox_source_feature_detection->isChecked()) + { + cameraThread.enableFeatureDetection(this->getAllParameters()); + } cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( @@ -6682,7 +7214,8 @@ void PreferencesDialog::testOdometry() _ui->doubleSpinBox_source_scanVoxelSize->value(), _ui->spinBox_source_scanNormalsK->value(), _ui->doubleSpinBox_source_scanNormalsRadius->value(), - (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value()); + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast(camera) == 0) { cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); @@ -6701,7 +7234,7 @@ void PreferencesDialog::testOdometry() } } - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent"); if(imuThread) { UEventsManager::createPipe(imuThread, &odomThread, "IMUEvent"); @@ -6739,10 +7272,15 @@ void PreferencesDialog::testCamera() Camera * camera = this->createCamera(); if(camera) { - CameraThread cameraThread(camera, this->getAllParameters()); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); + cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); + if(_ui->checkbox_source_feature_detection->isChecked()) + { + cameraThread.enableFeatureDetection(this->getAllParameters()); + } cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( @@ -6753,7 +7291,8 @@ void PreferencesDialog::testCamera() _ui->doubleSpinBox_source_scanVoxelSize->value(), _ui->spinBox_source_scanNormalsK->value(), _ui->doubleSpinBox_source_scanNormalsRadius->value(), - (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value()); + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast(camera) == 0) { cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); @@ -6771,7 +7310,7 @@ void PreferencesDialog::testCamera() cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString()); } } - UEventsManager::createPipe(&cameraThread, window, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, window, "SensorEvent"); cameraThread.start(); window->exec(); @@ -6835,8 +7374,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setStereoMode(false); // this forces restart _calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_rgb"); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); _calibrationDialog->exec(); _calibrationDialog->unregisterFromEventsManager(); @@ -6863,8 +7402,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setStereoMode(false); // this forces restart _calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_depth"); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); _calibrationDialog->exec(); _calibrationDialog->unregisterFromEventsManager(); @@ -6907,7 +7446,7 @@ void PreferencesDialog::calibrate() { return; } - SensorData rgbData = camera->takeImage(); + SensorData rgbData = camera->takeData(); UASSERT(rgbData.cameraModels().size() == 1); rgbModel = rgbData.cameraModels()[0]; delete camera; @@ -6916,7 +7455,7 @@ void PreferencesDialog::calibrate() { return; } - SensorData irData = camera->takeImage(); + SensorData irData = camera->takeData(); serial = camera->getSerial(); UASSERT(irData.cameraModels().size() == 1); irModel = irData.cameraModels()[0]; @@ -6992,8 +7531,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setSavingDirectory(this->getCameraInfoDir()); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); @@ -7036,29 +7575,16 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() } } - Src odomDriver; - if(_ui->comboBox_odom_sensor->currentIndex() == 0) + Src odomDriver = getOdomSourceDriver(); + if(odomDriver == kSrcUndef) { QMessageBox::warning(this, tr("Calibration"), tr("No odometry sensor selected!")); return; } - else if(_ui->comboBox_odom_sensor->currentIndex() == 1) - { - odomDriver = kSrcStereoRealSense2; - } - else if(_ui->comboBox_odom_sensor->currentIndex() == 2) - { - odomDriver = kSrcStereoZed; - } - else - { - UERROR("Odom sensor %d not implemented", _ui->comboBox_odom_sensor->currentIndex()); - return; - } - + // 3 steps calibration: RGB -> IR -> Extrinsic QMessageBox::StandardButton button = QMessageBox::question(this, tr("Calibration"), tr("We will calibrate the extrinsics. Important: Make sure " @@ -7152,7 +7678,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() delete camera; return; } - SensorData odomSensorData = camera->takeImage(); + SensorData odomSensorData = camera->takeData(); if(odomSensorData.cameraModels().size() == 1) { odomSensorModel = odomSensorData.cameraModels()[0]; } @@ -7176,7 +7702,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() delete camera; return; } - SensorData camData = camera->takeImage(); + SensorData camData = camera->takeData(); serial = camera->getSerial(); if(camData.cameraModels().size() == 1) { cameraModel = camData.cameraModels()[0]; @@ -7253,5 +7779,42 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() } } +void PreferencesDialog::testLidar() +{ + CameraViewer * window = new CameraViewer(this, this->getAllParameters()); + window->setWindowTitle(tr("Lidar viewer")); + window->setWindowFlags(Qt::Window); + window->resize(1280, 480+QPushButton().minimumHeight()); + window->registerToEventsManager(); + window->setDecimation(1); + + Lidar * lidar = this->createLidar(); + if(lidar) + { + SensorCaptureThread lidarThread(lidar, this->getAllParameters()); + lidarThread.setScanParameters( + _ui->checkBox_source_scanFromDepth->isChecked(), + _ui->spinBox_source_scanDownsampleStep->value(), + _ui->doubleSpinBox_source_scanRangeMin->value(), + _ui->doubleSpinBox_source_scanRangeMax->value(), + _ui->doubleSpinBox_source_scanVoxelSize->value(), + _ui->spinBox_source_scanNormalsK->value(), + _ui->doubleSpinBox_source_scanNormalsRadius->value(), + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); + + UEventsManager::createPipe(&lidarThread, window, "SensorEvent"); + + lidarThread.start(); + window->exec(); + delete window; + lidarThread.join(true); + } + else + { + delete window; + } +} + } diff --git a/guilib/src/StatsToolBox.cpp b/guilib/src/StatsToolBox.cpp index ce0daf3fc8..54933e944a 100644 --- a/guilib/src/StatsToolBox.cpp +++ b/guilib/src/StatsToolBox.cpp @@ -528,15 +528,18 @@ void StatsToolBox::clear() plots[0]->clearData(); } else - { - UERROR(""); - } - } - QList items = _statBox->currentWidget()->findChildren(); - for (int i = 0; iclearCache(); - } + { + UERROR(""); + } + } + if(_statBox->currentWidget()) + { + QList items = _statBox->currentWidget()->findChildren(); + for (int i = 0; iclearCache(); + } + } } void StatsToolBox::contextMenuEvent(QContextMenuEvent * event) @@ -567,7 +570,7 @@ void StatsToolBox::contextMenuEvent(QContextMenuEvent * event) } } - if(!plotName.isEmpty()) + if(!plotName.isEmpty() && _statBox->currentWidget()) { QList items = _statBox->currentWidget()->findChildren(); for(int i=0; i & curvesPerFigure, QStringList & curveNames) +void StatsToolBox::getFiguresSetup(QList & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds) { curvesPerFigure.clear(); curveNames.clear(); + curveThresholds.clear(); for(QMap::iterator i=_figures.begin(); i!=_figures.end(); ++i) { QList plots = i.value()->findChildren(); @@ -593,6 +597,10 @@ void StatsToolBox::getFiguresSetup(QList & curvesPerFigure, QStringList & c QStringList names = plots[0]->curveNames(); curvesPerFigure.append(names.size()); curveNames.append(names); + for(int j=0; jisThreshold(names[j])?QString::number(plots[0]->getThresholdValue(names[j])):"NA"); + } } else { @@ -625,6 +633,22 @@ void StatsToolBox::addCurve(const QString & name, bool newFigure, bool cacheOn) ULOGGER_ERROR("Not supposed to be here..."); } } +void StatsToolBox::addThreshold(const QString & name, qreal value) +{ + QString plotName = _plotMenu->actions().last()->text(); + QWidget * fig = _figures.value(plotName, (QWidget*)0); + if(fig) + { + UPlot * plot = fig->findChild(plotName); + if(plot) + plot->addThreshold(name, value); + } + else + { + UERROR("There are no figures, cannot add threshold \"%s\"", name.toStdString().c_str()); + } +} + void StatsToolBox::setWorkingDirectory(const QString & workingDirectory) { diff --git a/guilib/src/images/oakdpro.png b/guilib/src/images/oakdpro.png new file mode 100644 index 0000000000000000000000000000000000000000..9308b34a3ea69ae3d1bb288b8acb41d3c1bf3bba GIT binary patch literal 8691 zcmeHLc|6qn*QXT9o~=^G5Q`fQw!vUfL}Fq}k(4zQnrtB|B-tWs zwh$_6qKH&>Jzu)F+kJk&=k>hq>v?|vUCi?Rob!2~_j#XlzKio#7N&w*c5LC|;u6G| zp^pHc&cJIo-)7*wt7zwW;G-wV)}DR@9{{5Hcu}bCWDq@&Mh1}?R0Vyn!r!=27vDPHxfnQWEDL zWr#`a&ZmLCzq_*7j=tx!$bahm%IYn_z>V0GM8UK-m2+HN+~=sq##R_(<3ADv(n>jf zSGipelx@iwk70zP@q2~GDsJFKz4fg4xCvIVP=itiklmf#l1 zkQHSeYU$>nVf^hge!{X&p9s3omR_c3nKoS)nC7o5Ijp7nM)1uz)|^vmDMK;A$muKX zM!XNNi#?urB4qS9?L|AH_8>~bQbtj7i-&4COl8Nu8F6o0YmMePm*aNYUfy=kaJ`jE zp?Kw-d{y663Ju3G2XV+OmNxpF@$v|}b_EUpQ+LB4UrJ}Wt$kZFVY zq5SLVDYW!D?F<}reH>)vt#3Ob7k%D-h2duG_<)tiU6 znt~s{6@pIRG>;rJ<4#&VwVPp(<<-35bh+VN(%>;)*&JcRq=$!wxp<=GaFAp_5$%{Z%V|dVjR^;N+HDJ*2L^m=W zL?F9RJ@vq!DyqRCDoGD~R09je(u~QjRI?x-vQ3bMEiuTAs6zr9=x@hlw3rN?8L_r-v zAN(BxIMM^V(&;pmx_UrBfLZ`T&CAC{9j2qBqYj0u!{HFX1L7O#NyjrFp1$&H5I-=` zWM85Wl}4v}d4kq3@dPhFx*iw|*g=2r^Ppj|KjA%ne^&v}L!E)Asl(Ku>K-2IzwYp* zoA?7DzZ3emJA7?{!Bsy(_Vx1fA(Bn}$)0riUm-}upZjTkKJM%3kcjGJcd`fI>I+1L z{UzkuDd6XhzeHTKtdsl|-V}qi`su!=mrnvlP{uxFJl)I3*2~LX54=Viv=;5B84HX%2~WqP@pLkf92Aa3K@lhf)E1_R zg6p7Q+7PG~3i^w`7l}#<{NKFSh8v{&qsPpszCirI^`##ZZ$tL}arEQRow`2iAkg~I zqVU8YDfr_3$)t6j0M-u^(G~CMLI%!mzt6`X>(qbKdjt)H7J>o?_#;Rf5TrH{3enL- z!XR+C78FS)Xp`ZjABF#k?(0RN2jG3kMlOJkfL6dftZM~QUN7$6zj6t1B?CGGf*~MK zI0S~Yg=qt_p1;#bu)u`y$HgVWi$NRNx(-(*nKM1cagCvFr$>gDbU)Y*v6ot^hA=293!@YsW@s28A|uU@ zVjg;(Q=^18;a1-alPR54;UqBasXIO95W9a>z%g->x` zcBbM6YTPw^q#3gIF=1j`j6JK9P$~_Ju{a@qnQ11{V?P#HDovDNzc?8{$YtBxCERA8 zV@FSP6PR`hx6X;=<}}z!a4N$tk*~{zqcoJl+1DJs6D^@3QX+j8-x(;&-iNLs zI9!oyExy)H@kXWcq$|K|b$mBC&3k!?Yh2`bt zw*=H4TW%aTg3ERj!U{)VF%2c&5Td0=mwOW&KFqm_iW-@Sa3kb*Fu68c+*>{221z#4 zt9;PjQq-^^_a66VaIw8`p_1zg+#KK{U(B*=OeY&my#d8rflgH zz||AbJsO|6liO8AhzbXfsT#;`%j#)Yc`}?oSf3g^x4K*(TeTb&y5bTlw%k`e6TCQd z)M;fYzP2_os?o*yEBdRSJW4`JLPmWJ=$Nf?=CgH2%zY>I2cd=e9=A20yq*aR zV$i0SOidfh&lEnBs{ZoU>GX$u+0#p>zF)H1ly?^eeO}t?yrekkFO{mk?;z`P6RB>z zi5rJglVqV=z@3EI{xr~ix7E;v>}R#zox47EOwheC;ck`1%0Ah(TeGva7TrOptA351 z<~+gKPE_A2U@;+MFsVnG7sReEee{^@n4-_#Wwb9r423p}{Sso@jNY-iLijV(ofDuegEj%_PG5m)(Q zkCLRBBcyE3GouY}n0x4ewMgqUCfw2n9bs2-VfayF(`bNPgSp$530?4W_s&b3+NEp2 znL?tmhagXp)cC}0pHEd@vaS=k5)v9!H59DjIw@Hv0-VY5M ziU)jM{!N@vnL#OJ_O%FEDk6URx(+Skm0ka>A@#IQ5(nfz3}hWHrs zCr;oS9?%IvDC=n)jqH)qG1$NHHh<`;kY+maK#Ec%mn5c`wb~{ahm^0hTWehd%96}BM7nUkl$Fgdw4aJrz?x;b zcr7UgOpT0IfB&GNI4VcYxnOQnY>_z5xc~T=!>g%>V(thNNCG&q>dDiGr@M8aM23H} zyKZ*Xy>qHysC;DfZ0*J}uc3y8D|R`J@4ra9ZQ!={NP~rM&%9VV#LhxG7gao)f3v}8 zx^r|!`663c6ergARt-QgUxBlYm*jFc>&@?H!J7%UsM@T>% z=U^!W(e1-#Q`1jvw`DjdGWl*733L_bZIs4Yix|H;7aJn&oz~VXss8A*SbnO$g`RBa z+iiyr8Xpiq$K1^Swxhp)bLO4Uk_4v*-J#^!!bVXNL1alJQKT5;m|{#$zQN4o}URS&9$ zTRO&I_vEugR2oLNdWO6gn%n)715SskHlSh8RY&DzgZXpP!0m3@&jz#;kqs2S%;EFd z-kVfXXRjGqk10?O4tU-T=4b|d8XCKI)ak91{+y1G(0&>H5MgUMb>HJ}tUqb_jGru8 zqH2sN8e9Tt3=z%v>zhIy{VaZ~|$2zf=&lLv}P}yEIOP)tx z>Mdy>{G-Z4=})Sw7KM~}d0$pmHL9eazU~?PxN~&lFym|MfH4?-uOY_W=w&BMH{FM0 zJjdLezWhbyX4|!dhMHB;#-*i?F9$|R-Vr>8|faSjm> z&DV$-ZDS^Jks)EyB1sXr53lf7IW5^XAs_BkpY=^E-~9-yY)%>VHeSURh>yb02B^)3 zKfnd4#-!NYF;2Rt?1K0vtCu-A`FS-sa`qy7-(~g~Z(+63z{$}nC3}T*i-H#=?xnQA zgLlc^Qmj<+cj3CE!jvTxpQRbPW&!W#ZhYn4t;#!RVbvR zvy_C)@7R>b*Y*0+imd+FYwIQ@*;2lYyw|E2Z;QKDYR+(E^EZd)^BqU0x7CNc4<$0!O=$*ESENgKabZH1mlryEznEAlT$q{B}rs>-5sTvk42um-? zLVWIz@K*^Fk#ewI%%Z z^yZ2sV*;OR+^a#q-qhH3DgHEV)8469r=F0Iis^@6Ow3`9Z;*rQ?0?H!7%FRRS}|yX zYnt6#`+EFMr@!Y&8UxXV11_sgu8iik3xo4^$*OLMhu1z=eX6u@7|RWqSAE-%zu(qQ zF5}Icd1tv7fqV})8_0@>?KmK0(nd$#)OwLI^kKBpg&xxEu3e{^*;aC|#MrCotmP2` zOThA?y`8K8TdWmzGc}}jTL$=u6kqKY+v`0J#kulLj--6GHYHZO@Lem&V}fSojiDhh z#s;+lxp>hcaQXcR=2Wz_c4EhrfB%Hv$y%YyB_*|f+e&lw>uB{d?QOy{nC7-NY|8A- z{N7BpBQBROJBJl3YY&8$TdW*4o8Q>IJuT>hg%IXB%fHaFJpK8kFf#+(z?|F7iATZt zB{-(MEe%;Jy0&kLadxqGURI`N?46Y*86Mo}3U$R}Zy4zW)NkFshfj{)*VcEQYdf7{ z`Ci!Zz7e+Qw_U>vn?BZwXz$kpwKswW?%xzK7WeA3?cH)^v!aY_wW&kP{ky@5c6ToD z=XR4FN}jqh8dg_hueUWUyBFT#P1O&HnrOh5dA_U*v`iwK#qZiU)&+CnjlIrND@e^x zxuEPKe=6y9ynBttvs~)#i~B#1U$c7G7ulC_&HMAb=zgtFai!%GpN(q`_8k(q%x=-q z4@&Qxc864zXB$r7>b&<(7MfQ=Wzyg>X>r*Q$H^Uyz%7C9qZ`V;;W(9pif+{J@6hIH zW@YUMnrq{Y54tp#E^U0CcL8OZcZd>iP5f$UPq@_}9-U$}*4JzL@kr09_#9~~NyNqB z;AComVRh(PyMcqa+w8b;QeAyLOmdV>m%3FXofU|VQ>ia9FWwiuJy1Wt7Fv?P`W6T} zSdcsZK#qEG{I24I^C|jYhu_{1Tn2xZvACaf^hHnzp@F2Ukj_fv-M{m&x!Rs)i;PaKDqXr@cH%@U7&abfjv#MxGI) z)A4efPM1iyRFBfJn=c~kC$DR&^u60?pwe9AB9#RhNypap1vkDS=k-)*?!xpW@NPup z$k}2X#ACMXa{FptaB76n{6$7IR=1v=mbPR0N&;2oapjIpdOa#?=lV3&-3FrtUmWl+ z)_v@mscK}4ZEYE&=0qOVHxRz`l-KSVWn+1ISl{tYAC+(g?i;J6%j_mk)CxS{2{1o* zC&?kjB~qn{6Wud-kB}hfQZ@^k_`c0A5zwq5))9y=d;=T(VuKW;{T(x4OQ}9rzQuivh&!5qN6z$!WWAs zjvy;kL-KA#MfXNcx8hlP#EMvV=IJpEz0zzX`K-;I;P|?<{0D9}X`nLOckxd+Dox|h z_eq95g&S3T6f{RW;NqCTEn#NQ2dSZ&DJQ}1eqo5$LrMg`{LE6|iNGU>O;^C6+ZMTOaaoMri>_cMdAj99pnO2Dx zA(~No+>*W~-8zo>tbX@_jzpvqq3n65UAY1($Cez1n&9wfczT@oi%*PJZSKM`Tjjfd zJBs5@FnMa(liomXlqb_fXm4Y$N0?g@Jm{X~$fc;Q>W6<;2`_FLh`iDt(AOie_Ts zFb?l1O#G`V`37Y>E$kL)i-ZNP3{4fBA2`!*DI4h>-hpl-%!XhQ+eD>W-#PlX=}5I^ zZr|H+1(Zcl2FV??)Dsd#M%?I5&3qoAaI&SmD@D0G)tSv6ZV@*YJau|rkwD>p)p8h$ zK=@{YVM!XPH$N#+zoieQR#h)gRj+mlbFT^;U9C8ESvWrm_%8t$#>4_$aKJhIzW}Fn BbTI$` literal 0 HcmV?d00001 diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index eb1a292eba..4fcfb5af01 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -62,7 +62,7 @@ 0 0 307 - 380 + 389 @@ -80,16 +80,6 @@ - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -338,6 +328,53 @@ + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Edit Constraint + + + ... + + + + + + + Remove Prior + + + X + + + + + + + Qt::Horizontal + + + + 0 + 20 + + + + + + @@ -356,7 +393,7 @@ 0 0 306 - 380 + 389 @@ -520,16 +557,6 @@ - - - - - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -632,6 +659,53 @@ + + + + + + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Edit Constraint + + + ... + + + + + + + Remove Prior + + + X + + + + + + + Qt::Horizontal + + + + 0 + 20 + + + + + + @@ -843,8 +917,7 @@ - - + @@ -1269,18 +1342,80 @@ - - + + + + Align poses with ground truth + + + true + + + + + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false - - + + + + + + Root + + + + + + + Span to all maps + + + true + + + + + + + WM + + + + + + + + + Time grid (s) + + + + + + + RMSE (m) + + + + + + + + + + true + + + + + @@ -1289,7 +1424,7 @@ - + @@ -1299,45 +1434,61 @@ - - + + + + Align scans/clouds with ground truth + + + true + + - - + + - Poses + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Time optimization (s) + + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + <html><head/><body><p>N: Neighbor</p><p>NM: Neighbor Merged</p><p>G: Global</p><p>LS: Local by Space (Proximity)</p><p>LT: Local by Time (Proximity)</p><p>U: User</p><p>P: Prior</p><p>LM: Landmark</p><p>GR: Gravity</p></body></html> + Links (N, NM, G, LS, LT, U, P, LM, GR) - + Path length (m) - - + + - Time grid (s) + Poses - + @@ -1347,36 +1498,11 @@ - - - - - - Root - - - - - - - Span to all maps - - - true - - - - - - - WM - - - - + + - - + + @@ -1385,27 +1511,24 @@ - - + + - Align poses with ground truth + Ignore intermediate nodes true - - + + - - - - false + Time optimization (s) - + @@ -1415,49 +1538,22 @@ - - + + - + Align poses with GPS - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + true - - + + - Ignore intermediate nodes + - - true - - - - - - - RMSE (m) - - - - - - - Align scans/clouds with ground truth - - - true - - - - - - - - - + true @@ -1702,7 +1798,7 @@ 0 0 518 - 951 + 1007 @@ -1711,102 +1807,76 @@ - - - - - - - 1 - - - 16 - - - 16 + + + + OctoMap - - + + - Ground cell color + Crop radius when filtering empty space from 2d occupancy grid. true - - - - Crop radius when filtering empty space from 2d occupancy grid. + + + + pixels - - true + + 1 - - + + - Local grid: regenerate from saved grid instead of sensors + - - true + + false - - - - m - - - 3 + + + + - 0.000000000000000 + 1 - 99.000000000000000 - - - 0.010000000000000 + 16 - 0.000000000000000 - - - - - - - Empty cell color - - - true + 16 - - - + + + - - false + + 0 - - - - - - + + 16 + + + 16 - + @@ -1824,59 +1894,67 @@ - - + + - Show empty space + OctoMap: Rendering type true - - + + - Gain compensation radius (Constraints view) + + + + false + + + + + + + Voxel size (for clouds and scans) true - - + + - Show probabilistic occupancy grid in Graph View. + OctoMap: Tree depth true - - - - - Point Cloud - - - - - Cube - - - - - Volume - - + + + + + + + false + - - + + - Decimation (for images) + + + + + + + + Obstacle cell color true @@ -1884,52 +1962,56 @@ - + - Voxel size (for clouds and scans) + Gain compensation radius (Constraints view) true - - - - pixels + + + + - - 1 + + false - - - - + + + + Show probabilistic occupancy grid in Graph View. - - 0 + + true - - 16 + + + + + + For stereo data, show disparity instead of right image in main views. - - 16 + + true - - + + - Create RGB-D cloud from RGB projection on scan. + Ground cell color true - + m @@ -1951,7 +2033,17 @@ - + + + + Empty cell color + + + true + + + + @@ -1969,17 +2061,7 @@ - - - - - - - false - - - - + @@ -1997,83 +2079,145 @@ - - + + - Obstacle cell color + - - true + + false - - + + - false + true - - + + - OctoMap: Tree depth + Local grid: regenerate from saved grid instead of sensors true - - + + - OctoMap + Show frontiers + + + true - - + + - OctoMap: Rendering type + Create RGB-D cloud from RGB projection on scan. true - - + + - + Show empty space - + true - - + + + + + Point Cloud + + + + + Cube + + + + + Volume + + + + + + - For stereo data, show disparity instead of right image in main views. + Decimation (for images) true - - + + + + m + + + 3 + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.010000000000000 + + + 0.000000000000000 + + + + + - + Frontier cell color - - false + + true + + + + + + ... + + + + + + + true + + + + + @@ -2775,10 +2919,29 @@ + + + + Available if RTAB-Map is built with GridMap + + + Elevation + + + false + + + true + + + + + Adjust roll, pitch and height based on odometry pose + - Odom Frame + Odom true @@ -2946,6 +3109,16 @@ + + + + Grid + + + true + + + @@ -2956,6 +3129,22 @@ + + + + Available if RTAB-Map is built with GridMap + + + Elevation + + + false + + + true + + + @@ -3014,20 +3203,12 @@ View 3D map... - - - Refine all neighbor links... - - - Refine all neighbor links with the registration strategy set in "Core Parameters"->"Reg" - - - + - Refine all loop closure links... + Refine links... - Refine all loop closure links with the registration strategy set in "Core Parameters"->"Reg" + Refine selected links with the registration strategy set in "Core Parameters"->"Reg", "Vis" and/or "Icp" diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 59ff675454..736e9743d6 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -79,7 +79,7 @@ p, li { white-space: pre-wrap; } - Author : + Maintainer : @@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; } 0 - -438 + -675 596 - 1004 + 1050 @@ -185,70 +185,68 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With VINS-Fusion : true - - + + - Apache-2 + With OpenNI2 : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - + With K4W2 : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With OpenVINS : true - - + + @@ -260,8 +258,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -273,8 +271,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -286,108 +284,120 @@ p, li { white-space: pre-wrap; } - - + + - With DVO : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv2 + With RealSense : true - - + + - MIT + BSD true - - + + - With DepthAI : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + PSF true - - + + - With Octomap : + Apache v2 and/or GPLv2 true - - + + - With Freenect : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With stereo dc1394 : + Apache-2 true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Zed Open Capture : + With MYNTEYE S : true - - + + @@ -399,51 +409,58 @@ p, li { white-space: pre-wrap; } - - + + - With loam_velodyne : + With Ceres : true - - + + - + GPLv3 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With TORO : true - - + + - MIT + With GTSAM : true - - + + - With AliceVision : + Qt version : true - - + + Apache-2 @@ -452,31 +469,31 @@ p, li { white-space: pre-wrap; } - - + + - <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - Qt::RichText + With FastCV : true - - + + - Creative Commons [Attribution-NonCommercial-ShareAlike] + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -488,38 +505,38 @@ p, li { white-space: pre-wrap; } - - + + - With Freenect2 : + Penn Software License true - - + + - With g2o : + With Zed Open Capture : true - - + + - PSF + With libpointmatcher : true - - + + @@ -531,31 +548,28 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With SuperPoint Torch : true - - + + - MIT + With ORB Octree : true - - + + Apache v2 @@ -564,58 +578,74 @@ p, li { white-space: pre-wrap; } + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + - + - With ORB SLAM 2 : + With Viso2 : true - - + + - With MSCKF : + With FOVIS : true - - + + - BSD + VTK version : true - - + + - With Zed SDK : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache v2 + BSD true - - + + @@ -627,64 +657,88 @@ p, li { white-space: pre-wrap; } - - + + - + With Python3 : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With g2o : true - - + + - + OpenCV version : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + GPLv2 true - - + + - With stereo FlyCapture2 : + With K4A : + + + true + + + + + + + Open Source or Commercial true - - + + - PCL version : + BSD true - - + + - BSD + With Octomap : true - - + + BSD @@ -693,8 +747,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -706,8 +760,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -719,8 +773,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -732,48 +786,51 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With GridMap : true - - + + - With Viso2 : + With Freenect : true - - + + - Open Source or Commercial + With Freenect2 : true - - + + - With ORB Octree : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -785,18 +842,18 @@ p, li { white-space: pre-wrap; } - - + + - OpenCV version : + With loam_velodyne : true - - + + @@ -808,8 +865,18 @@ p, li { white-space: pre-wrap; } - - + + + + With ORB SLAM 2 : + + + true + + + + + @@ -821,18 +888,18 @@ p, li { white-space: pre-wrap; } - - + + - With GTSAM : + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> true - - + + BSD @@ -841,54 +908,48 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With CCCoreLib : true - - + + - GPLv2 + BSD true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache v2 true - - + + - Penn Software License + Apache v2 and/or GPLv2 true - - + + @@ -900,21 +961,18 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With CPU-TSDF : true - - + + @@ -926,78 +984,87 @@ p, li { white-space: pre-wrap; } - - + + - With Ceres : + With DepthAI : true - - + + - With OpenNI2 : + With DVO : true - - + + - With SuperPoint Torch : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Qt version : + BSD true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With FOVIS : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With MYNTEYE S : + GPLv3 true - - + + @@ -1009,18 +1076,18 @@ p, li { white-space: pre-wrap; } - - + + - With TORO : + With AliceVision : true - - + + @@ -1032,104 +1099,100 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + - MPL2 - - - true + - - - - - - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv2 + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> + + + Qt::RichText true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + MIT true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + With MRPT : true - - + + - LGPL + With OKVIS : true - - + + @@ -1141,38 +1204,31 @@ p, li { white-space: pre-wrap; } - - + + - With cvsba : - - - true + - - - - - - GPLv3 + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With FastCV : + BSD true - - + + @@ -1184,91 +1240,88 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With stereo FlyCapture2 : true - - + + - With Python3 : + GPLv3 true - - + + - With K4W2 : + GPLv3 true - - + + - With OpenChisel : + GPLv2 true - - + + - With RealSense2 : + BSD true - - + + - Apache v2 and/or GPLv2 + With stereo dc1394 : true - - + + - With OpenVINS : + GPLv3 true - - + + - GPLv3 + MIT true - - + + @@ -1280,8 +1333,8 @@ p, li { white-space: pre-wrap; } - - + + BSD @@ -1290,28 +1343,41 @@ p, li { white-space: pre-wrap; } - - + + - With OKVIS : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + PCL version : true - - + + + + With OpenChisel : + + + true + + + + + @@ -1323,8 +1389,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1336,28 +1402,28 @@ p, li { white-space: pre-wrap; } - - + + - With CCCoreLib : + With cvsba : true - - + + - With VINS-Fusion : + With Zed SDK : true - - + + @@ -1369,38 +1435,51 @@ p, li { white-space: pre-wrap; } - - + + - With K4A : + With RealSense2 : true - - + + - Apache v2 and/or GPLv2 + LGPL true - - + + - With CPU-TSDF : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + + + GPLv2 + + + true + + + + + @@ -1412,31 +1491,28 @@ p, li { white-space: pre-wrap; } - - + + - BSD + MPL2 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + @@ -1448,28 +1524,28 @@ p, li { white-space: pre-wrap; } - - + + - BSD + MIT true - - + + - With RealSense : + With MSCKF : true - - + + @@ -1481,28 +1557,18 @@ p, li { white-space: pre-wrap; } - - - - With libpointmatcher : - - - true - - - - - + + - VTK version : + Creative Commons [Attribution-NonCommercial-ShareAlike] true - - + + BSD @@ -1511,28 +1577,28 @@ p, li { white-space: pre-wrap; } - - + + - With MRPT : + MIT true - - + + - BSD + With Open3D : true - - + + @@ -1579,7 +1645,7 @@ p, li { white-space: pre-wrap; } - Copyright (C) 2010-2020 IntRoLab - Université de Sherbrooke + Copyright (C) 2010-2024 IntRoLab - Université de Sherbrooke Qt::AlignCenter diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index a59aa2c0f2..4667fe68c0 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -6,7 +6,7 @@ 0 0 - 1032 + 919 869 @@ -23,9 +23,9 @@ 0 - -2140 - 998 - 5850 + -1328 + 885 + 6169 @@ -1086,220 +1086,362 @@ false - - - - - m - - - 3 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.020000000000000 - - - - - - - Search radius. - - - true - - - - - - - 1 - - - 100 - - - 2 - - - - - - - Minimum neighbors in the search radius. - - - true - - - - - - - m - - - 2 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Ceiling filtering height. - - - true - - - - - - - m - - - 2 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Floor filtering height. - - - true - - - - - - - m - - - 2 - - - 0.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Footprint width. - - - true - - - - - - - m - - - 2 - - - 0.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - + + + + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + m + + + 3 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + 1 + + + 100 + + + 2 + + + + + + + Footprint length. + + + true + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint height. If negative, the footprint is filtered between [-height:height] around the base frame, otherwise it is [0:height] + + + true + + + + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Floor filtering height. This is done in map frame. + + + true + + + + + + + Search radius. + + + true + + + + + + + Footprint width. + + + true + + + + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Minimum neighbors in the search radius. + + + true + + + + + + + Ceiling filtering height. This is done in map frame. + + + true + + + + - - - - Footprint length. + + + + Off-Axis Filtering - + true - - - - - - m - - - 2 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Footprint height. If negative, the footprint is filtered between [-height:height] around the base frame, otherwise it is [0:height] - - + true + + + + + Keep only points with normals aligned with one/any of the map X-Y-Z axes. + + + true + + + + + + + + + +X + + + true + + + + + + + -Y + + + true + + + + + + + -X + + + true + + + + + + + +Y + + + true + + + + + + + +Z + + + true + + + + + + + -Z + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + deg + + + 1 + + + 0.000000000000000 + + + 179.000000000000000 + + + 1.000000000000000 + + + 10.000000000000000 + + + + + + + Max angle error to keep the points. + + + true + + + + + + diff --git a/guilib/src/ui/linkRefiningDialog.ui b/guilib/src/ui/linkRefiningDialog.ui new file mode 100644 index 0000000000..3bc362496f --- /dev/null +++ b/guilib/src/ui/linkRefiningDialog.ui @@ -0,0 +1,191 @@ + + + linkRefiningDialog + + + + 0 + 0 + 333 + 286 + + + + Link Refining + + + + + + Link Type + + + + + + + + + + Inter and Intra Sessions + + + + + Intra-only + + + + + Inter-only + + + + + + + + + + + Range + + + + + + From + + + + + + + To + + + + + + + Nodes: + + + + + + + 1 + + + 999999 + + + + + + + 1 + + + 999999 + + + + + + + Maps: + + + + + + + 0 + + + 999999 + + + 0 + + + + + + + 0 + + + 999999 + + + 0 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults + + + + + + + + + buttonBox + accepted() + linkRefiningDialog + accept() + + + 248 + 254 + + + 157 + 274 + + + + + buttonBox + rejected() + linkRefiningDialog + reject() + + + 316 + 260 + + + 286 + 274 + + + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 257d40e28b..ceb86a163d 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -105,16 +105,6 @@ Select source - - - RGB camera - - - - :/images/webcam.png:/images/webcam.png - - - RGB-D camera @@ -332,6 +322,16 @@ + + + OAK-D Pro + + + + :/images/oakdpro.png:/images/oakdpro.png + + + @@ -339,10 +339,17 @@ + + + + + LiDAR + + - + @@ -1244,18 +1251,6 @@ 240p - - - true - - - - :/images/webcam.png:/images/webcam.png - - - Usb camera - - Print loop closure IDs to console @@ -1722,6 +1717,22 @@ Depth AI + + + true + + + Depth AI + + + + + true + + + Velodyne VLP-16 + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 9f65689cc9..e1d9c32bd5 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6,7 +6,7 @@ 0 0 - 1052 + 1009 925 @@ -23,8 +23,8 @@ :/images/RTAB-Map.ico:/images/RTAB-Map.ico - - + + 0 @@ -63,9 +63,9 @@ 0 - -314 - 756 - 3657 + 0 + 713 + 4653 @@ -306,41 +306,117 @@ Save/Load Settings - + - - - Load settings (*.ini) ... - - - - - - - Save settings (*.ini) ... - - + + + + + Load settings (*.ini) ... + + + + + + + Save settings (*.ini) ... + + + + + + + Reset all settings + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + - - - Reset all settings + + + Presets + + + + + <html><head/><body><p><a href="https://github.com/introlab/rtabmap/tree/master/data"><span style=" text-decoration: underline; color:#0000ff;">Presets</span></a> are applied after resetting to defaults the 3D Rendering panel and Core parameters. You can create your own presets by saving/loading settings above.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + TOF Camera ICP + + + + + + + <html><head/><body><p>e.g., Kinect [v2, For Azure] and L515 cameras. Example <a href="http://official-rtab-map-forum.206.s1.nabble.com/Kinect-For-Azure-L515-ICP-lighting-invariant-mapping-td7187.html"><span style=" text-decoration: underline; color:#0000ff;">here</span></a>.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + LiDAR 3D ICP + + + + + + + e.g., Velodyne, RoboSense and Ouster LiDARs. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + - - - - Qt::Horizontal - - - - 40 - 20 - - - - @@ -2414,7 +2490,52 @@ Show a yellow background when the number of odometry inliers goes under this thr - + + + + Show in 3D map view. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. Set 0 to use same value from Local Occupancy Grid panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0.010000000000000 + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.750000000000000 + + + + @@ -2424,42 +2545,67 @@ Show a yellow background when the number of odometry inliers goes under this thr - - + + + + Partially checked: Height color map. Checked: RGB color map. + - Show in 3D map view. + + + + false + + + true + + + + + + + Show Elevation Map in 3D map view. + + + true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + Opacity. + + true + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - + m + + + 2 - 0.010000000000000 + 0.000000000000000 - 1.000000000000000 + 10.000000000000000 - 0.050000000000000 + 0.010000000000000 - 0.750000000000000 + 0.000000000000000 @@ -2903,704 +3049,561 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Source - + - - - - - Input rate (0 means as fast as possible). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Source type. Select specific driver below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - ... - - - - - - - - 100 - 0 - - - - - - - - - - - - - Hz - - - 1 - - - 100.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - Mirroring mode (flip image horizontally). It has no effect on database source. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - QComboBox::AdjustToContents - - - - RGB-D - - - - - Stereo - - - - - RGB - - - - - Database - - - - - - - - - 0 - 0 - - - - Test - - - - - - - - 0 - 0 - - - - Calibrate - - - - - - - Calibration files are saved in "camera_info" folder of the working directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - 0 - 0 - - - - Create Calibration - - - - - - - Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1 - - - - - - - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - - - 3 + + + Camera Sensor - - - - - - RGB-D - - - false - - - false - - + + + + + + + QComboBox::AdjustToContents + - - - Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + RGB-D + - - - - - QComboBox::AdjustToContents - - - - OpenNI-PCL - - - - - Freenect - - - - - OpenNI-CV - - - - - OpenNI-CV-ASUS - - - - - OpenNI2 - - - - - Freenect2 - - - - - RealSense - - - - - Images - - - - - Kinect for Windows SDK v2 - - - - - RealSense2 - - - - - Kinect for Azure - - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + + Stereo + - - - 0 - - - - - - - - 0 - 0 - - - - OpenNI - - - - - - - - - - - - - Path to a *.ONI file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - ... - - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + RGB + + + + + Database + + + + + None + + + + + + + + Source type. Select specific driver below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Hz + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Input rate (0 means as fast as possible). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Mirroring mode (flip image horizontally). It has no effect on database source. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + + + + + Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + Histogram + + + + + CLAHE + + + + + + + + Equalizes the histogram of grayscale images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Detect features inside camera thread using parameters set in Visual Registration panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + + 100 + 0 + + + + + - - - - - - - 0 - 0 - - - - OpenNI 2 - - - - - - - - - true - - - - - - - Auto exposure. - - - true - - - - - - - - - - - - - - - - - true - - - - - - - 65535 - - - - - - - Use timestamps and frame IDs from OpenNI2. - - - true - - - - - - - Auto white balance. - - - true - - - - - - - IR-Depth vertical shift. Positive toward up, negative toward down. - - - true - - - - - - - Mirroring. - - - true - - - - - - - - - - false - - + + + + + + + Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Calibrate + + + + + + + Calibration files are saved in "camera_info" folder of the working directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Create Calibration + + + + + + + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Test + + + + + + + + + + 1 + + + + + + + RGB-D + + + false + + + false + + + + + + Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + QComboBox::AdjustToContents + + + + OpenNI-PCL + - - - - pix - - - -9999 - - - 9999 - - + + + Freenect + - - - - Exposure. - - - true - - + + + OpenNI-CV + - - - - 1000 - - - 100 - - + + + OpenNI-CV-ASUS + - - - - Gain. - - - true - - + + + OpenNI2 + - - - - Path to a *.ONI file. - - - true - - + + + Freenect2 + - - - - ... - - + + + RealSense + - - - - - - - false + + + Images + + + + + Kinect for Windows SDK v2 + + + + + RealSense2 + + + + + Kinect for Azure + + + + + + + + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 4 + + + + + + + + 0 + 0 + + + OpenNI + + + + + + + + + + + + + Path to a *.ONI file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + - - - - IR-Depth horizontal shift. Positive toward left, negative toward right. + + + + + + + + Qt::Vertical - - true + + + 20 + 696 + - + - - + + + + + + Qt::Vertical @@ -3612,531 +3615,2292 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - pix - - - -9999 + + + + + + + + Qt::Vertical - - 9999 + + + 20 + 0 + - + - - - - Depth decimation. - - - true + + + + + + + + + 0 + 0 + + + OpenNI 2 + + + + + + Path to a *.ONI file. + + + true + + + + + + + + + + + + + + ... + + + + + + + Auto white balance. + + + true + + + + + + + + + + true + + + + + + + Auto exposure. + + + true + + + + + + + + + + true + + + + + + + Exposure. + + + true + + + + + + + 65535 + + + + + + + Gain. + + + true + + + + + + + 1000 + + + 100 + + + + + + + Use timestamps and frame IDs from OpenNI2. + + + true + + + + + + + + + + false + + + + + + + Mirroring. + + + true + + + + + + + + + + false + + + + + + + IR-Depth horizontal shift. Positive toward left, negative toward right. + + + true + + + + + + + pix + + + -9999 + + + 9999 + + + + + + + IR-Depth vertical shift. Positive toward up, negative toward down. + + + true + + + + + + + pix + + + -9999 + + + 9999 + + + + + + + Depth decimation. + + + true + + + + + + + + + + 1 + + + 65535 + + + + - - - - - - - 1 - - - 65535 + + + + + + + + + 0 + 0 + + + Freenect2 + + + + + + Depth edge aware filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 65.000000000000000 + + + 12.000000000000000 + + + + + + + QComboBox::AdjustToContents + + + + Registered Color to Depth SD + + + + + Registered Depth to Color SD (old way) + + + + + Registered Depth to Color HD + + + + + Registered Depth to Color HD (old way) + + + + + IR + Depth + + + + + + + + + + + true + + + + + + + Depth bilateral filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + Min depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 64.000000000000000 + + + 0.100000000000000 + + + 0.300000000000000 + + + + + + + Depth noise filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Max depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + Pipeline, any one of "gl cl clkde cuda cudakde cpu". If empty, default one is used. Depending on how libfreenect2 was built, the selected pipeline may not be available. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + - - - - - - - - - - 0 - 0 - - - - Freenect2 - - - - - - Depth edge aware filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + 0 + + + RealSense + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + + + + RGB camera source. For fisheye has depth available only if it is calibrated. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Color + + + + + Infrared + + + + + Fisheye + + + + + + + + Preset for depth stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Preset for RGB stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Use visual inertial odometry for ZR300. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Depth image scaled to RGB image size. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + - - - - 65.000000000000000 - - - 12.000000000000000 + + + + + + + + + 0 + 0 + + + RGB-D Images + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + For RGBD-SLAM datasets, it is 5. + + + 3 + + + 1.000000000000000 + + + 999999.000000000000000 + + + + + + + + + + + + + + Path to directory containing RGB images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + + + + 9999999 + + + + + + + ... + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Depth scale factor (depth = pixel value / factor). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + Path to directory containing depth images. The directory should have the same size has the RGB directory. The depth images should be already registered to RGB images. Assume that UINT16 images are in mm and FLOAT32 images are in m. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - QComboBox::AdjustToContents + + + + + + + + + 0 + 0 + - - - Registered Color to Depth SD - - - - - Registered Depth to Color SD (old way) - - - - - Registered Depth to Color HD - - - - - Registered Depth to Color HD (old way) - - - - - IR + Depth - - + + Kinect for Windows SDK v2 + + + + + + QComboBox::AdjustToContents + + + + Registered Color to Depth SD + + + + + Registered Depth to Color SD + + + + + Registered Depth to Color HD + + + + + + + + Format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - - - - true + + + + + + + + + 0 + 0 + + + RealSense2 + + + + + + + + + false + + + + + + + IR emitter enabled + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + IR mode: use IR camera instead of RGB camera. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Use depth image in IR mode (instead of right image). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + RGB/IR stream width. Set 1280 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + RGB/IR stream height. Set 720 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Hz + + + 999 + + + + + + + RGB/IR stream rate. Set 30 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + (L515) Depth stream width. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + (L515) Depth stream height. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Hz + + + 999 + + + + + + + (L515) Depth stream rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - Depth bilateral filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 0 + 0 + + + Kinect for Azure + + + + + + + + + false + + + + + + + + + + + + + + Use MKV file stamps as input rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Frames per second + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + + 0 + 0 + + + + + 5 + + + + + 15 + + + + + 30 + + + + + + + + + 720p + + + + + 1080p + + + + + 1440p + + + + + 1536p + + + + + 2160p + + + + + 3072p + + + + + + + + Path to a *.MKV file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use IR for RGB image (may work better in dark areas). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 320x288 (NFOV 2x2BINNED) + + + + + 640x576 (NFOV UNBINNED) + + + + + 512x512 (WFOV 2x2BINNED) + + + + + 1024x1024 (WFOV UNBINNED) + + + + + + + + Depth camera resolution. 2x2 binning mode extends the Z-range in comparison to the corresponding unbinned mode. Binning is done at the cost of lowering image resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>RGB camera resolution. Note that when color camera is used (IR mode is not checked below), to <span style=" font-weight:600;">avoid black borders in point clouds</span> generated, set ROI ratios under 3D Rendering settings to &quot;0.05 0.05 0.05 0.05&quot; under Map and Odom columns.</p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - - - - true - - + + + + + + + + + + + + + + + Stereo + + + false + + + false + + + + + + Grabber for stereo devices (i.e., Bumblebee2, RealSense2, DepthAI, Zed cameras). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + QComboBox::AdjustToContents + + + + DC1394 + - - - - Min depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + FlyCapture2 + - - - - Format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Images + - - - - 64.000000000000000 - - - 0.100000000000000 - - - 0.300000000000000 - - + + + Video + - - - - Depth noise filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + ZED sdk + - - - - Max depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Usb camera + - - - - - - - true + + + Tara Camera + + + + + RealSense2 + + + + + MyntEyeS + + + + + ZED Open Capture + + + + + DepthAI + + + + + + + + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Auto exposure compensation between left and right images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + 4 + + + + + + + + + + + + + + 0 + 0 + + + Stereo Images + + + + + + ... + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + Path to directory containing left images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Path to directory containing right images. The directory should have the same size has the left images directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + - - - - Pipeline, any one of "gl cl clkde cuda cudakde cpu". If empty, default one is used. Depending on how libfreenect2 was built, the selected pipeline may not be available. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 0 + 0 + + + Stereo Video (*.avi, *.mp4) + + + + + + ... + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + Side-by-Side (SBS) video or left video. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Right video. Should be empty if a SBS video is set above. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + - - - - + + + + + + + + + 0 + 0 + + + Zed sdk + + + + + + Self-Calibration. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + NONE + + + + + PERFORMANCE + + + + + QUALITY + + + + + ULTRA + + + + + NEURAL + + + + + + + + [SDK3] Texture confidence. Threshold to reject depth values based on their textureness confidence. A lower value means more confidence and precision (but less density). An upper value reduces filtering (more density, less certainty.). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. Not used when a SVO file is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Depth confidence. Filtering value for the disparity map (and by extension the depth map). A lower value means more confidence and precision (but less density), an upper value reduces the filtering (more density, less certainty). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Sensing mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 100 + + + + + + + 100 + + + + + + + + + ... + + + + + + + + + + + + + + + + + + + + + + + Quality. If NONE, the disparity is not computed on the GPU, but on CPU using OpenCV (see StereoBM panel for parameters). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to a *.SVO file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + STANDARD + + + + + FILL + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + HD2K + + + + + HD1080 + + + + + HD1200 + + + + + HD720 + + + + + SVGA + + + + + VGA + + + + + AUTO + + + + + - - - - - - - - - - 0 - 0 - - - - RealSense - - - - - - QComboBox::AdjustToContents - - - - Best Quality - - - - - Largest Image - - - - - Highest Framerate - - - - - - - - Preset for depth stream. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Preset for RGB stream. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - Best Quality - - - - - Largest Image - - - - - Highest Framerate - - - - - - - - Use visual inertial odometry for ZR300. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - - - - false - - - - - - - Depth image scaled to RGB image size. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents + + + + + + + 0 + 0 + - - - Color - - - - - Infrared - - - - - Fisheye - - + + Usb Camera + + + + + + Optional right device ID. It should be -1 if the stereo camera streams side-by-side images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + + + + + Stream width (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stream height (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + -1 + + + + + + + 0 + + + 10000 + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + - - - - RGB camera source. For fisheye has depth available only if it is calibrated. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + 0 + 0 + + + MYNT EYE S + + + + + + + + + + + + + Compute depth image with MYNT EYE S SDK. Ignored if images are not rectified from MYNT EYE S SDK. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Rectify images using MYNT EYE S SDK. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + 240 + + + 120 + + + + + + + 48 + + + 24 + + + + + + + + + + true + + + + + + + Image gain [0-48], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image brightness [0-240], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Auto-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image contrast [0-254], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 254 + + + 116 + + + + + + + IR control [0-160]. 0 means that the IR pattern is not projected. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 160 + + + 116 + + + + - - + + Qt::Vertical 20 - 40 + 0 - - - - - - - - - - 0 - 0 - - - - RGB-D Images - - - - - - Path to directory containing RGB images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - Depth scale factor (depth = pixel value / factor). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - Path to directory containing depth images. The directory should have the same size has the RGB directory. The depth images should be already registered to RGB images. Assume that UINT16 images are in mm and FLOAT32 images are in m. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - For RGBD-SLAM datasets, it is 5. - - - 3 - - - 1.000000000000000 - - - 999999.000000000000000 + + + + + + + 0 + 0 + + + Zed Open Capture + + + + + + + HD2K + + + + + HD1080 + + + + + HD720 + + + + + VGA + + + + + + + + Resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - + + Qt::Vertical @@ -4148,102 +5912,488 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - - - - - - - 0 - 0 - - - - Kinect for Windows SDK v2 - - - - - - QComboBox::AdjustToContents - - - - Registered Color to Depth SD - - - - - Registered Depth to Color SD - - - - - Registered Depth to Color HD - - - - - - - - Format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + 0 + + + DepthAI + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Disabled + + + + + 64 pixels + + + + + 96 pixels + + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + 720p + + + + + 800p + + + + + 400p + + + + + 480p + + + + + 1200p + + + + + + + + -1.000000000000000 + + + 1.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + IR laser dot projector intensity. 0 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + GFTT + + + + + SuperPoint + + + + + HF-Net + + + + + + + + Use the translation information from the board design data (not the calibration data). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Output mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + On-device feature detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Stereo + + + + + Mono+Depth + + + + + Color+Depth + + + + + + + + <html><head/><body><p>Disparity range increased from 0-95 to 0-190, combined from full resolution and downscaled images.</p></body></html> + + + Extended disparity. Suitable for short range objects. Currently incompatible with sub-pixel disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 255 + + + 200 + + + + + + + -1 + + + 255 + + + 5 + + + + + + + + + + + + + + Disparity confidence threshold (0-255). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + IR flood light intensity. 0 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to MyriadX blob file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>Computes disparity with sub-pixel interpolation (3 fractional bits by default). </p></body></html> + + + Subpixel mode: number of fractional bits. Suitable for long range. Currently incompatible with extended disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Disabled + + + + + Subpixel 3 + + + + + Subpixel 4 + + + + + Subpixel 5 + + + + + + + + + + + + + + + Computes and combines disparities in both L-R and R-L directions, and combine them. + + + Disparity left-right check threshold. Set -1 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. + + + Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). If alpha is set to -1, old rectification policy is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p> * Matching pixel by pixel for N disparities.</p><p> * Matching every 2nd pixel for M disparitites.</p><p> * Matching every 4th pixel for T disparities.</p><p> * In case of 96 disparities: N=48, M=32, T=16.</p><p> * This way the search range is extended to 176 disparities, by sparse matching.</p><p> * Note: when enabling this flag only depth map will be affected, disparity map is not.</p></body></html> + + + Disparity companding using sparse matching. Currently incompatible with extended disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + + + IMU published + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - + + Qt::Vertical @@ -4257,261 +6407,150 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - Qt::Vertical - - - - 20 - 283 - - - - - - - - - - - - - 0 - 0 - - - - RealSense2 - - - - - - Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Hz - - - 999 - - - - - - - (L515) Depth stream width. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - pix - - - 9999 - - - - - - - RGB/IR stream width. Set 1280 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Hz - - - 999 - - - - - - - Use depth image in IR mode (instead of right image). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - pix - - - 9999 - - - - - - - RGB/IR stream rate. Set 30 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - IR mode: use IR camera instead of RGB camera. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - (L515) Depth stream height. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - false - - + + + + + + + + + + + + + RGB + + + false + + + false + + + + + + + + Source type. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + + Usb camera + - - - - pix - - - 9999 - - + + + Images + - - - - (L515) Depth stream rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Video file + - - - - IR emitter enabled - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + 0 + + + + + + + USB Camera + + + + + + 0 + + + 10000 + + + + + + + Stream height (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stream width (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + + - - + + Qt::Vertical @@ -4523,2337 +6562,1589 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - RGB/IR stream height. Set 720 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + + + + Images Dataset + + + + + + false + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 999999999 + + + + + + + ... + + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 999999999 + + + + - - - - + + + + Qt::Vertical - - false + + + 0 + 0 + + + + + + + + + + + Video (AVI) + + + + + + false + + + + + + + ... + + + + - - - - pix + + + + Qt::Vertical - - 9999 + + + 0 + 0 + - + - - - - - - - - - - 0 - 0 - - - - Kinect for Azure - - - - - - - - - false - - - - - - - - - - - - - - Use MKV file stamps as input rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Frames per second - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - 0 - 0 - - - - - 5 - - - - - 15 - - - - - 30 - - - - - - - - - 720p - - - - - 1080p - - - - - 1440p - - - - - 1536p - - - - - 2160p - - - - - 3072p - - - - - - - - Path to a *.MKV file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use IR for RGB image (may work better in dark areas). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 320x288 (NFOV 2x2BINNED) - - - - - 640x576 (NFOV UNBINNED) - - - - - 512x512 (WFOV 2x2BINNED) - - - - - 1024x1024 (WFOV UNBINNED) - - - - - - - - Depth camera resolution. 2x2 binning mode extends the Z-range in comparison to the corresponding unbinned mode. Binning is done at the cost of lowering image resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>RGB camera resolution. Note that when color camera is used (IR mode is not checked below), to <span style=" font-weight:600;">avoid black borders in point clouds</span> generated, set ROI ratios under 3D Rendering settings to &quot;0.05 0.05 0.05 0.05&quot; under Map and Odom columns.</p></body></html> - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - + + + - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - Stereo - - - false - - - false - - + + - - - Grabber for stereo devices (i.e., Bumblebee2, Zed camera). + + + Database - - true + + false - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false - - - - - - - - QComboBox::AdjustToContents - - + + + + + 0 + + + 99999 + + + + + + + 0 + + + 99999 + + + + + + + + - DC1394 + Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - FlyCapture2 + - - + + + + - Images + Ignore goals saved in the database. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + - Video + + + + + + + + Open database viewer - - - ZED sdk + - - + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + - Usb camera + - - + + + + - Tara Camera + Ignore goal delay. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - RealSense2 + Ignore features. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - MyntEyeS + Use database stamps as input rate. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - ZED Open Capture + Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - DepthAI + Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Auto exposure compensation between left and right images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + + + + + + + + + + + + + + + + + + Ignore landmarks. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + -1 + + + 9999 + + + + + + + Start position (node ID). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ignore priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + - - - 10 + + + Qt::Vertical - - + + + 20 + 1375 + + + + + + + + + + + + Directory of images (optional settings) + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + Ground truth file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + + Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - 0 - 0 - - - - Stereo Images - - - - - - ... - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - Path to directory containing left images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Path to directory containing right images. The directory should have the same size has the left images directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - + + + + + Bayer mode. For convenience, if the images are bayered. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - 0 - 0 - - - - Stereo Video (*.avi, *.mp4) - - - - - - ... - - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - Side-by-Side (SBS) video or left video. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Right video. Should be empty if a SBS video is set above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - + + + + + ... + - - - - - - - 0 - 0 - - - - Zed sdk - - - - - - Self-Calibration. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - NONE - - - - - PERFORMANCE - - - - - QUALITY - - - - - ULTRA - - - - - - - - [SDK3] Texture confidence. Threshold to reject depth values based on their textureness confidence. A lower value means more confidence and precision (but less density). An upper value reduces filtering (more density, less certainty.). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Resolution. Not used when a SVO file is used. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Depth confidence. Filtering value for the disparity map (and by extension the depth map). A lower value means more confidence and precision (but less density), an upper value reduces the filtering (more density, less certainty). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Sensing mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 100 - - - - - - - 100 - - - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - - - Quality. If NONE, the disparity is not computed on the GPU, but on CPU using OpenCV (see StereoBM panel for parameters). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to a *.SVO file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - STANDARD - - - - - FILL - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - HD2K - - - - - HD1080 - - - - - HD720 - - - - - VGA - - - - - - - - + + + + + + - - - - - - - 0 - 0 - - - - Usb Camera - - - - - - Optional right device ID. It should be -1 if the stereo camera streams side-by-side images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 10000 - - - - - - - Stream width (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stream height (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -1 - - - - - - - 0 - - - 10000 - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - + + + + + Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + + Odometry file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + + Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - 0 - 0 - - - - MYNT EYE S - - - - - - - - - - - - - Compute depth image with MYNT EYE S SDK. Ignored if images are not rectified from MYNT EYE S SDK. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Rectify images using MYNT EYE S SDK. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - 240 - - - 120 - - - - - - - 48 - - - 24 - - - - - - - - - - true - - - - - - - Image gain [0-48], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image brightness [0-240], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Auto-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image contrast [0-254], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 254 - - - 116 - - - - - - - IR control [0-160]. 0 means that the IR pattern is not projected. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 160 - - - 116 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + ... + - - - - - - - 0 - 0 - - - - Zed Open Capture - - - - - - - HD2K - - - - - HD1080 - - - - - HD720 - - - - - VGA - - - - - - - - Resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + + - - - - - - - 0 - 0 - - - - DepthAI - - - - - - Resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Output depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Depth confidence. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - 720p - - - - - 800p - - - - - 400p - - - - - 480p - - - - - 1200p - - - - - - - - 0 - - - 255 - - - 200 - - - - - - - IMU firmware update - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - IMU published - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + + - - - - - - - - - - - - - RGB - - - false - - - false - - - - - - + + + + + + + + + + + + + + + + + + + Maximum laser scan points. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Synchronize capture rate with timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 99999999 + + + + + + + <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + QComboBox::AdjustToContents + + - Source type. + Raw - - true + + + + RGBD-SLAM (motion capture) - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + KITTI - - - - - - 0 + + + + TORO - - - Usb camera - - - - - Images - - - - - Video file - - - - - - + + - Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + g2o - - true + + + + NewCollege - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Malaga Urban - - - - + + - + St Lucia Stereo - - - - - - - - 0 - - - - - - - USB Camera - - - - - - 0 - - - 10000 - - - - - - - Stream height (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stream width (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 10000 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + Karlsruhe + + + + + EuRoC MAV + + + + + RGBD-SLAM + + + + + RGBD-SLAM + ID + + - - - - - - Images Dataset - - - - - - false - - - - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 999999999 - - - - - - - ... - - - - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 999999999 - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - + + + + + ... + - - - - - - Video (AVI) - - - - - - false - - - - - - - ... - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> + + + 0 0 0 0 0 0 + - - - - - - - - - - - - - Database - - - false - - - false - - - - - - 0 - - - 99999 - - - - - - - 0 - - - 99999 - - - - - - - - - - Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Ignore goals saved in the database. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - - - Open database viewer - - - - - - - :/images/mag_glass.png:/images/mag_glass.png - - - - - - - - - - - - - - Ignore goal delay. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore features. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use database stamps as input rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - Ignore landmarks. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -1 - - - 9999 - - - - - - - Start position (node ID). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - - - - - Directory of images (optional settings) - - - - 0 - - - 0 - - - 0 - - - 0 - + + + + + Local transform from /base_link to /laser_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Disabled + + + + + BG + + + + + GB + + + + + RG + + + + + GR + + + + + + + + Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + QComboBox::AdjustToContents + + + + Raw + + + + + RGBD-SLAM (motion capture) + + + + + KITTI + + + + + TORO + + + + + g2o + + + + + NewCollege + + + + + Malaga Urban + + + + + St Lucia Stereo + + + + + Karlsruhe + + + + + EuRoC MAV + + + + + RGBD-SLAM + + + + + RGBD-SLAM + ID + + + + + + + + ... + + + + + + + Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 4 + + + 9.990000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + Local transform from /base_link to /imu_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to file containing optional IMU data (*.csv [EuRoC format]). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + EuRoC: 200 Hz -> 250 Hz + + + 99999999 + + + + + + + Load config file for each frame. For calibration, global calibration path above should be empty. Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + Generate depth image from laser scan + + + true + + + false + + + + + + Interpolate depth values to fill holes + + + true + + + + + + Vertically + + + true + + + + + + + Horizontally + + + + + + + + + + Fill holes from the image border. + + + + + + + + + - - - - - Ground truth file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Bayer mode. For convenience, if the images are bayered. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Odometry file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Maximum laser scan points. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Synchronize capture rate with timestamps. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 99999999 - - - - - - - <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> - - - QComboBox::AdjustToContents - - - - Raw - - - - - RGBD-SLAM (motion capture) - - - - - KITTI - - - - - TORO - - - - - g2o - - - - - NewCollege - - - - - Malaga Urban - - - - - St Lucia Stereo - - - - - Karlsruhe - - - - - EuRoC MAV - - - - - RGBD-SLAM - - - - - RGBD-SLAM + ID - - - - - - - - ... - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - Local transform from /base_link to /scan_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - Disabled - - - - - BG - - - - - GB + + + Depth Image Filtering + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + Open database viewer + + + + + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + + + Distortion model (output from depth calibration). + + + true + + + + + + + + + Bilateral Filtering of the Depth Image - - - - RG + + true - - + + + + + + + + 4 + + + 0.000100000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.005000000000000 + + + + + + + Standard deviation of the Gaussian for the intensity difference. Set the standard deviation of the Gaussian used to control how much an adjacent pixel is downweighted because of the intensity difference (depth in our case). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 1 + + + 0.100000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 5.000000000000000 + + + + + + + Size of the Gaussian bilateral filter window to use. Set the standard deviation of the Gaussian used by the bilateral filter for the spatial neighborhood/window. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + IMU Filtering + + + + + + + + + None + + + + + Madgwick Filter + + + + + Complementary Filter + + + + + + + + IMU filtering strategy used to estimate the orientation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + false + + + + + + + Convert IMU in base frame before filtering. This can help to initialize correctly the yaw. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + Madgwick Filter + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Gain. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Zeta. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + Complementary Filter + + + + + + Do bias estimation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Gain acc. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Bias alpha. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + + + + + + + + Do adaptive gain. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + + + + Visual Odometry Sensor + + + + + + + + QComboBox::AdjustToContents + + + + None + + + + + RealSense2 (T265) + + + - GR + ZED sdk - - + + - Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + Driver. true @@ -6863,87 +8154,299 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> - - - QComboBox::AdjustToContents - - - - Raw - - - + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + - RGBD-SLAM (motion capture) + Calibration file path (*.yaml) for the visual odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. - - - - KITTI + + true - - - - TORO + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + - g2o + Calibrate extrinsics between visual odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - NewCollege + - - + + + + + + ms + + + 1 + + + -999.000000000000000 + + + 999.000000000000000 + + + + + - Malaga Urban + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - St Lucia Stereo + Time offset between camera and visual odometry sensors. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000100000000000 + + + 999.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + - Karlsruhe + Scale factor between camera and visual odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - EuRoC MAV + Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and visual odometry sensors are the same sensor.</p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Calibrate Extrinsics + + + + + + + + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p><br/></p></body></html> + + + 0.009 0.021 0.027 0.000 -0.018 0.005 + + + + + + + Maximum wait time to get the pose for latest data captured. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ms + + + 0 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 100.000000000000000 + + + + + + + + + + + + + LiDAR Sensor + + + false + + + false + + + + + + If you want to use ICP registration, the sensor data should have laser scan. Laser scans can be created from the depth images (see option below) or loaded from the source selected. These parameters can be used to reduce the point cloud size directly in the capturing thread. + + + true + + + + + + + + + QComboBox::AdjustToContents + - RGBD-SLAM + None - RGBD-SLAM + ID + VLP-16 - - - - ... + + + + LiDAR odometry will update the camera local transform to match its real pose at the LiDAR stamp. - - - - - Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. + Grabber for LiDAR devices (e.g., Velodyne). The capture will be synchronized with LiDAR frame rate. If a camera driver is set, images will be added to LiDAR capture. true @@ -6953,42 +8456,17 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - s - - - 4 - - - 9.990000000000000 - - - 0.010000000000000 - - - 0.020000000000000 - - - - - + + - Local transform from /base_link to /imu_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - Path to file containing optional IMU data (*.csv [EuRoC format]). + Generate laser scan from depth image. true @@ -6998,1118 +8476,630 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - ... - - - - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - EuRoC: 200 Hz -> 250 Hz - - - 99999999 - - - - - - - Load config file for each frame. For calibration, global calibration path above should be empty. Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - Generate depth image from laser scan - - - true - - - false - - - - - - Interpolate depth values to fill holes + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + m - + + 9999.000000000000000 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + m + + + 3 + + + 0.010000000000000 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 1 + + + 99999999 + + + + + + + Minimum range, filter points below that range. 0 means disabled. + + true - - - - - Vertically - - - true - - - - - - - Horizontally - - - - + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + Lidar deskewing. The input scan should have time channel and "Visual Odometry Sensor" should be used. If only IMU is available through the camera, enable publishing of inter IMU under "IMU filtering" above and enable deskewing in Odometry parameters instead. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Voxel size for uniform sampling. Note that it is done after downsampling. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 9999.000000000000000 + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Local transform from /base_link to /laser_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum range, filter points above that range. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 0 + + + 99999999 + + + 0 + + + + + + + K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 0.010000000000000 + + + + + + + Force ground normals to be all upward (e.g., 0.8). Useful to make sure Velodyne's scans on ground have all normals upward. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + Downsample step size for laser scans. If your laser scans are created from depth images, the step is used as decimation of the depth images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + - Fill holes from the image border. + Test - - - - - - - Odometry Sensor - - - - - - ms - - + + + 1 - - -999.000000000000000 - - - 999.000000000000000 - - - - - - - - - - - - - - Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and odometry sensors are the same sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - None - - - - - RealSense2 (T265) - - - - - ZED sdk - - - - - - - - - 0 - 0 - - - - Calibrate Extrinsics - - - - - - - Calibrate extrinsics between odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Time offset between camera and odometry sensors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Calibration file path (*.yaml) for the odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p><br/></p></body></html> - - - 0.009 0.021 0.027 0.000 -0.018 0.005 - - - - - - - - - ... - - - - - - - - 100 - 0 - - - - - - - - - - - - - Scale factor between camera and odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 4 - - - 0.000100000000000 - - - 999.000000000000000 - - - 0.100000000000000 - - - 1.000000000000000 - + + + + + + Qt::Vertical + + + + 20 + 255 + + + + + + + + + + + + VLP16 + + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 255 + + + 192 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 255 + + + 168 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 250 + + + 1 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 250 + + + 201 + + + + + + + + + <html><head/><body><p>If the lidar is not connected to a GPS, the lidar's clock may be not in sync with the host computer's clock. By enabling this, as soon as a packet is received, it will be stamped with host time &quot;now&quot;. Note that it affects only the final scan timsestamp, not the individual relative timestamp for each point. It is recommended to enable this if you combine it with other sensors above and it is not connected to a GPS.</p></body></html> + + + Use host time. See tooltip for more info. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + ... + + + + + + + IP Address. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + This can speedup deskewing. + + + Output organized point cloud. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to a *.PCAP file. IP/Port/Host time are ignored if PCAP file is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + Port. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 99999 + + + 2368 + + + + + + + <html><head/><body><p>This makes synchronization closer with other sensors above as they will be pulled only after the scan is fully received.</p></body></html> + + + Make the scan timestamp corresponding to timestamp of the last point of the rotation, otherwise it will be the timestamp of the first point of the rotation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + - - - - Depth Image Filtering + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + RTAB-Map Settings + + + + + + Hz + + + 3 + + + 1.000000000000000 - - - - - - - ... - - - - - - - - 100 - 0 - - - - - - - - - - - Open database viewer - - - - - - - :/images/mag_glass.png:/images/mag_glass.png - - - - - - - Distortion model (output from depth calibration). - - - true - - - - - - - - - Bilateral Filtering of the Depth Image - - - true - - - - - - - - - 4 - - - 0.000100000000000 - - - 1.000000000000000 - - - 0.001000000000000 - - - 0.005000000000000 - - - - - - - Standard deviation of the Gaussian for the intensity difference. Set the standard deviation of the Gaussian used to control how much an adjacent pixel is downweighted because of the intensity difference (depth in our case). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 1 - - - 0.100000000000000 - - - 100.000000000000000 - - - 1.000000000000000 - - - 5.000000000000000 - - - - - - - Size of the Gaussian bilateral filter window to use. Set the standard deviation of the Gaussian used by the bilateral filter for the spatial neighborhood/window. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - IMU Filtering + + + + + + + true - - - - - - - - None - - - - - Madgwick Filter - - - - - Complementary Filter - - - - - - - - IMU filtering strategy used to estimate the orientation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. IMU filtering strategy will be ignored. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Convert IMU in base frame before filtering. This can help to initialize correctly the yaw. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - - - 2 - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - Madgwick Filter - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - Gain. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - Zeta. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - Complementary Filter - - - - - - Do bias estimation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Gain acc. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - Bias alpha. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - - - - - - - - Do adaptive gain. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - Laser scans + + + + SLAM mode. + If set to false, global localization is performed, so without adding new data to the map. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + - false + true - - - - - If you want to use ICP registration, the sensor data should have laser scan. Laser scans can be created from the depth images (see option below) or loaded from the source selected. These parameters can be used to reduce the point cloud size directly in the capturing thread. - - - true - - - - - - - - - m - - - 9999.000000000000000 - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 0 - - - 99999999 - - - 0 - - - - - - - Downsample step size for laser scans. If your laser scans are created from depth images, the step is used as decimation of the depth images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - m - - - 3 - - - 0.010000000000000 - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 1 - - - 99999999 - - - - - - - Voxel size for uniform sampling. Note that it is done after downsampling. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 0.010000000000000 - - - - - - - Force ground normals to be all upward (e.g., 0.8). Useful to make sure Velodyne's scans on ground have all normals upward. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 9999.000000000000000 - - - - - - - Minimum range, filter points below that range. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Maximum range, filter points above that range. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Generate laser scan from depth image. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 1.000000000000000 - - - 0.010000000000000 - - - - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - - - - - - - - RTAB-Map Settings - - - - - - Hz + + + + Activate metric RGB-D SLAM. +If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information. - - 3 + + true - - 1.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + @@ -8118,11 +9108,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - SLAM mode. - If set to false, global localization is performed, so without adding new data to the map. + Publish statistics. true @@ -8132,57 +9121,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - true - - - - - + + - Activate metric RGB-D SLAM. -If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - true - - - - - - - Publish statistics. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Detection rate (0 means inf). RTAB-Map will filter input images to satisfy this rate. + Detection rate (0 means inf). RTAB-Map will filter input images to satisfy this rate. true @@ -8951,43 +9893,43 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 + + + + Image pre decimation. Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Vocabulary->Depth As Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. - - 16 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - 1 + + + + - - 16 + + true - - - - 1 - - - 9999 + + + + - - 15 + + false - - + + - Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set). + Image post decimation. Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than Pre Decimation, data already decimated is saved (no need to re-decimate the image). true @@ -8997,29 +9939,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - 50 - 0 - + + + + + + + false + + + + + + + 1 - 1.000000000000000 + 16 - - 0.010000000000000 + + + + + + - - 0.200000000000000 + + false - - + + - Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory. + Raw descriptors kept in memory. true @@ -9029,41 +9982,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + + 50 + 0 + + - 999 + 1.000000000000000 - 1 + 0.010000000000000 - 2 - - - - - - - - - - false + 0.200000000000000 - - + + @@ -9085,8 +10037,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -9095,23 +10047,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + - Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. + Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set). true @@ -9121,8 +10060,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -9131,10 +10070,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Raw descriptors kept in memory. + Multi-threaded compression. true @@ -9144,10 +10083,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Image pre decimation. Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Vocabulary->Depth As Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. + T_recent : Ratio of locations after the last loop closure in WM that cannot be transferred. true @@ -9157,23 +10096,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - True=Generate location Ids, False=use input image ids. + - + true - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - + + - Maximum locations retrieved at the same time from LTM to WM. + Save intermediate node data. true @@ -9183,18 +10119,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. - + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + @@ -9203,20 +10142,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + 1 - - true + + 16 - - + + - Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters). + On transfer, signatures are sorted by weight->ID (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID. true @@ -9226,6 +10165,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 1 + + + 9999 + + + 15 + + + @@ -9239,8 +10191,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 999 + + + 1 + + + 2 + + + + + @@ -9249,10 +10214,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - T_recent : Ratio of locations after the last loop closure in WM that cannot be transferred. + Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory. true @@ -9262,10 +10227,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Image post decimation. Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than Pre Decimation, data already decimated is saved (no need to re-decimate the image). + Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters). true @@ -9275,10 +10240,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - On transfer, signatures are sorted by weight->ID (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID. + True=Generate location Ids, False=use input image ids. true @@ -9288,18 +10253,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Maximum locations retrieved at the same time from LTM to WM. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + @@ -9308,10 +10276,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet. + - Multi-threaded compression. + Rotate images so that upside is up if they are not already. true @@ -9321,16 +10292,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Save intermediate node data. - - - true + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false @@ -10749,7 +11717,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. Loop Closure Detection - + @@ -10910,6 +11878,36 @@ see Sqlite3 doc 'PRAGMA synchronous'. + + + + Global descriptor extractor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Disabled + + + + + PyDescriptor + + + + @@ -11008,6 +12006,100 @@ see Sqlite3 doc 'PRAGMA synchronous'. + + + + PyDescriptor + + + + + + <html><head/><body><p>Python wrapper for python descriptor like <a href="https://github.com/uzh-rpg/netvlad_tf_open"><span style=" text-decoration: underline; color:#0000ff;">NetVLAD</span></a>. Download the interface <a href="https://github.com/introlab/rtabmap/tree/master/corelib/src/pydescriptor"><span style=" text-decoration: underline; color:#0000ff;">script</span></a> and copy it in the python folder of NetVLAD, then set its path below.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + 0 + + + 999999 + + + 1 + + + 4096 + + + + + + + + + + [Required] Path to rtabmap python script. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Descriptor dimension. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + @@ -11081,21 +12173,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Map Update - - + + - Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess). - - - true + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + - + m @@ -11104,10 +12197,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). true @@ -11117,33 +12210,55 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - rad + m - 2 + 1 - 3.140000000000000 + 99.000000000000000 0.100000000000000 + + 1.000000000000000 + - - + + - + Maximum angular speed to update the map (0 means not limit). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. + Ignore off diagonal values of the odometry covariance matrix. true @@ -11153,55 +12268,52 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 2 - - - 1.000000000000000 + + + + Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links. - - 0.100000000000000 + + true - - 0.100000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - m + rad - 1 + 2 - 99.000000000000000 + 3.140000000000000 0.100000000000000 - - 1.000000000000000 - - - + + - + Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11211,10 +12323,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Maximum linear speed to update the map (0 means not limit). + Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. true @@ -11224,43 +12336,63 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - - - m/s + + + + Do local bundle adjustment with neighborhood of the loop closure. - - 3 + + true - - 0.100000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + m + + 1.000000000000000 + + + + + + + rad/s + - 3 + 2 + + + 3.140000000000000 0.100000000000000 - - + + - Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. + + + + + + + + Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. true @@ -11270,27 +12402,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 9999 - - - - m - - - 1.000000000000000 - - - - - + + - Ignore off diagonal values of the odometry covariance matrix. + Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority). true @@ -11300,23 +12422,30 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links. + + + + m/s - - true + + 3 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.100000000000000 - - + + - Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. + + + + + + + + Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. true @@ -11327,9 +12456,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. + Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess). true @@ -11339,33 +12468,62 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + + + - rad/s + 2 - 3.140000000000000 + 1.000000000000000 0.100000000000000 + + 0.100000000000000 + - - + + - - + + + + m + + + 4 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + - Maximum angular speed to update the map (0 means not limit). + Maximum linear speed to update the map (0 means not limit). true @@ -11375,13 +12533,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - - + + - Do local bundle adjustment with neighborhood of the loop closure. + Ratio of working memory for which local nodes are immunized from transfer. true @@ -11391,7 +12549,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + If true, rtabmap will assume the robot is restarting 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. @@ -11404,24 +12562,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - - + + - Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). + Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11431,10 +12582,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + m + + + 3 + + + 0.100000000000000 + + + + + - Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority). + Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. true @@ -11444,17 +12608,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching. + Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. true @@ -11464,10 +12621,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Ratio of working memory for which local nodes are immunized from transfer. + Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). true @@ -11477,17 +12634,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - + + - Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). + + + + + + + + Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching. true @@ -11497,10 +12661,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. + Force odometry poses to be 3DoF if Motion Estimation is 3DoF. true @@ -11510,8 +12674,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -12964,7 +14128,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + Altitude delta. Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + m @@ -12983,20 +14160,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Occupancy threshold. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Probability of a miss. + Footprint radius used to clear all obstacles under the graph. true @@ -13006,32 +14186,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 9999 + + + - + m - 3 + 0 - 0.001000000000000 + 0.000000000000000 - 99.000000000000000 + 9999.000000000000000 - 0.100000000000000 + 10.000000000000000 - 0.010000000000000 + 0.000000000000000 - - + + - Minimum map size. + Probability of a miss. true @@ -13041,10 +14228,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Altitude delta. Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building). + Probability clamping threshold minimum. true @@ -13054,21 +14241,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - - - + + + + 4 + 0.000000000000000 @@ -13079,47 +14259,79 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.100000000000000 - 0.000000000000000 + 0.500000000000000 - - + + + + Erode obstacle cells. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Minimum map size. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - + m - 4 + 3 - 0.000000000000000 + 0.001000000000000 - 1.000000000000000 + 99.000000000000000 0.100000000000000 - 0.500000000000000 + 0.010000000000000 - - - - Full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not. + + + + - - true + + 0.500000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 - - + + @@ -13127,33 +14339,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000000000000000 - 0.500000000000000 + 1.000000000000000 0.100000000000000 - 0.500000000000000 + 0.000000000000000 - - - - Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + 16 - - + + - Probability of a hit. + Maximum nodes assembled in the map starting from the last node (0=unlimited). true @@ -13163,10 +14369,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Maximum nodes assembled in the map starting from the last node (0=unlimited). + Probability clamping threshold maximum. true @@ -13176,10 +14382,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Occupancy threshold. + Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap. true @@ -13189,41 +14395,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - m + - 0 + 4 0.000000000000000 - 9999.000000000000000 + 1.000000000000000 - 10.000000000000000 + 0.100000000000000 - 0.000000000000000 + 0.500000000000000 - - + + - - 4 - 0.000000000000000 - 1.000000000000000 + 0.500000000000000 0.100000000000000 @@ -13233,7 +14436,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -13255,10 +14458,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Erode obstacle cells. + + + + false + + + + + + + Probability of a hit. true @@ -13268,75 +14481,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 0.500000000000000 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.500000000000000 - - - - - - - Probability clamping threshold minimum. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Footprint radius used to clear all obstacles under the graph. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Probability clamping threshold maximum. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999 - - - - - + + - Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap. + Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value. true @@ -13346,13 +14494,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 16 - - - @@ -14044,7 +15185,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Odometry - + @@ -14060,10 +15201,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Guess based on previous motion. + 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)). true @@ -14073,10 +15214,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame. + Data buffer size (0 means inf). true @@ -14086,23 +15227,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - [Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + QComboBox::AdjustToContents + + + No filtering + + + + + Kalman filtering + + + + + Particle filtering + + - - + + - Odometry strategy. + Guess based on previous motion. true @@ -14184,10 +15334,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Align odometry with ground on initialization. + Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. true @@ -14197,51 +15347,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - - - 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)). + + + + 1.000000000000000 - - true + + 0.100000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.000000000000000 - - + + - - + + - 999999 + 9999 - - - - - - Test odometry + + 100 - - + + - Fill info with data (inliers/outliers features to be shown in Odometry view). + [Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame. true @@ -14251,10 +15390,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Data buffer size (0 means inf). + Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -> Visual Feature -> Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. true @@ -14264,20 +15403,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 - - - 32 - - - - - + + - Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. + Registration approach. Default means the same registration approach used in Motion Estimation panel (same used by the loop closure detector). true @@ -14287,65 +15416,53 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -> Visual Feature -> Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + 3 + QComboBox::AdjustToContents - No filtering + Visual - Kalman filtering + Geometry (ICP) - Particle filtering + Visual + Geometry + + + + + [Default] - - + + - - - - - - - - 1.000000000000000 - - - 0.100000000000000 + Fill info with data (inliers/outliers features to be shown in Odometry view). - - 0.000000000000000 + + true - - - - - - + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -14362,30 +15479,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999999 + + + + - - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.500000000000000 + + + + Test odometry - - + + - Registration approach. Default means the same registration approach used in Motion Estimation panel (same used by the loop closure detector). + Align odometry with ground on initialization. true @@ -14395,40 +15506,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 3 - - - QComboBox::AdjustToContents + + + + 999999 - - - Visual - - - - - Geometry (ICP) - - - - - Visual + Geometry - - - - - [Default] - - - - + + - [Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame. + [Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame. true @@ -14439,12 +15527,44 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + 1 + - 9999 + 32 + + + + + + + s + + + 3 + + + 10.000000000000000 + + + 0.100000000000000 - 100 + 1.000000000000000 + + + + + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 @@ -14461,22 +15581,63 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - s + + + + Odometry strategy. - - 3 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + - 10.000000000000000 + 999999 - - 0.100000000000000 + + + + + + [Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame. - - 1.000000000000000 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + @@ -14485,7 +15646,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 12 + 5 @@ -14801,22 +15962,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Qt::Vertical - - - - 20 - 0 - - - - + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -14885,7 +16046,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag [Visual] Correspondences computation: Optical Flow - + @@ -14902,10 +16063,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Window size. + Iterations. true @@ -14915,26 +16076,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 0 - - - 999999 - - - 1 - - - 3 - - - - - + + - Iterations. + Max level. true @@ -14944,10 +16089,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Epsilon. + Window size. true @@ -14957,26 +16102,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 1 - - - 999999 - - - 1 - - - 30 - - - - - + + - Max level. + Epsilon. true @@ -15005,18 +16134,37 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Qt::Vertical - - - - 20 - 0 - + + + + 0 - + + 999999 + + + 1 + + + 3 + + + + + + + 1 + + + 999999 + + + 1 + + + 30 + + @@ -15029,7 +16177,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 20 - 40 + 0 @@ -16388,32 +17536,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + false + - m + - 4 + 2 0.000000000000000 - 9.999900000000000 + 999.000000000000000 - 0.010000000000000 + 1.000000000000000 - 0.076000000000000 + 0.000000000000000 - - + + - Close/Far threshold. Baseline times. + Fake IR projector baseline used only when stereo is not used. true @@ -16423,32 +17574,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 1 - - - 0.100000000000000 - - - 999.000000000000000 + + + + Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2. - - 1.000000000000000 + + true - - 40.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Fake IR projector baseline used only when stereo is not used. + Maximum ORB features extracted per frame. true @@ -16458,13 +17600,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + ... + + - - + + - Path to ORB vocabulary (*.txt). + Close/Far threshold. Baseline times. true @@ -16474,13 +17620,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - ... + + + + 99999 + + + 1000 + + + @@ -16494,64 +17646,60 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - false + + + + Path to ORB vocabulary (*.txt). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - + m - 2 + 4 0.000000000000000 - 999.000000000000000 + 9.999900000000000 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.076000000000000 - - - - Maximum ORB features extracted per frame. + + + + - - true + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.100000000000000 - - - - - 99999 - - - 1000 - - - - - - - Maximum size of the feature map (0 means infinite). + 999.000000000000000 - - true + + 1.000000000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 40.000000000000000 @@ -16567,11 +17715,172 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Enable IMU. Only supported with ORB_SLAM3. + + + true + + + + + + + + 8 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Accelerometer "random walk". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Accelerometer "white noise". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Gyroscope "white noise". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 8 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + + + Gyroscope "random walk". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + IMU sampling rate (0 to estimate from input data). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 8 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000001000000000 + + + 0.000001000000000 + + + + + + + 8 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000100000000000 + + + 0.000100000000000 + + + + + + + 0 + + + 0.000000000000000 + + + 10000.000000000000000 + + + 0.000000000000000 + + + + + + + + - + Qt::Vertical @@ -16813,114 +18122,1632 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - VLP-16 + VLP-16 + + + + + HDL-32 + + + + + HDL-64E + + + + + + + + Map resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 2 + + + 0.010000000000000 + + + 9.000000000000000 + + + 0.000100000000000 + + + 0.200000000000000 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + MSCKF_VIO + + + + + + <html><head/><body><p>MSCKF_VIO: <a href="https://github.com/KumarRobotics/msckf_vio"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/KumarRobotics/msckf_vio</span></a></p><p>Stereo and IMU inputs required.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image processing + + + + + + 1 + + + + + + + Grid row. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + + + + + Grid col. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Grid min feature num. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Grid max feature num. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Pyramid levels. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + + + + + Patch size. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Fast threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Max iteration. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Tracking precision. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Ransac treshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Stereo threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Odometry estimation + + + + + + + + + Max cam state size. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 2 + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Position std threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Rotation threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Translation threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Tracking rate threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 3 + + + 99.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Optimization translation threshold. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Noise gyro. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Noise acceleration. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Noise gyro bias. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Noise acceleration bias. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Noise feature. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Initial covariance velocity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 5 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Initial covariance gyro bias. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 3 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Initial covariance acceleration bias. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 8 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Initial covariance extrincic rotation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 6 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.500000000000000 + + + + + + + Initial covariance extrinsic translation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + VINS-Fusion + + + + + + <html><head/><body><p>VINS-Fusion: <a href="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/HKUST-Aerial-Robotics/VINS-Fusion</span></a></p><p>IMU input optional depending on the parameters in the config file. Currently tested only with EuRoC data set.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + Path to VINS-Fusion config file (*.yaml). While VINS-Fusion requires calibrations in the config file, they are ignored as those from received images are used instead. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + OpenVINS + + + + + + <html><head/><body><p>OpenVINS: <a href="https://github.com/rpng/open_vins"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/rpng/open_vins</span></a></p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Tracker + + + + + + + + + + + + + If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should use KLT tracking, or descriptor matcher + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 200 + + + + + + + Number of points (per camera) we will extract and try to track + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 15 + + + + + + + Will check after doing KLT track and remove any features closer than this + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform 1d triangulation instead of 3d + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If we should perform Levenberg-Marquardt refinement + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 5 + + + + + + + Max runs for Levenberg-Marquardt + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1000.000000000000000 + + + 10.000000000000000 + + + 40.000000000000000 + + + + + + + Max baseline ratio to accept triangulated features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 99999.000000000000000 + + + 1000.000000000000000 + + + 10000.000000000000000 + + + + + + + Max condition number of linear triangulation matrix accept triangulated features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + State + + + + + + + + + + + + + If first-estimate Jacobians should be used (enable for good consistency) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + QComboBox::AdjustToContents + + + + DISCRETE + + + + + RK4 + + + + + ANALYTICAL + + + + + + + + Numerical integration methods + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If the transform between camera and IMU should be optimized (R_ItoC, p_CinI) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If camera intrinsics should be optimized (focal, center, distortion) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If timeoffset between camera and IMU should be optimized + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If imu intrinsics should be calibrated (rotation and skew-scale matrix) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If gyroscope gravity sensitivity (Tg) should be calibrated + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 11 + + + + + + + Max clone size of sliding window + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 50 + + + + + + + Max number of estimated SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 10000 + + + 25 + + + + + + + Max number of SLAM features we allow to be included in a single EKF update. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + 50 + + + + + - HDL-32 + Max number of MSCKF features we will use at a given image timestep. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + GLOBAL_3D + + + + + GLOBAL_FULL_INVERSE_DEPTH + + + + + ANCHORED_3D + + + + + ANCHORED_FULL_INVERSE_DEPTH + + + + + ANCHORED_MSCKF_INVERSE_DEPTH + + + + + ANCHORED_INVERSE_DEPTH_SINGLE + + + + + + - HDL-64E + What representation our features are in (msckf features) - - - - - - - Map resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 2 - - - 0.010000000000000 - - - 9.000000000000000 - - - 0.000100000000000 - - - 0.200000000000000 - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - MSCKF_VIO - - - - - - <html><head/><body><p>MSCKF_VIO: <a href="https://github.com/KumarRobotics/msckf_vio"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/KumarRobotics/msckf_vio</span></a></p><p>Stereo and IMU inputs required. Currently tested only with EuRoC data set.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 4 + + + QComboBox::AdjustToContents + + + + GLOBAL_3D + + + + + GLOBAL_FULL_INVERSE_DEPTH + + + + + ANCHORED_3D + + + + + ANCHORED_FULL_INVERSE_DEPTH + + + + + ANCHORED_MSCKF_INVERSE_DEPTH + + + + + ANCHORED_INVERSE_DEPTH_SINGLE + + + + + + + + What representation our features are in (slam features) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 0.000000000000000 + + + 0.500000000000000 + + + 0.000000000000000 + + + + + + + Delay before initializing (helps with stability from bad initialization...) + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 5 + + + 0.010000000000000 + + + 9.810000000000000 + + + + + + + Magnitude of gravity in this location + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + + + Mask for left image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + + + + + Mask for right image + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - + - Image processing + Initializer - + - + + + s + - 1 + 0.000000000000000 + + + 0.500000000000000 + + + 2.000000000000000 - + - Grid row. + Amount of time we will initialize over true @@ -16931,16 +19758,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - 1 + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 - + - Grid col. + Variance threshold on our acceleration to be classified as moving true @@ -16951,12 +19787,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + 0.000000000000000 + + + 0.100000000000000 + + + 10.000000000000000 + + - + - Grid min feature num. + Max disparity to consider the platform stationary (dependent on resolution) true @@ -16967,12 +19813,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + 0 + + + 10000 + + + 50 + + - + - Grid max feature num. + How many features to track during initialization (saves on computation) true @@ -16983,12 +19839,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + + - + - Pyramid levels. + If we should perform dynamic initialization true @@ -16999,16 +19859,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 1 + + + - + - Patch size. + If we should optimize and recover the calibration in our MLE true @@ -17019,12 +19879,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + 0 + + + 50 + + - + - Fast threshold. + Max number of MLE iterations for dynamic initialization true @@ -17035,12 +19902,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + s + + + 0.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + - + - Max iteration. + Max time for MLE optimization true @@ -17051,25 +19931,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - 0.000000000000000 - - - 1.000000000000000 - - - 0.100000000000000 + 1 - 0.500000000000000 + 6 - + - Tracking precision. + Max number of MLE threads for dynamic initialization true @@ -17080,28 +19954,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 1 - + - 0.000000000000000 - - - 99.000000000000000 - - - 0.100000000000000 + 4 - 0.500000000000000 + 6 - + - Ransac treshold. + Number of poses to use during initialization (max should be cam freq * window) true @@ -17112,28 +19977,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 1 + + + deg 0.000000000000000 - 99.000000000000000 - - - 0.100000000000000 + 45.000000000000000 - 0.500000000000000 + 10.000000000000000 - + - Stereo threshold. + Minimum degrees we need to rotate before we try to init (sum of norm) true @@ -17143,22 +20005,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - Odometry estimation - - - - + + + + 1.000000000000000 + + + 999.000000000000000 + + + 10.000000000000000 + + - - + + - Max cam state size. + Magnitude we will inflate initial covariance of orientation true @@ -17168,29 +20031,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 2 - + + - 0.000000000000000 + 1.000000000000000 - 99.000000000000000 - - - 0.100000000000000 + 999.000000000000000 - 0.500000000000000 + 100.000000000000000 - - + + - Position std threshold. + Magnitude we will inflate initial covariance of velocity true @@ -17200,26 +20057,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 + + + + 1.000000000000000 - 99.000000000000000 - - - 0.100000000000000 + 999.000000000000000 - 0.500000000000000 + 10.000000000000000 - - + + - Rotation threshold. + Magnitude we will inflate initial covariance of gyroscope bias true @@ -17229,26 +20083,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - 0.000000000000000 + 1.000000000000000 - 99.000000000000000 - - - 0.100000000000000 + 999.000000000000000 - 0.500000000000000 + 100.000000000000000 - - + + - Translation threshold. + Magnitude we will inflate initial covariance of accelerometer bias true @@ -17258,23 +20109,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 0.000000000000000 + - 99.000000000000000 + 1.000000000000000 - 0.100000000000000 + 0.000000000001000 - 0.500000000000000 + 0.000000000000000 - - + + - Tracking rate threshold. + Minimum reciprocal condition number acceptable for our covariance recovery true @@ -17284,26 +20138,55 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 3 + + + + + + + ZUPT + + + + + + + + + + + + + If we should try to use zero velocity update + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 - 99.000000000000000 + 10.000000000000000 0.100000000000000 - 0.500000000000000 + 0.000000000000000 - - + + - Optimization translation threshold. + Chi2 multiplier for zero velocity true @@ -17313,10 +20196,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 + + + + 0.000000000000000 1.000000000000000 @@ -17325,14 +20208,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.100000000000000 - 0.500000000000000 + 0.100000000000000 - - + + - Noise gyro. + Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt) true @@ -17342,26 +20225,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 + + + + 0.000000000000000 - 1.000000000000000 + 100.000000000000000 - 0.100000000000000 + 1.000000000000000 - 0.500000000000000 + 10.000000000000000 - - + + - Noise acceleration. + Multiplier of our zupt measurement IMU noise matrix (default should be 1.0) true @@ -17371,13 +20254,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 + + + + 0.000000000000000 - 1.000000000000000 + 10.000000000000000 0.100000000000000 @@ -17387,10 +20270,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Noise gyro bias. + Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt) true @@ -17400,26 +20283,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.500000000000000 + + + + - - + + - Noise acceleration bias. + If we should only use the zupt at the very beginning static initialization phase true @@ -17429,8 +20303,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + Noise / Chi2 + + + + + + m/s^2/sqrt(Hz) + 6 @@ -17438,17 +20324,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 1.000000000000000 - 0.100000000000000 + 0.000100000000000 - 0.500000000000000 + 0.010000000000000 - - + + - Noise feature. + Accel "white noise" true @@ -17458,26 +20344,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + m/s^3/sqrt(Hz) + - 4 + 6 1.000000000000000 - 0.100000000000000 + 0.000100000000000 - 0.500000000000000 + 0.001000000000000 - - + + - Initial covariance velocity. + Accel bias diffusion true @@ -17487,26 +20376,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + rad/s/sqrt(Hz) + - 5 + 6 1.000000000000000 - 0.100000000000000 + 0.000100000000000 - 0.500000000000000 + 0.001000000000000 - - + + - Initial covariance gyro bias. + Gyro "white noise" true @@ -17516,26 +20408,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + rad/s^2/sqrt(Hz) + - 3 + 6 1.000000000000000 - 0.100000000000000 + 0.000100000000000 - 0.500000000000000 + 0.000100000000000 - - + + - Initial covariance acceleration bias. + Gyro bias diffusion true @@ -17545,26 +20440,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 8 + + + + 0.000000000000000 - 1.000000000000000 + 10.000000000000000 0.100000000000000 - 0.500000000000000 + 1.000000000000000 - - + + - Initial covariance extrincic rotation. + Pixel noise for MSCKF features true @@ -17574,147 +20469,94 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 6 + + + + 0.000000000000000 - 1.000000000000000 + 10.000000000000000 0.100000000000000 - 0.500000000000000 + 1.000000000000000 - - + + - Initial covariance extrinsic translation. + Chi2 multiplier for MSCKF features true Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - VINS-Fusion - - - - - - <html><head/><body><p>VINS-Fusion: <a href="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/HKUST-Aerial-Robotics/VINS-Fusion</span></a></p><p>IMU input optional depending on the parameters in the config file. Currently tested only with EuRoC data set.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - Path to VINS-Fusion config file (*.yaml). While VINS-Fusion requires calibrations in the config file, they are ignored as those from received images are used instead. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - OpenVINS - - - - - - <html><head/><body><p>OpenVINS: <a href="https://github.com/rpng/open_vins"><span style=" text-decoration: underline; color:#0000ff;">https://github.com/rpng/open_vins</span></a></p><p>Currently tested only with EuRoC data set.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Pixel noise for SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Chi2 multiplier for SLAM features + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -17728,7 +20570,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 20 - 1049 + 0 @@ -17878,19 +20720,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Qt::Vertical - - - - 20 - 0 - - - - @@ -18030,6 +20859,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -18040,7 +20882,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 0 + 2 @@ -18356,16 +21198,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - true - - - @@ -19259,10 +22091,10 @@ Lower the ratio -> higher the precision. Motion Estimation: 3D to 2D (PnP) - - + + - Refine iterations. + Reprojection error. true @@ -19291,17 +22123,23 @@ Lower the ratio -> higher the precision. - - - - Reprojection error. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + AUTO + + + + + ANY + + + + + HOMOGENEOUS + + @@ -19323,26 +22161,58 @@ Lower the ratio -> higher the precision. - - + + + + Flags. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + - 0 + 0.000000000000000 - 999999 + 99.000000000000000 - 1 + 0.100000000000000 - 1 + 0.000000000000000 - - + + - Flags. + Multi-camera random sampling policy. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy. true @@ -19352,7 +22222,7 @@ Lower the ratio -> higher the precision. - + Max linear variance between 3D point correspondences after PnP. 0 means disabled. @@ -19365,25 +22235,78 @@ Lower the ratio -> higher the precision. + + + + Reprojection error. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - - - + + + 2 - + + 999999 + + 4 + + + + + + Refine iterations. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - 0.000000000000000 + 0 - 99.000000000000000 + 999999 - 0.100000000000000 + 1 - 0.000000000000000 + 1 + + + + + + + Compute variance for each linear component instead of using the combined XYZ variance for all linear components. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + @@ -19669,7 +22592,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 60.000000000000000 @@ -19692,7 +22615,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 5.000000000000000 + 0.100000000000000 @@ -19953,14 +22876,15 @@ Lower the ratio -> higher the precision. - - - - m - - - 6 + + + + + + + + 0.000000000000000 @@ -19971,14 +22895,14 @@ Lower the ratio -> higher the precision. 0.010000000000000 - 0.000000000000000 + 0.700000000000000 - - + + - Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". + Maximum range filtering. Set to 0 to disable. true @@ -19988,23 +22912,23 @@ Lower the ratio -> higher the precision. - - - - Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. + + + + 1 - - true + + 1000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 10 - - + + - Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. + Uniform sampling voxel size. Set to 0 to disable. true @@ -20014,7 +22938,7 @@ Lower the ratio -> higher the precision. - + 1 @@ -20027,10 +22951,10 @@ Lower the ratio -> higher the precision. - - + + - Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. + Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are "pcd", "ply" or "vtk". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten. true @@ -20040,10 +22964,36 @@ Lower the ratio -> higher the precision. - - + + + + m + + + 3 + + + 0.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 + + + + + + + 0.010000000000000 + + + + + - Uniform sampling voxel size. Set to 0 to disable. + Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. true @@ -20053,30 +23003,26 @@ Lower the ratio -> higher the precision. - - - - 1 + + + + + + + 2 - 1000 - - - 10 + 1.000000000000000 - - - - 0.010000000000000 - - + + - Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. + Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. true @@ -20086,10 +23032,10 @@ Lower the ratio -> higher the precision. - - + + - Strategy. + Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. true @@ -20099,10 +23045,10 @@ Lower the ratio -> higher the precision. - - + + - Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. + Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. true @@ -20112,29 +23058,33 @@ Lower the ratio -> higher the precision. - - - - 3 + + + + + + + + - 0.001000000000000 + 1 - 9999.989999999999782 + 999999 - 0.010000000000000 + 1 - 0.100000000000000 + 1 - - + + - Minimum range filtering. Set to 0 to disable. + Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. true @@ -20144,17 +23094,29 @@ Lower the ratio -> higher the precision. - - - - + + + + m + + + 2 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 - - + + - Maximum range filtering. Set to 0 to disable. + Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. true @@ -20164,10 +23126,10 @@ Lower the ratio -> higher the precision. - - - - 0.000000000000000 + + + + 3 1.000000000000000 @@ -20175,28 +23137,25 @@ Lower the ratio -> higher the precision. 0.010000000000000 - - 0.700000000000000 - - - - - 2 + + + + Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. - - 1.000000000000000 + + true - - 0.010000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. + Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -20206,48 +23165,103 @@ Lower the ratio -> higher the precision. - - + + - m + 2 - 0.000000000000000 + 0.010000000000000 - + 1.000000000000000 + + 0.100000000000000 + - 0.000000000000000 + 0.850000000000000 - - - - m - + + - 2 + 3 + + + 0.001000000000000 - 10.000000000000000 + 9999.989999999999782 0.010000000000000 - 0.200000000000000 + 0.100000000000000 + + + + + + + Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. + + + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + Reject + + + + + Constrained PointToPoint + + + + + PointToPoint + + - - + + - Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. + Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. true @@ -20257,32 +23271,10 @@ Lower the ratio -> higher the precision. - - - - - - - 2 - - - 0.010000000000000 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.850000000000000 - - - - - + + - Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + Max iterations. true @@ -20293,9 +23285,9 @@ Lower the ratio -> higher the precision. - + - Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + Point to plane ICP. true @@ -20305,29 +23297,32 @@ Lower the ratio -> higher the precision. - - + + m - 3 + 6 0.000000000000000 + + 1.000000000000000 + 0.010000000000000 - 0.005000000000000 + 0.000000000000000 - - + + - Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. + Minimum range filtering. Set to 0 to disable. true @@ -20337,32 +23332,6 @@ Lower the ratio -> higher the precision. - - - - - Reject - - - - - Constrained PointToPoint - - - - - PointToPoint - - - - - - - - - - - @@ -20382,23 +23351,29 @@ Lower the ratio -> higher the precision. - - - - Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + + + + rad - - true + + 2 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 3.140000000000000 + + + 0.010000000000000 + + + 0.780000000000000 - - + + - Point to plane ICP. + Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -20408,10 +23383,10 @@ Lower the ratio -> higher the precision. - - + + - Max iterations. + Strategy. true @@ -20421,17 +23396,23 @@ Lower the ratio -> higher the precision. - - + + + + m + - 3 + 2 - 1.000000000000000 + 10.000000000000000 0.010000000000000 + + 0.200000000000000 + @@ -20447,41 +23428,6 @@ Lower the ratio -> higher the precision. - - - - 1 - - - 999999 - - - 1 - - - 1 - - - - - - - rad - - - 2 - - - 3.140000000000000 - - - 0.010000000000000 - - - 0.780000000000000 - - - @@ -20504,10 +23450,10 @@ Lower the ratio -> higher the precision. - - + + - Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are "pcd", "ply" or "vtk". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten. + Reciprocal correspondences. To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence. true @@ -20517,8 +23463,31 @@ Lower the ratio -> higher the precision. - - + + + + Flag to enable filters above: 1="from" cloud only, 2="to" cloud only, 3=both. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 3 + + + 3 + + @@ -20886,7 +23855,7 @@ Lower the ratio -> higher the precision. 0.000000000000000 - 1.000000000000000 + 1000.000000000000000 0.001000000000000 @@ -21136,6 +24105,16 @@ Lower the ratio -> higher the precision. + + + + iSAM2: Do graph optimization incrementally to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported in this mode. + + + true + + + @@ -21146,6 +24125,65 @@ Lower the ratio -> higher the precision. + + + + 4 + + + 0.000100000000000 + + + 999.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + + + Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info. + + + true + + + + + + + + + + + + + + 1 + + + 10000 + + + 10 + + + + + + + Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info. + + + true + + + @@ -23844,7 +26882,7 @@ Lower the ratio -> higher the precision. - + diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 26f275d35c..3377dbf3e3 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -1055,6 +1055,7 @@ UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, qreal thesholdVal UPlotCurve(name, parent), _orientation(orientation) { + _threshold = thesholdValue; if(_orientation == Qt::Horizontal) { this->addValue(0, thesholdValue); @@ -1080,6 +1081,7 @@ void UPlotCurveThreshold::setThreshold(qreal threshold) if(_items.size() == 3) { UPlotItem * item = 0; + _threshold = threshold; if(_orientation == Qt::Horizontal) { item = (UPlotItem*)_items.at(0); @@ -2193,10 +2195,10 @@ bool UPlot::addCurve(UPlotCurve * curve, bool ownershipTransferred) return false; } -QStringList UPlot::curveNames() +QStringList UPlot::curveNames() const { QStringList names; - for(QList::iterator iter = _curves.begin(); iter!=_curves.end(); ++iter) + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) { if(*iter) { @@ -2206,9 +2208,38 @@ QStringList UPlot::curveNames() return names; } -bool UPlot::contains(const QString & curveName) +bool UPlot::isThreshold(const QString & curveName) const { - for(QList::iterator iter = _curves.begin(); iter!=_curves.end(); ++iter) + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) + { + if(*iter && (*iter)->name().compare(curveName) == 0) + { + return qobject_cast(*iter) != 0; + } + } + return false; +} + +double UPlot::getThresholdValue(const QString & curveName) const +{ + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) + { + if(*iter && (*iter)->name().compare(curveName) == 0) + { + UPlotCurveThreshold* curve = qobject_cast(*iter); + if(curve) + { + return curve->getThreshold(); + } + } + } + UERROR("Curve \"%s\" not found as theshold!", curveName.toStdString().c_str()); + return 0; +} + +bool UPlot::contains(const QString & curveName) const +{ + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) { if(*iter && (*iter)->name().compare(curveName) == 0) { diff --git a/package.xml b/package.xml index 482eeb6032..a1e6e949a5 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.21.0 + 0.21.5 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe @@ -14,15 +14,15 @@ proj cv_bridge - libfreenect-dev libg2o - libopenni-dev + gtsam libpcl-all-dev libpointmatcher libsqlite3-dev octomap + grid_map_core qt_gui_cpp zlib diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index bf8a4641b3..f95eee8a35 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -22,6 +22,7 @@ ENDIF(OPENCV_NONFREE_FOUND) IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( CameraRGBD ) + ADD_SUBDIRECTORY( LidarViewer ) ADD_SUBDIRECTORY( DatabaseViewer ) ADD_SUBDIRECTORY( EpipolarGeometry ) ADD_SUBDIRECTORY( OdometryViewer ) diff --git a/tools/Calibration/main.cpp b/tools/Calibration/main.cpp index 5fb7f14659..97557eb774 100644 --- a/tools/Calibration/main.cpp +++ b/tools/Calibration/main.cpp @@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/gui/CalibrationDialog.h" @@ -271,7 +271,7 @@ int main(int argc, char * argv[]) UFATAL("Calibration for driver %d not available.", driver); } - rtabmap::CameraThread * cameraThread = 0; + rtabmap::SensorCaptureThread * cameraThread = 0; if(camera) { @@ -281,7 +281,7 @@ int main(int argc, char * argv[]) delete camera; exit(1); } - cameraThread = new rtabmap::CameraThread(camera); + cameraThread = new rtabmap::SensorCaptureThread(camera); } dialog.registerToEventsManager(); diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 9116bf550f..6ff6d5d025 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -346,7 +346,7 @@ int main(int argc, char * argv[]) exit(1); } - rtabmap::SensorData data = camera->takeImage(); + rtabmap::SensorData data = camera->takeData(); if (data.imageRaw().empty()) { printf("Cloud not get frame from the camera!\n"); @@ -522,7 +522,7 @@ int main(int argc, char * argv[]) printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str()); } ++id; - data = camera->takeImage(); + data = camera->takeData(); } printf("Closing...\n"); if(viewer) diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index d467630724..bf76d06bd0 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -28,12 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include #include -#include +#include +#include #include #include #include @@ -55,7 +55,7 @@ void showUsage() exit(1); } -rtabmap::CameraThread * cam = 0; +rtabmap::SensorCaptureThread * cam = 0; QApplication * app = 0; // catch ctrl-c void sighandler(int sig) @@ -144,10 +144,11 @@ int main (int argc, char * argv[]) return -1; } ParametersMap parameters = dialog.getAllParameters(); - cam = new CameraThread(camera, parameters); + cam = new SensorCaptureThread(camera, parameters); cam->setMirroringEnabled(dialog.isSourceMirroring()); cam->setColorOnly(dialog.isSourceRGBDColorOnly()); cam->setImageDecimation(dialog.getSourceImageDecimation()); + cam->setHistogramMethod(dialog.getSourceHistogramMethod()); cam->setStereoToDepth(dialog.isSourceStereoDepthGenerated()); cam->setStereoExposureCompensation(dialog.isSourceStereoExposureCompensation()); cam->setScanParameters( diff --git a/tools/DatabaseViewer/main.cpp b/tools/DatabaseViewer/main.cpp index 8bc2116aba..0b96e41382 100644 --- a/tools/DatabaseViewer/main.cpp +++ b/tools/DatabaseViewer/main.cpp @@ -32,6 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) +#include +#endif + int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); @@ -41,8 +45,9 @@ int main(int argc, char * argv[]) CoInitialize(nullptr); #endif -#if VTK_MAJOR_VERSION >= 8 - vtkObject::GlobalWarningDisplayOff(); +#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1) + // needed to ensure appropriate OpenGL context is created for VTK rendering. + QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); #endif QApplication * app = new QApplication(argc, argv); @@ -50,9 +55,9 @@ int main(int argc, char * argv[]) mainWindow->showNormal(); - if(argc == 2) + if(argc >= 2) { - mainWindow->openDatabase(argv[1]); + mainWindow->openDatabase(argv[argc-1], rtabmap::Parameters::parseArguments(argc, argv, true)); } // Now wait for application to finish diff --git a/tools/DetectMoreLoopClosures/main.cpp b/tools/DetectMoreLoopClosures/main.cpp index 9e8ce5adb2..dba760bb1d 100644 --- a/tools/DetectMoreLoopClosures/main.cpp +++ b/tools/DetectMoreLoopClosures/main.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include #include #include @@ -244,14 +244,14 @@ int main(int argc, char * argv[]) else if(detected > 0 && haveOptimizedMap) { printf("The database has a global occupancy grid, regenerating one with new optimized graph!\n"); - OccupancyGrid grid(parameters); + LocalGridCache cache; + OccupancyGrid grid(&cache, parameters); std::map optimizedPoses = rtabmap.getLocalOptimizedPoses(); for(std::map::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter) { - cv::Mat occupancyGrid; SensorData data = rtabmap.getMemory()->getNodeData(iter->first, false, false, false, true); data.uncompressData(); - grid.addToCache(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw()); + cache.add(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint()); } grid.update(optimizedPoses); cv::Mat map = grid.getMap(xMin, yMin); diff --git a/tools/EurocDataset/CMakeLists.txt b/tools/EurocDataset/CMakeLists.txt index 4d0f04f1e2..e766ee1321 100644 --- a/tools/EurocDataset/CMakeLists.txt +++ b/tools/EurocDataset/CMakeLists.txt @@ -1,27 +1,34 @@ FIND_PACKAGE(yaml-cpp QUIET) -IF(NOT yaml-cpp_FOUND) +IF(yaml-cpp_FOUND) + IF (TARGET yaml-cpp::yaml-cpp) + # yaml-cpp 0.8.0 uses target yaml-cpp::yaml-cpp. + SET(YAML_CPP_LIBRARIES yaml-cpp::yaml-cpp) + ELSEIF (TARGET yaml-cpp) + # yaml-cpp 0.7.0 uses target yaml-cpp (VCPKG). + SET(YAML_CPP_LIBRARIES yaml-cpp) + ENDIF() +ELSE() find_package(PkgConfig QUIET) IF(PKG_CONFIG_FOUND) pkg_check_modules(yaml_cpp QUIET yaml-cpp) IF(yaml_cpp_FOUND) SET(YAML_CPP_LIBRARIES ${yaml_cpp_LIBRARIES}) - SET(YAML_CPP_INCLUDE_DIRS ${yaml_cpp_INCLUDEDIR}) + SET(YAML_CPP_INCLUDE_DIR ${yaml_cpp_INCLUDEDIR}) SET(yaml-cpp_FOUND ${yaml_cpp_FOUND}) ENDIF(yaml_cpp_FOUND) ENDIF(PKG_CONFIG_FOUND) -ENDIF(NOT yaml-cpp_FOUND) +ENDIF(yaml-cpp_FOUND) IF(yaml-cpp_FOUND) SET(INCLUDE_DIRS - ${YAML_CPP_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIR} ) SET(LIBRARIES ${YAML_CPP_LIBRARIES} - yaml-cpp ) INCLUDE_DIRECTORIES(${INCLUDE_DIRS} yaml-cpp) diff --git a/tools/EurocDataset/main.cpp b/tools/EurocDataset/main.cpp index 75ab880acb..34c4f1e530 100644 --- a/tools/EurocDataset/main.cpp +++ b/tools/EurocDataset/main.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -43,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UProcessInfo.h" #include "rtabmap/core/IMUFilter.h" #include +#include #include #include #include @@ -280,7 +280,7 @@ int main(int argc, char * argv[]) // We use CameraThread only to use postUpdate() method Transform baseToImu(0,0,1,0, 0,-1,0,0, 1,0,0,0); - CameraThread cameraThread(new + SensorCaptureThread cameraThread(new CameraStereoImages( pathLeftImages, pathRightImages, @@ -387,9 +387,9 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; + SensorCaptureInfo cameraInfo; UDEBUG(""); - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorData data = cameraThread.camera()->takeData(&cameraInfo); UDEBUG(""); int iteration = 0; double start = data.stamp(); @@ -486,6 +486,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); @@ -558,9 +559,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks()); diff --git a/tools/Info/main.cpp b/tools/Info/main.cpp index 3d39cac593..b2dcf5cdb3 100644 --- a/tools/Info/main.cpp +++ b/tools/Info/main.cpp @@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include #include @@ -59,6 +59,7 @@ void showUsage() " Options:\n" " --diff Show only modified parameters.\n" " --diff \"other_map.db\" Compare parameters with other database.\n" + " --dump \"config.ini\" Dump parameters in ini file.\n" "\n"); exit(1); } @@ -81,19 +82,33 @@ int main(int argc, char * argv[]) } std::string otherDatabasePath; + std::string dumpFilePath; bool diff = false; for(int i=1; igetLastParameters(); + if(!dumpFilePath.empty()) + { + Parameters::writeINI(dumpFilePath, parameters); + printf("%ld parameters exported to \"%s\".\n", parameters.size(), dumpFilePath.c_str()); + return 0; + } ParametersMap defaultParameters = Parameters::getDefaultParameters(); ParametersMap removedParameters = Parameters::getBackwardCompatibilityMap(); std::string otherDatabasePathName; @@ -326,7 +347,9 @@ int main(int argc, char * argv[]) driver->getAllLinks(links, true, true); bool reducedGraph = false; std::vector linkTypes(Link::kEnd, 0); - for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + std::vector > linkLengths(Link::kEnd); + std::multimap uniqueLinks = graph::filterDuplicateLinks(links); + for(std::multimap::iterator iter=uniqueLinks.begin(); iter!=uniqueLinks.end(); ++iter) { if(iter->second.type() == Link::kNeighborMerged) { @@ -335,6 +358,7 @@ int main(int argc, char * argv[]) if(iter->second.type()>=0 && iter->second.type()second.type()]; + linkLengths[iter->second.type()].push_back(iter->second.transform().getNorm()); } } if(reducedGraph) @@ -387,7 +411,19 @@ int main(int argc, char * argv[]) std::cout << (uFormat("Links:\n")); for(size_t i=0; i0) + { + std = std::sqrt(std); + } + std::cout << (uFormat("%s%d\t(length avg: %.2fm, std: %.2fm, max: %.2fm)\n", + pad(uFormat(" %s:", Link::typeName((Link::Type)i).c_str())).c_str(), + linkTypes[i], + avg, + std, + max)); } std::cout << ("\n"); long total = 0; diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index fe30590e21..6ba82e798c 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UProcessInfo.h" #include +#include #include #include @@ -373,7 +373,7 @@ int main(int argc, char * argv[]) false, // assume that images are already rectified 0.0f); } - CameraThread cameraThread(camera, parameters); + SensorCaptureThread cameraThread(camera, parameters); ((CameraImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false); if(exposureCompensation) { @@ -434,8 +434,8 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorCaptureInfo cameraInfo; + SensorData data = cameraThread.camera()->takeData(&cameraInfo); int iteration = 0; ///////////////////////////// @@ -491,6 +491,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); @@ -568,9 +569,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks()); diff --git a/tools/LidarViewer/CMakeLists.txt b/tools/LidarViewer/CMakeLists.txt new file mode 100644 index 0000000000..6350690091 --- /dev/null +++ b/tools/LidarViewer/CMakeLists.txt @@ -0,0 +1,32 @@ + +IF(NOT WITH_QT) + # visualization module required + FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) +ENDIF(NOT WITH_QT) + +SET(INCLUDE_DIRS + ${PROJECT_BINARY_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/utilite/include + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +SET(LIBRARIES + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +add_definitions(${PCL_DEFINITIONS}) + +INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) + +ADD_EXECUTABLE(lidar_viewer main.cpp) +TARGET_LINK_LIBRARIES(lidar_viewer rtabmap_core rtabmap_utilite ${LIBRARIES}) + +SET_TARGET_PROPERTIES( lidar_viewer + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_viewer) + +INSTALL(TARGETS lidar_viewer + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime + BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime) \ No newline at end of file diff --git a/tools/LidarViewer/main.cpp b/tools/LidarViewer/main.cpp new file mode 100644 index 0000000000..fcd62678a3 --- /dev/null +++ b/tools/LidarViewer/main.cpp @@ -0,0 +1,144 @@ +/* +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. +*/ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include +#include + +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMath.h" +#include "rtabmap/utilite/UFile.h" +#include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UConversion.h" +#include +#include +#include + +#include +#include +#include //fps calculations +#include +#include +#include +#include +#include + +using namespace pcl; +using namespace pcl::console; +using namespace pcl::visualization; + +void showUsage() +{ + printf("\nUsage:\n" + "rtabmap-lidar_viewer IP PORT driver\n" + " driver Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n"); + exit(1); +} + +// catch ctrl-c +bool running = true; +void sighandler(int sig) +{ + printf("\nSignal %d caught...\n", sig); + running = false; +} + +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kInfo); + //ULogger::setPrintTime(false); + //ULogger::setPrintWhere(false); + + int driver = 0; + std::string ip; + int port = 2368; + if(argc < 4) + { + showUsage(); + } + else + { + ip = argv[1]; + port = uStr2Int(argv[2]); + driver = atoi(argv[3]); + if(driver < 0 || driver > 0) + { + UERROR("driver should be 0."); + showUsage(); + } + } + printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port); + + rtabmap::LidarVLP16 * lidar = 0; + if(driver == 0) + { + lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + } + else + { + UFATAL(""); + } + + if(!lidar->init()) + { + printf("Lidar init failed! Please select another driver (see \"--help\").\n"); + delete lidar; + exit(1); + } + + pcl::visualization::CloudViewer * viewer = new pcl::visualization::CloudViewer("cloud"); + + // to catch the ctrl-c + signal(SIGABRT, &sighandler); + signal(SIGTERM, &sighandler); + signal(SIGINT, &sighandler); + + rtabmap::SensorData data = lidar->takeScan(); + while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running) + { + pcl::PointCloud::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform()); + viewer->showCloud(cloud, "cloud"); + printf("Scan size: %ld points\n", cloud->size()); + + int c = cv::waitKey(10); // wait 10 ms or for key stroke + if(c == 27) + break; // if ESC, break and quit + + data = lidar->takeScan(); + } + printf("Closing...\n"); + if(viewer) + { + delete viewer; + } + delete lidar; + return 0; +} diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 1e7a1dedfb..565cc1ce15 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -34,7 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include @@ -42,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include void showUsage() { @@ -388,7 +388,7 @@ int main (int argc, char * argv[]) { if(camera->isCalibrated()) { - rtabmap::CameraThread cameraThread(camera, parameters); + rtabmap::SensorCaptureThread cameraThread(camera, parameters); cameraThread.setScanParameters(icp, decimation<1?1:decimation, 0, maxDepth, voxelSize, normalsK, normalsRadius); diff --git a/tools/Report/main.cpp b/tools/Report/main.cpp index fbc15ac95d..4185eae8ee 100644 --- a/tools/Report/main.cpp +++ b/tools/Report/main.cpp @@ -62,6 +62,12 @@ void showUsage() " and compute error based on the scaled path.\n" " --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n" " and valid ground truth indices to [path]_indices.txt \n" + " --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n" + " override the ground truth set in database if there is one.\n" + " If extension is *.db, the optimized poses of that database will\n" + " be used as ground truth.\n" + " --gt_max_t # Maximum time interval (sec) to interpolate between a pose and\n" + " corresponding ground truth (default 1 sec).\n" " --inc Incremental optimization. \n" " --stats Show available statistics \"Statistic/Id\" to plot or get localization statistics (if path is a file). \n" #ifdef WITH_QT @@ -147,6 +153,8 @@ int main(int argc, char * argv[]) std::string exportPrefix = "Stat"; int showLoc = 0; float locDelay = 60; + std::string gtFile; + double gtMaxInterval = 1; std::vector statsToShow; #ifdef WITH_QT std::map figures; @@ -227,6 +235,44 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(strcmp(argv[i],"--gt") == 0) + { + ++i; + if(i10) + { + printf("\"--gt_max_t\" option should be between 0 and 10 sec, parsed %f sec.\n", gtMaxInterval); + showUsage(); + } + } + else + { + printf("Missing value for \"--gt\" option.\n"); + showUsage(); + } + } else if(strcmp(argv[i],"--loc") == 0) { ++i; @@ -324,6 +370,57 @@ int main(int argc, char * argv[]) statsToShow.clear(); } + // External Ground Truth + std::map externalGtPoses; + if(!gtFile.empty()) + { + if(UFile::getExtension(gtFile) == "db") + { + DBDriver * driver = DBDriver::create(); + if(driver->openConnection(gtFile)) + { + std::map poses = driver->loadOptimizedPoses(); + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + Transform p, gt; + GPS gps; + int m=-1, w=-1; + std::string l; + double s; + std::vector v; + EnvSensors sensors; + if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors)) + { + externalGtPoses.insert(std::make_pair(s, iter->second)); + } + } + } + delete driver; + } + else + { + // 10 can import format 1, 10 and 11. + std::map poses; + std::map stamps; + if(rtabmap::graph::importPoses(gtFile, 10, poses, 0, &stamps)) + { + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + externalGtPoses.insert(std::make_pair(stamps.at(iter->first), iter->second)); + } + } + } + if(externalGtPoses.size() < 2) + { + printf("Failed loading ground truth poses from \"%s\" (\"--gt\" option).\n", gtFile.c_str()); + showUsage(); + } + else + { + printf("Loading %ld ground truth poses from \"%s\".\n", externalGtPoses.size(), gtFile.c_str()); + } + } + std::string fileName; std::list paths; paths.push_back(path); @@ -404,6 +501,11 @@ int main(int argc, char * argv[]) ULogger::setLevel(logLevel); std::set ids; driver->getAllNodeIds(ids, false, false, ignoreInterNodes); + std::set allIds = ids; + if(ignoreInterNodes) + { + driver->getAllNodeIds(allIds, false, false, false); + } std::map, double> > stats = driver->getAllStatistics(); std::map odomPoses, gtPoses; std::map odomStamps; @@ -523,7 +625,8 @@ int main(int argc, char * argv[]) std::map > localizationSessionStats; double previousStamp = 0.0; - for(std::set::iterator iter=ids.begin(); iter!=ids.end(); ++iter) + std::map allWeights; + for(std::set::iterator iter=allIds.begin(); iter!=allIds.end(); ++iter) { Transform p, gt; GPS gps; @@ -534,119 +637,137 @@ int main(int argc, char * argv[]) EnvSensors sensors; if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors)) { - odomPoses.insert(std::make_pair(*iter, p)); - odomStamps.insert(std::make_pair(*iter, s)); - if(!gt.isNull()) + allWeights.insert(std::make_pair(*iter, w)); + if((!ignoreInterNodes || w!=-1)) { - gtPoses.insert(std::make_pair(*iter, gt)); - } - - if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end()) - { - continue; - } - - if(*iter >= startIdPerDb && uContains(stats, *iter)) - { - const std::map & stat = stats.at(*iter).first; - if(uContains(stat, Statistics::kGtTranslational_rmse())) + odomPoses.insert(std::make_pair(*iter, p)); + odomStamps.insert(std::make_pair(*iter, s)); + if(!externalGtPoses.empty()) { - rmse = stat.at(Statistics::kGtTranslational_rmse()); - if(maxRMSE==-1 || maxRMSE < rmse) + std::map::iterator nextIter = externalGtPoses.upper_bound(s); + if(nextIter!=externalGtPoses.end()) { - maxRMSE = rmse; + std::map::iterator previousIter = nextIter; + --previousIter; + if(s == previousIter->first || (nextIter->first-s <= gtMaxInterval && s-previousIter->first <= gtMaxInterval)) + { + UASSERT(s-previousIter->first >= 0); + gtPoses.insert(std::make_pair(*iter, previousIter->second.interpolate((s-previousIter->first)/(nextIter->first-previousIter->first),nextIter->second))); + } } } - if(uContains(stat, std::string("Camera/TotalTime/ms"))) + else if(!gt.isNull()) { - cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms"))); + gtPoses.insert(std::make_pair(*iter, gt)); } - if(uContains(stat, std::string("Odometry/TotalTime/ms"))) - { - odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms"))); - } - else if(uContains(stat, std::string("Odometry/TimeEstimation/ms"))) + + if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end()) { - odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms"))); + continue; } - if(uContains(stat, std::string("RtabmapROS/TotalTime/ms"))) + if(*iter >= startIdPerDb && uContains(stats, *iter)) { - if(w!=-1) + const std::map & stat = stats.at(*iter).first; + if(uContains(stat, Statistics::kGtTranslational_rmse())) { - slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms")); + rmse = stat.at(Statistics::kGtTranslational_rmse()); + if(maxRMSE==-1 || maxRMSE < rmse) + { + maxRMSE = rmse; + } } - } - else if(uContains(stat, Statistics::kTimingTotal())) - { - if(w!=-1) + if(uContains(stat, std::string("Camera/TotalTime/ms"))) { - slamTime.push_back(stat.at(Statistics::kTimingTotal())); + cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms"))); + } + if(uContains(stat, std::string("Odometry/TotalTime/ms"))) + { + odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms"))); + } + else if(uContains(stat, std::string("Odometry/TimeEstimation/ms"))) + { + odomTime.push_back(stat.at(std::string("Odometry/TimeEstimation/ms"))); } - } - if(uContains(stat, std::string(Statistics::kMemoryRAM_usage()))) - { - float ram = stat.at(Statistics::kMemoryRAM_usage()); - if(maxMapRAM==-1 || maxMapRAM < ram) + if(uContains(stat, std::string("RtabmapROS/TotalTime/ms"))) { - maxMapRAM = ram; + if(w!=-1) + { + slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms")); + } } - } - if(uContains(stat, std::string("Odometry/RAM_usage/MB"))) - { - float ram = stat.at("Odometry/RAM_usage/MB"); - if(maxOdomRAM==-1 || maxOdomRAM < ram) + else if(uContains(stat, Statistics::kTimingTotal())) + { + if(w!=-1) + { + slamTime.push_back(stat.at(Statistics::kTimingTotal())); + } + } + + if(uContains(stat, std::string(Statistics::kMemoryRAM_usage()))) { - maxOdomRAM = ram; + float ram = stat.at(Statistics::kMemoryRAM_usage()); + if(maxMapRAM==-1 || maxMapRAM < ram) + { + maxMapRAM = ram; + } + } + if(uContains(stat, std::string("Odometry/RAM_usage/MB"))) + { + float ram = stat.at("Odometry/RAM_usage/MB"); + if(maxOdomRAM==-1 || maxOdomRAM < ram) + { + maxOdomRAM = ram; + } } - } #ifdef WITH_QT - for(std::map::iterator jter=curves.begin(); jter!=curves.end(); ++jter) - { + for(std::map::iterator jter=curves.begin(); jter!=curves.end(); ++jter) + { #else - for(std::map > > >::iterator jter=localizationMultiStats.begin(); - jter!=localizationMultiStats.end(); - ++jter) - { -#endif - if(uContains(stat, jter->first)) + for(std::map > > >::iterator jter=localizationMultiStats.begin(); + jter!=localizationMultiStats.end(); + ++jter) { - double y = stat.at(jter->first); -#ifdef WITH_QT - double x = s; - if(useIds) +#endif + if(uContains(stat, jter->first)) { - x = *iter; - } - jter->second->addValue(x,y); + double y = stat.at(jter->first); +#ifdef WITH_QT + double x = s; + if(useIds) + { + x = *iter; + } + jter->second->addValue(x,y); #endif - if(!localizationMultiStats.empty()) - { - if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first)) + if(!localizationMultiStats.empty()) { - // changed session - for(std::map >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter) + if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first)) { - LocStats values = LocStats::from(localizationSessionStats.at(kter->first)); - localizationMultiStats.at(kter->first).rbegin()->second.push_back(values); - localizationSessionStats.at(kter->first).clear(); - } + // changed session + for(std::map >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter) + { + LocStats values = LocStats::from(localizationSessionStats.at(kter->first)); + localizationMultiStats.at(kter->first).rbegin()->second.push_back(values); + localizationSessionStats.at(kter->first).clear(); + } - previousStamp = s; - } + previousStamp = s; + } - if(!uContains(localizationSessionStats, jter->first)) - { - localizationSessionStats.insert(std::make_pair(jter->first, std::vector())); + if(!uContains(localizationSessionStats, jter->first)) + { + localizationSessionStats.insert(std::make_pair(jter->first, std::vector())); + } + localizationSessionStats.at(jter->first).push_back(y); } - localizationSessionStats.at(jter->first).push_back(y); } } + previousStamp = s; } - previousStamp = s; } } } @@ -667,12 +788,54 @@ int main(int argc, char * argv[]) std::multimap links; std::multimap allLinks; driver->getAllLinks(allLinks, true, true); + if(ignoreInterNodes) + { + std::multimap allBiLinks; + for(std::multimap::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter) + { + if(jter->second.from() != jter->second.to() && + (jter->second.type() == Link::kNeighbor || + jter->second.type() == Link::kNeighborMerged)) + { + allBiLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse())); + } + allBiLinks.insert(*jter); + } + allLinks=allBiLinks; + } std::multimap loopClosureLinks; for(std::multimap::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter) { if(jter->second.from() == jter->second.to() || graph::findLink(links, jter->second.from(), jter->second.to(), true) == links.end()) { - links.insert(*jter); + Link link = jter->second; + // remove intermediate nodes? + if(link.from() != link.to() && + ignoreInterNodes && + (link.type() == Link::kNeighbor || + link.type() == Link::kNeighborMerged)) + { + while(uContains(allWeights, link.to()) && allWeights.at(link.to()) < 0) + { + std::multimap::iterator uter = allLinks.find(link.to()); + while(uter != allLinks.end() && + uter->first==link.to() && + uter->second.from()>uter->second.to()) + { + ++uter; + } + if(uter != allLinks.end()) + { + link = link.merge(uter->second, uter->second.type()); + allLinks.erase(uter->first); + } + else + { + break; + } + } + } + links.insert(std::make_pair(jter->first, link)); } if( jter->second.type() != Link::kNeighbor && jter->second.type() != Link::kNeighborMerged && @@ -860,19 +1023,19 @@ int main(int argc, char * argv[]) } else { - std::vector gtPoses; + std::vector gtPosesTmp; std::vector rPoses; for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { if(groundTruth.find(iter->first) != groundTruth.end()) { - gtPoses.push_back(groundTruth.at(iter->first)); + gtPosesTmp.push_back(groundTruth.at(iter->first)); rPoses.push_back(poses.at(iter->first)); } } - if(!gtPoses.empty()) + if(!gtPosesTmp.empty()) { - graph::calcRelativeErrors(gtPoses, rPoses, relative_t_err, relative_r_err); + graph::calcRelativeErrors(gtPosesTmp, rPoses, relative_t_err, relative_r_err); } } } @@ -1010,12 +1173,12 @@ int main(int argc, char * argv[]) } } } - printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n", + printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%s, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n", fileName.c_str(), (int)ids.size(), bestScale, bestRMSE, - maxRMSE, + maxRMSE!=-1?uFormat("%.3fm", maxRMSE).c_str():"NA", bestVoRMSE, bestRMSEAng, !outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(), diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index b077707a84..71db0d4b41 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -29,12 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #ifdef RTABMAP_OCTOMAP -#include +#include #endif -#include +#include +#include #include #include -#include +#include #include #include #include @@ -778,11 +779,12 @@ int main(int argc, char * argv[]) dbReader->init(); - OccupancyGrid grid(parameters); - grid.setCloudAssembling(assemble3dMap); + LocalGridCache mapCache; + OccupancyGrid grid(&mapCache, parameters); #ifdef RTABMAP_OCTOMAP - OctoMap octomap(parameters); + OctoMap octomap(&mapCache, parameters); #endif + CloudMap cloudMap(&mapCache, parameters); float linearUpdate = Parameters::defaultRGBDLinearUpdate(); float angularUpdate = Parameters::defaultRGBDAngularUpdate(); @@ -814,9 +816,9 @@ int main(int argc, char * argv[]) printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str()); std::map globalMapStats; int processed = 0; - CameraInfo info; - SensorData data = dbReader->takeImage(&info); - CameraThread camThread(dbReader, parameters); // take ownership of dbReader + SensorCaptureInfo info; + SensorData data = dbReader->takeData(&info); + SensorCaptureThread camThread(dbReader, parameters); // take ownership of dbReader camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius); if(scanFromDepth) { @@ -847,11 +849,11 @@ int main(int argc, char * argv[]) while(skippedFrames-- > 0) { ++processed; - data = dbReader->takeImage(); + data = dbReader->takeData(); } } - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(scanFromDepth) { data.setLaserScan(LaserScan()); @@ -930,6 +932,7 @@ int main(int argc, char * argv[]) double timeRtabmap = t.ticks(); double timeUpdateInit = 0.0; double timeUpdateGrid = 0.0; + double timeUpdateCloudMap = 0.0; #ifdef RTABMAP_OCTOMAP double timeUpdateOctoMap = 0.0; #endif @@ -942,34 +945,45 @@ int main(int argc, char * argv[]) { bool updateGridMap = false; bool updateOctoMap = false; - if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end()) + bool updateCloudMap = false; + if(assemble2dMap && grid.addedNodes().find(id) == grid.addedNodes().end()) { updateGridMap = true; } + if(assemble3dMap && cloudMap.addedNodes().find(id) == cloudMap.addedNodes().end()) + { + updateCloudMap = true; + } #ifdef RTABMAP_OCTOMAP if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end()) { updateOctoMap = true; } #endif - if(updateGridMap || updateOctoMap) + if(updateGridMap || updateOctoMap || updateCloudMap) { cv::Mat ground, obstacles, empty; stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty); + float cellSize = stats.getLastSignatureData().sensorData().gridCellSize(); + const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint(); timeUpdateInit = t.ticks(); + mapCache.add(id, ground, obstacles, empty, cellSize, viewpoint); + if(updateGridMap) { - grid.addToCache(id, ground, obstacles, empty); grid.update(stats.poses()); timeUpdateGrid = t.ticks() + timeUpdateInit; } + if(updateCloudMap) + { + cloudMap.update(stats.poses()); + timeUpdateCloudMap = t.ticks() + timeUpdateInit; + } #ifdef RTABMAP_OCTOMAP if(updateOctoMap) { - const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint(); - octomap.addToCache(id, ground, obstacles, empty, viewpoint); octomap.update(stats.poses()); timeUpdateOctoMap = t.ticks() + timeUpdateInit; } @@ -979,6 +993,7 @@ int main(int argc, char * argv[]) } globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f)); + globalMapStats.insert(std::make_pair(std::string("GlobalGrid/CloudUpdate/ms"), timeUpdateCloudMap*1000.0f)); #ifdef RTABMAP_OCTOMAP //Simulate publishing double timePub2dOctoMap = 0.0; @@ -998,9 +1013,9 @@ int main(int argc, char * argv[]) globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f)); globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f)); globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f)); - globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f)); + globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateCloudMap+timeUpdateOctoMap+timePub2dOctoMap+timePub3dOctoMap+timeRtabmap)*1000.0f)); #else - globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeRtabmap)*1000.0f)); + globalMapStats.insert(std::make_pair(std::string("GlobalGrid/TotalWithRtabmap/ms"), (timeUpdateGrid+timeUpdateCloudMap+timeRtabmap)*1000.0f)); #endif } } @@ -1084,7 +1099,7 @@ int main(int argc, char * argv[]) while(skippedFrames-- > 0) { processed++; - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at(0,0)>=9999) { printf("High variance detected, triggering a new map...\n"); @@ -1098,7 +1113,7 @@ int main(int argc, char * argv[]) } } - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(scanFromDepth) { data.setLaserScan(LaserScan()); @@ -1214,7 +1229,7 @@ int main(int argc, char * argv[]) if(assemble3dMap) { std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_obstacles.pcd"; - if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0) + if(pcl::io::savePCDFileBinary(outputPath, *cloudMap.getMapObstacles()) == 0) { printf("Saving 3d obstacles \"%s\"... done!\n", outputPath.c_str()); } @@ -1222,10 +1237,10 @@ int main(int argc, char * argv[]) { printf("Saving 3d obstacles \"%s\"... failed!\n", outputPath.c_str()); } - if(grid.getMapGround()->size()) + if(cloudMap.getMapGround()->size()) { outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_ground.pcd"; - if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapGround()) == 0) + if(pcl::io::savePCDFileBinary(outputPath, *cloudMap.getMapGround()) == 0) { printf("Saving 3d ground \"%s\"... done!\n", outputPath.c_str()); } @@ -1234,10 +1249,10 @@ int main(int argc, char * argv[]) printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str()); } } - if(grid.getMapEmptyCells()->size()) + if(cloudMap.getMapEmptyCells()->size()) { outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd"; - if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0) + if(pcl::io::savePCDFileBinary(outputPath, *cloudMap.getMapEmptyCells()) == 0) { printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str()); } diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 8240996974..02f86fdbf9 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraRGBD.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UProcessInfo.h" #include +#include #include #include @@ -200,7 +200,7 @@ int main(int argc, char * argv[]) //parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx()))); model.save(path); - CameraThread cameraThread(new + SensorCaptureThread cameraThread(new CameraRGBDImages( pathRgbImages, pathDepthImages, @@ -239,14 +239,13 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorCaptureInfo cameraInfo; + SensorData data = cameraThread.camera()->takeData(&cameraInfo); int iteration = 0; ///////////////////////////// // Processing dataset begin ///////////////////////////// - cv::Mat covariance; int odomKeyFrames = 0; double previousStamp = 0.0; int skipCount = 0; @@ -256,9 +255,9 @@ int main(int argc, char * argv[]) { ++skipCount; - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); continue; } skipCount = 0; @@ -277,6 +276,12 @@ int main(int argc, char * argv[]) odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1); } } + + if(iteration!=0 && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at(0,0)>=9999) + { + UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at(0,0)); + rtabmap.triggerNewMap(); + } if(odomInfo.keyFrameAdded) { @@ -303,10 +308,6 @@ int main(int argc, char * argv[]) data.setFeatures(std::vector(), std::vector(), cv::Mat());// remove features processData = intermediateNodes; } - if(covariance.empty() || odomInfo.reg.covariance.at(0,0) > covariance.at(0,0)) - { - covariance = odomInfo.reg.covariance; - } timer.restart(); if(processData) @@ -318,6 +319,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); @@ -337,8 +339,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize)); OdometryEvent e(SensorData(), Transform(), odomInfo); - rtabmap.process(data, pose, covariance, e.velocity(), externalStats); - covariance = cv::Mat(); + rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats); } ++iteration; @@ -354,8 +355,6 @@ int main(int argc, char * argv[]) if(rmse >= 0.0f) { - //printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad", - // iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at(0,0)), sqrt(odomInfo.reg.covariance.at(3,3))); printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm", iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse); } @@ -376,9 +375,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks()); @@ -428,7 +427,7 @@ int main(int argc, char * argv[]) } } - + // compute RMSE statistics float translational_rmse = 0.0f; float translational_mean = 0.0f; diff --git a/utilite/include/rtabmap/utilite/UStl.h b/utilite/include/rtabmap/utilite/UStl.h index 7a85c91d30..442ecb0734 100644 --- a/utilite/include/rtabmap/utilite/UStl.h +++ b/utilite/include/rtabmap/utilite/UStl.h @@ -186,11 +186,9 @@ template inline std::set uKeysSet(const std::map & m) { std::set s; - int i=0; for(typename std::map::const_iterator iter = m.begin(); iter!=m.end(); ++iter) { s.insert(s.end(), iter->first); - ++i; } return s; } From b9aef6f1609c033a5a672faba0339827ae2e177b Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 30 Apr 2024 15:17:54 +0200 Subject: [PATCH 12/13] fix ci --- corelib/include/rtabmap/core/VWDictionary.h | 1 + 1 file changed, 1 insertion(+) diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index a8745e2ba8..5328413037 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include "rtabmap/core/Parameters.h" +#include namespace rtabmap { From 5b83f5e2ee9ebaca23c4c19496d63ec831b497e2 Mon Sep 17 00:00:00 2001 From: Long Vuong Date: Tue, 30 Apr 2024 15:34:18 +0200 Subject: [PATCH 13/13] fix ci --- corelib/include/rtabmap/core/FlannIndex.h | 1 + 1 file changed, 1 insertion(+) diff --git a/corelib/include/rtabmap/core/FlannIndex.h b/corelib/include/rtabmap/core/FlannIndex.h index 220bd8a723..a9b7f46358 100644 --- a/corelib/include/rtabmap/core/FlannIndex.h +++ b/corelib/include/rtabmap/core/FlannIndex.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap {