Merge branch 'develop'

JenkinsFile

@@ -2,5 +2,4 @@ SolArModulePipeline {
     moduleName="SolARModuleG2O"
     dirName="SolARBuild"
     runTests=true
-    android=true
 }

SolARModuleG2O.pro

@@ -9,7 +9,7 @@ INSTALLSUBDIR = SolARBuild
 TARGET = SolARModuleG2O
 
 FRAMEWORK = $$TARGET
-VERSION=0.11.0
+VERSION=1.0.0
 
 DEFINES += MYVERSION=$${VERSION}
 DEFINES += TEMPLATE_LIBRARY
@@ -49,7 +49,7 @@ unix {
     QMAKE_POST_LINK += "make install install_deps"
 }
 
-unix:!android {
+unix {
     QMAKE_CXXFLAGS += -Wignored-qualifiers
 }
 
@@ -58,15 +58,6 @@ linux {
     LIBS += -L/home/linuxbrew/.linuxbrew/lib # temporary fix caused by grpc with -lre2 ... without -L in grpc.pc
 }
 
-macx {
-    DEFINES += _MACOS_TARGET_
-    QMAKE_MAC_SDK= macosx
-    QMAKE_CFLAGS += -mmacosx-version-min=10.7 -std=c11 #-x objective-c++
-    QMAKE_CXXFLAGS += -mmacosx-version-min=10.7 -std=c11 -std=c++11 -O3 -fPIC#-x objective-c++
-    QMAKE_LFLAGS += -mmacosx-version-min=10.7 -v -lstdc++
-    LIBS += -lstdc++ -lc -lpthread
-}
-
 win32 {
     DEFINES +=WINDOWS
     DEFINES += WIN64 UNICODE _UNICODE
@@ -77,11 +68,6 @@ win32 {
     QMAKE_CXXFLAGS += /bigobj
 }
 
-android {
-    QMAKE_LFLAGS += -nostdlib++
-    ANDROID_ABIS="arm64-v8a"
-}
-
 
 header_files.path = $${PROJECTDEPLOYDIR}/interfaces
 header_files.files = $$files($${PWD}/interfaces/*.h*)

bcom-SolARModuleG2O.pc.in

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

interfaces/SolAROptimizationG2O.h

@@ -21,6 +21,7 @@
 #include "api/solver/map/IBundler.h"
 #include "api/storage/ICovisibilityGraphManager.h"
 #include "api/storage/IKeyframesManager.h"
+#include "api/storage/ICameraParametersManager.h"
 #include "api/storage/IPointCloudManager.h"
 #include "api/geom/I3DTransform.h"
 
@@ -36,6 +37,7 @@ namespace G2O {
  *
  * @SolARComponentInjectablesBegin
  * @SolARComponentInjectable{SolAR::api::storage::IPointCloudManager}
+ * @SolARComponentInjectable{SolAR::api::storage::ICameraParametersManager}
  * @SolARComponentInjectable{SolAR::api::storage::IKeyframesManager}
  * @SolARComponentInjectable{SolAR::api::storage::ICovisibilityGraphManager}
  * @SolARComponentInjectablesEnd
@@ -87,25 +89,19 @@ class SOLARG2O_EXPORT_API SolAROptimizationG2O : public org::bcom::xpcf::Configu
 
 	/// @brief solve a non-linear problem related to bundle adjustement statement expressed as:
 	/// minArg(pts3ds,intrinsics,extrinsics) = MIN_cam_i(MIN_3d_j(pts2d_j - reproje(pt3ds_j,intrinsics_i,extrinsics_i)),
-	/// @param[in, out] K: camera calibration parameters responsible of 3D points generation.
-	/// @param[in, out] D: camera distorsion parameters responsible of 3D points generation
 	/// @param[in] selectKeyframes : selected views to bundle following a given strategies. If it is empty then take all keyframes into account to perform global bundle adjustment.
 	/// @return the mean re-projection error after optimization.
-	double bundleAdjustment(datastructure::CamCalibration & K, datastructure::CamDistortion & D, const std::vector<uint32_t> & selectKeyframes = {}) override;
+	double bundleAdjustment(const std::vector<uint32_t> & selectKeyframes = {}) override;
 
 	/// @brief solve a non-linear problem related to sim3D optimization between two overlaped keyframes of two different maps:
-	/// @param[in] K1: camera calibration parameters responsible of 3D points generation from map 1.
-	/// @param[in] K2: camera calibration parameters responsible of 3D points generation from map 2.
 	/// @param[in] keyframe1: first overlapping keyframe from map 1.
 	/// @param[in] keyframe2: second overlapping keyframe from map 2.
 	/// @param[in] matches: matches between two keyframes.
 	/// @param[in] pts3D1: first set of 3D points.
 	/// @param[in] pts3D2: second set of 3D points.
 	/// @param[in, out] pose: Sim3 matrix pose between map1 and map2
 	/// @return the mean re-projection error.
-	double optimizeSim3(datastructure::CamCalibration& K1,
-						datastructure::CamCalibration& K2,
-						const SRef<Keyframe>& keyframe1,
+    double optimizeSim3(const SRef<Keyframe>& keyframe1,
 						const SRef<Keyframe>& keyframe2,
 						const std::vector<DescriptorMatch>& matches,
 						const std::vector<Point3Df> & pts3D1,
@@ -127,9 +123,11 @@ class SOLARG2O_EXPORT_API SolAROptimizationG2O : public org::bcom::xpcf::Configu
 	int							m_fixedMap = 0;
 	int							m_fixedKeyframes = 0;
 	int							m_fixedScale = 0;
+    SRef<datastructure::Map>                        m_map;
     SRef<api::geom::I3DTransform>					m_transform3D;
 	SRef<api::storage::IPointCloudManager>			m_pointCloudManager;
 	SRef<api::storage::IKeyframesManager>			m_keyframesManager;
+    SRef<api::storage::ICameraParametersManager>	m_cameraParametersManager;
 	SRef<api::storage::ICovisibilityGraphManager>	m_covisibilityGraphManager;
 };
 

packagedependencies.txt

@@ -1,2 +1,2 @@
-SolARFramework|0.11.0|SolARFramework|SolARBuild@github|https://github.com/SolarFramework/SolarFramework/releases/download
-g2o|20200410|g2o|conan-solar@conan|conan-solar#https://artifact.b-com.com/api/conan/solar-conan-local#0
+SolARFramework|1.0.0|SolARFramework|SolARBuild@github|https://github.com/SolarFramework/SolarFramework/releases/download
+g2o#1_0_0|20200410|g2o|conan-solar@conan|conan-solar

src/SolAROptimizationG2O.cpp

@@ -44,6 +44,7 @@ SolAROptimizationG2O::SolAROptimizationG2O():ConfigurableBase(xpcf::toUUID<SolAR
 {
     addInterface<api::solver::map::IBundler>(this);
     declareInjectable<IPointCloudManager>(m_pointCloudManager);
+    declareInjectable<ICameraParametersManager>(m_cameraParametersManager);
     declareInjectable<IKeyframesManager>(m_keyframesManager);
     declareInjectable<ICovisibilityGraphManager>(m_covisibilityGraphManager);
     declareInjectable<api::geom::I3DTransform>(m_transform3D);
@@ -70,8 +71,10 @@ SolAROptimizationG2O::~SolAROptimizationG2O()
 FrameworkReturnCode SolAROptimizationG2O::setMap(const SRef<datastructure::Map> map)
 {
 	m_pointCloudManager->setPointCloud(map->getConstPointCloud());
+    m_cameraParametersManager->setCameraParametersCollection(map->getConstCameraParametersCollection());
 	m_keyframesManager->setKeyframeCollection(map->getConstKeyframeCollection());
-	m_covisibilityGraphManager->setCovisibilityGraph(map->getConstCovisibilityGraph());
+    m_covisibilityGraphManager->setCovisibilityGraph(map->getConstCovisibilityGraph());
+    m_map = map;
 	return FrameworkReturnCode::_SUCCESS;
 }
 
@@ -100,9 +103,9 @@ Transform3Df toSolarPose(const g2o::SE3Quat &SE3)
     return pose;
 }
 
-double SolAROptimizationG2O::bundleAdjustment(CamCalibration & K, ATTRIBUTE(maybe_unused) CamDistortion & D, const std::vector<uint32_t> & selectKeyframes)
+double SolAROptimizationG2O::bundleAdjustment(const std::vector<uint32_t> & selectKeyframes)
 {
-	// get cloud points and keyframes to optimize
+    // get cloud points and keyframes to optimize
 	int iterations;
 	std::vector< SRef<Keyframe>> keyframes;
 	std::vector<SRef<CloudPoint>> tmpCloudPoints, cloudPoints;
@@ -182,11 +185,19 @@ double SolAROptimizationG2O::bundleAdjustment(CamCalibration & K, ATTRIBUTE(mayb
 		iterations = m_iterationsGlobal;
 		LOG_INFO("Global bundle adjustment");
 		// get all keyframes
-		m_keyframesManager->getAllKeyframes(keyframes);
-		for (const auto &kf : keyframes)
-			idxKeyFrames.insert(kf->getId());
+        if (m_keyframesManager->getAllKeyframes(keyframes) == FrameworkReturnCode::_SUCCESS) {
+            for (const auto &kf : keyframes)
+                idxKeyFrames.insert(kf->getId());
+        }
+        else {
+            LOG_ERROR("Error while trying to get all keyframes from KeyframeManager");
+            return 0;
+        }
 		// get all point cloud
-		m_pointCloudManager->getAllPoints(tmpCloudPoints);
+        if (m_pointCloudManager->getAllPoints(tmpCloudPoints) != FrameworkReturnCode::_SUCCESS) {
+            LOG_ERROR("Error while trying to get all points of the cloud from PointCloudManager");
+            return 0;
+        }
 	}
 
 	// filter cloud point more than 1 visibility
@@ -207,7 +218,7 @@ double SolAROptimizationG2O::bundleAdjustment(CamCalibration & K, ATTRIBUTE(mayb
 		vSE3->setEstimate(toSE3Quat(keyframes[i]->getPose().inverse()));
 		const uint32_t &kfId = keyframes[i]->getId();
 		vSE3->setId(kfId);
-		if (m_fixedKeyframes)
+        if (m_fixedKeyframes || keyframes[i]->isFixedPose())
 			vSE3->setFixed(true);
 		else
 			vSE3->setFixed(kfId == 0);
@@ -275,12 +286,17 @@ double SolAROptimizationG2O::bundleAdjustment(CamCalibration & K, ATTRIBUTE(mayb
 				g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
 				e->setRobustKernel(rk);
 				rk->setDelta(thHuber2D);
-			}
-
-			e->fx = K(0, 0);
-			e->fy = K(1, 1);
-			e->cx = K(0, 2);
-			e->cy = K(1, 2);
+			}		
+            SRef<CameraParameters> camParams;
+            if (m_cameraParametersManager->getCameraParameters(kf->getCameraID(), camParams) != FrameworkReturnCode :: _SUCCESS)
+            {
+                LOG_WARNING("Camera parameteres with id {} does not exists in the camera parameters manager", kf->getCameraID());
+                continue;
+            }
+            e->fx = camParams->intrinsic(0, 0);
+            e->fy = camParams->intrinsic(1, 1);
+            e->cx = camParams->intrinsic(0, 2);
+            e->cy = camParams->intrinsic(1, 2);
 
 			optimizer.addEdge(e);
 			nbObservations++;
@@ -368,7 +384,7 @@ double SolAROptimizationG2O::bundleAdjustment(CamCalibration & K, ATTRIBUTE(mayb
 	return totalErr / nbErr;
 }
 
-double SolAROptimizationG2O::optimizeSim3(CamCalibration & K1, CamCalibration & K2, const SRef<Keyframe>& keyframe1, const SRef<Keyframe>& keyframe2, const std::vector<DescriptorMatch>& matches, const std::vector<Point3Df> & pts3D1, const std::vector<Point3Df> & pts3D2, Transform3Df & pose)
+double SolAROptimizationG2O::optimizeSim3(const SRef<Keyframe>& keyframe1, const SRef<Keyframe>& keyframe2, const std::vector<DescriptorMatch>& matches, const std::vector<Point3Df> & pts3D1, const std::vector<Point3Df> & pts3D2, Transform3Df & pose)
 {
 	g2o::SparseOptimizer optimizer;
 	auto linearSolver = std::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>();
@@ -385,19 +401,30 @@ double SolAROptimizationG2O::optimizeSim3(CamCalibration & K1, CamCalibration &
 	translation = sim3_K1_k2.translation() / scale;
 	g2o::Sim3 g2oS12(rot.cast<double>(), translation.cast<double>(), static_cast<double>(scale));
 
+    SRef<CameraParameters> camParamsK1, camParamsK2;
+    if (m_cameraParametersManager->getCameraParameters(keyframe1->getCameraID(), camParamsK1) != FrameworkReturnCode :: _SUCCESS)
+    {
+        LOG_WARNING("Camera parameteres with id {} does not exists in the camera parameters manager", keyframe1->getCameraID());
+        return -1;
+    }
+    if (m_cameraParametersManager->getCameraParameters(keyframe2->getCameraID(), camParamsK2) != FrameworkReturnCode :: _SUCCESS)
+    {
+        LOG_WARNING("Camera parameteres with id {} does not exists in the camera parameters manager", keyframe2->getCameraID());
+        return -1;
+    }
 	g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
 	vSim3->_fix_scale = (bool)m_fixedScale;
 	vSim3->setEstimate(g2oS12);
 	vSim3->setId(0);
 	vSim3->setFixed(false);
-	vSim3->_principle_point1[0] = K1(0, 2);
-	vSim3->_principle_point1[1] = K1(1, 2);
-	vSim3->_focal_length1[0] = K1(0, 0);
-	vSim3->_focal_length1[1] = K1(1, 1);
-	vSim3->_principle_point2[0] = K2(0, 2);
-	vSim3->_principle_point2[1] = K2(1, 2);
-	vSim3->_focal_length2[0] = K2(0, 0);
-	vSim3->_focal_length2[1] = K2(1, 1);
+    vSim3->_principle_point1[0] = camParamsK1->intrinsic(0, 2);
+    vSim3->_principle_point1[1] = camParamsK1->intrinsic(1, 2);
+    vSim3->_focal_length1[0] = camParamsK1->intrinsic(0, 0);
+    vSim3->_focal_length1[1] = camParamsK1->intrinsic(1, 1);
+    vSim3->_principle_point2[0] = camParamsK2->intrinsic(0, 2);
+    vSim3->_principle_point2[1] = camParamsK2->intrinsic(1, 2);
+    vSim3->_focal_length2[0] = camParamsK2->intrinsic(0, 0);
+    vSim3->_focal_length2[1] = camParamsK2->intrinsic(1, 1);
 	optimizer.addVertex(vSim3);
 
 	// Set MapPoint vertices

tests/SolARTest_ModuleG2O_Bundler/SolARTest_ModuleG2O_Bundler.pro

@@ -6,7 +6,7 @@ QMAKE_PROJECT_DEPTH = 0
 
 ## global defintions : target lib name, version
 TARGET = SolARTest_ModuleG2O_Bundler
-VERSION=0.11.0
+VERSION=1.0.0
 PROJECTDEPLOYDIR = $${PWD}/../deploy
 
 DEFINES += MYVERSION=$${VERSION}
@@ -54,10 +54,6 @@ linux {
         LIBS += -L/home/linuxbrew/.linuxbrew/lib # temporary fix caused by grpc with -lre2 ... without -L in grpc.pc
 }
 
-macx {
-    QMAKE_MAC_SDK= macosx
-    QMAKE_CXXFLAGS += -fasm-blocks -x objective-c++
-}
 
 win32 {
     QMAKE_LFLAGS += /MACHINE:X64
@@ -69,10 +65,6 @@ win32 {
     INCLUDEPATH += $$(WINDOWSSDKDIR)lib/winv6.3/um/x64
 }
 
-android {
-    ANDROID_ABIS="arm64-v8a"
-}
-
 linux {
   run_install.path = $${TARGETDEPLOYDIR}
   run_install.files = $${PWD}/../run.sh