Integration of OpenCV's ArUco Marker Detection (see new parameter "RGBD/MarkerDetection")

2026-09-02 09:30:25 +08:00 · 2019-02-18 18:19:55 -05:00
parent 08f3e6c08e
commit d6ca37a9e7
18 changed files with 859 additions and 23 deletions
--- a/corelib/include/rtabmap/core/Landmark.h
+++ b/corelib/include/rtabmap/core/Landmark.h
@@ -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() {}
--- a/corelib/include/rtabmap/core/MarkerDetector.h
+++ b/corelib/include/rtabmap/core/MarkerDetector.h
@@ -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_ */
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.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
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -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();

--- a/corelib/include/rtabmap/core/Statistics.h
+++ b/corelib/include/rtabmap/core/Statistics.h
@@ -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);
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -96,6 +96,8 @@ SET(SRC_FILES
 	
 	OccupancyGrid.cpp
 	
+	MarkerDetector.cpp
+	
 	GainCompensator.cpp
 		
 	rtflann/ext/lz4.c
--- a/corelib/src/MarkerDetector.cpp
+++ b/corelib/src/MarkerDetector.cpp
@@ -0,0 +1,141 @@
+/*
+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.
+*/
+
+#include <rtabmap/core/MarkerDetector.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+MarkerDetector::MarkerDetector(const ParametersMap & parameters) :
+		markerLength_(Parameters::defaultArucoMarkerLength()),
+		dictionaryId_(Parameters::defaultArucoDictionary())
+{
+#ifdef HAVE_OPENCV_ARUCO
+	detectorParams_ = cv::aruco::DetectorParameters::create();
+	detectorParams_->cornerRefinementMethod = Parameters::defaultArucoCornerRefinementMethod();
+	parseParameters(parameters);
+#endif
+}
+
+MarkerDetector::~MarkerDetector() {
+
+}
+
+void MarkerDetector::parseParameters(const ParametersMap & parameters)
+{
+#ifdef HAVE_OPENCV_ARUCO
+	detectorParams_->adaptiveThreshWinSizeMin = 3;
+	detectorParams_->adaptiveThreshWinSizeMax = 23;
+	detectorParams_->adaptiveThreshWinSizeStep = 10;
+	detectorParams_->adaptiveThreshConstant = 7;
+	detectorParams_->minMarkerPerimeterRate = 0.03;
+	detectorParams_->maxMarkerPerimeterRate = 4.0;
+	detectorParams_->polygonalApproxAccuracyRate = 0.03;
+	detectorParams_->minCornerDistanceRate = 0.05;
+	detectorParams_->minDistanceToBorder = 3;
+	detectorParams_->minMarkerDistanceRate = 0.05;
+	Parameters::parse(parameters, Parameters::kArucoCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
+	detectorParams_->cornerRefinementWinSize = 5;
+	detectorParams_->cornerRefinementMaxIterations = 30;
+	detectorParams_->cornerRefinementMinAccuracy = 0.1;
+	detectorParams_->markerBorderBits = 1;
+	detectorParams_->perspectiveRemovePixelPerCell = 4;
+	detectorParams_->perspectiveRemoveIgnoredMarginPerCell = 0.13;
+	detectorParams_->maxErroneousBitsInBorderRate = 0.35;
+	detectorParams_->minOtsuStdDev = 5.0;
+	detectorParams_->errorCorrectionRate = 0.6;
+
+	Parameters::parse(parameters, Parameters::kArucoMarkerLength(), markerLength_);
+	Parameters::parse(parameters, Parameters::kArucoDictionary(), dictionaryId_);
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+	if(dictionaryId_ >= 17)
+	{
+		UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
+				"current version is %s. Setting dictionary type to default (%d)",
+				CV_VERSION,
+				Parameters::defaultArucoDictionary());
+		dictionaryId_ = Parameters::defaultArucoDictionary();
+	}
+#endif
+	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
+#endif
+}
+
+std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, cv::Mat * imageWithDetections)
+{
+	std::map<int, Transform> detections;
+
+#ifdef HAVE_OPENCV_ARUCO
+
+	std::vector< int > ids;
+	std::vector< std::vector< cv::Point2f > > corners, rejected;
+	std::vector< cv::Vec3d > rvecs, tvecs;
+
+	// detect markers and estimate pose
+	cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected);
+	UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
+	if(ids.size() > 0)
+	{
+		cv::aruco::estimatePoseSingleMarkers(corners, markerLength_, model.K(), model.D(), rvecs, tvecs);
+		for(size_t i=0; i<ids.size(); ++i)
+		{
+			cv::Mat R;
+			cv::Rodrigues(rvecs[i], R);
+			Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
+						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
+						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
+
+			Transform pose = model.localTransform() * t;
+			detections.insert(std::make_pair(ids[i], pose));
+			UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+		}
+	}
+
+	if(imageWithDetections)
+	{
+		image.copyTo(*imageWithDetections);
+		if(ids.size() > 0)
+		{
+			cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
+
+			for(unsigned int i = 0; i < ids.size(); i++)
+			{
+				cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], markerLength_ * 0.5f);
+			}
+		}
+	}
+
+#else
+	ROS_ERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
+#endif
+
+	return detections;
+}
+
+
+} /* namespace rtabmap */
+
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -61,6 +61,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
 #include <rtabmap/core/OccupancyGrid.h>
+#include <rtabmap/core/MarkerDetector.h>
 #include <opencv2/imgproc/types_c.h>

 namespace rtabmap {
@@ -106,6 +107,9 @@ Memory::Memory(const ParametersMap & parameters) :
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
 	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
+	_detectMarkers(Parameters::defaultRGBDMarkerDetection()),
+	_markerLinVariance(Parameters::defaultArucoVarianceLinear()),
+	_markerAngVariance(Parameters::defaultArucoVarianceAngular()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -137,6 +141,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_registrationIcpMulti = new RegistrationIcp(paramsMulti);

 	_occupancy = new OccupancyGrid(parameters);
+	_markerDetector = new MarkerDetector(parameters);
 	this->parseParameters(parameters);
 }

@@ -561,6 +566,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
 	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
 	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
+	Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers);
+	Parameters::parse(params, Parameters::kArucoVarianceLinear(), _markerLinVariance);
+	Parameters::parse(params, Parameters::kArucoVarianceAngular(), _markerAngVariance);

 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -674,6 +682,11 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_occupancy->parseParameters(params);
 	}

+	if(_markerDetector)
+	{
+		_markerDetector->parseParameters(params);
+	}
+
 	// do this after all params are parsed
 	// SLAM mode vs Localization mode
 	iter = params.find(Parameters::kMemIncrementalMemory());
@@ -4577,6 +4590,53 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

+	Landmarks landmarks = data.landmarks();
+	if(_detectMarkers)
+	{
+		UDEBUG("Detecting markers...");
+		if(landmarks.empty())
+		{
+			std::map<int, Transform> markers;
+			if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
+			{
+				if(data.cameraModels().size() > 1)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						UWARN("Detecting markers in multi-camera setup is not yet implemented, detecting only in first camera. This message is only printed once.");
+					}
+					warned = true;
+				}
+				markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0]);
+			}
+			else if(data.stereoCameraModel().isValidForProjection())
+			{
+				markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left());
+			}
+			for(std::map<int, Transform>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
+			{
+				if(iter->first <= 0)
+				{
+					UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
+					continue;
+				}
+				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+				covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
+				covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
+				landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second, covariance)));
+			}
+			UDEBUG("Markers detected = %d", (int)markers.size());
+		}
+		else
+		{
+			UWARN("Input data has already landmarks, cannot do marker detection.");
+		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemMarkers_detection(), t*1000.0f);
+		UDEBUG("time markers detection = %fs", t);
+	}
+
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
 	std::vector<CameraModel> cameraModels = data.cameraModels();
@@ -5025,7 +5085,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}

 	//landmarks
-	for(Landmarks::const_iterator iter = data.landmarks().begin(); iter!=data.landmarks().end(); ++iter)
+	for(Landmarks::const_iterator iter = landmarks.begin(); iter!=landmarks.end(); ++iter)
 	{
 		if(iter->second.id() > 0)
 		{
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -2526,7 +2526,9 @@ bool Rtabmap::process(
 			{
 				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
 				{
-					if(iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kVirtualClosure)
+					if( iter->second.type() != Link::kNeighbor &&
+						iter->second.type() != Link::kVirtualClosure &&
+						iter->second.type() != Link::kLandmark)
 					{
 						UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
 						poses.clear();