Merge remote-tracking branch 'origin/feature/SLAMOptimization' into develop

Author: cutullic

.gitignore

@@ -57,8 +57,10 @@ Release/
 *-packagedependencies-unix.*
 *-packagedependencies-android.*
 *.pc
-*-Debug
-*-Release
+*-Debug*
+*-Release*
+*_Debug*
+*_Release*
 
 # Executables
 *.exe

SolARModuleTools.pro

@@ -6,7 +6,7 @@ CONFIG -= qt
 INSTALLSUBDIR = SolARBuild
 TARGET = SolARModuleTools
 FRAMEWORK = $$TARGET
-VERSION=0.9.0
+VERSION=0.9.1
 
 DEFINES += MYVERSION=$${VERSION}
 DEFINES += TEMPLATE_LIBRARY

bcom-SolARModuleTools.pc.in

@@ -5,7 +5,7 @@ libdir=${exec_prefix}/lib
 includedir=${prefix}/interfaces
 Name: SolARModuleTools
 Description:
-Version: 0.9.0
+Version: 0.9.1
 Requires:
 Libs: -L${libdir} -l${libname}
 Libs.private: ${libdir}/${pfx}${libname}.${lext}

interfaces/SolARFiducialMarkerPoseEstimator.h

@@ -26,7 +26,9 @@
 #include "api/features/IDescriptorsExtractorSBPattern.h"
 #include "api/features/ISBPatternReIndexer.h"
 #include "api/geom/IImage2WorldMapper.h"
+#include "api/geom/IProject.h"
 #include "api/solver/pose/I3DTransformFinderFrom2D3D.h"
+#include "api/features/ICornerRefinement.h"
 #include "datastructure/Image.h"
 #include "datastructure/FiducialMarker.h"
 #include "SolARToolsAPI.h"
@@ -53,6 +55,7 @@ namespace TOOLS {
 * @SolARComponentInjectable{SolAR::api::features::ISBPatternReIndexer}
 * @SolARComponentInjectable{SolAR::api::geom::IImage2WorldMapper}
 * @SolARComponentInjectable{SolAR::api::solver::pose::I3DTransformFinderFrom2D3D}
+* @SolARComponentInjectable{SolAR::api::features::ICornerRefinement}
 * @SolARComponentInjectablesEnd
 *
 * @SolARComponentPropertiesBegin
@@ -112,10 +115,13 @@ class SOLAR_TOOLS_EXPORT_API SolARFiducialMarkerPoseEstimator : public org::bcom
 	SRef<api::features::ISBPatternReIndexer>			m_patternReIndexer;
 	SRef<api::geom::IImage2WorldMapper>					m_img2worldMapper;
 	SRef<api::solver::pose::I3DTransformFinderFrom2D3D>	m_pnp;
+	SRef<api::features::ICornerRefinement>				m_cornerRefinement;
+	SRef<api::geom::IProject>							m_projector;
 	SRef<datastructure::DescriptorBuffer>				m_markerPatternDescriptor;
 	int													m_nbThreshold = 3;
 	int													m_minThreshold = -1;
 	int													m_maxThreshold = 220;
+	float												m_maxReprojError = 0.5f;
 };
 
 }

interfaces/SolARSLAMBootstrapper.h

@@ -28,6 +28,7 @@
 #include "api/solver/map/IKeyframeSelector.h"
 #include "api/display/IMatchesOverlay.h"
 #include "api/solver/pose/I3DTransformFinderFrom2D2D.h"
+#include "api/geom/IUndistortPoints.h"
 #include "SolARToolsAPI.h"
 #include "xpcf/component/ConfigurableBase.h"
 
@@ -75,6 +76,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMBootstrapper : public org::bcom::xpcf::Con
 	SolARSLAMBootstrapper();
 	///@brief SolAR3DTransformEstimationFrom3D3D destructor;
 	~SolARSLAMBootstrapper() = default;
+	
 	/// @brief this method is used to set intrinsic parameters and distorsion of the camera
 	/// @param[in] Camera calibration matrix parameters.
 	/// @param[in] Camera distorsion parameters.
@@ -88,6 +90,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMBootstrapper : public org::bcom::xpcf::Con
     FrameworkReturnCode process(const SRef<datastructure::Image> image, SRef<datastructure::Image> & view, const datastructure::Transform3Df & pose = datastructure::Transform3Df::Identity()) override;
 
 	void unloadComponent() override final;
+	org::bcom::xpcf::XPCFErrorCode onConfigured() override final;
 
 private:
 	/// bootstrap uses marker
@@ -99,6 +102,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMBootstrapper : public org::bcom::xpcf::Con
 	int													m_hasPose = 1;
 	int													m_nbMinInitPointCloud = 50;
     float												m_angleThres = 0.1f;
+    float												m_ratioDistanceIsKeyframe = 0.05f;
 	bool												m_bootstrapOk = false;
 	bool												m_initKeyframe1 = false;
 	SRef<datastructure::Keyframe>										m_keyframe1, m_keyframe2;
@@ -113,6 +117,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMBootstrapper : public org::bcom::xpcf::Con
 	SRef<api::solver::map::IMapFilter>					m_mapFilter;
 	SRef<api::solver::map::IKeyframeSelector>			m_keyframeSelector;
 	SRef<api::solver::pose::I3DTransformFinderFrom2D2D>	m_poseFinderFrom2D2D;
+	SRef<api::geom::IUndistortPoints>					m_undistortPoints;
 	SRef<api::display::IMatchesOverlay>					m_matchesOverlay;
 };
 

interfaces/SolARSLAMMapping.h

@@ -105,8 +105,12 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMMapping : public org::bcom::xpcf::Configur
 
 private:
 	float																		m_minWeightNeighbor = 1.f;
-	int																			m_minTrackedPoints = 100;
+	int																			m_minTrackedPoints = 200;
 	int																			m_maxNbNeighborKfs = 5;
+	int																			m_nbPassedFrames = 0;
+	int																			m_nbVisibilityAtLeast = 30;
+	int																			m_nbPassedFrameAtLeast = 5;
+	float																		m_ratioCPRefKeyframe = 0.5;
 	SRef<datastructure::Keyframe>												m_updatedReferenceKeyframe;
 	datastructure::CamCalibration												m_camMatrix;
 	datastructure::CamDistortion												m_camDistortion;

interfaces/SolARSLAMTracking.h

@@ -93,6 +93,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMTracking : public org::bcom::xpcf::Configu
     FrameworkReturnCode process(const SRef<datastructure::Frame> frame, SRef<datastructure::Image> &displayImage) override;
 
 	void unloadComponent() override final;
+	org::bcom::xpcf::XPCFErrorCode onConfigured() override final;
 
 private:
 	void updateLocalMap();
@@ -104,6 +105,7 @@ class SOLAR_TOOLS_EXPORT_API SolARSLAMTracking : public org::bcom::xpcf::Configu
 	bool												m_isLostTrack = false;
 	float												m_minWeightNeighbor = 10.f;
 	float												m_thresAngleViewDirection = 0.7f;
+	float												m_reprojErrorThreshold;
 	int													m_displayTrackedPoints = 1;
 	bool												m_isUpdateReferenceKeyframe = false;
 	std::mutex											m_refKeyframeMutex;

packagedependencies.txt

@@ -1 +1 @@
-SolARFramework|0.9.0|SolARFramework|SolARBuild@github|https://github.com/SolarFramework/SolarFramework/releases/download
+SolARFramework|0.9.1|SolARFramework|SolARBuild@github|https://github.com/SolarFramework/SolarFramework/releases/download

src/SolARFiducialMarkerPoseEstimator.cpp

@@ -43,15 +43,19 @@ SolARFiducialMarkerPoseEstimator::SolARFiducialMarkerPoseEstimator():Configurabl
 	declareInjectable<api::features::ISBPatternReIndexer>(m_patternReIndexer);
 	declareInjectable<api::geom::IImage2WorldMapper>(m_img2worldMapper);
 	declareInjectable<api::solver::pose::I3DTransformFinderFrom2D3D>(m_pnp);
+	declareInjectable<api::features::ICornerRefinement>(m_cornerRefinement);
+	declareInjectable<api::geom::IProject>(m_projector);
 	declareProperty("nbThreshold", m_nbThreshold);
 	declareProperty("minThreshold", m_minThreshold);
 	declareProperty("maxThreshold", m_maxThreshold);
+	declareProperty("maxReprojError", m_maxReprojError);
 }
 
 void SolARFiducialMarkerPoseEstimator::setCameraParameters(const CamCalibration & intrinsicParams, const CamDistortion & distortionParams) {
 	m_camMatrix = intrinsicParams;
 	m_camDistortion = distortionParams;
 	m_pnp->setCameraParameters(m_camMatrix, m_camDistortion);
+	m_projector->setCameraParameters(m_camMatrix, m_camDistortion);
 	// components initialisation for marker detection
 	m_binaryMarker->loadMarker();
 	m_patternDescriptorExtractor->extract(m_binaryMarker->getPattern(), m_markerPatternDescriptor);
@@ -136,10 +140,22 @@ FrameworkReturnCode SolARFiducialMarkerPoseEstimator::estimate(const SRef<Image>
 				m_patternReIndexer->reindex(recognizedContours, patternMatches, pattern2DPoints, img2DPoints);
 				// Compute the 3D position of each corner of the marker
 				m_img2worldMapper->map(pattern2DPoints, pattern3DPoints);
+				// Refine corner locations
+				m_cornerRefinement->refine(greyImage, img2DPoints);
 				// Compute the pose of the camera using a Perspective n Points algorithm using only the 4 corners of the marker
 				if (m_pnp->estimate(img2DPoints, pattern3DPoints, pose) == FrameworkReturnCode::_SUCCESS)
 				{
-					return FrameworkReturnCode::_SUCCESS;
+					std::vector<Point2Df> projected2DPts;
+					m_projector->project(pattern3DPoints, projected2DPts, pose);
+					float errorReproj(0.f);
+					for (int j = 0; j < projected2DPts.size(); ++j)
+						errorReproj += (projected2DPts[j] - img2DPoints[j]).norm();
+					errorReproj /= projected2DPts.size();
+					LOG_DEBUG("Mean reprojection error: {}", errorReproj);
+					if (errorReproj < m_maxReprojError)
+						return FrameworkReturnCode::_SUCCESS;
+					pose = Transform3Df::Identity();
+					return FrameworkReturnCode::_ERROR_;
 				}
 			}
 		}

src/SolARLoopClosureDetector.cpp

@@ -61,8 +61,11 @@ FrameworkReturnCode SolARLoopClosureDetector::detect(const SRef<Keyframe> queryK
 	for (auto &it : retKeyframesIndex) {
 		std::vector<uint32_t> paths;
 		m_covisibilityGraph->getShortestPath(queryKeyframeId, it, paths);
-		if (paths.size() > 3)
+		if (paths.size() > 3) {
 			candidatesId.push_back(it);
+			if (candidatesId.size() >= 3)
+				break;
+		}
 	}
 	std::vector<SRef<Keyframe>> candidateKeyframes;
 	std::vector<Transform3Df> candidateKeyframePoses;