mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 09:30:25 +08:00
Integration of OpenCV's ArUco Marker Detection (see new parameter "RGBD/MarkerDetection")
This commit is contained in:
matlabbe
@@ -49,12 +49,12 @@ public:
|
||||
UASSERT(id_>0);
|
||||
UASSERT(!pose_.isNull());
|
||||
UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(0,0)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(1,1)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(2,2)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(3,3)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(4,4)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(5,5)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
|
||||
UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
|
||||
}
|
||||
|
||||
virtual ~Landmark() {}
|
||||
|
||||
59
corelib/include/rtabmap/core/MarkerDetector.h
Normal file
59
corelib/include/rtabmap/core/MarkerDetector.h
Normal file
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_
|
||||
#define CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <opencv2/opencv_modules.hpp>
|
||||
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#include <opencv2/aruco.hpp>
|
||||
#endif
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class MarkerDetector {
|
||||
public:
|
||||
MarkerDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~MarkerDetector();
|
||||
void parseParameters(const ParametersMap & parameters);
|
||||
std::map<int, Transform> detect(const cv::Mat & image, const CameraModel & model, cv::Mat * imageWithDetections = 0);
|
||||
|
||||
private:
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
|
||||
float markerLength_;
|
||||
int dictionaryId_;
|
||||
cv::Ptr<cv::aruco::Dictionary> dictionary_;
|
||||
#endif
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
|
||||
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_ */
|
||||
@@ -57,6 +57,7 @@ class RegistrationInfo;
|
||||
class RegistrationIcp;
|
||||
class Stereo;
|
||||
class OccupancyGrid;
|
||||
class MarkerDetector;
|
||||
|
||||
class RTABMAP_EXP Memory
|
||||
{
|
||||
@@ -316,6 +317,9 @@ private:
|
||||
bool _imagesAlreadyRectified;
|
||||
bool _rectifyOnlyFeatures;
|
||||
bool _covOffDiagonalIgnored;
|
||||
bool _detectMarkers;
|
||||
float _markerLinVariance;
|
||||
float _markerAngVariance;
|
||||
|
||||
int _idCount;
|
||||
int _idMapCount;
|
||||
@@ -349,6 +353,8 @@ private:
|
||||
RegistrationIcp * _registrationIcpMulti;
|
||||
|
||||
OccupancyGrid * _occupancy;
|
||||
|
||||
MarkerDetector * _markerDetector;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
@@ -355,6 +355,7 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes.");
|
||||
RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure, bool, false, "Do local bundle adjustment with neighborhood of the loop closure.");
|
||||
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for 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 \"Aruco\" 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.");
|
||||
|
||||
// Local/Proximity loop closure detection
|
||||
@@ -709,6 +710,12 @@ class RTABMAP_EXP Parameters
|
||||
RTABMAP_PARAM(GridGlobal, ProbClampingMin, float, 0.1192, "Probability clamping minimum (value between 0 and 1).");
|
||||
RTABMAP_PARAM(GridGlobal, ProbClampingMax, float, 0.971, "Probability clamping maximum (value between 0 and 1).");
|
||||
|
||||
RTABMAP_PARAM(Aruco, Dictionary, int, 0, "Dictionary to use: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2, DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
|
||||
RTABMAP_PARAM(Aruco, MarkerLength, float, 0.1, "The length (m) of the markers' side.");
|
||||
RTABMAP_PARAM(Aruco, VarianceLinear, float, 0.001, "Linear variance to set on marker detections.");
|
||||
RTABMAP_PARAM(Aruco, VarianceAngular, float, 0.001, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
|
||||
RTABMAP_PARAM(Aruco, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag 2)");
|
||||
|
||||
public:
|
||||
virtual ~Parameters();
|
||||
|
||||
|
||||
@@ -147,6 +147,7 @@ class RTABMAP_EXP Statistics
|
||||
RTABMAP_STATS(TimingMem, Post_decimation, ms);
|
||||
RTABMAP_STATS(TimingMem, Scan_filtering, ms);
|
||||
RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
|
||||
RTABMAP_STATS(TimingMem, Markers_detection, ms);
|
||||
|
||||
RTABMAP_STATS(Keypoint, Dictionary_size, words);
|
||||
RTABMAP_STATS(Keypoint, Indexed_words, words);
|
||||
|
||||
Reference in New Issue
Block a user