diff --git a/.gitignore b/.gitignore index 2505b155..1e82164c 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,7 @@ *.lo *.o *.obj -build/ +*build* # Precompiled Headers *.gch diff --git a/.gitmodules b/.gitmodules index b643edbb..8b137891 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1 @@ -[submodule "SolARWrapper"] - path = SolARWrapper - url = https://github.com/SolarFramework/SwigWrapper.git - branch = . + diff --git a/JenkinsFile b/JenkinsFile index ede97463..5757ff61 100644 --- a/JenkinsFile +++ b/JenkinsFile @@ -1,5 +1,4 @@ SolArModulePipeline { moduleName="SolARFramework" dirName="SolARBuild" - android=true } \ No newline at end of file diff --git a/JenkinsFile_All b/JenkinsFile_All index 5c5ffae5..f8d306dc 100644 --- a/JenkinsFile_All +++ b/JenkinsFile_All @@ -38,16 +38,6 @@ pipeline { buildModule("SolARModuleOpenGL",BRANCH,RELEASE,PRE_RELEASE,REMAKEN_VERSION,QMAKE_RULES_VERSION,SKIP_TESTS,UPLOAD_CONAN_PACKAGE) } } - stage("SolARModuleOpenGV"){ - steps { - buildModule("SolARModuleOpenGV",BRANCH,RELEASE,PRE_RELEASE,REMAKEN_VERSION,QMAKE_RULES_VERSION,SKIP_TESTS,UPLOAD_CONAN_PACKAGE) - } - } - stage("SolARModuleCeres"){ - steps { - buildModule("SolARModuleCeres",BRANCH,RELEASE,PRE_RELEASE,REMAKEN_VERSION,QMAKE_RULES_VERSION,SKIP_TESTS,UPLOAD_CONAN_PACKAGE) - } - } stage("SolARModuleTools"){ steps { buildModule("SolARModuleTools",BRANCH,RELEASE,PRE_RELEASE,REMAKEN_VERSION,QMAKE_RULES_VERSION,SKIP_TESTS,UPLOAD_CONAN_PACKAGE) diff --git a/SolARFramework.pri b/SolARFramework.pri old mode 100755 new mode 100644 index fbb5f42c..ad2a2d59 --- a/SolARFramework.pri +++ b/SolARFramework.pri @@ -1,4 +1,5 @@ HEADERS += interfaces/api/display/I2DOverlay.h \ + $$PWD/interfaces/api/pipeline/IServiceManagerPipeline.h \ interfaces/api/pipeline/IAsyncRelocalizationPipeline.h \ interfaces/api/solver/pose/ITrackablePose.h \ interfaces/api/input/devices/IDepthCamera.h \ @@ -46,12 +47,16 @@ interfaces/api/input/devices/IDevice.h \ interfaces/api/input/devices/IIMU.h \ interfaces/api/input/devices/IRGBDCamera.h \ interfaces/api/input/devices/IStereoCameraCalibration.h \ +interfaces/api/input/files/IMeshLoader.h \ interfaces/api/input/files/IPointCloudLoader.h \ interfaces/api/input/files/ITrackableLoader.h \ interfaces/api/input/files/IWorldGraphLoader.h \ interfaces/api/loop/ILoopClosureDetector.h \ interfaces/api/loop/ILoopCorrector.h \ interfaces/api/loop/IOverlapDetector.h \ +interfaces/api/output/files/IMeshExporter.h \ +interfaces/api/output/files/IPointCloudExporter.h \ +interfaces/api/pipeline/IDenseMappingPipeline.h \ interfaces/api/pipeline/IMappingPipeline.h \ interfaces/api/pipeline/IPipeline.h \ interfaces/api/pipeline/IPoseEstimationPipeline.h \ @@ -62,6 +67,9 @@ interfaces/api/pointCloud/IPCFilterCentroid.h \ interfaces/api/reloc/IKeyframeRetriever.h \ interfaces/api/reloc/IRelocalizer.h \ interfaces/api/reloc/IRegression.h \ +interfaces/api/sfm/IMeshing.h \ +interfaces/api/sfm/IMultiViewStereo.h \ +interfaces/api/sfm/IStructureFromMotion.h \ interfaces/api/sink/ISinkPoseImage.h \ interfaces/api/sink/ISinkPoseTextureBuffer.h \ interfaces/api/sink/ISinkReturnCode.h \ @@ -90,6 +98,7 @@ interfaces/api/source/ISourceImage.h \ interfaces/api/source/ISourceReturnCode.h \ interfaces/api/storage/ICovisibilityGraphManager.h \ interfaces/api/storage/IKeyframesManager.h \ +interfaces/api/storage/ICameraParametersManager.h \ interfaces/api/storage/IPointCloudManager.h \ interfaces/api/storage/IMapManager.h \ interfaces/api/tracking/IOpticalFlowEstimator.h \ @@ -101,6 +110,7 @@ interfaces/core/SolARFramework.h \ interfaces/core/SolARFrameworkDefinitions.h \ interfaces/datastructure/BufferInternal.hpp \ interfaces/datastructure/CameraDefinitions.h \ +interfaces/datastructure/CameraParametersCollection.h \ interfaces/datastructure/CloudPoint.h \ interfaces/datastructure/CoordinateSystem.h \ interfaces/datastructure/DescriptorBuffer.h \ @@ -115,6 +125,7 @@ interfaces/datastructure/ImageMarker.h \ interfaces/datastructure/Keyframe.h \ interfaces/datastructure/Keypoint.h \ interfaces/datastructure/MathDefinitions.h \ +interfaces/datastructure/Mesh.h \ interfaces/datastructure/PointCloud.h \ interfaces/datastructure/PrimitiveInformation.h \ interfaces/datastructure/SquaredBinaryPattern.h \ @@ -134,10 +145,12 @@ interfaces/base/geom/AReprojectionStereo.h \ interfaces/base/pipeline/AMappingPipeline.h \ interfaces/api/segm/IInstanceSegmentation.h \ interfaces/api/segm/ISemanticSegmentation.h \ +interfaces/api/segm/IPanopticSegmentation.h \ interfaces/api/display/IMaskOverlay.h SOURCES += src/core/Log.cpp \ src/core/SolARFramework.cpp \ +src/datastructure/CameraParametersCollection.cpp \ src/datastructure/CloudPoint.cpp \ src/datastructure/CoordinateSystem.cpp \ src/datastructure/DescriptorBuffer.cpp \ @@ -159,6 +172,7 @@ src/datastructure/CovisibilityGraph.cpp \ src/datastructure/KeyframeRetrieval.cpp \ src/datastructure/KeyframeCollection.cpp \ src/datastructure/Map.cpp \ +src/datastructure/Mesh.cpp \ src/base/features/ADescriptorMatcher.cpp \ src/base/features/ADescriptorMatcherGeometric.cpp \ src/base/features/ADescriptorMatcherRegion.cpp \ diff --git a/SolARFramework.pro b/SolARFramework.pro index 249f0c46..1d40e5f1 100755 --- a/SolARFramework.pro +++ b/SolARFramework.pro @@ -10,7 +10,7 @@ QMAKE_PROJECT_DEPTH = 0 INSTALLSUBDIR = SolARBuild TARGET = SolARFramework FRAMEWORK = $$TARGET -VERSION=0.11.0 +VERSION=1.0.0 DEFINES += MYVERSION=$${VERSION} DEFINES += TEMPLATE_LIBRARY @@ -51,15 +51,6 @@ linux { QMAKE_LFLAGS += -ldl } -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 -fPIC#-x objective-c++ - QMAKE_LFLAGS += -mmacosx-version-min=10.7 -v -lstdc++ - LIBS += -lstdc++ -lc -lpthread -} - win32 { DEFINES += WIN64 UNICODE _UNICODE @@ -69,13 +60,6 @@ win32 { QMAKE_CXXFLAGS_RELEASE += /O2 } -android { - # Needed to access android sink for spdlog - DEFINES += __ANDROID__ - ANDROID_ABIS="arm64-v8a" -} - - header_interfaces.path = $${PROJECTDEPLOYDIR}/interfaces/ header_interfaces.files = $$files($${PWD}/interfaces/*.h*) @@ -93,6 +77,8 @@ header_interfaces_input_devices.path = $${PROJECTDEPLOYDIR}/interfaces/api/input header_interfaces_input_devices.files = $$files($${PWD}/interfaces/api/input/devices/*.h*) header_interfaces_input_files.path = $${PROJECTDEPLOYDIR}/interfaces/api/input/files/ header_interfaces_input_files.files = $$files($${PWD}/interfaces/api/input/files/*.h*) +header_interfaces_output_files.path = $${PROJECTDEPLOYDIR}/interfaces/api/output/files/ +header_interfaces_output_files.files = $$files($${PWD}/interfaces/api/output/files/*.h*) header_interfaces_pointCloud.path = $${PROJECTDEPLOYDIR}/interfaces/api/pointCloud/ header_interfaces_pointCloud.files = $$files($${PWD}/interfaces/api/pointCloud/*.h*) header_interfaces_reloc.path = $${PROJECTDEPLOYDIR}/interfaces/api/reloc/ @@ -129,6 +115,8 @@ header_base_pipeline.path = $${PROJECTDEPLOYDIR}/interfaces/base/pipeline/ header_base_pipeline.files += $$files($${PWD}/interfaces/base/pipeline/*.h*) header_interfaces_segm.path = $${PROJECTDEPLOYDIR}/interfaces/api/segm/ header_interfaces_segm.files = $$files($${PWD}/interfaces/api/segm/*.h*) +header_interfaces_sfm.path = $${PROJECTDEPLOYDIR}/interfaces/api/sfm/ +header_interfaces_sfm.files += $$files($${PWD}/interfaces/api/sfm/*.h*) INCLUDEPATH += $${PWD}/interfaces @@ -140,6 +128,7 @@ INSTALLS += header_interfaces_fusion INSTALLS += header_interfaces_geom INSTALLS += header_interfaces_image INSTALLS += header_interfaces_input_devices header_interfaces_input_files +INSTALLS += header_interfaces_output_files INSTALLS += header_interfaces_pointCloud INSTALLS += header_interfaces_reloc INSTALLS += header_interfaces_sink @@ -152,27 +141,20 @@ INSTALLS += header_interfaces_core INSTALLS += header_interfaces_datastructure INSTALLS += header_interfaces_pipeline INSTALLS += header_interfaces_loop +INSTALLS += header_interfaces_segm INSTALLS += header_interfaces_slam +INSTALLS += header_interfaces_sfm INSTALLS += header_base_features INSTALLS += header_base_geom INSTALLS += header_base_pipeline -INSTALLS += header_interfaces_segm + OTHER_FILES += \ packagedependencies.txt \ packagedependencies-win.txt \ packagedependencies-linux.txt \ - packagedependencies-mac.txt \ - packagedependencies-android.txt \ extra-packages.txt \ extra-packages-linux.txt #NOTE : Must be placed at the end of the .pro include ($$shell_quote($$shell_path($${QMAKE_REMAKEN_RULES_ROOT}/remaken_install_target.pri)))) # Shell_quote & shell_path required for visual on windows - -DISTFILES += - -HEADERS += \ - interfaces/api/pipeline/IAsyncRelocalizationPipeline.h - - diff --git a/SolARWrapper b/SolARWrapper deleted file mode 160000 index e970cd72..00000000 --- a/SolARWrapper +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e970cd72bf2492c7f87b2ac7f5c47804c45fbf53 diff --git a/bcom-SolARFramework.pc.in b/bcom-SolARFramework.pc.in index ebe3734d..6deca616 100644 --- a/bcom-SolARFramework.pc.in +++ b/bcom-SolARFramework.pc.in @@ -5,7 +5,7 @@ libdir=${exec_prefix}/lib includedir=${prefix}/interfaces Name: SolARFramework Description: -Version: 0.11.0 +Version: 1.0.0 Requires: Libs: -L${libdir} -l${libname} Libs.private: ${libdir}/${pfx}${libname}.${lext} diff --git a/interfaces/api/display/I3DOverlay.h b/interfaces/api/display/I3DOverlay.h index ea480efa..5ed9aba5 100644 --- a/interfaces/api/display/I3DOverlay.h +++ b/interfaces/api/display/I3DOverlay.h @@ -43,14 +43,14 @@ class [[xpcf::clientUUID("ce48f688-bb48-4d61-800c-e504c0d060a8")]] [[xpcf::serve /// @brief I3DOverlay default destructor virtual ~I3DOverlay() = default; - /// @brief Set the intrinsic parameters and distorsion of the camera - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsic_parameters, const SolAR::datastructure::CamDistortion & distorsion_parameters) = 0; - /// @brief Draw a box on the given Image /// The box is displayed according to the pose given in parameter. The reference of the box is positionned on the center of its bottom face. /// @param[in] Transfomr3Df The pose of the camera from which the box is viewed. + /// @param[in] camParams The camera parameters. /// @param[in,out] displayImage The image on which the box will be drawn - virtual void draw (const SolAR::datastructure::Transform3Df & pose, SRef displayImage) = 0; + virtual void draw (const SolAR::datastructure::Transform3Df & pose, + const SolAR::datastructure::CameraParameters & camParams, + SRef displayImage) = 0; }; } diff --git a/interfaces/api/features/IDescriptorMatcherGeometric.h b/interfaces/api/features/IDescriptorMatcherGeometric.h index 54acbded..9e739cec 100644 --- a/interfaces/api/features/IDescriptorMatcherGeometric.h +++ b/interfaces/api/features/IDescriptorMatcherGeometric.h @@ -56,9 +56,11 @@ class [[xpcf::clientUUID("1bd62a3f-3376-45c3-a980-94d042ae509f")]] [[xpcf::serve /// @param[in] undistortedKeypoints2 The second set of undistorted keypoints. /// @param[in] pose1 The first pose. /// @param[in] pose2 The second pose. - /// @param[in] camParams The intrinsic parameters of the camera. + /// @param[in] camParams1 The intrinsic parameters of the camera 1. + /// @param[in] camParams2 The intrinsic parameters of the camera 2. /// @param[out] matches A vector of matches representing pairs of indices relatively to the first and second set of descriptors. - /// @param[in] mask The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask1 The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask2 The indices of descriptors in the second frame are used for matching to the first frame. If it is empty then all will be used. /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode match(const SRef descriptors1, const SRef descriptors2, @@ -66,22 +68,28 @@ class [[xpcf::clientUUID("1bd62a3f-3376-45c3-a980-94d042ae509f")]] [[xpcf::serve const std::vector &undistortedKeypoints2, const SolAR::datastructure::Transform3Df& pose1, const SolAR::datastructure::Transform3Df& pose2, - const SolAR::datastructure::CameraParameters & camParams, - std::vector & matches, - const std::vector& mask = {}) = 0; + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, + std::vector & matches, + const std::vector& mask1 = {}, + const std::vector& mask2 = {}) = 0; /// @brief Match two sets of descriptors from two frames based on epipolar constraint. /// @param[in] frame1 The first frame containing descriptors and undistorted keypoints. /// @param[in] frame2 The second frame containing descriptors and undistorted keypoints. - /// @param[in] camParams The intrinsic parameters of the camera. + /// @param[in] camParams1 The intrinsic parameters of the camera 1. + /// @param[in] camParams2 The intrinsic parameters of the camera 2. /// @param[out] matches A vector of matches representing pairs of indices relatively to the first and second set of descriptors. - /// @param[in] mask The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask1 The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask2 The indices of descriptors in the second frame are used for matching to the first frame. If it is empty then all will be used. /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode match(const SRef frame1, const SRef frame2, - const SolAR::datastructure::CameraParameters & camParams, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector & matches, - const std::vector& mask = {}) = 0; + const std::vector& mask1 = {}, + const std::vector& mask2 = {}) = 0; }; } } diff --git a/interfaces/api/features/IImageMatcher.h b/interfaces/api/features/IImageMatcher.h index e29422b1..5259e4df 100644 --- a/interfaces/api/features/IImageMatcher.h +++ b/interfaces/api/features/IImageMatcher.h @@ -71,8 +71,8 @@ class [[xpcf::clientUUID("0fcb5bf1-7251-4c7d-a3cc-3da7b4c306f4")]] [[xpcf::serve } // end of namespace SolAR XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::features::IImageMatcher, - "157ec340-0682-4e6c-bf69-e4d95fa760d3", - "IImageMatcher", - "IImageMatcher interface description"); + "157ec340-0682-4e6c-bf69-e4d95fa760d3", + "IImageMatcher", + "IImageMatcher interface description"); #endif // IIMAGEMATCHER_H diff --git a/interfaces/api/features/IMatchesFilter.h b/interfaces/api/features/IMatchesFilter.h index ca1a9d29..bbb903ae 100644 --- a/interfaces/api/features/IMatchesFilter.h +++ b/interfaces/api/features/IMatchesFilter.h @@ -42,14 +42,16 @@ namespace features { /// @param[in] Original keypoints associated to desc_2. /// @param[in] camera pose 1. /// @param[in] camera pose 2. - /// @param[in] camera's intrinsic parameters. + /// @param[in] first camera's intrinsic parameters. + /// @param[in] second camera's intrinsic parameters. virtual void filter(ATTRIBUTE(maybe_unused) const std::vector & inputMatches, ATTRIBUTE(maybe_unused) std::vector & outputMatches, ATTRIBUTE(maybe_unused) const std::vector & inputKeyPoints1, ATTRIBUTE(maybe_unused) const std::vector & inputKeyPoints2, ATTRIBUTE(maybe_unused) const SolAR::datastructure::Transform3Df &pose1, ATTRIBUTE(maybe_unused) const SolAR::datastructure::Transform3Df &pose2, - ATTRIBUTE(maybe_unused) const SolAR::datastructure::CamCalibration &intrinsicParams) {}; + ATTRIBUTE(maybe_unused) const SolAR::datastructure::CamCalibration &intrinsicParams1, + ATTRIBUTE(maybe_unused) const SolAR::datastructure::CamCalibration &intrinsicParams2 = SolAR::datastructure::CamCalibration::Zero()) {}; }; } } diff --git a/interfaces/api/geom/IProject.h b/interfaces/api/geom/IProject.h index 1cadd116..ae8efc3f 100644 --- a/interfaces/api/geom/IProject.h +++ b/interfaces/api/geom/IProject.h @@ -42,28 +42,27 @@ class [[xpcf::clientUUID("64351b8a-7801-4ca9-841f-a4254506abc3")]] [[xpcf::serve /// @brief I3DTransform default destructor virtual ~IProject() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief This method project a set of 3D points in the image plane /// @param[in] inputPoints the set of 3D points to project - /// @param[out] imagePoints the resulting set of 2D points defined in the image coordinate systemn /// @param[in] pose the 3D pose of the camera (a 4x4 float matrix) + /// @param[in] camParams the camera parameters. + /// @param[out] imagePoints the resulting set of 2D points defined in the image coordinate systemn /// @return FrameworkReturnCode::_SUCCESS_ if 3D projection succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode project(const std::vector & inputPoints, - std::vector & imagePoints, - const SolAR::datastructure::Transform3Df& pose = SolAR::datastructure::Transform3Df::Identity()) = 0; + const SolAR::datastructure::Transform3Df& pose, + const SolAR::datastructure::CameraParameters & camParams, + std::vector & imagePoints) = 0; /// @brief This method project a set of 3D cloud points in the image plane /// @param[in] inputPoints the set of 3D cloud points to project - /// @param[out] imagePoints the resulting set of 2D points defined in the image coordinate systemn /// @param[in] pose the 3D pose of the camera (a 4x4 float matrix) + /// @param[in] camParams the camera parameters. + /// @param[out] imagePoints the resulting set of 2D points defined in the image coordinate systemn /// @return FrameworkReturnCode::_SUCCESS_ if 3D projection succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode project(const std::vector> & inputPoints, - std::vector & imagePoints, - const SolAR::datastructure::Transform3Df& pose = SolAR::datastructure::Transform3Df::Identity()) = 0; + const SolAR::datastructure::Transform3Df& pose, + const SolAR::datastructure::CameraParameters & camParams, + std::vector & imagePoints) = 0; }; diff --git a/interfaces/api/geom/IUndistortPoints.h b/interfaces/api/geom/IUndistortPoints.h index b049cc37..9167d999 100644 --- a/interfaces/api/geom/IUndistortPoints.h +++ b/interfaces/api/geom/IUndistortPoints.h @@ -44,23 +44,21 @@ class [[xpcf::clientUUID("9833188d-b7c6-4600-9032-35b0c4119eea")]] [[xpcf::serve /// @brief This method corrects undistortsion to a set of 2D points /// @param[in] inputPoints the set of 2D points to correct + /// @param[in] camParams the camera parameters /// @param[out] outputPoints the undistorted 2D Points /// @return FrameworkReturnCode::_SUCCESS_ if 2D transformation succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode undistort(const std::vector & inputPoints, + const SolAR::datastructure::CameraParameters & camParams, std::vector & outputPoints) = 0; /// @brief This method corrects undistortsion to a set of 2D keypoints /// @param[in] inputKeypoints the set of 2D keypoints to correct + /// @param[in] camParams the camera parameters /// @param[out] outputKeypoints the undistorted 2D keypoints /// @return FrameworkReturnCode::_SUCCESS_ if 2D transformation succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode undistort(const std::vector & inputKeypoints, - std::vector & outputKeypoints) = 0; - - /// @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. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, - const SolAR::datastructure::CamDistortion & distorsionParams) = 0; + const SolAR::datastructure::CameraParameters & camParams, + std::vector & outputKeypoints) = 0; }; diff --git a/interfaces/api/geom/IUnproject.h b/interfaces/api/geom/IUnproject.h index 48728be7..28b2ff38 100644 --- a/interfaces/api/geom/IUnproject.h +++ b/interfaces/api/geom/IUnproject.h @@ -42,28 +42,27 @@ class [[xpcf::clientUUID("7e71a7db-6a67-4c28-ad40-1922370fdc00")]] [[xpcf::serve /// @brief IUnproject default destructor virtual ~IUnproject() = 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. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief This method unproject a set of 2D image points in the 3D world coordinate system /// @param[in] imagePoints the set of 2D points to unproject - /// @param[out] worldPoints a set of world 3D points resulting from the unprojection of the 2D image points /// @param[in] pose the 3D pose of the camera (a 4x4 float matrix) + /// @param[in] camParams the camera parameters + /// @param[out] worldPoints a set of world 3D points resulting from the unprojection of the 2D image points /// @return FrameworkReturnCode::_SUCCESS_ if 3D projection succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode unproject(const std::vector & imagePoints, - std::vector & worldPoints, - const SolAR::datastructure::Transform3Df & pose = SolAR::datastructure::Transform3Df::Identity()) = 0; + const SolAR::datastructure::Transform3Df & pose, + const SolAR::datastructure::CameraParameters & camParams, + std::vector & worldPoints) = 0; /// @brief This method unproject a set of 2D image points in the 3D world coordinate system /// @param[in] imageKeypoints the set of 2D keypoints to unproject - /// @param[out] worldPoints a set of world 3D points resulting from the unprojection of the 2D image points /// @param[in] pose the 3D pose of the camera (a 4x4 float matrix) + /// @param[in] camParams the camera parameters + /// @param[out] worldPoints a set of world 3D points resulting from the unprojection of the 2D image points /// @return FrameworkReturnCode::_SUCCESS_ if 3D projection succeed, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode unproject(const std::vector & imageKeypoints, - std::vector & worldPoints, - const SolAR::datastructure::Transform3Df & pose = SolAR::datastructure::Transform3Df::Identity()) = 0; + const SolAR::datastructure::Transform3Df & pose, + const SolAR::datastructure::CameraParameters & camParams, + std::vector & worldPoints) = 0; }; diff --git a/interfaces/api/input/files/IMeshLoader.h b/interfaces/api/input/files/IMeshLoader.h new file mode 100644 index 00000000..dfc11e5a --- /dev/null +++ b/interfaces/api/input/files/IMeshLoader.h @@ -0,0 +1,57 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_IMESHLOADER_H +#define SOLAR_IMESHLOADER_H + +#include "xpcf/api/IComponentIntrospect.h" +#include "core/Messages.h" +#include "datastructure/Mesh.h" + +namespace SolAR { +namespace api { +namespace input { +namespace files { +/** + * @class IMeshLoader + * @brief Loads a mesh from a file. + * UUID: 315c7cd4-462d-11ed-b878-0242ac120002 + * + */ +class [[xpcf::clientUUID("5580de7a-462d-11ed-b878-0242ac120002")]] [[xpcf::serverUUID("5973d118-462d-11ed-b878-0242ac120002")]] IMeshLoader : + virtual public org::bcom::xpcf::IComponentIntrospect { +public: + IMeshLoader() = default; + virtual ~IMeshLoader() = default; + + /// @brief Load a Mesh from a file + /// @param[out] Mesh: the point cloud to fill + /// @return FrameworkReturnCode::_SUCCESS if load succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode load(SRef & Mesh) = 0; +}; + +} +} +} +} + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::input::files::IMeshLoader, + "315c7cd4-462d-11ed-b878-0242ac120002", + "IMeshLoader", + "Load a mesh from a file"); + + +#endif // SOLAR_IMESHLOADER_H diff --git a/interfaces/api/input/files/IPointCloudLoader.h b/interfaces/api/input/files/IPointCloudLoader.h index 8029216f..5a71abbb 100644 --- a/interfaces/api/input/files/IPointCloudLoader.h +++ b/interfaces/api/input/files/IPointCloudLoader.h @@ -38,10 +38,9 @@ class [[xpcf::clientUUID("dd80a6fa-db2f-4ba7-ba69-5207a68cfb03")]] [[xpcf::serve virtual ~IPointCloudLoader() = default; /// @brief Load a PointCloud from a filepath (.pcd or .ply) - /// @param[in] filepath: the path to the mesh file /// @param[out] pointCloud: the point cloud to fill /// @return FrameworkReturnCode::_SUCCESS if load succeed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode load(const std::string & filepath, SRef & pointCloud) = 0; + virtual FrameworkReturnCode load(SRef & pointCloud) = 0; }; } diff --git a/interfaces/api/loop/ILoopClosureDetector.h b/interfaces/api/loop/ILoopClosureDetector.h index 486307f6..faebe209 100644 --- a/interfaces/api/loop/ILoopClosureDetector.h +++ b/interfaces/api/loop/ILoopClosureDetector.h @@ -42,12 +42,6 @@ class [[xpcf::clientUUID("ee57ff66-30d0-11ec-8d3d-0242ac130003")]] [[xpcf::serve ///@brief ILoopClosureDetector default destructor virtual ~ILoopClosureDetector() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams: Camera calibration matrix parameters. - /// @param[in] distortionParams: Camera distortion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, - const SolAR::datastructure::CamDistortion & distortionParams) = 0; - /// @brief Detect a loop closure from a given keyframe. /// @param[in] queryKeyframe: the query keyframe. /// @param[out] detectedLoopKeyframe: the detected loop keyframe. diff --git a/interfaces/api/loop/ILoopCorrector.h b/interfaces/api/loop/ILoopCorrector.h index d8770a51..aa4af8d0 100644 --- a/interfaces/api/loop/ILoopCorrector.h +++ b/interfaces/api/loop/ILoopCorrector.h @@ -42,12 +42,6 @@ class [[xpcf::clientUUID("51f449f8-c9df-4c3a-ac57-7ca95debfdbc")]] [[xpcf::serve ///@brief ILoopCorrector default destructor virtual ~ILoopCorrector() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams: Camera calibration matrix parameters. - /// @param[in] distortionParams: Camera distortion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, - const SolAR::datastructure::CamDistortion & distortionParams) = 0; - /// @brief corrects a loop of keyframes and their associated point clouds from a loop detection result. /// @param[in] queryKeyframe: the query keyframe. /// @param[in] detectedLoopKeyframe: the detected loop keyframe. diff --git a/interfaces/api/loop/IOverlapDetector.h b/interfaces/api/loop/IOverlapDetector.h index 1e0fd730..d7ba7b0b 100644 --- a/interfaces/api/loop/IOverlapDetector.h +++ b/interfaces/api/loop/IOverlapDetector.h @@ -42,12 +42,6 @@ class [[xpcf::clientUUID("ccc5e7f8-2f9c-4b2f-a7f1-f151a9aa2191")]] [[xpcf::serve ///@brief IOverlapDetector default destructor virtual ~IOverlapDetector() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams: Camera calibration matrix parameters. - /// @param[in] distortionParams: Camera distortion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, - const SolAR::datastructure::CamDistortion & distortionParams) = 0; - /// @brief Detect overlap between two floating maps with different refences. /// @param[in] globalMap global map as reference. /// @param[in] floatingMap floating map as the map to merge. diff --git a/interfaces/api/output/files/IMeshExporter.h b/interfaces/api/output/files/IMeshExporter.h new file mode 100644 index 00000000..5baf6252 --- /dev/null +++ b/interfaces/api/output/files/IMeshExporter.h @@ -0,0 +1,55 @@ +/** + * @copyright Copyright (c) 2022 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_IMESHEXPORTER_H +#define SOLAR_IMESHEXPORTER_H + +#include + +#include "core/Messages.h" +#include "datastructure/Mesh.h" + +namespace SolAR::api::output::files +{ + +/** + * @class IMeshExporter + * @brief Exports a mesh to a file. + * UUID: faf50760-462c-11ed-b878-0242ac120002 + * + */ +class [[xpcf::clientUUID("0e1263d8-462d-11ed-b878-0242ac120002")]] [[xpcf::serverUUID("14cbf040-462d-11ed-b878-0242ac120002")]] IMeshExporter : + virtual public org::bcom::xpcf::IComponentIntrospect { +public: + IMeshExporter() = default; + virtual ~IMeshExporter() = default; + + /// @brief Export a Mesh to a file + /// @param[in] mesh: the mesh to export + /// @return FrameworkReturnCode::_SUCCESS if export succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode exportMesh(const SRef & mesh) = 0; +}; + +} // namespace SolAR::api::output::files + + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::output::files::IMeshExporter, + "faf50760-462c-11ed-b878-0242ac120002", + "IMeshExporter", + "Export a mesh to a file"); + + +#endif // SOLAR_IMESHEXPORTER_H diff --git a/interfaces/api/output/files/IPointCloudExporter.h b/interfaces/api/output/files/IPointCloudExporter.h new file mode 100644 index 00000000..ed4b0afc --- /dev/null +++ b/interfaces/api/output/files/IPointCloudExporter.h @@ -0,0 +1,55 @@ +/** + * @copyright Copyright (c) 2022 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_IPOINTCLOUDEXPORTER_H +#define SOLAR_IPOINTCLOUDEXPORTER_H + +#include + +#include "core/Messages.h" +#include "datastructure/PointCloud.h" + +namespace SolAR::api::output::files +{ + +/** + * @class IPointCloudExporter + * @brief Exports a point cloud to a file. + * UUID: 8e2ec8f9-ed99-473d-84eb-7cb35eff6a1e + * + */ +class [[xpcf::clientUUID("7d924b06-238c-11ed-861d-0242ac120002")]] [[xpcf::serverUUID("858c7124-238c-11ed-861d-0242ac120002")]] IPointCloudExporter : + virtual public org::bcom::xpcf::IComponentIntrospect { +public: + IPointCloudExporter() = default; + virtual ~IPointCloudExporter() = default; + + /// @brief Export a PointCloud to a file + /// @param[in] pointCloud: the point cloud to export + /// @return FrameworkReturnCode::_SUCCESS if export succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode exportPointCloud(const SRef & pointCloud) = 0; +}; + +} // namespace SolAR::api::output::files + + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::output::files::IPointCloudExporter, + "8e2ec8f9-ed99-473d-84eb-7cb35eff6a1e", + "IPointCloudExporter", + "Export a point cloud to a file"); + + +#endif // SOLAR_IPOINTCLOUDEXPORTER_H diff --git a/interfaces/api/pipeline/IAsyncRelocalizationPipeline.h b/interfaces/api/pipeline/IAsyncRelocalizationPipeline.h index ee24ced0..d24a38cd 100644 --- a/interfaces/api/pipeline/IAsyncRelocalizationPipeline.h +++ b/interfaces/api/pipeline/IAsyncRelocalizationPipeline.h @@ -19,11 +19,14 @@ #include "api/pipeline/IPipeline.h" +#include "api/pipeline/IMappingPipeline.h" #include "datastructure/CameraDefinitions.h" #include "datastructure/Image.h" +#include "datastructure/Map.h" +#include "datastructure/MathDefinitions.h" +#include "datastructure/PointCloud.h" #include "xpcf/core/helpers.h" - namespace SolAR { namespace api { namespace pipeline { @@ -64,8 +67,11 @@ typedef enum { * This class provides the interface to define an asynchronous relocalization processing pipeline. */ -class [[xpcf::clientUUID("91a569da-5695-11ec-bf63-0242ac130002")]] [[xpcf::serverUUID("95913e8e-5695-11ec-bf63-0242ac130002")]] IAsyncRelocalizationPipeline : - virtual public IPipeline { +class [[xpcf::clientUUID("91a569da-5695-11ec-bf63-0242ac130002")]] [[xpcf::serverUUID("95913e8e-5695-11ec-bf63-0242ac130002")]] +#ifndef DOXYGEN_SHOULD_SKIP_THIS // Doxygen does not support custom DSL + XPCF_GRPC_CLIENT_RECV_SIZE("-1") XPCF_GRPC_CLIENT_SEND_SIZE("-1") +#endif + IAsyncRelocalizationPipeline : virtual public IPipeline { public: /// @brief IAsyncRelocalizationPipeline default constructor IAsyncRelocalizationPipeline() = default; @@ -74,67 +80,179 @@ class [[xpcf::clientUUID("91a569da-5695-11ec-bf63-0242ac130002")]] [[xpcf::serve virtual ~IAsyncRelocalizationPipeline() = default; using IPipeline::init; + using IPipeline::start; + using IPipeline::stop; + + /// @brief Register a new client and return its UUID to use for future requests + /// @param[out] the UUID for this new client + /// @return FrameworkReturnCode::_SUCCESS if the client is registered with its UUID, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode registerClient(std::string & uuid) = 0; + + /// @brief Unregister a client using its UUID + /// @param[in] the UUID of the client to unregister + /// @return FrameworkReturnCode::_SUCCESS if the client is unregistered, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode unregisterClient(const std::string & uuid) = 0; + + /// @brief Return all current clients UUID + /// @param[out] uuidList: the list of UUID of all clients currently registered + /// @return FrameworkReturnCode::_SUCCESS if the method succeeds, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode getAllClientsUUID(std::vector & uuidList) const = 0; + + /// @brief Initialization of the pipeline + /// @param[in] uuid: UUID of the client + /// @return FrameworkReturnCode::_SUCCESS if the init succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode init(const std::string & uuid) = 0; /// @brief Init the pipeline and specify the mode for the pipeline processing + /// @param[in] uuid: UUID of the client /// @param[in] pipelineMode: mode to use for pipeline processing /// @return FrameworkReturnCode::_SUCCESS if the mode is correctly initialized, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode init(PipelineMode pipelineMode) = 0; + virtual FrameworkReturnCode init(const std::string & uuid, PipelineMode pipelineMode) = 0; + + /// @brief Start the pipeline + /// @param[in] uuid: UUID of the client + /// @return FrameworkReturnCode::_SUCCESS if the stard succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode start(const std::string & uuid) = 0; + + /// @brief Stop the pipeline. + /// @param[in] uuid: UUID of the client + /// @return FrameworkReturnCode::_SUCCESS if the stop succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode stop(const std::string & uuid) = 0; /// @brief Return the current mode used for the pipeline processing - /// @return current mode - PipelineMode getProcessingMode() const { - return m_PipelineMode; - } + /// @param[in] uuid: UUID of the client + /// @param[out] pipelineMode: the current pipeline mode + /// @return FrameworkReturnCode::_SUCCESS if the method succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode getProcessingMode(const std::string & uuid, PipelineMode & pipelineMode) const = 0; /// @brief Set the camera parameters - /// @param[in] cameraParams: the camera parameters (its resolution and its focal) + /// @param[in] uuid: UUID of the client + /// @param[in] cameraParams the camera parameters (its resolution and its focal) + /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setCameraParameters(const std::string & uuid, + const SolAR::datastructure::CameraParameters & cameraParams) = 0; + + /// @brief Set the camera parameters (use for stereo camera) + /// @param[in] uuid: UUID of the client + /// @param[in] cameraParams1 the camera parameters of the first camera + /// @param[in] cameraParams2 the camera parameters of the second camera /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams) = 0; + virtual FrameworkReturnCode setCameraParameters(const std::string & uuid, + const SolAR::datastructure::CameraParameters & cameraParams1, + const SolAR::datastructure::CameraParameters & cameraParams2) = 0; + + /// @brief Set the rectification parameters (use for stereo camera) + /// @param[in] uuid: UUID of the client + /// @param[in] rectCam1 the rectification parameters of the first camera + /// @param[in] rectCam2 the rectification parameters of the second camera + /// @return FrameworkReturnCode::_SUCCESS if the rectification parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setRectificationParameters(const std::string & uuid, + const SolAR::datastructure::RectificationParameters & rectCam1, + const SolAR::datastructure::RectificationParameters & rectCam2) = 0; /// @brief Get the camera parameters - /// @param[out] cameraParams: the camera parameters (its resolution and its focal) + /// @param[in] uuid: UUID of the client + /// @param[out] cameraParams the camera parameters (its resolution and its focal) /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly returned, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode getCameraParameters(SolAR::datastructure::CameraParameters & cameraParams) const = 0; + virtual FrameworkReturnCode getCameraParameters(const std::string & uuid, + SolAR::datastructure::CameraParameters & cameraParams) const = 0; /// @brief Request the asynchronous relocalization pipeline to process a new image to calculate /// the corresponding 3D transformation to the SolAR coordinates system - /// @param[in] image: the image to process - /// @param[in] pose: the original pose in the client coordinates system - /// @param[in] timestamp: the timestamp of the image - /// @param[out] transform3DStatus: the status of the current 3D transformation matrix - /// @param[out] transform3D: the current 3D transformation matrix (if available) - /// @param[out] confidence: the confidence score of the 3D transformation matrix + /// @param[in] uuid: UUID of the client + /// @param[in] images the images to process + /// @param[in] poses the poses associated to images in the client coordinates system + /// @param[in] timestamp the timestamp of the image + /// @param[out] transform3DStatus the status of the current 3D transformation matrix + /// @param[out] transform3D the current 3D transformation matrix (if available) + /// @param[out] confidence the confidence score of the 3D transformation matrix + /// @param[out] mappingStatus the status of the current mapping processing /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode relocalizeProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + FrameworkReturnCode relocalizeProcessRequest(const std::string & uuid, + const std::vector> & images, + const std::vector & poses, + const std::chrono::system_clock::time_point & timestamp, + TransformStatus & transform3DStatus, + SolAR::datastructure::Transform3Df & transform3D, + float_t & confidence, + MappingStatus & mappingStatus) + { + SolAR::datastructure::Transform3Df worldTransform(SolAR::datastructure::Maths::Matrix4f::Zero()); + return relocalizeProcessRequest(uuid, + images, + poses, + /* fixedPose = */ false, + worldTransform, + timestamp, + transform3DStatus, + transform3D, + confidence, + mappingStatus); + } + + /// @brief Request the asynchronous relocalization pipeline to process a new image to calculate + /// the corresponding 3D transformation to the SolAR coordinates system + /// @param[in] uuid: UUID of the client + /// @param[in] images the images to process + /// @param[in] poses the poses associated to images in the client coordinates system + /// @param[in] fixedPose the input poses are considered as ground truth + /// @param[in] worldTransform SolAR (ex: marker) to World origin (ex: BIM origin). Pass zero-filled matrix if not set. + /// @param[in] timestamp the timestamp of the image + /// @param[out] transform3DStatus the status of the current 3D transformation matrix + /// @param[out] transform3D the current 3D transformation matrix (if available) + /// @param[out] confidence the confidence score of the 3D transformation matrix + /// @param[out] mappingStatus the status of the current mapping processing + /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode relocalizeProcessRequest(const std::string & uuid, + const std::vector> & images, + const std::vector & poses, + bool fixedPose, + const SolAR::datastructure::Transform3Df & worldTransform, const std::chrono::system_clock::time_point & timestamp, TransformStatus & transform3DStatus, SolAR::datastructure::Transform3Df & transform3D, - float_t & confidence) = 0; + float_t & confidence, + MappingStatus & mappingStatus) = 0; /// @brief Request the asynchronous relocalization pipeline to get the 3D transform offset /// between the device coordinate system and the SolAR coordinate system - /// @param[out] transform3DStatus: the status of the current 3D transformation matrix - /// @param[out] transform3D : the current 3D transformation matrix (if available) - /// @param[out] confidence: the confidence score of the 3D transformation matrix + /// @param[in] uuid: UUID of the client + /// @param[out] transform3DStatus the status of the current 3D transformation matrix + /// @param[out] transform3D the current 3D transformation matrix (if available) + /// @param[out] confidence the confidence score of the 3D transformation matrix /// @return FrameworkReturnCode::_SUCCESS if the 3D transform is available, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode get3DTransformRequest(TransformStatus & transform3DStatus, + virtual FrameworkReturnCode get3DTransformRequest(const std::string & uuid, + TransformStatus & transform3DStatus, SolAR::datastructure::Transform3Df & transform3D, float_t & confidence) = 0; /// @brief Return the last pose processed by the pipeline - /// @param[out] pose: the last pose if available - /// @param[in] poseType: the type of the requested pose + /// @param[in] uuid: UUID of the client + /// @param[out] pose the last pose if available + /// @param[in] poseType the type of the requested pose /// - in the SolAR coordinate system (by default) /// - in the device coordinate system /// @return FrameworkReturnCode::_SUCCESS if the last pose is available, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode getLastPose(SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode getLastPose(const std::string & uuid, + SolAR::datastructure::Transform3Df & pose, const PoseType poseType = SOLAR_POSE) const = 0; + /// @brief Request the global map stored by the map update pipeline + /// @param[out] map: the output global map + /// @return FrameworkReturnCode::_SUCCESS if the global map is available, else FrameworkReturnCode::_ERROR_ + [[grpc::client_receiveSize("-1")]] virtual FrameworkReturnCode getMapRequest( + SRef & map) const = 0; + /// @brief Reset the map stored by the map update pipeline /// @return FrameworkReturnCode::_SUCCESS if the map is correctly reset, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode resetMap() const = 0; + /// @brief Request the point cloud of the global map stored by the map update pipeline + /// @param[out] pointCloud: the output point cloud + /// @return FrameworkReturnCode::_SUCCESS if the point cloud is available, else FrameworkReturnCode::_ERROR_ + [[grpc::client_receiveSize("-1")]] virtual FrameworkReturnCode getPointCloudRequest( + SRef & pointCloud) const = 0; + protected: /// @brief Mode to use for the pipeline processing (Relocalization and Mapping by default) PipelineMode m_PipelineMode = RELOCALIZATION_AND_MAPPING; diff --git a/interfaces/api/pipeline/IDenseMappingPipeline.h b/interfaces/api/pipeline/IDenseMappingPipeline.h new file mode 100644 index 00000000..a768c0f8 --- /dev/null +++ b/interfaces/api/pipeline/IDenseMappingPipeline.h @@ -0,0 +1,90 @@ +/** + * @copyright Copyright (c) 2020 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_DENSEMAPPINGPIPELINE_H +#define SOLAR_DENSEMAPPINGPIPELINE_H + + +#include "api/pipeline/IPipeline.h" +#include "datastructure/Map.h" +#include "datastructure/Mesh.h" +#include "xpcf/core/helpers.h" + + +namespace SolAR { +namespace api { +namespace pipeline { + +/// +/// @typedef DenseMappingStatus +/// @brief Indicate the status of the dense mapping pipeline +/// +typedef enum { + MVS = 0, // Multiview Stereo step to densify points + MESHING = 1, // meshing step to create a mesh from the dense point cloud + DONE = 2 // dense reconstruction is done +} DenseMappingStatus; + +/** + * @class IDenseMappingPipeline + * @brief Defines pipeline to create a dense point cloud or a mesh from a sparse map. + * UUID: 34ab0434-480b-11ed-b878-0242ac120002 + * + * This class provides the interface to define a dense mapping processing pipeline. + */ + +class [[xpcf::clientUUID("54fc1ed0-480b-11ed-b878-0242ac120002")]] [[xpcf::serverUUID("5ac9298e-480b-11ed-b878-0242ac120002")]] +#ifndef DOXYGEN_SHOULD_SKIP_THIS // Doxygen does not support custom DSL + XPCF_GRPC_CLIENT_RECV_SIZE("-1") XPCF_GRPC_CLIENT_SEND_SIZE("-1") +#endif + IDenseMappingPipeline : virtual public IPipeline { +public: + /// @brief IDenseMappingPipeline default constructor + IDenseMappingPipeline() = default; + + /// @brief IDenseMappingPipeline default destructor + virtual ~IDenseMappingPipeline() = default; + + /// @brief Request to the dense mapping pipeline to process a sparse map + /// @param[in] sparseMap: the sparse map to use to create a dense map + /// @param[in] createMesh: true to create a mesh, otherwise, just a dense point cloud is created + /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ + [[grpc::client_sendSize("-1")]] virtual FrameworkReturnCode denseMappingProcessRequest(const SRef & sparseMap, + const bool createMesh) = 0; + + /// @brief Provide the dense pottn cloud resulting from the dense mapping of the sparse map + /// @param[out] outputPointCloud: the resulting dense point cloud + /// @param[out] status: the current status of the dense mapping pipeline + /// @return FrameworkReturnCode::_SUCCESS if data are available, else FrameworkReturnCode::_ERROR_ + [[grpc::client_receiveSize("-1")]] virtual FrameworkReturnCode getPointCloud(SRef & outputPointCloud, DenseMappingStatus & status) const = 0; + + /// @brief Provide the mesh resulting from the dense mapping of the sparse map + /// @param[out] outputMesh: the resulting mesh + /// @param[out] status: the current status of the dense mapping pipeline + /// @return FrameworkReturnCode::_SUCCESS if data are available, else FrameworkReturnCode::_ERROR_ + [[grpc::client_receiveSize("-1")]] virtual FrameworkReturnCode getMesh(SRef& outputMesh, DenseMappingStatus & status) const = 0; + +}; +} +} +} + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::pipeline::IDenseMappingPipeline, + "34ab0434-480b-11ed-b878-0242ac120002", + "IDenseMappingPipeline", + "The interface to define a dense mapping processing pipeline") + +#endif // SOLAR_DENSEMAPPINGPIPELINE_H diff --git a/interfaces/api/pipeline/IMapUpdatePipeline.h b/interfaces/api/pipeline/IMapUpdatePipeline.h index 98fcbdb3..2c9a5ce4 100644 --- a/interfaces/api/pipeline/IMapUpdatePipeline.h +++ b/interfaces/api/pipeline/IMapUpdatePipeline.h @@ -47,7 +47,7 @@ class [[xpcf::clientUUID("d9da863c-c9ff-4562-a3a2-329ac1f44008")]] [[xpcf::serve /// @brief IMapUpdatePipeline default destructor virtual ~IMapUpdatePipeline() = default; - /// @brief Set the camera parameters + /// @brief Set the camera parameters /// @param[in] cameraParams: the camera parameters (its resolution and its focal) /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams) = 0; @@ -72,6 +72,12 @@ class [[xpcf::clientUUID("d9da863c-c9ff-4562-a3a2-329ac1f44008")]] [[xpcf::serve /// @brief Reset the map stored by the map update pipeline /// @return FrameworkReturnCode::_SUCCESS if the map is correctly reset, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode resetMap() = 0; + + /// @brief Request to the map update pipeline to get the point cloud of the global map + /// @param[out] pointCloud: the output point cloud + /// @return FrameworkReturnCode::_SUCCESS if the point cloud is available, else FrameworkReturnCode::_ERROR_ + [[grpc::client_receiveSize("-1")]] virtual FrameworkReturnCode getPointCloudRequest(SRef & pointCloud) const = 0; + }; } } diff --git a/interfaces/api/pipeline/IMappingPipeline.h b/interfaces/api/pipeline/IMappingPipeline.h index 9a6c9b81..fe6c3333 100644 --- a/interfaces/api/pipeline/IMappingPipeline.h +++ b/interfaces/api/pipeline/IMappingPipeline.h @@ -61,41 +61,73 @@ class [[xpcf::clientUUID("110a089c-0bb1-488e-b24b-c1b96bc9df3b")]] [[xpcf::serve /// @brief IMappingPipeline default destructor virtual ~IMappingPipeline() = default; + using IPipeline::init; + + /// @brief Initialization of the pipeline with the URL of an available Relocalization Service + /// @param[in] relocalizationServiceURL the URL of an available Relocalization Service + /// @return FrameworkReturnCode::_SUCCESS if the init succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode init(const std::string relocalizationServiceURL) = 0; + /// @brief Set the camera parameters - /// @param[in] cameraParams: the camera parameters (its resolution and its focal) + /// @param[in] cameraParams the camera parameters (its resolution and its focal) /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams) = 0; + /// @brief Set the camera parameters (use for stereo camera) + /// @param[in] cameraParams1 the camera parameters of the first camera + /// @param[in] cameraParams2 the camera parameters of the second camera + /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams1, + const SolAR::datastructure::CameraParameters & cameraParams2) = 0; + + /// @brief Set the rectification parameters (use for stereo camera) + /// @param[in] rectCam1 the rectification parameters of the first camera + /// @param[in] rectCam2 the rectification parameters of the second camera + /// @return FrameworkReturnCode::_SUCCESS if the rectification parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setRectificationParameters(const SolAR::datastructure::RectificationParameters & rectCam1, + const SolAR::datastructure::RectificationParameters & rectCam2) = 0; + + /// @brief Set the 3D transformation from SolAR to world spaces + /// @param[in] transform the transformation matrix from SolAR to World + /// @return FrameworkReturnCode::_SUCCESS if the transform is correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode set3DTransformSolARToWorld(const SolAR::datastructure::Transform3Df & transform) = 0; + /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the device coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the device coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[in] transform the transformation matrix from the device coordinate system to the world coordinate system /// @param[out] updatedTransform the refined transformation by a loop closure detection /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, const SolAR::datastructure::Transform3Df & transform, SolAR::datastructure::Transform3Df & updatedTransform, MappingStatus & status) = 0; /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the world coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the world coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, MappingStatus & status) = 0; /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the world coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the world coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[out] updatedTransform the refined transformation by a loop closure detection /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, SolAR::datastructure::Transform3Df & updatedTransform, MappingStatus & status) = 0; diff --git a/interfaces/api/pipeline/IRelocalizationPipeline.h b/interfaces/api/pipeline/IRelocalizationPipeline.h index a9fd88bf..03f0dd6e 100644 --- a/interfaces/api/pipeline/IRelocalizationPipeline.h +++ b/interfaces/api/pipeline/IRelocalizationPipeline.h @@ -63,9 +63,10 @@ class [[xpcf::clientUUID("597d510d-452a-4da2-9c3a-8d4b8d15c584")]] [[xpcf::serve /// @param[in] image: the image to process /// @param[out] pose: the new calculated pose /// @param[out] confidence: the confidence score + /// @param[in] poseCoarse: (optional) coarse pose which needs to be refined by reloc, by default, poseCoarse equals identity matrix meaning that no coarse pose is provided /// @return FrameworkReturnCode::_SUCCESS if the processing is successful, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode relocalizeProcessRequest(const SRef image, - SolAR::datastructure::Transform3Df& pose, float_t & confidence) = 0; + SolAR::datastructure::Transform3Df& pose, float_t & confidence, const SolAR::datastructure::Transform3Df& poseCoarse = SolAR::datastructure::Transform3Df::Identity()) = 0; /// @brief Request to the relocalization pipeline to get the map /// @param[out] map the output map diff --git a/interfaces/api/pipeline/IServiceManagerPipeline.h b/interfaces/api/pipeline/IServiceManagerPipeline.h new file mode 100644 index 00000000..5089fec9 --- /dev/null +++ b/interfaces/api/pipeline/IServiceManagerPipeline.h @@ -0,0 +1,107 @@ +/** + * @copyright Copyright (c) 2022 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_SERVICEMANAGERPIPELINE_H +#define SOLAR_SERVICEMANAGERPIPELINE_H + +#include "api/pipeline/IPipeline.h" +#include "xpcf/core/helpers.h" + +namespace SolAR { +namespace api { +namespace pipeline { + +/// +/// @typedef ServiceType +/// @brief Indicate the type of the service +/// +enum class ServiceType { + MAP_UPDATE_SERVICE = 0, + RELOCALIZATION_SERVICE = 1, + RELOCALIZATION_MARKERS_SERVICE = 2, + MAPPING_SERVICE = 3, + MAPPING_STEREO_SERVICE = 4 +}; + +/** + * @class IServiceManagerPipeline + * @brief Defines the service manager interface. + * UUID: 48c83eda-6432-11ed-81ce-0242ac120002 + * + * This class provides the interface to define a manager for all SolAR remote services. + */ + +class [[xpcf::clientUUID("600ca5f4-6432-11ed-81ce-0242ac120002")]] [[xpcf::serverUUID("65c3a736-6432-11ed-81ce-0242ac120002")]] IServiceManagerPipeline : + virtual public IPipeline { +public: + /// @brief IServiceManagerPipeline default constructor + IServiceManagerPipeline() = default; + + /// @brief IServiceManagerPipeline default destructor + virtual ~IServiceManagerPipeline() = default; + + /// @brief Register a new service to the service manager + /// @param[in] serviceType: type of the service + /// @param[in] serviceURL: URL of the service + /// @return FrameworkReturnCode::_SUCCESS if the service is registered, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode registerService(const ServiceType serviceType, const std::string & serviceURL) = 0; + + /// @brief Unregister a service from the service manager + /// @param[in] serviceType: type of the service + /// @param[in] serviceURL: URL of the service + /// @return FrameworkReturnCode::_SUCCESS if the service is unregistered, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode unregisterService(const ServiceType serviceType, const std::string & serviceURL) = 0; + + /// @brief Get an available URL for a specific service type + /// @param[in] serviceType: type of the service + /// @param[out] serviceURL: URL of the service + /// @return FrameworkReturnCode::_SUCCESS if a service URL is available, else + /// FrameworkReturnCode::_NO_SERVICE_REGISTERED if no service of the given type is registered + /// FrameworkReturnCode::_NO_SERVICE_AVAILABLE if no service of the given type is available + /// FrameworkReturnCode::_ERROR_ for other errors + virtual FrameworkReturnCode getService(const ServiceType serviceType, std::string & serviceURL) const = 0; + + /// @brief Get an available URL for a specific service type, and lock it for the given client UUID + /// If a service of the specific type is already locked for the given client UUID, return its URL + /// @param[in] serviceType: type of the service + /// @param[in] clientUUID: UUID of the client + /// @param[out] serviceURL: URL of the service + /// @return FrameworkReturnCode::_SUCCESS if a service URL is available and the service locked, else + /// FrameworkReturnCode::_NO_SERVICE_REGISTERED if no service of the given type is registered + /// FrameworkReturnCode::_NO_SERVICE_AVAILABLE if no service of the given type is available + /// FrameworkReturnCode::_ERROR_ for other errors + virtual FrameworkReturnCode getAndLockService(const ServiceType serviceType, const std::string & clientUUID, + std::string & serviceURL) = 0; + + /// @brief Unlock the service of the given type, for the given client UUID + /// @param[in] serviceType: type of the service + /// @param[in] clientUUID: UUID of the client + /// @return FrameworkReturnCode::_SUCCESS if the service is unlocked, else + /// FrameworkReturnCode::_NO_SERVICE_LOCKED if no service is locked for the client UUID + /// FrameworkReturnCode::_ERROR_ for other errors + virtual FrameworkReturnCode unlockService(const ServiceType serviceType, const std::string & clientUUID) = 0; +}; + +} // pipeline +} // api +} // SolAR + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::pipeline::IServiceManagerPipeline, + "48c83eda-6432-11ed-81ce-0242ac120002", + "IServiceManagerPipeline", + "The interface to define the service manager") + +#endif // SOLAR_SERVICEMANAGERPIPELINE_H diff --git a/interfaces/api/reloc/IKeyframeRetriever.h b/interfaces/api/reloc/IKeyframeRetriever.h index 243187d1..fd5d0a18 100644 --- a/interfaces/api/reloc/IKeyframeRetriever.h +++ b/interfaces/api/reloc/IKeyframeRetriever.h @@ -48,8 +48,9 @@ class XPCF_IGNORE IKeyframeRetriever : /// @brief Add a keyframe to the retrieval model /// @param[in] keyframe: the keyframe to add to the retrieval model + /// @param[in] useMatchedDescriptor: if true bow feature will be computed merely from descriptors which are matched to other frames, by default is set to false meaning that all descriptors will be used /// @return FrameworkReturnCode::_SUCCESS if the keyfram adding succeed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode addKeyframe(const SRef keyframe) = 0; + virtual FrameworkReturnCode addKeyframe(const SRef keyframe, bool useMatchedDescriptor=false) = 0; /// @brief Suppress a keyframe from the retrieval model /// @param[in] keyframe_id: the keyframe to supress from the retrieval model @@ -115,6 +116,9 @@ class XPCF_IGNORE IKeyframeRetriever : /// @brief This method is to set the keyframe retrieval /// @param[in] keyframeRetrieval the keyframe retrieval of map virtual void setKeyframeRetrieval(const SRef keyframeRetrieval) = 0; + + /// @brief This method is to reset the keyframe retrieval + virtual void resetKeyframeRetrieval() = 0; }; } diff --git a/interfaces/api/segm/IPanopticSegmentation.h b/interfaces/api/segm/IPanopticSegmentation.h new file mode 100644 index 00000000..1d45fe74 --- /dev/null +++ b/interfaces/api/segm/IPanopticSegmentation.h @@ -0,0 +1,67 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IPANOPTICSEGMENTATION_H +#define IPANOPTICSEGMENTATION_H + +#include "datastructure/Image.h" +#include "datastructure/GeometryDefinitions.h" +#include "core/Messages.h" + +namespace SolAR { +namespace api { +namespace segm { + + +/** + * @class IPanopticSegmentation + * @brief Perform 2D Panoptic segmentation. + * UUID: 60701300-75e2-4215-a5e1-3a2b0aa3193a + */ + +class [[xpcf::clientUUID("26e61305-b483-487a-8701-4a9612af5db6")]] [[xpcf::serverUUID("5c3e0d59-101c-40f8-8efe-0190c4816d81")]] IPanopticSegmentation : + virtual public org::bcom::xpcf::IComponentIntrospect { +public: + ///@brief IPanopticSegmentation default constructor. + IPanopticSegmentation() = default; + + ///@brief IPanopticSegmentation default destructor. + virtual ~IPanopticSegmentation() = default; + + /// @brief Perform 2D Panoptic segmentation + /// @param[in] image The input image. + /// @param[out] labelMap The label map corresponding to the input image. + /// @param[out] boxes The bounding boxes of each detected object. + /// @param[out] classObjectIds The class id and object id of each bounding box. + /// @param[out] scores The corresponding confidence scores. + /// @return FrameworkReturnCode::_SUCCESS if the segmentation succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode segment(const SRef image, + SRef &labelMap, + std::vector &boxes, + std::vector> &classObjectIds, + std::vector &scores) = 0; +}; + +} +} +} // end of namespace SolAR + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::segm::IPanopticSegmentation, + "60701300-75e2-4215-a5e1-3a2b0aa3193a", + "IPanopticSegmentation", + "Interface SolAR::api::segm::IPanopticSegmentation"); + +#endif // IPANOPTICSEGMENTATION_H diff --git a/interfaces/api/sfm/IMeshing.h b/interfaces/api/sfm/IMeshing.h new file mode 100644 index 00000000..98482f95 --- /dev/null +++ b/interfaces/api/sfm/IMeshing.h @@ -0,0 +1,62 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IMESHING_H +#define IMESHING_H + + +#include "xpcf/api/IComponentIntrospect.h" +#include "datastructure/Map.h" +#include "datastructure/Mesh.h" + +namespace SolAR { +namespace api { +namespace sfm { + +/** + * @class IMeshing + * @brief Create a 3D mesh from a 3D dense map. + * UUID: 7d810e96-fd9d-4029-a102-61fe3883a633 + */ + +class XPCF_IGNORE IMeshing : virtual public org::bcom::xpcf::IComponentIntrospect +{ +public: + ///@brief IStructureFromMotion default constructor. + IMeshing() = default; + + ///@brief IMeshing default destructor. + virtual ~IMeshing() override = default; + + /// @brief Create mesh from a dense 3D point cloud + /// @param[in] densePointCloud: the dense poitn cloud to mesh with triangles + /// @param[out] mesh: the resulting meshes + /// @return FrameworkReturnCode::_SUCCESS if the meshing succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode createMesh(const SRef& densePointCloud, + SRef& mesh) = 0; +}; + + +} // namespace sfm +} // namespace api +} // namespace SolAR + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::sfm::IMeshing, + "7d810e96-fd9d-4029-a102-61fe3883a633", + "IMeshing", + "IMeshing interface description"); + +#endif // IMESHING_H diff --git a/interfaces/api/sfm/IMultiViewStereo.h b/interfaces/api/sfm/IMultiViewStereo.h new file mode 100644 index 00000000..b76ae24c --- /dev/null +++ b/interfaces/api/sfm/IMultiViewStereo.h @@ -0,0 +1,53 @@ +#ifndef IMULTIVIEWSTEREO_H +#define IMULTIVIEWSTEREO_H + +#include "xpcf/api/IComponentIntrospect.h" +#include "core/Messages.h" +#include "datastructure/MathDefinitions.h" +#include "datastructure/GeometryDefinitions.h" +#include "datastructure/Image.h" +#include "datastructure/DescriptorBuffer.h" +#include "datastructure/Keypoint.h" +#include "datastructure/Map.h" + +namespace SolAR { +namespace api { +namespace sfm { + +/** + * @class IMultiViewStereo + * @brief Create a dense point cloud from images with corresponding poses. + * UUID: d73ae23f-e1ce-4abe-91cd-9aa2f34c8dff + * + */ + +class XPCF_IGNORE IMultiViewStereo : virtual public org::bcom::xpcf::IComponentIntrospect +{ +public: + ///@brief IStructureFromMotion default constructor. + IMultiViewStereo() = default; + + ///@brief IStructureFromMotion default destructor. + virtual ~IMultiViewStereo() override = default; + + + + /// @brief Create dense point cloud + /// @param[in] map: the sparse map to densify + /// @param[in] densePointCloud: the dense point cloud resulting from the densification of the sparse map + /// @return FrameworkReturnCode::_SUCCESS if the keyfram adding succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode createDensePointCloud(const SRef& map, + SRef& densePointCloud) = 0; //last argument for test +}; + + +} // namespace mvs +} // namespace api +} // namespace SolAR + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::sfm::IMultiViewStereo, + "d73ae23f-e1ce-4abe-91cd-9aa2f34c8dff", + "IMultiViewStereo", + "IMultiViewStereo interface description"); + +#endif // IMULTIVIEWSTEREO_H diff --git a/interfaces/api/sfm/IStructureFromMotion.h b/interfaces/api/sfm/IStructureFromMotion.h new file mode 100644 index 00000000..5e138390 --- /dev/null +++ b/interfaces/api/sfm/IStructureFromMotion.h @@ -0,0 +1,67 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ISTRUCTUREFROMMOTION_H +#define ISTRUCTUREFROMMOTION_H + + +#include "xpcf/api/IComponentIntrospect.h" +#include "core/Messages.h" +#include "datastructure/MathDefinitions.h" +#include "datastructure/GeometryDefinitions.h" +#include "datastructure/Image.h" +#include "datastructure/DescriptorBuffer.h" +#include "datastructure/Keypoint.h" +#include "datastructure/Map.h" + +namespace SolAR { +namespace api { +namespace sfm { + +/** + * @class IStructureFromMotion + * @brief Create a sparse point cloud and estimate camera poses from a set of images. + * UUID: 3681e09b-1704-4a08-b1cd-42d5a7c961b4 + * + */ + +class XPCF_IGNORE IStructureFromMotion : virtual public org::bcom::xpcf::IComponentIntrospect +{ +public: + ///@brief IStructureFromMotion default constructor. + IStructureFromMotion() = default; + + ///@brief IStructureFromMotion default destructor. + virtual ~IStructureFromMotion() override = default; + + /// @brief Create map + /// @param[in] keyframe: the keyframe to add to the bag of words + /// @return FrameworkReturnCode::_SUCCESS if the keyfram adding succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode createMap(std::vector>& images, + SRef& map) = 0; //last argument for test +}; + + +} // namespace sfm +} // namespace api +} // namespace SolAR + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::sfm::IStructureFromMotion, + "3681e09b-1704-4a08-b1cd-42d5a7c961b4", + "IStructureFromMotion", + "IStructureFromMotion interface description"); + +#endif // ISTRUCTUREFROMMOTION_H diff --git a/interfaces/api/slam/IBootstrapper.h b/interfaces/api/slam/IBootstrapper.h index 0e460675..b0190938 100644 --- a/interfaces/api/slam/IBootstrapper.h +++ b/interfaces/api/slam/IBootstrapper.h @@ -45,12 +45,6 @@ class [[xpcf::clientUUID("d593b615-efcf-4b4c-82eb-148065f85008")]] [[xpcf::serve /// @brief IBootstrapper default destructor virtual ~IBootstrapper() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, - const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief This method uses images to boostrap mapping /// @param[in] frame input image to process /// @param[out] view output image to visualize diff --git a/interfaces/api/slam/IMapping.h b/interfaces/api/slam/IMapping.h index 64693f0c..c67c783e 100644 --- a/interfaces/api/slam/IMapping.h +++ b/interfaces/api/slam/IMapping.h @@ -44,10 +44,6 @@ class [[xpcf::clientUUID("9edd1642-4b42-4c08-a11f-e46839f7fd63")]] [[xpcf::serve /// @brief IMapping default destructor virtual ~IMapping() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] camParams Camera parameters. - virtual void setCameraParameters(const SolAR::datastructure::CameraParameters & camParams) = 0; - /// @brief check the mapping process is idle /// @return true if the mapping process is idle, else false virtual bool idle() = 0; diff --git a/interfaces/api/slam/ITracking.h b/interfaces/api/slam/ITracking.h index a44c84e9..728b0876 100644 --- a/interfaces/api/slam/ITracking.h +++ b/interfaces/api/slam/ITracking.h @@ -44,11 +44,6 @@ class [[xpcf::clientUUID("064ae968-4fc7-448b-a485-468a112e4fa3")]] [[xpcf::serve /// @brief IBootstrapper default destructor virtual ~ITracking() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams: camera calibration matrix parameters. - /// @param[in] distorsionParams: camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief this method is used to set new keyframe for tracking process /// @param[in] newKeyframme the new keyframe virtual void setNewKeyframe(const SRef newKeyframe) = 0; diff --git a/interfaces/api/solver/map/IBundler.h b/interfaces/api/solver/map/IBundler.h index 80294d97..5ea7d253 100644 --- a/interfaces/api/solver/map/IBundler.h +++ b/interfaces/api/solver/map/IBundler.h @@ -42,27 +42,19 @@ class [[xpcf::clientUUID("258dfecc-850e-4ec7-9b25-77c5bca8694a")]] [[xpcf::serve /// @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. + /// @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. - virtual double bundleAdjustment(SolAR::datastructure::CamCalibration & K, - SolAR::datastructure::CamDistortion & D, - const std::vector & selectKeyframes = {}) = 0; + virtual double bundleAdjustment(const std::vector & selectKeyframes = {}) = 0; /// @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] 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. - virtual double optimizeSim3(SolAR::datastructure::CamCalibration& K1, - SolAR::datastructure::CamCalibration& K2, - const SRef& keyframe1, + virtual double optimizeSim3(const SRef& keyframe1, const SRef& keyframe2, const std::vector& matches, const std::vector & pts3D1, diff --git a/interfaces/api/solver/map/IMapUpdate.h b/interfaces/api/solver/map/IMapUpdate.h index 3b9b8d6a..30fae8d1 100644 --- a/interfaces/api/solver/map/IMapUpdate.h +++ b/interfaces/api/solver/map/IMapUpdate.h @@ -47,10 +47,6 @@ class [[xpcf::clientUUID("2c2b7702-6a34-4add-b645-547ef0185253")]] [[xpcf::serve /// @brief IMapUpdate default destructor virtual ~IMapUpdate() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] camParams Camera parameters. - virtual void setCameraParameters(const SolAR::datastructure::CameraParameters & camParams) = 0; - /// @brief Update the global map. /// @param[in,out] globalMap the global map /// @param[in] newKeyframeIds the ids of new keyframes. diff --git a/interfaces/api/solver/map/ITriangulator.h b/interfaces/api/solver/map/ITriangulator.h index ff9f120d..26cc0e70 100644 --- a/interfaces/api/solver/map/ITriangulator.h +++ b/interfaces/api/solver/map/ITriangulator.h @@ -47,18 +47,15 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve /// @brief ITriangulator default destructor virtual ~ITriangulator() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief triangulate pairs of points 2d captured from two views with differents poses (with respect to the camera instrinsic parameters). /// @param[in] pointsView1 set of 2D points seen in view_1. /// @param[in] pointsView2 set of 2D points seen in view_2. /// @param[in] matches the matches between the keypoints of the view1 and the keypoints of the view 2. /// @param[in] working_views a pair representing the id of the two views /// @param[in] poseView1 camera pose in the world coordinates system of the view_1 expressed as a Transform3D. - /// @param[in] poseView2 camera pose in the world coordinates system of the view_2 expressed as a Transform3D.. + /// @param[in] poseView2 camera pose in the world coordinates system of the view_2 expressed as a Transform3D. + /// @param[in] camParams1 the parameters of the camera 1. + /// @param[in] camParams1 the parameters of the camera 2. /// @param[out] pcloud set of triangulated 3d_points. /// @return the mean re-projection error (mean distance in pixels between the original 2D points and the projection of the reconstructed 3D points) virtual double triangulate(const std::vector & pointsView1, @@ -67,6 +64,8 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve const std::pair & working_views, const SolAR::datastructure::Transform3Df & poseView1, const SolAR::datastructure::Transform3Df & poseView2, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector> & pcloud)=0; /// @brief triangulate pairs of points 2d captured from two views with differents poses (with respect to the camera instrinsic parameters). @@ -75,7 +74,9 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve /// @param[in] matches the matches between the keypoints of the view1 and the keypoints of the view 2. /// @param[in] working_views a pair representing the id of the two views /// @param[in] poseView1 camera pose in the world coordinates system of the view_1 expressed as a Transform3D. - /// @param[in] poseView2 camera pose in the world coordinates system of the view_2 expressed as a Transform3D.. + /// @param[in] poseView2 camera pose in the world coordinates system of the view_2 expressed as a Transform3D. + /// @param[in] camParams1 the parameters of the camera 1. + /// @param[in] camParams1 the parameters of the camera 2. /// @param[out] pcloud set of triangulated 3d_points. /// @return the mean re-projection error (mean distance in pixels between the original 2D points and the projection of the reconstructed 3D points) virtual double triangulate(const std::vector & keypointsView1, @@ -84,6 +85,8 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve const std::pair & working_views, const SolAR::datastructure::Transform3Df & poseView1, const SolAR::datastructure::Transform3Df & poseView2, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector> & pcloud)=0; /// @brief triangulate pairs of points 2d captured from two views with differents poses (with respect to the camera instrinsic parameters). @@ -94,7 +97,9 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve /// @param[in] matches the matches between the keypoints of the view1 and the keypoints of the view 2. /// @param[in] working_views a pair representing the id of the two views /// @param[in] poseView1 Camera pose in the world coordinates system of the view_1 expressed as a Transform3D. - /// @param[in] poseView2 Camera pose in the world coordinates system of the view_2 expressed as a Transform3D.. + /// @param[in] poseView2 Camera pose in the world coordinates system of the view_2 expressed as a Transform3D. + /// @param[in] camParams1 the parameters of the camera 1. + /// @param[in] camParams1 the parameters of the camera 2. /// @param[out] pcloud Set of triangulated 3d_points. /// @return the mean re-projection error (mean distance in pixels between the original 2D points and the projection of the reconstructed 3D points) virtual double triangulate( const std::vector & keypointsView1, @@ -105,6 +110,8 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve const std::pair & working_views, const SolAR::datastructure::Transform3Df & poseView1, const SolAR::datastructure::Transform3Df & poseView2, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector> & pcloud) =0; /// @brief calculating 3D cloud points by triangulating pairs of matched features or using depth information of keypoints. @@ -112,6 +119,8 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve /// @param[in] frame2 the second frame. /// @param[in] matches the matches between these two frames. /// @param[in] working_views a pair representing the id of the two views + /// @param[in] camParams1 the parameters of the camera 1. + /// @param[in] camParams1 the parameters of the camera 2 /// @param[out] pcloud Set of triangulated 3d_points. /// @param[in] onlyDepth if it is true, using only depth information of keypoints for computing 3D cloud points. /// @return the mean re-projection error (mean distance in pixels between the original 2D points and the projection of the reconstructed 3D points) @@ -119,6 +128,8 @@ class [[xpcf::clientUUID("1b6be40f-030c-4436-a920-913d6e4421f4")]] [[xpcf::serve SRef frame2, const std::vector &matches, const std::pair & working_views, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector> & pcloud, const bool& onlyDepth = false) = 0; }; diff --git a/interfaces/api/solver/pose/I2Dto3DTransformDecomposer.h b/interfaces/api/solver/pose/I2Dto3DTransformDecomposer.h index e7717b45..8b1ecd55 100644 --- a/interfaces/api/solver/pose/I2Dto3DTransformDecomposer.h +++ b/interfaces/api/solver/pose/I2Dto3DTransformDecomposer.h @@ -42,16 +42,14 @@ class [[xpcf::clientUUID("861a828d-bdda-424b-a00b-3a6a7ef48f8e")]] [[xpcf::serve /// @brief I2Dto3DTransformDecomposer default destructor. virtual ~I2Dto3DTransformDecomposer() = 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. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief decompose a transform 2d to a transform 3d (4 possible poses {R1,t1},{R1,t2}, {R2,t1}, {R2,t2}). - /// @param[in] Transform 2D (fundamental matrxi, homgraphy..). + /// @param[in] F the fundamental matrix. + /// @param[in] camParams the camera parameters. /// @param[out] Set (04 possibles cases) of the decomposed camera poses in the world coordinate system expressed as Transform3D. /// @return true if succeed, else false - virtual bool decompose(const SolAR::datastructure::Transform2Df & F, std::vector & decomposedPoses)= 0; + virtual bool decompose(const SolAR::datastructure::Transform2Df & F, + const SolAR::datastructure::CameraParameters & camParams, + std::vector & decomposedPoses)= 0; }; } } diff --git a/interfaces/api/solver/pose/I3DTransformFinderFrom2D2D.h b/interfaces/api/solver/pose/I3DTransformFinderFrom2D2D.h index 8a804381..6815f844 100644 --- a/interfaces/api/solver/pose/I3DTransformFinderFrom2D2D.h +++ b/interfaces/api/solver/pose/I3DTransformFinderFrom2D2D.h @@ -44,20 +44,17 @@ class [[xpcf::clientUUID("62f0a994-3ad4-4322-8059-7513153d074a")]] [[xpcf::serve ///@brief I3DTransformFinderFrom2D2D default destructor. virtual ~I3DTransformFinderFrom2D2D() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief Estimates camera pose from a set of 2D points of the first image which match with a set of 2D points of the second image. /// @param[in] pointsView1 Set of 2D points seen in view 1. /// @param[in] pointsView2 Set of 2D points seen in view 2 and matching with the 2D points of the view 1. + /// @param[in] camParams the camera parameters. /// @param[in] poseView1 Camera pose (3D transform of the camera of the view1 defined in world corrdinate system). /// @param[out] poseView2 Camera pose (3D transform of the camera of the view2 defined in world corrdinate system). /// @param[in,out] inlierMatches a vector of matches that will be used for the pose estimation. This vector wll be updates as some input matches will be considered as outliers. If this vector is empty, we consider that the ith point of pointsView1 matches with the ith point of pointsView2. /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const std::vector & pointsView1, const std::vector & pointsView2, + const SolAR::datastructure::CameraParameters & camParams, const SolAR::datastructure::Transform3Df & poseView1, SolAR::datastructure::Transform3Df & poseView2, std::vector & inlierMatches) =0; @@ -65,12 +62,14 @@ class [[xpcf::clientUUID("62f0a994-3ad4-4322-8059-7513153d074a")]] [[xpcf::serve /// @brief Estimates camera pose from a set of keypoints of the first image which match with a set of keypoints of the second image. /// @param[in] pointsView1 Set of keypoints seen in view 1. /// @param[in] pointsView2 Set of keypoints seen in view 2 and matching with the 2D points of the view 1. + /// @param[in] camParams the camera parameters. /// @param[in] poseView1 Camera pose (3D transform of the camera of the view1 defined in world corrdinate system). /// @param[out] poseView2 Camera pose (3D transform of the camera of the view2 defined in world corrdinate system). /// @param[in,out] inlierMatches a vector of matches that will be used for the pose estimation. This vector wll be updates as some input matches will be considered as outliers. If this vector is empty, we consider that the ith point of pointsView1 matches with the ith point of pointsView2. /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const std::vector & pointsView1, const std::vector & pointsView2, + const SolAR::datastructure::CameraParameters & camParams, const SolAR::datastructure::Transform3Df& poseView1, SolAR::datastructure::Transform3Df & poseView2, std::vector& inlierMatches) =0; diff --git a/interfaces/api/solver/pose/I3DTransformFinderFrom2D3D.h b/interfaces/api/solver/pose/I3DTransformFinderFrom2D3D.h index 4ea9455f..d5c421e8 100644 --- a/interfaces/api/solver/pose/I3DTransformFinderFrom2D3D.h +++ b/interfaces/api/solver/pose/I3DTransformFinderFrom2D3D.h @@ -43,19 +43,16 @@ class [[xpcf::clientUUID("201fb9e7-9452-42e2-a587-9e9b3e49c889")]] [[xpcf::serve ///@brief I3DTransformFinderFrom2D3D default destructor. virtual ~I3DTransformFinderFrom2D3D() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - - /// @brief Estimates camera pose from a set of 2D image points of their corresponding 3D world points. - /// @param[in] imagePoints set of 2d_points seen in view_1. - /// @param[in] worldPoints set of 3d_points corresponding to view_1. + /// @brief Estimates camera pose from a set of 2D image points of their corresponding 3D world points. + /// @param[in] imagePoints the set of 2D image points. + /// @param[in] worldPoints the set of 3d world points. + /// @param[in] camParams the camera parameters. /// @param[out] pose camera pose (pose of the camera defined in world corrdinate system) expressed as a Transform3D. /// @param[in] initialPose (Optional) a transform3D to initialize the pose (reducing the convergence time and improving its success). /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const std::vector & imagePoints, const std::vector & worldPoints, + const SolAR::datastructure::CameraParameters & camParams, SolAR::datastructure::Transform3Df & pose, const SolAR::datastructure::Transform3Df initialPose = SolAR::datastructure::Transform3Df::Identity()) =0; }; diff --git a/interfaces/api/solver/pose/I3DTransformSACFinderFrom2D3D.h b/interfaces/api/solver/pose/I3DTransformSACFinderFrom2D3D.h index 606d2e1a..24ecb634 100644 --- a/interfaces/api/solver/pose/I3DTransformSACFinderFrom2D3D.h +++ b/interfaces/api/solver/pose/I3DTransformSACFinderFrom2D3D.h @@ -43,20 +43,17 @@ class [[xpcf::clientUUID("502c323f-1063-4efc-8ac6-c45469632546")]] [[xpcf::serve ///@brief I3DTransformSACFinderFrom2D3D default destructor. virtual ~I3DTransformSACFinderFrom2D3D() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] intrinsicParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief Estimates camera pose from a set of 2D image points of their corresponding 3D world points. - /// @param[in] imagePoints: set of 2d_points seen in view_1. - /// @param[in] worldPoints: set of 3d_points corresponding to view_1. - /// @param[out] inliers: indices of inlier correspondences. - /// @param[out] pose: camera pose (pose of the camera defined in world corrdinate system) expressed as a Transform3D. - /// @param[in] initialPose: (Optional) a transform3D to initialize the pose (reducing the convergence time and improving its success). + /// @param[in] inputPoints the set of 3D cloud points to project + /// @param[in] pose the 3D pose of the camera (a 4x4 float matrix) + /// @param[in] camParams the camera parameters. + /// @param[out] inliers indices of inlier correspondences. + /// @param[out] pose camera pose (pose of the camera defined in world corrdinate system) expressed as a Transform3D. + /// @param[in] initialPose (Optional) a transform3D to initialize the pose (reducing the convergence time and improving its success). /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const std::vector & imagePoints, const std::vector & worldPoints, + const SolAR::datastructure::CameraParameters & camParams, std::vector & inliers, SolAR::datastructure::Transform3Df & pose, const SolAR::datastructure::Transform3Df initialPose = SolAR::datastructure::Transform3Df::Identity()) =0; diff --git a/interfaces/api/solver/pose/I3DTransformSACFinderFrom3D3D.h b/interfaces/api/solver/pose/I3DTransformSACFinderFrom3D3D.h index 34508901..e4977c61 100644 --- a/interfaces/api/solver/pose/I3DTransformSACFinderFrom3D3D.h +++ b/interfaces/api/solver/pose/I3DTransformSACFinderFrom3D3D.h @@ -44,32 +44,32 @@ class [[xpcf::clientUUID("c5d93531-de0c-4424-b48f-00709822dd4a")]] [[xpcf::serve ///@brief I3DTransformSACFinderFrom3D3D default destructor. virtual ~I3DTransformSACFinderFrom3D3D() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams: Camera calibration matrix parameters. - /// @param[in] distortionParams: Camera distortion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distortionParams) = 0; - /// @brief Estimates camera pose from a set of 3D-3D point correspondences. - /// @param[in] firstPoints3D: first set of 3D points. - /// @param[in] secondPoints3D: second set of 3D points. - /// @param[out] pose: 3D transformation maps the first set of 3D points to the second one. - /// @param[out] inliers: indices of inlier correspondences. + /// @param[in] firstPoints3D first set of 3D points. + /// @param[in] secondPoints3D second set of 3D points. + /// @param[out] pose 3D transformation maps the first set of 3D points to the second one. + /// @param[out] inliers indices of inlier correspondences. virtual FrameworkReturnCode estimate(const std::vector & firstPoints3D, const std::vector & secondPoints3D, SolAR::datastructure::Transform3Df & pose, std::vector &inliers) = 0; /// @brief Estimates camera pose from a set of 3D-3D point correspondences. - /// @param[in] firstKeyframe: first keyframe. - /// @param[in] secondKeyframe: second keyframe. - /// @param[in] matches: matches between two keyframes. - /// @param[in] firstPoints3D: first set of 3D points. - /// @param[in] secondPoints3D: second set of 3D points. - /// @param[out] pose: 3D transformation maps the first set of 3D points to the second one. - /// @param[out] inliers: indices of inlier correspondences. + /// @param[in] firstKeyframe first keyframe. + /// @param[in] secondKeyframe second keyframe. + /// @param[in] firstCameraParameters parameters of the camera capturing the first keyframe. + /// @param[in] secondCameraParameters parameters of the camera capturing the second keyframe. + /// @param[in] secondKeyframe second keyframe. + /// @param[in] matches matches between two keyframes. + /// @param[in] firstPoints3D first set of 3D points. + /// @param[in] secondPoints3D second set of 3D points. + /// @param[out] pose 3D transformation maps the first set of 3D points to the second one. + /// @param[out] inliers indices of inlier correspondences. /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const SRef firstKeyframe, const SRef secondKeyframe, + const SolAR::datastructure::CameraParameters & firstCameraParameters, + const SolAR::datastructure::CameraParameters & secondCameraParameters, const std::vector &matches, const std::vector & firstPoints3D, const std::vector & secondPoints3D, diff --git a/interfaces/api/solver/pose/IMultiTrackablesPose.h b/interfaces/api/solver/pose/IMultiTrackablesPose.h index 3b5fe4c0..e92047b5 100644 --- a/interfaces/api/solver/pose/IMultiTrackablesPose.h +++ b/interfaces/api/solver/pose/IMultiTrackablesPose.h @@ -43,20 +43,17 @@ class [[xpcf::clientUUID("cf2959b3-cc73-46fa-8082-0809a3a2dcd3")]] [[xpcf::serve ///@brief IMultiTrackablesPose default destructor. virtual ~IMultiTrackablesPose() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief this method is used to set the set of trackables used to estimate the pose. /// @param[in] trackables the set of trackables used to estimate the pose. virtual FrameworkReturnCode setTrackables(const std::vector> trackables) =0; /// @brief Estimates camera pose based on a set of trackables. /// @param[in] image input image. + /// @param[in] camParams the camera parameters. /// @param[out] pose camera pose. /// @return FrameworkReturnCode::_SUCCESS if the estimation succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const SRef image, + const SolAR::datastructure::CameraParameters & camParams, SolAR::datastructure::Transform3Df & pose) =0; }; diff --git a/interfaces/api/solver/pose/ITrackablePose.h b/interfaces/api/solver/pose/ITrackablePose.h index a9960f74..a562978c 100644 --- a/interfaces/api/solver/pose/ITrackablePose.h +++ b/interfaces/api/solver/pose/ITrackablePose.h @@ -43,20 +43,17 @@ class [[xpcf::clientUUID("ad79b898-f2b0-446f-835e-7daf3458fe50")]] [[xpcf::serve ///@brief ITrackablePose default destructor. virtual ~ITrackablePose() = default; - /// @brief this method is used to set intrinsic parameters and distorsion of the camera - /// @param[in] intrinsicParams camera calibration matrix parameters. - /// @param[in] distorsionParams camera distorsion parameters. - virtual void setCameraParameters(const SolAR::datastructure::CamCalibration & intrinsicParams, const SolAR::datastructure::CamDistortion & distorsionParams) = 0; - /// @brief this method is used to set the trackable used to estimate the pose. /// @param[in] the trackable used to estimate the pose. virtual FrameworkReturnCode setTrackable(const SRef trackable) =0; /// @brief Estimates camera pose based on a fiducial marker. - /// @param[in] image: input image. - /// @param[out] pose: camera pose. + /// @param[in] image input image. + /// @param[in] camParams the camera parameters. + /// @param[out] pose camera pose. /// @return FrameworkReturnCode::_SUCCESS if the estimation succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode estimate(const SRef image, + const SolAR::datastructure::CameraParameters & camParams, SolAR::datastructure::Transform3Df & pose) =0; }; diff --git a/interfaces/api/storage/ICameraParametersManager.h b/interfaces/api/storage/ICameraParametersManager.h new file mode 100644 index 00000000..9a463a2b --- /dev/null +++ b/interfaces/api/storage/ICameraParametersManager.h @@ -0,0 +1,129 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_ICAMERASPARAMETERSSMANAGER_H +#define SOLAR_ICAMERASPARAMETERSSMANAGER_H + + +#include "xpcf/api/IComponentIntrospect.h" +#include "core/Messages.h" +#include "datastructure/CameraParametersCollection.h" +#include + +namespace SolAR { +namespace api { +namespace storage { + +/** + * @class ICameraParametersManager + * @brief Allows to store a set of cameraParameters. + * UUID: 31f151fc-326d-11ed-a261-0242ac120002 + * + * This storage component can be accessed by processing components to share persistent data. + */ + +class XPCF_IGNORE ICameraParametersManager : + virtual public org::bcom::xpcf::IComponentIntrospect { +public: + /// @brief ICameraParameterstorage default constructor + ICameraParametersManager() = default; + + /// @brief ICameraParameterstorage default destructor + virtual ~ICameraParametersManager() = default; + + /// @brief This method allow to add cameraParameters to the CameraParameters manager component + /// @param[in] cameraParameters the cameraParameters to add to the set of persistent CameraParameters + /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode addCameraParameters(const SRef cameraParameters) = 0; + + /// @brief This method allow to add cameraParameters to the CameraParameters manager component + /// @param[in] frame the frame to add to the set of persistent CameraParameters + /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode addCameraParameters(SolAR::datastructure::CameraParameters & cameraParameters) = 0; + + /// @brief This method allows to get cameraParameters by their id + /// @param[in] id id of the cameraParameters to get + /// @param[out] cameraParameters cameraParameters stored in the CameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode getCameraParameters(const uint32_t id, SRef & cameraParameters) const = 0; + + /// @brief This method allows to get cameraParameters by their id + /// @param[in] id id of the cameraParameters to get + /// @param[out] cameraParameters cameraParameters stored in the CameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode getCameraParameters(const uint32_t id, SolAR::datastructure::CameraParameters & cameraParameters) const = 0; + + /// @brief This method allows to get a set of CameraParameters by their ids + /// @param[in] ids a vector of ids of the cameraParameters to get + /// @param[out] cameraParameters a vector of cameraParameters stored in the CameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode getCameraParameters(const std::vector & ids, std::vector> & cameraParameters) const = 0; + + /// @brief This method allows to get all cameraParameters + /// @param[out] cameraParameters the set of cameraParameters + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode getAllCameraParameters(std::vector> & CameraParameters) const = 0; + + /// @brief This method allow to suppress cameraParameters by its id + /// @param[in] id id of cameraParameters to suppress + /// @return FrameworkReturnCode::_SUCCESS_ if the suppression succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode suppressCameraParameters(const uint32_t id) = 0; + + /// @brief This method allows to know if cameraParameters are already stored in the component + /// @param[in] id id of this cameraParameters + /// @return true if exist, else false + virtual bool isExistCameraParameters(const uint32_t id) const = 0; + + /// @brief This method allows to get the number of CameraParameters stored in the point cloud + /// @return The number of CameraParameters + virtual int getNbCameraParameters() const = 0; + + /// @brief This method allows to save all cameraParameters to the external file + /// @param[in] file the file name + /// @return FrameworkReturnCode::_SUCCESS_ if the suppression succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode saveToFile(const std::string & file) const = 0; + + /// @brief This method allows to load all CameraParameters from the external file + /// @param[in] file the file name + /// @return FrameworkReturnCode::_SUCCESS_ if the suppression succeed, else FrameworkReturnCode::_ERROR. + virtual FrameworkReturnCode loadFromFile(const std::string & file) = 0; + + /// @brief This method returns the CameraParameters collection + /// @return the CameraParameters collection + virtual const SRef & getConstCameraParametersCollection() const = 0; + + /// @brief This method returns the CameraParameters collection + /// @param[out] CameraParametersCollection the CameraParameters collection of map + /// @return the CameraParameters collection + virtual std::unique_lock getCameraParametersCollection(SRef& cameraParametersCollection) = 0; + + /// @brief This method is to set the CameraParameters collection + /// @param[in] cameraParameters the CameraParameters collection of map + virtual void setCameraParametersCollection(const SRef cameraParametersCollection) = 0; +}; + +} +} +} + +XPCF_DEFINE_INTERFACE_TRAITS(SolAR::api::storage::ICameraParametersManager, + "31f151fc-326d-11ed-a261-0242ac120002", + "ICameraParametersManager", + "A component interface for storing a set of persistent CameraParameters accessible by processing components."); + + +#endif //SOLAR_ICAMERASPARAMETERSSMANAGER_H + diff --git a/interfaces/api/storage/IMapManager.h b/interfaces/api/storage/IMapManager.h index d01fc31f..35e28672 100644 --- a/interfaces/api/storage/IMapManager.h +++ b/interfaces/api/storage/IMapManager.h @@ -28,6 +28,7 @@ #include "core/Messages.h" #include "datastructure/GeometryDefinitions.h" #include "datastructure/Map.h" +#include "api/storage/ICameraParametersManager.h" #include "api/storage/ICovisibilityGraphManager.h" #include "api/storage/IKeyframesManager.h" #include "api/storage/IPointCloudManager.h" @@ -106,6 +107,28 @@ class [[xpcf::clientUUID("9c68f766-3b6f-427f-91d3-1e5126d27326")]] [[xpcf::serve /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode removeKeyframe(const SRef keyframe) = 0; + /// @brief Add camera parameters to map manager + /// @param[in] cameraParameters the camera paramaters to add to the map manager + /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode addCameraParameters(const SRef cameraParameters) = 0; + + /// @brief Remove camera parameters from map manager + /// @param[in] cameraParameters the camera parameters to remove from the map manager + /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode removeCameraParameters(const SRef cameraParameters) = 0; + + /// @brief Get camera parameters from map manager + /// @param[in] id the id of the camera parameters + /// @param[out] cameraParameters the camera parameters to get from the map manager + /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode getCameraParameters(const uint32_t id, SRef & cameraParameters) = 0; + + /// @brief Get camera parameters from map manager + /// @param[in] id the id of the camera parameters + /// @param[out] cameraParameters the camera parameters to get from the map manager + /// @return FrameworkReturnCode::_SUCCESS if succeed, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode getCameraParameters(const uint32_t id, SolAR::datastructure::CameraParameters & cameraParameters) = 0; + /// @brief Prune cloud points of a map /// @param[in] cloudPoints: the cloud points are checked to prune virtual int pointCloudPruning(const std::vector> &cloudPoints = {}) = 0; @@ -114,6 +137,9 @@ class [[xpcf::clientUUID("9c68f766-3b6f-427f-91d3-1e5126d27326")]] [[xpcf::serve /// @param[in] keyframes: the keyframes are checked to prune virtual int keyframePruning(const std::vector> &keyframes = {}) = 0; + /// @brief Prune visibilities of a map + virtual FrameworkReturnCode visibilityPruning() = 0; + /// @brief Save the map to the external file /// @return FrameworkReturnCode::_SUCCESS_ if the backup succeeds, else FrameworkReturnCode::_ERROR. virtual FrameworkReturnCode saveToFile() const = 0; diff --git a/interfaces/base/features/ADescriptorMatcherGeometric.h b/interfaces/base/features/ADescriptorMatcherGeometric.h index c894fc88..c87614dc 100644 --- a/interfaces/base/features/ADescriptorMatcherGeometric.h +++ b/interfaces/base/features/ADescriptorMatcherGeometric.h @@ -44,9 +44,11 @@ class XPCF_IGNORE SOLARFRAMEWORK_API ADescriptorMatcherGeometric : public org::b /// @param[in] undistortedKeypoints2 The second set of undistorted keypoints. /// @param[in] pose1 The first pose. /// @param[in] pose2 The second pose. - /// @param[in] camParams The intrinsic parameters of the camera. + /// @param[in] camParams1 The intrinsic parameters of the camera 1. + /// @param[in] camParams2 The intrinsic parameters of the camera 2. /// @param[out] matches A vector of matches representing pairs of indices relatively to the first and second set of descriptors. - /// @param[in] mask The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask1 The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask2 The indices of descriptors in the second frame are used for matching to the first frame. If it is empty then all will be used. /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode match(const SRef descriptors1, const SRef descriptors2, @@ -54,23 +56,29 @@ class XPCF_IGNORE SOLARFRAMEWORK_API ADescriptorMatcherGeometric : public org::b const std::vector &undistortedKeypoints2, const SolAR::datastructure::Transform3Df& pose1, const SolAR::datastructure::Transform3Df& pose2, - const SolAR::datastructure::CameraParameters & camParams, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector & matches, - const std::vector& mask = {}) override + const std::vector& mask1 = {}, + const std::vector& mask2 = {}) override { return FrameworkReturnCode::_NOT_IMPLEMENTED; } /// @brief Match two sets of descriptors from two frames based on epipolar constraint. /// @param[in] frame1 The first frame containing descriptors and undistorted keypoints. /// @param[in] frame2 The second frame containing descriptors and undistorted keypoints. - /// @param[in] camParams The intrinsic parameters of the camera. /// @param[out] matches A vector of matches representing pairs of indices relatively to the first and second set of descriptors. /// @param[in] mask The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_ + /// @param[in] mask1 The indices of descriptors in the first frame are used for matching to the second frame. If it is empty then all will be used. + /// @param[in] mask2 The indices of descriptors in the second frame are used for matching to the first frame. If it is empty then all will be used. + /// @return FrameworkReturnCode::_SUCCESS if matching succeed, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode match(const SRef frame1, const SRef frame2, - const SolAR::datastructure::CameraParameters & camParams, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector & matches, - const std::vector& mask = {}) override; + const std::vector& mask1 = {}, + const std::vector& mask2 = {}) override; }; } } diff --git a/interfaces/base/pipeline/AMappingPipeline.h b/interfaces/base/pipeline/AMappingPipeline.h index a9c4af20..a65f8871 100644 --- a/interfaces/base/pipeline/AMappingPipeline.h +++ b/interfaces/base/pipeline/AMappingPipeline.h @@ -41,39 +41,67 @@ class XPCF_IGNORE SOLARFRAMEWORK_API AMappingPipeline : public org::bcom::xpcf:: /// @param[in] cameraParams: the camera parameters (its resolution and its focal) /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams) override + { return FrameworkReturnCode::_NOT_IMPLEMENTED; } + + /// @brief Set the camera parameters (use for stereo camera) + /// @param[in] cameraParams1 the camera parameters of the first camera + /// @param[in] cameraParams2 the camera parameters of the second camera + /// @return FrameworkReturnCode::_SUCCESS if the camera parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setCameraParameters(const SolAR::datastructure::CameraParameters & cameraParams1, + const SolAR::datastructure::CameraParameters & cameraParams2) override + { return FrameworkReturnCode::_NOT_IMPLEMENTED; } + + /// @brief Set the rectification parameters (use for stereo camera) + /// @param[in] rectCam1 the rectification parameters of the first camera + /// @param[in] rectCam2 the rectification parameters of the second camera + /// @return FrameworkReturnCode::_SUCCESS if the rectification parameters are correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode setRectificationParameters(const SolAR::datastructure::RectificationParameters & rectCam1, + const SolAR::datastructure::RectificationParameters & rectCam2) override + { return FrameworkReturnCode::_NOT_IMPLEMENTED; } + + /// @brief Set the 3D transformation from SolAR to world spaces + /// @param[in] transform the transformation matrix from SolAR to World + /// @return FrameworkReturnCode::_SUCCESS if the transform is correctly set, else FrameworkReturnCode::_ERROR_ + virtual FrameworkReturnCode set3DTransformSolARToWorld(const SolAR::datastructure::Transform3Df & transform) override { return FrameworkReturnCode::_NOT_IMPLEMENTED; } /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the device coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the device coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[in] transform the transformation matrix from the device coordinate system to the world coordinate system /// @param[out] updatedTransform the refined transformation by a loop closure detection /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, const SolAR::datastructure::Transform3Df & transform, SolAR::datastructure::Transform3Df & updatedTransform, SolAR::api::pipeline::MappingStatus & status) override { return FrameworkReturnCode::_NOT_IMPLEMENTED; } /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the world coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the world coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, SolAR::api::pipeline::MappingStatus & status) override; /// @brief Request to the mapping pipeline to process a new image/pose - /// @param[in] image the input image to process - /// @param[in] pose the input pose in the world coordinate system + /// @param[in] images the input images to process + /// @param[in] poses the input poses in the world coordinate system + /// @param[in] fixedPose the input poses are considered as ground truth /// @param[out] updatedTransform the refined transformation by a loop closure detection /// @param[out] status the current status of the mapping pipeline /// @return FrameworkReturnCode::_SUCCESS if the data are ready to be processed, else FrameworkReturnCode::_ERROR_ - virtual FrameworkReturnCode mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, + virtual FrameworkReturnCode mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, SolAR::datastructure::Transform3Df & updatedTransform, SolAR::api::pipeline::MappingStatus & status) override; diff --git a/interfaces/core/Messages.h b/interfaces/core/Messages.h index 780142ff..463ee8d9 100644 --- a/interfaces/core/Messages.h +++ b/interfaces/core/Messages.h @@ -38,6 +38,16 @@ enum class FrameworkReturnCode:long{ _ERROR_LOAD_IMAGE = -10, /** Cannot acces Image */ _ERROR_ACCESS_IMAGE = -11, + + // for services + /** Service is busy */ + _BUSY = -20, + /** No service registered (Service Manager) */ + _NO_SERVICE_REGISTERED = -21, + /** No service available (Service Manager) */ + _NO_SERVICE_AVAILABLE = -22, + /** Service not locked by a client (Service Manager) */ + _NO_SERVICE_LOCKED = -23 }; } // end of namespace SolAR diff --git a/interfaces/core/SerializationDefinitions.h b/interfaces/core/SerializationDefinitions.h index 1c71b848..5757d05e 100644 --- a/interfaces/core/SerializationDefinitions.h +++ b/interfaces/core/SerializationDefinitions.h @@ -50,7 +50,7 @@ #include using OutputArchive = ::boost::archive::text_oarchive; using InputArchive = ::boost::archive::text_iarchive; -#elif SOLAR_USE_XML_SERIALIZATION +#elif defined (SOLAR_USE_XML_SERIALIZATION) #include #include using OutputArchive = ::boost::archive::xml_oarchive; diff --git a/interfaces/datastructure/CameraDefinitions.h b/interfaces/datastructure/CameraDefinitions.h index 693f712b..6157a34c 100644 --- a/interfaces/datastructure/CameraDefinitions.h +++ b/interfaces/datastructure/CameraDefinitions.h @@ -200,7 +200,7 @@ inline void from_json(const BasicJsonType& j, Eigen::Matrix data = j.template get>(); int size = (int)std::sqrt(data.size()); - assert((size * size == data.size()) && "Must be a squared matrix"); + assert((size * size == (int) data.size()) && "Must be a squared matrix"); eigenData.resize(size, size); for (int x = 0; x < size; ++x) for (int y = 0; y < size; ++y) @@ -337,7 +337,7 @@ inline bool saveToFile(const SolAR::datastructure::CameraRigParameters& camParam { nlohmann::ordered_json j; // write camera parameters to json - int nbCameras = camParams.cameraParams.size(); + int nbCameras = (int) camParams.cameraParams.size(); j["NbCameras"] = nbCameras; int countCamera(0); for (const auto& it : camParams.cameraParams) { @@ -345,7 +345,7 @@ inline bool saveToFile(const SolAR::datastructure::CameraRigParameters& camParam countCamera++; } // write rectifications to json - int nbRects = camParams.rectificationParams.size(); + int nbRects = (int) camParams.rectificationParams.size(); j["NbRectifications"] = nbRects; int countRect(0); for (const auto& it : camParams.rectificationParams) { @@ -357,7 +357,7 @@ inline bool saveToFile(const SolAR::datastructure::CameraRigParameters& camParam countRect++; } // write transformations to json - int nbTransformations = camParams.transformations.size(); + int nbTransformations = (int) camParams.transformations.size(); j["NbTransformations"] = nbTransformations; int countTrans(0); for (const auto& it : camParams.transformations) { diff --git a/interfaces/datastructure/CameraParametersCollection.h b/interfaces/datastructure/CameraParametersCollection.h new file mode 100644 index 00000000..a245859e --- /dev/null +++ b/interfaces/datastructure/CameraParametersCollection.h @@ -0,0 +1,101 @@ +#ifndef CAMERAPARAMETERSCOLLECTION_H +#define CAMERAPARAMETERSCOLLECTION_H + +#include "core/SolARFrameworkDefinitions.h" +#include "datastructure/GeometryDefinitions.h" +#include "datastructure/CameraDefinitions.h" +#include "datastructure/Lockable.h" +#include "core/Messages.h" +#include "xpcf/core/refs.h" +#include + +// Definition of CameraParametersCollection Class // +// part of SolAR namespace // + +namespace SolAR { +namespace datastructure { + +/** +* @class CameraParametersCollection +* @brief A set of CameraParameters. +* This class provides a set of CameraParameters. +*/ +class SOLARFRAMEWORK_API CameraParametersCollection : public Lockable { +public: + + /// + /// @brief CameraParametersCollection constructor. + /// + CameraParametersCollection() = default; + CameraParametersCollection(const CameraParametersCollection& other) = default; + CameraParametersCollection& operator=(const CameraParametersCollection& other) = default; + + /// + /// \brief ~CameraParametersCollection + /// + ~CameraParametersCollection() = default; + + /// @brief This method allow to add CameraParameters to the CameraParameters manager component + /// @param[in] cameraParameters the CameraParameters to add to the set of persistent CameraParameterss + /// @param[in] defineCameraParametersId if true an id will be set for the added CameraParameters, if false the id of the CameraParameters will be used + /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode addCameraParameters(const SRef cameraParameters, bool defineCameraParametersId = true); + + /// @brief This method allow to add a frame to the key frame manager component + /// @param[in] cameraParameters the cameraParameters to add to the set of persistent CameraParameterss + /// @param[in] defineCameraParametersId if true an id will be set for the added cameraParameters, if false the id of the cameraParameters will be used + /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode addCameraParameters(SolAR::datastructure::CameraParameters & cameraParameters, bool defineCameraParametersId = true); + + /// @brief This method allows to get a cameraParameters by its id + /// @param[in] id of the cameraParameters to get + /// @param[out] cameraParameters the cameraParameters stored in the cameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode getCameraParameters(const uint32_t id, SRef & cameraParameters) const; + + /// @brief This method allows to get a cameraParameters by its id + /// @param[in] id of the cameraParameters to get + /// @param[out] cameraParameters the cameraParameters stored in the cameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode getCameraParameters(const uint32_t id, SolAR::datastructure::CameraParameters & cameraParameters) const; + + /// @brief This method allows to get a set of cameraParameters by their ids + /// @param[in] a vector of ids of the cameraParameters to get + /// @param[out] a vector of cameraParameters stored in the cameraParameters manager + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode getCameraParameters(const std::vector & ids, std::vector>& cameraParameters) const; + + /// @brief This method allows to get all cameraParameters + /// @param[out] the set of cameraParameters + /// @return FrameworkReturnCode::_SUCCESS_ if succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode getAllCameraParameters(std::vector>& CameraParameters) const; + + /// @brief This method allow to suppress a cameraParameters by its id + /// @param[in] id of the cameraParameters to suppress + /// @return FrameworkReturnCode::_SUCCESS_ if the suppression succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode suppressCameraParameters(const uint32_t id); + + /// @brief This method allows to know if a cameraParameters is already stored in the component + /// @param[in] Id of this cameraParameters + /// @return true if exist, else false + bool isExistCameraParameters(const uint32_t id) const; + + /// @brief This method allows to get the number of cameraParameters stored in the collection of cameraParameters + /// @return The number of cameraParameters + int getNbCameraParameters() const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + + std::map> m_cameraParameters; + uint32_t m_id = 0; +}; + +DECLARESERIALIZE(CameraParametersCollection); + +} +} // end of namespace SolAR + +#endif // CAMERAPARAMETERSCOLLECTION_H diff --git a/interfaces/datastructure/CloudPoint.h b/interfaces/datastructure/CloudPoint.h index 5566cf53..d6b8ffc6 100644 --- a/interfaces/datastructure/CloudPoint.h +++ b/interfaces/datastructure/CloudPoint.h @@ -39,6 +39,14 @@ namespace datastructure { */ class SOLARFRAMEWORK_API CloudPoint : public Point3Df, public PrimitiveInformation { public: + typedef enum { + Color = 0x01, + ViewDirection = 0x02, + ReprojectionError = 0x04, + Visibility = 0x08, + Descriptor = 0x10 + } CloudPointType; + CloudPoint() = default; /// @brief CloudPoint constructor with a Point3Df. @@ -272,13 +280,13 @@ class SOLARFRAMEWORK_API CloudPoint : public Point3Df, public PrimitiveInformat void serialize(Archive &ar, const unsigned int version); private: - uint32_t m_id = 0; + uint32_t m_cloudPointSupportedTypes = 0; + uint32_t m_id = 0; SRef m_descriptor = nullptr; std::map m_visibility = {}; Vector3f m_rgb = {0.0, 0.0, 0.0}; Vector3f m_viewDirection = {0.0, 0.0, 0.0}; double m_reproj_error = 0.0; - bool m_isFeatureCP; }; DECLARESERIALIZE(CloudPoint); diff --git a/interfaces/datastructure/Frame.h b/interfaces/datastructure/Frame.h index b104a1a0..fca7337b 100644 --- a/interfaces/datastructure/Frame.h +++ b/interfaces/datastructure/Frame.h @@ -10,6 +10,7 @@ #include "datastructure/DescriptorMatch.h" #include "datastructure/CloudPoint.h" #include +#include "datastructure/CameraDefinitions.h" #include namespace SolAR { @@ -27,24 +28,27 @@ class SOLARFRAMEWORK_API Frame { Frame() = default; Frame(const SRef frame); - Frame(const SRef keyframe); + Frame(const SRef keyframe); explicit Frame(const std::vector & keypoints, const SRef descriptors, - const SRef view, - const Transform3Df pose = Transform3Df::Identity()); + const SRef view, + const uint32_t camID = 0, + const Transform3Df pose = Transform3Df::Identity()); explicit Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, const SRef refKeyframe, + const uint32_t camID = 0, const Transform3Df pose = Transform3Df::Identity()); explicit Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, + const uint32_t camID = 0, const Transform3Df pose = Transform3Df::Identity()); /// @brief ~Frame @@ -66,6 +70,14 @@ class SOLARFRAMEWORK_API Frame { /// @param[in] pose: camera pose void setPose(const Transform3Df & pose); + /// @brief check if the pose of this frame is fixed and no need to optimize + /// @return true if fixed pose, otherwise return false + bool isFixedPose() const; + + /// @brief This method is to set this frame having a fixed pose. + /// @param[in] value the value (true/false) + void setFixedPose(bool value); + /// @brief get keypoints /// @return keypoints const std::vector & getKeypoints() const; @@ -78,6 +90,12 @@ class SOLARFRAMEWORK_API Frame { /// @brief set keypoints /// @param[in] kpts: keypoints void setKeypoints(const std::vector& kpts); + + /// @brief update keypoint class id + /// @param[in] i index of keypoint + /// @param[in] classId semantic class Id of this keypoint + /// @return true if update class id successfully + bool updateKeypointClassId(int i, int classId); /// @brief get undistorted keypoints /// @return undistorted keypoints @@ -132,6 +150,22 @@ class SOLARFRAMEWORK_API Frame { /// @return true if remove successfully bool removeVisibility(const uint32_t& id_keypoint, const uint32_t& id_cloudPoint); + /// @brief set camera parameters + /// @param[in] camParams the camera parameters + void setCameraID(const uint32_t camID); + + /// @brief get camera parameters + /// @return the camera parameters + const uint32_t& getCameraID() const; + + /// @brief set image name + /// @param[in] imageName: image name + void setImageName(const std::string &imageName); + + /// @brief get image name + /// @return image name + const std::string& getImageName() const; + private: friend class boost::serialization::access; template @@ -144,6 +178,9 @@ class SOLARFRAMEWORK_API Frame { SRef m_descriptors; std::vector m_keypoints; std::vector m_keypointsUndistort; + std::string m_imageName; + uint32_t m_camID; + bool m_isFixedPose = false; //A map storing the 3D points visibility, where the first element corresponds to the index of the keypoint of the frame, and the second element to the index of the corresponding cloudPoint. std::map m_mapVisibility; diff --git a/interfaces/datastructure/Image.h b/interfaces/datastructure/Image.h index 7b7598f7..8ad28908 100644 --- a/interfaces/datastructure/Image.h +++ b/interfaces/datastructure/Image.h @@ -37,7 +37,7 @@ class SOLARFRAMEWORK_API ImageInternal { ImageInternal() = default; explicit ImageInternal(uint32_t size); explicit ImageInternal(void* data, uint32_t size); - ~ImageInternal() = default; + virtual ~ImageInternal(); void setBufferSize(uint32_t size); inline uint32_t getBufferSize() { return m_bufferSize; } void setData(void * data, uint32_t size); @@ -255,6 +255,18 @@ class SOLARFRAMEWORK_API Image { /// @return _SUCCESS if the image is saved, _ERROR_ otherwise. FrameworkReturnCode load(std::string imagePath); + /// @brief Rotate image 90 degrees + /// @return _SUCCESS if the image is rotated, _ERROR_ otherwise. + FrameworkReturnCode rotate90(); + + /// @brief Rotate image 180 degrees + /// @return _SUCCESS if the image is rotated, _ERROR_ otherwise. + FrameworkReturnCode rotate180(); + + /// @brief Rotate image 270 degrees + /// @return _SUCCESS if the image is rotated, _ERROR_ otherwise. + FrameworkReturnCode rotate270(); + private: friend class boost::serialization::access; protected: @@ -275,6 +287,13 @@ class SOLARFRAMEWORK_API Image { SRef m_internalImpl; uint32_t computeImageBufferSize(); + // Rotate degrees + enum class RotateQuantity { + DEGREE_90, + DEGREE_180, + DEGREE_270 + }; + FrameworkReturnCode rotate(RotateQuantity); Sizei m_size; enum ImageLayout m_layout; enum PixelOrder m_pixOrder; diff --git a/interfaces/datastructure/Keyframe.h b/interfaces/datastructure/Keyframe.h index 44f960ed..67236d93 100644 --- a/interfaces/datastructure/Keyframe.h +++ b/interfaces/datastructure/Keyframe.h @@ -36,25 +36,28 @@ class SOLARFRAMEWORK_API Keyframe : public Frame, public PrimitiveInformation { public: Keyframe() = default; - Keyframe(SRef frame) : Frame(frame), m_id(0) {}; + Keyframe(SRef frame) : Frame(frame) {}; explicit Keyframe(const std::vector & keypoints, SRef descriptors, SRef view, - Transform3Df pose = Transform3Df::Identity()) : Frame(keypoints, descriptors, view, pose), m_id(0) {}; + const uint32_t camID = 0, + Transform3Df pose = Transform3Df::Identity()) : Frame(keypoints, descriptors, view, camID, pose) {}; explicit Keyframe(const std::vector & keypoints, const std::vector & undistortedKeypoints, SRef descriptors, SRef view, SRef refKeyframe, - Transform3Df pose = Transform3Df::Identity()): Frame(keypoints, undistortedKeypoints, descriptors, view, refKeyframe, pose), m_id(0){}; + const uint32_t camID = 0, + Transform3Df pose = Transform3Df::Identity()): Frame(keypoints, undistortedKeypoints, descriptors, view, refKeyframe, camID, pose) {}; explicit Keyframe(const std::vector & keypoints, const std::vector & undistortedKeypoints, SRef descriptors, SRef view, - Transform3Df pose = Transform3Df::Identity()): Frame(keypoints, undistortedKeypoints, descriptors, view, pose), m_id(0){}; + const uint32_t camID = 0, + Transform3Df pose = Transform3Df::Identity()): Frame(keypoints, undistortedKeypoints, descriptors, view, camID, pose) {}; ~Keyframe() = default; @@ -69,6 +72,19 @@ class SOLARFRAMEWORK_API Keyframe : public Frame, public PrimitiveInformation { /// void setId(const uint32_t& id_keyframe); + /// + /// @brief Set keypoint matched + /// @param[in] id_keypoint: keypoint id + /// @return boolean + /// + bool setKeypointStatusToMatched(uint32_t id_keypoint); + + /// + /// @brief Get keypoint matched map + /// @return list of boolean + /// + const std::vector& getIsKeypointMatched() const; + private: friend class boost::serialization::access; template @@ -76,6 +92,7 @@ class SOLARFRAMEWORK_API Keyframe : public Frame, public PrimitiveInformation { private: uint32_t m_id; + std::vector m_isKeypointMatched; // boolean map true or false indicating if keypoint matched to other keyframes during mapping }; DECLARESERIALIZE(Keyframe); diff --git a/interfaces/datastructure/KeyframeCollection.h b/interfaces/datastructure/KeyframeCollection.h index 1ca5d7f0..c61cf622 100644 --- a/interfaces/datastructure/KeyframeCollection.h +++ b/interfaces/datastructure/KeyframeCollection.h @@ -37,13 +37,15 @@ class SOLARFRAMEWORK_API KeyframeCollection : public Lockable { /// @brief This method allow to add a frame to the keyframe manager component /// @param[in] frame the frame to add to the set of persistent keyframes + /// @param[in] defineKeyframeId if true an id will be set for the added keyframe, if false the id of the keyframe will be used /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. - FrameworkReturnCode addKeyframe(const SRef keyframe); + FrameworkReturnCode addKeyframe(const SRef keyframe, bool defineKeyframeId = true); /// @brief This method allow to add a frame to the key frame manager component - /// @param[in] frame the frame to add to the set of persistent keyframes + /// @param[in] frame the frame to add to the set of persistent keyframes + /// @param[in] defineKeyframeId if true an id will be set for the added keyframe, if false the id of the keyframe will be used /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. - FrameworkReturnCode addKeyframe(const SolAR::datastructure::Keyframe & keyframe); + FrameworkReturnCode addKeyframe(const SolAR::datastructure::Keyframe & keyframe, bool defineKeyframeId = true); /// @brief This method allows to get a keyframe by its id /// @param[in] id of the keyframe to get @@ -92,7 +94,7 @@ class SOLARFRAMEWORK_API KeyframeCollection : public Lockable { std::map>m_keyframes; SolAR::datastructure::DescriptorType m_descriptorType; - uint32_t m_id; + uint32_t m_id = 0; }; DECLARESERIALIZE(KeyframeCollection); diff --git a/interfaces/datastructure/KeyframeRetrieval.h b/interfaces/datastructure/KeyframeRetrieval.h index e1b69e2e..f10c34b7 100644 --- a/interfaces/datastructure/KeyframeRetrieval.h +++ b/interfaces/datastructure/KeyframeRetrieval.h @@ -79,6 +79,9 @@ class SOLARFRAMEWORK_API KeyframeRetrieval : public Lockable { /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. FrameworkReturnCode getInvertedIndex(uint32_t nodeId, std::set& invertedIndex); + /// @brief This method allows to reset BoW features as well as inverted index map + void reset(); + private: friend class boost::serialization::access; template diff --git a/interfaces/datastructure/Keypoint.h b/interfaces/datastructure/Keypoint.h index cc491629..4da31a8d 100644 --- a/interfaces/datastructure/Keypoint.h +++ b/interfaces/datastructure/Keypoint.h @@ -122,6 +122,16 @@ class SOLARFRAMEWORK_API Keypoint : public Point2Df { /// @brief This method allows to set the depth value of an Keypoint in the camera coordinate system /// @param[in] depth Depth value inline void setDepth(float depth) {m_depth = depth;} + + /// @brief This method allows to set the class Id of a Keypoint + /// @param[in] classId class id of the key point + inline void setClassId(int classId) {m_class_id = classId;} + + /// @brief This method allows to set the RGB values of a Keypoint + /// @param[in] r Red channel intensity level between 0 and 255 + /// @param[in] g Green channel intensity level between 0 and 255 + /// @param[in] b Blue channel intensity level between 0 and 255 + inline void setRGB(float r, float g, float b) {m_rgb(0)=r; m_rgb(1)=g; m_rgb(2)=b;} private: friend class boost::serialization::access; @@ -134,7 +144,7 @@ class SOLARFRAMEWORK_API Keypoint : public Point2Df { float m_angle; float m_response; int m_octave; - int m_class_id; + int m_class_id = -1; // default value -1 meaning that no class is assigned Vector3f m_rgb = { 0.0, 0.0, 0.0 }; float m_depth = -1.f; }; diff --git a/interfaces/datastructure/Map.h b/interfaces/datastructure/Map.h index 27b7c4a8..5ae2ebff 100644 --- a/interfaces/datastructure/Map.h +++ b/interfaces/datastructure/Map.h @@ -21,11 +21,13 @@ #include "core/SolARFrameworkDefinitions.h" #include "datastructure/GeometryDefinitions.h" #include "datastructure/Identification.h" +#include "datastructure/CameraParametersCollection.h" #include "datastructure/CoordinateSystem.h" #include "datastructure/PointCloud.h" #include "datastructure/KeyframeCollection.h" #include "datastructure/CovisibilityGraph.h" #include "datastructure/KeyframeRetrieval.h" +#include "datastructure/Trackable.h" #include "xpcf/core/refs.h" #include @@ -40,13 +42,14 @@ namespace datastructure { * @brief A generic map composed of an identification and a coordinate system. * This class provides a generic map. */ -class SOLARFRAMEWORK_API Map { +class SOLARFRAMEWORK_API Map : public Trackable { public: typedef enum { _PointCloud = 0x01, _Keyframe = 0x02, _CovisibilityGraph = 0x04, - _KFRetriever = 0x08 + _KFRetriever = 0x08, + _CameraParemeters = 0x10 } MapType; /// @@ -133,19 +136,38 @@ class SOLARFRAMEWORK_API Map { /// const SRef & getConstKeyframeCollection() const; - /// + /// /// @brief This method returns the keyframe collection /// @param[out] keyframeCollection the keyframe collection of map /// @return the keyframe collection /// std::unique_lock getKeyframeCollection(SRef& keyframeCollection); - + /// /// @brief This method is to set the keyframe collection /// @param[in] keyframeCollection the keyframe collection of map /// void setKeyframeCollection(const SRef keyframeCollection); + /// + /// @brief This method returns the camera parameters collection + /// @return the camera parameters collection + /// + const SRef & getConstCameraParametersCollection() const; + + /// + /// @brief This method returns the camera parameters collection + /// @param[out] keyframeCollection the camera parameters collection of map + /// @return a mutex + /// + std::unique_lock getCameraParametersCollection(SRef& cameraParametersCollection); + + /// + /// @brief This method is to set the camera parameters collection + /// @param[in] cameraParametersCollection the camera parameters collection of map + /// + void setCameraParametersCollection(const SRef cameraParametersCollection); + /// /// @brief This method returns the covisibility graph /// @return the covisibility graph @@ -184,20 +206,28 @@ class SOLARFRAMEWORK_API Map { /// void setKeyframeRetrieval(const SRef keyframeRetrieval); + /// + /// @brief This method is to get trackable type + /// @return the trackable's type + /// + TrackableType getType() const override; + private: friend class boost::serialization::access; template void serialize(Archive &ar, const unsigned int version); - uint32_t m_mapSupportedTypes; - SRef m_identification; - SRef m_coordinateSystem; - SRef m_pointCloud; - SRef m_keyframeCollection; - SRef m_covisibilityGraph; - SRef m_keyframeRetrieval; + uint32_t m_mapSupportedTypes; + SRef m_identification; + SRef m_coordinateSystem; + SRef m_pointCloud; + SRef m_keyframeCollection; + SRef m_covisibilityGraph; + SRef m_keyframeRetrieval; + SRef m_cameraParametersCollection; }; + DECLARESERIALIZE(Map); } } // end of namespace SolAR diff --git a/interfaces/datastructure/MathDefinitions.h b/interfaces/datastructure/MathDefinitions.h index 4c831d18..122a35bd 100644 --- a/interfaces/datastructure/MathDefinitions.h +++ b/interfaces/datastructure/MathDefinitions.h @@ -92,12 +92,42 @@ typedef Maths::Vector3f Vector3f; */ typedef Vector Vector3b; +/** + * @typedef Vector3ui + * @brief A vector of 3 unsigned int + */ +typedef Vector Vector3ui; + /** * @typedef Vector3d * @brief A vector of 3 doubles. */ typedef Maths::Vector3d Vector3d; +/** + * @typedef Vector2f + * @brief A vector of 2 floats. + */ +typedef Maths::Vector2f Vector2f; + +/** + * @typedef Vector2b + * @brief A vector of 2 unsigned char. + */ +typedef Vector Vector2b; + +/** + * @typedef Vector2ui + * @brief A vector of 2 unsigned int + */ +typedef Vector Vector2ui; + +/** + * @typedef Vector2d + * @brief A vector of 2 doubles. + */ +typedef Maths::Vector2d Vector2d; + template using RowVector = Maths::Matrix; @@ -148,12 +178,88 @@ typedef Quaternion Quaternionf; */ typedef Maths::Matrix Rotation; +/** + * @typedef Rotation + * @brief A 3x3 matrix defining a 3D rotation. + */ +typedef Maths::Matrix Rotationd; + /** * @typedef Projection * @brief A 3x4 matrix defining a projection matrix. */ typedef Maths::Matrix Projection; +/** + * @brief Compute average quaternion from a list of input quaternions. + * The algorithm used is described here: + * https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf + * @param[in] quaternions vector of Vector4f quaternions + * @return average quaternion Vector4f + */ +static Vector4f quaternionAverage(const std::vector& quaternions) { + if (quaternions.size() == 0) + return Vector4f::Zero(); + else if (quaternions.size() == 1) + return quaternions.front(); + + // first build a 4x4 matrix which is the elementwise sum of the product of each quaternion with itself + Maths::Matrix4f A = Maths::Matrix4f::Zero(); + for (const auto& q : quaternions) + A += q * q.transpose(); + // normalise with the number of quaternions + A /= static_cast(quaternions.size()); + // Compute the SVD of this 4x4 matrix + Maths::JacobiSVD svd(A, Maths::ComputeThinU | Maths::ComputeThinV); + Maths::VectorXf singularValues = svd.singularValues(); + Maths::MatrixXf matU = svd.matrixU(); + // find the eigen vector corresponding to the largest eigen value + int index = 0; + float value = singularValues(0); + for (int i=1; i < static_cast(singularValues.rows()); ++i) { + if (singularValues(i) > value) { + value = singularValues(i); + index = i; + } + } + return Vector4f(matU(0, index), matU(1, index), matU(2, index), matU(3, index)); +} + +/** + * @brief Compute average quaternion from a list of input quaternions. + * @param[in] quaternions vector of Quaternionf + * @return average quaternion Quaternionf + */ +static Quaternionf quaternionAverage(const std::vector& quaternions) { + std::vector quaternionsVecs; + for (const auto& q : quaternions) + quaternionsVecs.emplace_back(q.x(), q.y(), q.z(), q.w()); + return Quaternionf(quaternionAverage(quaternionsVecs)); +} + +/** + * @brief Compute average transform3D from a list of input transform3Ds. + * @param[in] transforms list of transform3D + * @return average transform3D + */ +static Transform3Df transform3DAverage(const std::vector& transforms) { + if (transforms.size() == 0) + return Transform3Df(Maths::Matrix4f::Zero()); + else if (transforms.size() == 1) + return transforms.front(); + Vector3f translations(0.f, 0.f, 0.f); + std::vector quaternions; + for (const auto& t : transforms) { + translations += t.translation(); + quaternions.emplace_back(t.rotation()); + } + translations /= static_cast(transforms.size()); + Transform3Df transform3D; + transform3D.linear() = quaternionAverage(quaternions).toRotationMatrix(); + transform3D.translation() = translations; + return transform3D; +} + } } diff --git a/interfaces/datastructure/Mesh.h b/interfaces/datastructure/Mesh.h new file mode 100644 index 00000000..301677b4 --- /dev/null +++ b/interfaces/datastructure/Mesh.h @@ -0,0 +1,120 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_MESH_H +#define SOLAR_MESH_H + +#ifndef _BCOM_SHARED +#define _BCOM_SHARED +#endif // _BCOM_SHARED + +#include "xpcf/api/IComponentIntrospect.h" +#include "core/SolARFrameworkDefinitions.h" +#include "datastructure/GeometryDefinitions.h" +#include "datastructure/MathDefinitions.h" +#include + + +// Definition of Mesh Class // +// part of SolAR namespace // +namespace SolAR { +namespace datastructure { +/** + * @class Mesh + * @brief A textured mesh of triangles base on indexed face sets. + */ +class SOLARFRAMEWORK_API Mesh { +public: + Mesh() = default; + + /// @brief CloudPoint constructor with a Point3Df. + /// @param[in] points vector of the 3D points of the mesh. + /// @param[in] indexedFaceSets indexed face sets representing the id of points for each triangle of the mesh. + /// @param[in] normals (optional): vector of normal for each 3D point.. + /// @param[in] colors (optional): color of each point of the mesh (RGB). + /// @param[in] texCoordinates (optional): Coordinate texture of each 3D point of the mesh.. + explicit Mesh( const std::vector points, + const std::vector indexedFaceSets, + const std::vector normals = {}, + const std::vector colors = {}, + const std::vector texCoordinates = {}); + + /// + /// \brief ~Mesh + /// + ~Mesh(); + + /// @brief This method returns the 3D points of the mesh + /// @return the vector of 3D points + inline std::vector getPoints () const { return m_points; } + + /// @brief This method allows to set the 3D points of the mesh + /// @param[in] points The 3D points of the mesh + inline void setPoints(std::vector points) {m_points = points;} + + /// @brief This method returns the indexed face set of the mesh + /// @return the vector indexed face set + inline std::vector getIndexedFaceSets () const { return m_indexedFaceSets; } + + /// @brief This method allows to set the indexed face sets of the mesh + /// @param[in] indexedFaceSets The indexed face sets of the mesh + inline void setIndexedFaceSets(std::vector indexedFaceSets) {m_indexedFaceSets = indexedFaceSets;} + + /// @brief This method returns the normals to the points of the mesh + /// @return the normals to the points of the mesh + inline std::vector getNormals () const { return m_normals; } + + /// @brief This method allows to set the normals to the points of the mesh + /// @param[in] normals The normals to the points of the mesh + inline void setNormals(std::vector normals) {m_normals = normals;} + + + /// @brief This method returns the colors of the points of the mesh + /// @return the colors of the points of the mesh + inline std::vector getColors () const { return m_colors; } + + /// @brief This method allows to set the colors of the points of the mesh + /// @param[in] colors The colors of the points of the mesh + inline void setColors(std::vector colors) {m_colors = colors;} + + /// @brief This method returns the texture coordinates of the points of the mesh + /// @return the texture coordinates of the points of the mesh + inline std::vector getTexCoordinates () const { return m_texCoordinates; } + + /// @brief This method allows to set the texture coordinates of the points of the mesh + /// @param[in] texCoordinates The texture coordinates of the points of the mesh + inline void setTexCoordinates(std::vector texCoordinates) {m_texCoordinates = texCoordinates;} + + +private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + +private: + std::vector m_points; + std::vector m_indexedFaceSets; + std::vector m_normals; + std::vector m_colors; + std::vector m_texCoordinates; +}; + +DECLARESERIALIZE(Mesh); + +} // end of namespace datastructure +} // end of namespace SolAR + +#endif // SOLAR_MESH_H diff --git a/interfaces/datastructure/PointCloud.h b/interfaces/datastructure/PointCloud.h index 70a17772..236ce12a 100644 --- a/interfaces/datastructure/PointCloud.h +++ b/interfaces/datastructure/PointCloud.h @@ -65,10 +65,11 @@ class SOLARFRAMEWORK_API PointCloud : public Lockable { /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. FrameworkReturnCode addPoint(const SolAR::datastructure::CloudPoint &point); - /// @brief This method allow to add a vector of 3D points to the point cloud - /// @param[in] a vector of the 3D points to add to the persistent point cloud - /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. - FrameworkReturnCode addPoints(const std::vector &points); + /// @brief This method allow to add a vector of 3D points to the point cloud + /// @param[in] a vector of the 3D points to add to the persistent point cloud + /// @param[in] definePointId if true an id will be set for the added point, else the id of the point will be used + /// @return FrameworkReturnCode::_SUCCESS_ if the addition succeed, else FrameworkReturnCode::_ERROR. + FrameworkReturnCode addPoints(const std::vector &points, bool definePointId = true); /// @brief This method allows to get a 3D point stored in the point cloud by its id /// @param[in] id of the point to get diff --git a/interfaces/datastructure/Trackable.h b/interfaces/datastructure/Trackable.h index 7574cc48..cf9d7358 100644 --- a/interfaces/datastructure/Trackable.h +++ b/interfaces/datastructure/Trackable.h @@ -38,7 +38,8 @@ enum TrackableType { UNKNOWN, FIDUCIAL_MARKER, IMAGE_MARKER, - QRCODE_MARKER + QRCODE_MARKER, + MAP }; diff --git a/packagedependencies-android.txt b/packagedependencies-android.txt deleted file mode 100644 index c5cde072..00000000 --- a/packagedependencies-android.txt +++ /dev/null @@ -1 +0,0 @@ -openimageio|2.3.13.0|openimageio|conan-solar@conan|conan-solar#https://artifact.b-com.com/api/conan/solar-conan-local#0|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#boost:numa=False#boost:without_stacktrace=True#fPIC=False#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_openvdb=False#with_tbb=False#with_tools=False diff --git a/packagedependencies-linux.txt b/packagedependencies-linux.txt index e7e0833e..e6534b0b 100644 --- a/packagedependencies-linux.txt +++ b/packagedependencies-linux.txt @@ -1 +1 @@ -openimageio|2.3.13.0|openimageio|conan-solar@conan|conan-solar#https://artifact.b-com.com/api/conan/solar-conan-local#0|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#boost:numa=False#boost:without_stacktrace=True#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_tools=False \ No newline at end of file +openimageio#1_0_0|2.3.13.0|openimageio|conan-solar@conan|conan-solar|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_tools=False diff --git a/packagedependencies-mac.txt b/packagedependencies-mac.txt deleted file mode 100644 index 389249b6..00000000 --- a/packagedependencies-mac.txt +++ /dev/null @@ -1 +0,0 @@ -openimageio|2.3.13.0|openimageio|conan-solar@conan|conan-solar#https://artifact.b-com.com/api/conan/solar-conan-local#0|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#boost:numa=False#boost:without_stacktrace=True#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_tools=False diff --git a/packagedependencies-win.txt b/packagedependencies-win.txt index bd0b54fb..9e7744cf 100644 --- a/packagedependencies-win.txt +++ b/packagedependencies-win.txt @@ -1,2 +1,2 @@ -openimageio|2.3.13.0|openimageio|conan-solar@conan|conan-solar#https://artifact.b-com.com/api/conan/solar-conan-local#0|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#boost:numa=False#boost:without_stacktrace=True#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_tools=False +openimageio#1_0_0|2.3.13.0|openimageio|conan-solar@conan|conan-solar|default|boost:shared=True#boost:zlib=False#boost:bzip2=False#with_ffmpeg=False#with_freetype=False#with_giflib=False#with_hdf5=False#with_libheif=False#with_libwebp=False#with_opencolorio=False#with_opencv=False#with_ptex=False#with_tools=False diff --git a/packagedependencies.txt b/packagedependencies.txt index 336d0a8c..b34276bb 100644 --- a/packagedependencies.txt +++ b/packagedependencies.txt @@ -1,4 +1,4 @@ eigen|3.3.9|eigen|conan|conan-center|na| spdlog|0.14.0|spdlog|thirdParties@github|https://github.com/SolarFramework/binaries/releases/download|shared| nlohmann_json|3.9.1|nlohmann_json|conan|conan-center|na| -xpcf|2.5.3|xpcf|http|https://github.com/b-com-software-basis/xpcf/releases/download|shared| +xpcf|2.6.2|xpcf|http|https://github.com/b-com-software-basis/xpcf/releases/download|shared| diff --git a/src/base/features/ADescriptorMatcherGeometric.cpp b/src/base/features/ADescriptorMatcherGeometric.cpp index 37970cd2..e593cdff 100644 --- a/src/base/features/ADescriptorMatcherGeometric.cpp +++ b/src/base/features/ADescriptorMatcherGeometric.cpp @@ -29,13 +29,14 @@ ADescriptorMatcherGeometric::ADescriptorMatcherGeometric(std::map frame1, const SRef frame2, - const SolAR::datastructure::CameraParameters & camParams, + const SolAR::datastructure::CameraParameters & camParams1, + const SolAR::datastructure::CameraParameters & camParams2, std::vector & matches, - const std::vector& mask) -{ + const std::vector& mask1, const std::vector& mask2) +{ return match(frame1->getDescriptors(), frame2->getDescriptors(), - frame1->getUndistortedKeypoints(), frame2->getUndistortedKeypoints(), frame1->getPose(), - frame2->getPose(), camParams, matches, mask); + frame1->getUndistortedKeypoints(), frame2->getUndistortedKeypoints(), frame1->getPose(), + frame2->getPose(), camParams1, camParams2, matches, mask1, mask2); } } diff --git a/src/base/pipeline/AMappingPipeline.cpp b/src/base/pipeline/AMappingPipeline.cpp index c9d333d0..4f8086e5 100644 --- a/src/base/pipeline/AMappingPipeline.cpp +++ b/src/base/pipeline/AMappingPipeline.cpp @@ -27,20 +27,22 @@ AMappingPipeline::AMappingPipeline(std::map componentIn declareInterface(this); } -FrameworkReturnCode AMappingPipeline::mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, - SolAR::api::pipeline::MappingStatus & status) +FrameworkReturnCode AMappingPipeline::mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, + SolAR::api::pipeline::MappingStatus & status) { datastructure::Transform3Df updatedTransform; - return mappingProcessRequest(image, pose, datastructure::Transform3Df::Identity(), updatedTransform, status); + return mappingProcessRequest(images, poses, fixedPose, datastructure::Transform3Df::Identity(), updatedTransform, status); } -FrameworkReturnCode AMappingPipeline::mappingProcessRequest(const SRef image, - const SolAR::datastructure::Transform3Df & pose, - SolAR::datastructure::Transform3Df & updatedTransform, - SolAR::api::pipeline::MappingStatus & status) +FrameworkReturnCode AMappingPipeline::mappingProcessRequest(const std::vector> & images, + const std::vector & poses, + bool fixedPose, + SolAR::datastructure::Transform3Df & updatedTransform, + SolAR::api::pipeline::MappingStatus & status) { - return mappingProcessRequest(image, pose, datastructure::Transform3Df::Identity(), updatedTransform, status); + return mappingProcessRequest(images, poses, fixedPose, datastructure::Transform3Df::Identity(), updatedTransform, status); } diff --git a/src/datastructure/CameraParametersCollection.cpp b/src/datastructure/CameraParametersCollection.cpp new file mode 100644 index 00000000..2f9e3c1e --- /dev/null +++ b/src/datastructure/CameraParametersCollection.cpp @@ -0,0 +1,151 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "datastructure/CameraParametersCollection.h" +#include "xpcf/core/helpers.h" +#include "core/Log.h" + +namespace xpcf = org::bcom::xpcf; + +namespace SolAR { +namespace datastructure { + +FrameworkReturnCode CameraParametersCollection::addCameraParameters(const SRef cameraParameters, bool defineCameraParametersId) +{ + uint32_t id; + if (defineCameraParametersId==true) + { + id = m_id++; + cameraParameters->id = id; + } + else + { + id = cameraParameters->id; + if (m_id<=id) + m_id = id+1; + + } + m_cameraParameters[id] = cameraParameters; + LOG_DEBUG("Add camera[{}] = {}", id, cameraParameters->id); + return FrameworkReturnCode::_SUCCESS; +} + +FrameworkReturnCode CameraParametersCollection::addCameraParameters(CameraParameters & cameraParameters, bool defineCameraParametersId) +{ + SRef cameraParameters_ptr = xpcf::utils::make_shared(cameraParameters); + uint32_t id; + if (defineCameraParametersId==true) + { + id = m_id++; + cameraParameters.id = id; + + } + else + { + id = cameraParameters.id; + if (m_id<=id) + m_id = id+1; + + } + cameraParameters_ptr->id = id; + m_cameraParameters[id] = cameraParameters_ptr; + LOG_DEBUG("Add camera[{}] = {}", id, cameraParameters_ptr->id); + return FrameworkReturnCode::_SUCCESS; +} + +FrameworkReturnCode CameraParametersCollection::getCameraParameters(const uint32_t id, SRef & cameraParameters) const +{ + std::map< uint32_t, SRef>::const_iterator cameraParametersIt = m_cameraParameters.find(id); + if (cameraParametersIt != m_cameraParameters.end()) { + cameraParameters = cameraParametersIt->second; + return FrameworkReturnCode::_SUCCESS; + } + else { + LOG_DEBUG("Cannot find cameraParameters with id {} to get", id); + return FrameworkReturnCode::_ERROR_; + } +} + +FrameworkReturnCode CameraParametersCollection::getCameraParameters(const uint32_t id, CameraParameters & cameraParameters) const +{ + std::map< uint32_t, SRef>::const_iterator cameraParametersIt = m_cameraParameters.find(id); + if (cameraParametersIt != m_cameraParameters.end()) { + cameraParameters = *(cameraParametersIt->second); + return FrameworkReturnCode::_SUCCESS; + } + else { + LOG_DEBUG("Cannot find cameraParameters with id {} to get", id); + return FrameworkReturnCode::_ERROR_; + } +} + +FrameworkReturnCode CameraParametersCollection::getCameraParameters(const std::vector& ids, std::vector>& cameraParameters) const +{ + for (auto &it : ids) { + std::map< uint32_t, SRef>::const_iterator cameraParametersIt = m_cameraParameters.find(it); + if (cameraParametersIt == m_cameraParameters.end()) { + LOG_DEBUG("Cannot find cameraParameters with id {} to get", it); + return FrameworkReturnCode::_ERROR_; + } + cameraParameters.push_back(cameraParametersIt->second); + } + return FrameworkReturnCode::_SUCCESS; +} + +FrameworkReturnCode CameraParametersCollection::getAllCameraParameters(std::vector>& cameraParameters) const +{ + for (auto cameraParametersIt = m_cameraParameters.begin(); cameraParametersIt != m_cameraParameters.end(); cameraParametersIt++) + cameraParameters.push_back(cameraParametersIt->second); + return FrameworkReturnCode::_SUCCESS; +} + +FrameworkReturnCode CameraParametersCollection::suppressCameraParameters(const uint32_t id) +{ + std::map< uint32_t, SRef>::iterator cameraParametersIt = m_cameraParameters.find(id); + if (cameraParametersIt != m_cameraParameters.end()) { + m_cameraParameters.erase(cameraParametersIt); + return FrameworkReturnCode::_SUCCESS; + } + else { + LOG_DEBUG("Cannot find cameraParameters with id {} to suppress", id); + return FrameworkReturnCode::_ERROR_; + } +} + +bool CameraParametersCollection::isExistCameraParameters(const uint32_t id) const +{ + if (m_cameraParameters.find(id) != m_cameraParameters.end()) + return true; + else + return false; +} + +int CameraParametersCollection::getNbCameraParameters() const +{ + return static_cast(m_cameraParameters.size()); +} + +template +void CameraParametersCollection::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) +{ + ar & m_id; + ar & m_cameraParameters; +} + +IMPLEMENTSERIALIZE(CameraParametersCollection); + +} // end of namespace datastructure +} // end of namespace SolAR diff --git a/src/datastructure/CloudPoint.cpp b/src/datastructure/CloudPoint.cpp index b383aad8..a7ef925e 100644 --- a/src/datastructure/CloudPoint.cpp +++ b/src/datastructure/CloudPoint.cpp @@ -26,22 +26,44 @@ CloudPoint::~CloudPoint(){ } CloudPoint::CloudPoint(const Point3Df& point, float r, float g, float b, float nx, float ny, float nz, double reproj_error) : - Point3Df(point), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_isFeatureCP(false) {} + Point3Df(point), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ViewDirection | CloudPointType::ReprojectionError; +} CloudPoint::CloudPoint(float x, float y, float z, float r, float g, float b, float nx, float ny, float nz, double reproj_error) : - Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_isFeatureCP(false) {} + Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ViewDirection | CloudPointType::ReprojectionError; +} CloudPoint::CloudPoint(float x, float y, float z, float r, float g, float b, double reproj_error, const std::map& visibility) : - Point3Df(x, y, z), m_rgb(r, g, b), m_reproj_error(reproj_error), m_visibility(visibility), m_isFeatureCP(true) {} + Point3Df(x, y, z), m_rgb(r, g, b), m_reproj_error(reproj_error), m_visibility(visibility) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ViewDirection | CloudPointType::Visibility; +} CloudPoint::CloudPoint(float x, float y, float z, float r, float g, float b, float nx, float ny, float nz, double reproj_error, const std::map& visibility) : - Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_visibility(visibility), m_isFeatureCP(true) {} + Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_visibility(visibility) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ViewDirection | CloudPointType::ReprojectionError | CloudPointType::Visibility ; +} CloudPoint::CloudPoint(float x, float y, float z, float r, float g, float b, double reproj_error, const std::map& visibility, SRef descriptor) : - Point3Df(x, y, z), m_rgb(r, g, b), m_reproj_error(reproj_error), m_visibility(visibility), m_descriptor(descriptor), m_isFeatureCP(true) {} + Point3Df(x, y, z), m_rgb(r, g, b), m_reproj_error(reproj_error), m_visibility(visibility), m_descriptor(descriptor) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ReprojectionError | CloudPointType::Visibility; + if (descriptor != nullptr) + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Descriptor; +} CloudPoint::CloudPoint(float x, float y, float z, float r, float g, float b, float nx, float ny, float nz, double reproj_error, const std::map& visibility, SRef descriptor) : - Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_visibility(visibility), m_descriptor(descriptor), m_isFeatureCP(true) {} + Point3Df(x, y, z), m_rgb(r, g, b), m_viewDirection(nx, ny, nz), m_reproj_error(reproj_error), m_visibility(visibility), m_descriptor(descriptor) +{ + m_cloudPointSupportedTypes = CloudPointType::Color | CloudPointType::ViewDirection | CloudPointType::ReprojectionError | CloudPointType::Visibility; + if (descriptor != nullptr) + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Descriptor; +} const uint32_t& CloudPoint::getId() const{ return m_id; @@ -56,7 +78,11 @@ const SRef& CloudPoint::getDescriptor() const{ } void CloudPoint::setDescriptor(const SRef &descriptor) { - m_descriptor = descriptor; + if (descriptor != nullptr) + { + m_descriptor = descriptor; + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Descriptor; + } } void CloudPoint::addNewDescriptor(const DescriptorView & descriptor) @@ -65,6 +91,7 @@ void CloudPoint::addNewDescriptor(const DescriptorView & descriptor) DescriptorBuffer newDescriptorCP = ((*m_descriptor * m_visibility.size()) + newDescriptor) / (m_visibility.size() + 1); DescriptorBuffer newDescriptorCPConvertedType = newDescriptorCP.convertTo(m_descriptor->getDescriptorDataType()); *m_descriptor = newDescriptorCPConvertedType; + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Descriptor; } const Vector3f& CloudPoint::getRGB() const{ @@ -85,6 +112,7 @@ const float& CloudPoint::getB() const { void CloudPoint::setRGB(const Vector3f &rgb) { m_rgb = rgb; + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Color; } const Vector3f & CloudPoint::getViewDirection() const @@ -95,16 +123,19 @@ const Vector3f & CloudPoint::getViewDirection() const void CloudPoint::setViewDirection(const Vector3f & viewDirection) { m_viewDirection = viewDirection.normalized(); + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::ViewDirection; } void CloudPoint::addNewViewDirection(const Vector3f & viewDirection) { m_viewDirection = ((m_viewDirection * m_visibility.size() + viewDirection) / (m_visibility.size() + 1)).normalized(); + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::ViewDirection; } void CloudPoint::setReprojError(const double & error) { m_reproj_error = error; + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::ReprojectionError; } const double& CloudPoint::getReprojError() const @@ -117,7 +148,8 @@ const std::map& CloudPoint::getVisibility() const { } void CloudPoint::addVisibility(const uint32_t& keyframe_id, const uint32_t& keypoint_id) { - m_visibility[keyframe_id] = keypoint_id; + m_visibility[keyframe_id] = keypoint_id; + m_cloudPointSupportedTypes = m_cloudPointSupportedTypes | CloudPointType::Visibility; } bool CloudPoint::removeVisibility(const uint32_t& keyframe_id) @@ -134,16 +166,22 @@ template void CloudPoint::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { ar.template register_type(); + ar & m_cloudPointSupportedTypes; + ar & boost::serialization::base_object(*this); ar & boost::serialization::base_object(*this); ar & m_id; - ar & m_descriptor; - ar & m_visibility; - ar & boost::serialization::make_array(m_rgb.data(), 3); - ar & boost::serialization::make_array(m_viewDirection.data(), 3); - ar & m_reproj_error; - ar & m_isFeatureCP; + if (m_cloudPointSupportedTypes & CloudPointType::Descriptor) + ar & m_descriptor; + if (m_cloudPointSupportedTypes & CloudPointType::Visibility) + ar & m_visibility; + if (m_cloudPointSupportedTypes & CloudPointType::Color) + ar & boost::serialization::make_array(m_rgb.data(), 3); + if (m_cloudPointSupportedTypes & CloudPointType::ViewDirection) + ar & boost::serialization::make_array(m_viewDirection.data(), 3); + if (m_cloudPointSupportedTypes & CloudPointType::ReprojectionError) + ar & m_reproj_error; } IMPLEMENTSERIALIZE(CloudPoint); diff --git a/src/datastructure/CovisibilityGraph.cpp b/src/datastructure/CovisibilityGraph.cpp index 11def1fa..e5b5b61a 100644 --- a/src/datastructure/CovisibilityGraph.cpp +++ b/src/datastructure/CovisibilityGraph.cpp @@ -277,16 +277,18 @@ FrameworkReturnCode CovisibilityGraph::getShortestPath(const uint32_t node1_id, //take current node const auto &tn = trees[curNode]; //see ifs neighbors - for (auto neigh : m_edges.at(tn.node)) { - if (!visited[neigh]) { - visited[neigh] = true; - trees.push_back(TreeNode(neigh, static_cast(curNode))); - if (neigh == node2_id) { - found = true; - break; - } - } - } + if (m_edges.find(tn.node) != m_edges.end()) { + for (auto neigh : m_edges.at(tn.node)) { + if (!visited[neigh]) { + visited[neigh] = true; + trees.push_back(TreeNode(neigh, static_cast(curNode))); + if (neigh == node2_id) { + found = true; + break; + } + } + } + } curNode++; } if (!found)//no path diff --git a/src/datastructure/Frame.cpp b/src/datastructure/Frame.cpp index 4494634a..e984362c 100644 --- a/src/datastructure/Frame.cpp +++ b/src/datastructure/Frame.cpp @@ -28,16 +28,15 @@ std::mutex m_mutexVisibility; namespace SolAR { namespace datastructure { -Frame::Frame(const SRef frame) : m_keypoints(frame->getKeypoints()), m_keypointsUndistort(frame->getUndistortedKeypoints()), m_descriptors(frame->getDescriptors()), m_view(frame->getView()), m_referenceKeyFrame(frame->getReferenceKeyframe()), m_pose(frame->getPose()), m_mapVisibility(frame->getVisibility()){} +Frame::Frame(const SRef frame) : m_keypoints(frame->getKeypoints()), m_keypointsUndistort(frame->getUndistortedKeypoints()), m_descriptors(frame->getDescriptors()), m_view(frame->getView()), m_referenceKeyFrame(frame->getReferenceKeyframe()), m_pose(frame->getPose()), m_mapVisibility(frame->getVisibility()), m_imageName(frame->getImageName()), m_camID(frame->getCameraID()), m_isFixedPose(frame->isFixedPose()){} -Frame::Frame(const SRef keyframe) : m_keypoints(keyframe->getKeypoints()), m_keypointsUndistort(keyframe->getUndistortedKeypoints()), m_descriptors(keyframe->getDescriptors()), m_view(keyframe->getView()), m_referenceKeyFrame(keyframe->getReferenceKeyframe()), m_pose(keyframe->getPose()), m_mapVisibility(keyframe->getVisibility()) { -} +Frame::Frame(const SRef keyframe) : m_keypoints(keyframe->getKeypoints()), m_keypointsUndistort(keyframe->getUndistortedKeypoints()), m_descriptors(keyframe->getDescriptors()), m_view(keyframe->getView()), m_referenceKeyFrame(keyframe->getReferenceKeyframe()), m_pose(keyframe->getPose()), m_mapVisibility(keyframe->getVisibility()), m_imageName(keyframe->getImageName()), m_camID(keyframe->getCameraID()), m_isFixedPose(keyframe->isFixedPose()) {} -Frame::Frame(const std::vector& keypoints, const SRef descriptors, const SRef view, const Transform3Df pose) : m_keypoints(keypoints), m_descriptors(descriptors), m_view(view), m_pose(pose) {} +Frame::Frame(const std::vector& keypoints, const SRef descriptors, const SRef view, const uint32_t camID, const Transform3Df pose) : m_keypoints(keypoints), m_descriptors(descriptors), m_view(view), m_camID(camID), m_pose(pose) {} -Frame::Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, SRef refKeyframe, const Transform3Df pose): m_keypoints(keypoints), m_keypointsUndistort(undistortedKeypoints), m_descriptors(descriptors), m_view(view), m_referenceKeyFrame(refKeyframe), m_pose(pose){} +Frame::Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, SRef refKeyframe, const uint32_t camID, const Transform3Df pose): m_keypoints(keypoints), m_keypointsUndistort(undistortedKeypoints), m_descriptors(descriptors), m_view(view), m_referenceKeyFrame(refKeyframe), m_camID(camID), m_pose(pose){} -Frame::Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, const Transform3Df pose): m_keypoints(keypoints), m_keypointsUndistort(undistortedKeypoints), m_descriptors(descriptors), m_view(view), m_pose(pose){} +Frame::Frame(const std::vector & keypoints, const std::vector & undistortedKeypoints, const SRef descriptors, const SRef view, const uint32_t camID, const Transform3Df pose): m_keypoints(keypoints), m_keypointsUndistort(undistortedKeypoints), m_descriptors(descriptors), m_view(view), m_camID(camID), m_pose(pose){} const SRef& Frame::getView() const { @@ -61,11 +60,32 @@ void Frame::setPose(const Transform3Df & pose) m_pose = pose; } +bool Frame::isFixedPose() const +{ + return m_isFixedPose; +} + +void Frame::setFixedPose(bool value) +{ + m_isFixedPose = value; +} + void Frame::setKeypoints(const std::vector & kpts){ std::unique_lock lock(m_mutexKeypoint); m_keypoints = kpts; } +bool Frame::updateKeypointClassId(int i, int classId) +{ + if (i < 0 || i >= static_cast(m_keypoints.size()) || i >= static_cast(m_keypointsUndistort.size())) { + std::cerr << "keypoint index " << i << " out of range" << std::endl; + return false; + } + m_keypoints[i].setClassId(classId); + m_keypointsUndistort[i].setClassId(classId); + return true; +} + const std::vector& Frame::getUndistortedKeypoints() const { std::unique_lock lock(m_mutexKeypoint); @@ -155,14 +175,37 @@ const SRef& Frame::getReferenceKeyframe() const return m_referenceKeyFrame; } +void Frame::setCameraID(const uint32_t camID) +{ + m_camID = camID; +} + +const uint32_t & Frame::getCameraID() const +{ + return m_camID; +} + +void Frame::setImageName(const std::string &imageName) +{ + m_imageName = imageName; +} + +const std::string& Frame::getImageName() const +{ + return m_imageName; +} + template void Frame::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { ar & boost::serialization::make_array(m_pose.data(), 12); ar & m_view; ar & m_descriptors; ar & m_keypoints; - ar & m_keypointsUndistort; - ar & m_mapVisibility; + ar & m_keypointsUndistort; + ar & m_imageName; + ar & m_camID; + ar & m_isFixedPose; + ar & m_mapVisibility; } IMPLEMENTSERIALIZE(Frame); diff --git a/src/datastructure/Image.cpp b/src/datastructure/Image.cpp index 705b10ab..3967021e 100644 --- a/src/datastructure/Image.cpp +++ b/src/datastructure/Image.cpp @@ -44,6 +44,11 @@ ImageInternal::ImageInternal(void* data,uint32_t size) setData(data,size); } +ImageInternal::~ImageInternal() +{ + m_storageData.clear(); +} + void ImageInternal::setBufferSize(uint32_t size) { m_bufferSize = size; @@ -135,11 +140,11 @@ Image::Image(void* imageData, uint32_t width, uint32_t height, enum ImageLayout const OIIO::ImageSpec & spec = in->spec(); OIIO::imagesize_t buffersize = spec.image_bytes(true); - unsigned char* pixels = new unsigned char [buffersize]; - in->read_image(OIIO::TypeDesc::UNKNOWN, pixels); + auto pixels = std::make_unique(buffersize); + in->read_image(OIIO::TypeDesc::UNKNOWN, pixels.get()); m_internalImpl = utils::make_shared(); - m_internalImpl->setData(pixels, spec.image_bytes(true)); + m_internalImpl->setData(pixels.get(), spec.image_bytes(true)); in->close(); } } @@ -232,6 +237,7 @@ static std::map> SolAR2OIIOLayout = FrameworkReturnCode Image::save(std::string imagePath) const { Image::ImageEncoding encoding; + if (boost::algorithm::ends_with(imagePath, ".jpg") || boost::algorithm::ends_with(imagePath, ".jpeg")) encoding = ENCODING_JPEG; else if (boost::algorithm::ends_with(imagePath, ".png")) @@ -358,6 +364,7 @@ void Image::save(Archive & ar, const unsigned int version) const out->close (); ar & file_buffer; + file_buffer.clear(); } else { ar & m_internalImpl; @@ -379,8 +386,8 @@ FrameworkReturnCode Image::load(std::string imagePath) m_nbChannels = spec.nchannels; OIIO::imagesize_t buffersize = spec.image_bytes(true); - unsigned char* pixels = new unsigned char [buffersize]; - in->read_image(OIIO::TypeDesc::UNKNOWN, pixels); + auto pixels = std::make_unique(buffersize); + in->read_image(OIIO::TypeDesc::UNKNOWN, pixels.get()); if (OIIO2SolARType.find(spec.format) != OIIO2SolARType.end()) { @@ -417,7 +424,7 @@ FrameworkReturnCode Image::load(std::string imagePath) } m_internalImpl = utils::make_shared(); - m_internalImpl->setData(pixels, spec.image_bytes(true)); + m_internalImpl->setData(pixels.get(), spec.image_bytes(true)); in->close(); return FrameworkReturnCode::_SUCCESS; @@ -426,7 +433,7 @@ FrameworkReturnCode Image::load(std::string imagePath) template void Image::load(Archive & ar, const unsigned int version) { - ar & m_size; + ar & m_size; ar & m_layout; ar & m_pixOrder; ar & m_type; @@ -461,18 +468,96 @@ void Image::load(Archive & ar, const unsigned int version) const OIIO::ImageSpec & spec = in->spec(); OIIO::imagesize_t buffersize = spec.image_bytes(true); - unsigned char* pixels = new unsigned char [buffersize]; - in->read_image(OIIO::TypeDesc::UNKNOWN, pixels); + auto pixels = std::make_unique(buffersize); + in->read_image(OIIO::TypeDesc::UNKNOWN, pixels.get()); m_internalImpl = utils::make_shared(); - m_internalImpl->setData(pixels, spec.image_bytes(true)); + m_internalImpl->setData(pixels.get(), spec.image_bytes(true)); in->close(); + decodingBuffer.clear(); + memreader.close(); } else { ar & m_internalImpl; } } +FrameworkReturnCode Image::rotate(RotateQuantity degrees) +{ + OIIO::TypeDesc type; + if (SolAR2OIIOType.find(m_type) != SolAR2OIIOType.end()) + type = SolAR2OIIOType.at(m_type); + else + type = OIIO::TypeDesc::UNKNOWN; + + OIIO::ImageSpec spec = OIIO::ImageSpec(m_size.width, m_size.height, m_nbChannels, type); + + spec.nchannels = m_nbChannels; + if (SolAR2OIIOLayout.find(m_layout) != SolAR2OIIOLayout.end()) + spec.channelnames = SolAR2OIIOLayout.at(m_layout); + + spec.attribute ("oiio:ColorSpace", "sRGB"); + + OIIO::ImageBuf sourceBuf = OIIO::ImageBuf(spec, m_internalImpl->data()); + + // Convert to BGR or GRB to RGB channel format + if (m_layout == Image::ImageLayout::LAYOUT_BGR) + sourceBuf = OIIO::ImageBufAlgo::channels(sourceBuf, 3, { 2, 1, 0 }); + + OIIO::ImageBuf rotatedBuf; + bool rotateSuccess = false; + switch (degrees) { + case RotateQuantity::DEGREE_90: + rotateSuccess = OIIO::ImageBufAlgo::rotate90(rotatedBuf, sourceBuf); + break; + case RotateQuantity::DEGREE_180: + rotateSuccess = OIIO::ImageBufAlgo::rotate180(rotatedBuf, sourceBuf); + break; + case RotateQuantity::DEGREE_270: + rotateSuccess = OIIO::ImageBufAlgo::rotate270(rotatedBuf, sourceBuf); + break; + default: + // not supported by OpenImageIO + std::cout << "Image rotation which is not 90, 180 or 270 degrees is not supported" << std::endl; + return FrameworkReturnCode::_ERROR_; + } + + if (!rotateSuccess) + return FrameworkReturnCode::_ERROR_; + + if (rotatedBuf.has_error()) + { + std::cout << "error: " << rotatedBuf.geterror() << std::endl; + return FrameworkReturnCode::_ERROR_; + } + + if (degrees == RotateQuantity::DEGREE_90 || degrees == RotateQuantity::DEGREE_270) + setSize(m_size.height, m_size.width); + + std::vector result; + result.resize (m_size.width * m_size.height * m_nbBitsPerComponent * m_nbChannels); + + if (!rotatedBuf.get_pixels(OIIO::ROI::All(), type, &result[0])) + return FrameworkReturnCode::_ERROR_; + m_internalImpl->setData(&result[0], m_internalImpl->getBufferSize()); + return FrameworkReturnCode::_SUCCESS; +} + +FrameworkReturnCode Image::rotate90() +{ + return rotate(Image::RotateQuantity::DEGREE_90); +} + +FrameworkReturnCode Image::rotate180() +{ + return rotate(Image::RotateQuantity::DEGREE_180); +} + +FrameworkReturnCode Image::rotate270() +{ + return rotate(Image::RotateQuantity::DEGREE_270); +} + IMPLEMENTSERIALIZE(Image); } } diff --git a/src/datastructure/Keyframe.cpp b/src/datastructure/Keyframe.cpp index 86a6d171..2dfca882 100644 --- a/src/datastructure/Keyframe.cpp +++ b/src/datastructure/Keyframe.cpp @@ -33,11 +33,30 @@ void Keyframe::setId(const uint32_t& id_keyframe) m_id = id_keyframe; } +bool Keyframe::setKeypointStatusToMatched(uint32_t id_keypoint) +{ + if (m_isKeypointMatched.empty()) { + if (m_keypoints.empty()) + return false; + m_isKeypointMatched.resize(m_keypoints.size(), false); + } + if (id_keypoint >= m_isKeypointMatched.size()) + return false; + m_isKeypointMatched[id_keypoint] = true; + return true; +} + +const std::vector& Keyframe::getIsKeypointMatched() const +{ + return m_isKeypointMatched; +} + template void Keyframe::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { ar & boost::serialization::base_object(*this); ar & boost::serialization::base_object(*this); ar & m_id; + ar & m_isKeypointMatched; } IMPLEMENTSERIALIZE(Keyframe); diff --git a/src/datastructure/KeyframeCollection.cpp b/src/datastructure/KeyframeCollection.cpp index db361323..4eb0c2f1 100644 --- a/src/datastructure/KeyframeCollection.cpp +++ b/src/datastructure/KeyframeCollection.cpp @@ -23,20 +23,29 @@ namespace xpcf = org::bcom::xpcf; namespace SolAR { namespace datastructure { -FrameworkReturnCode KeyframeCollection::addKeyframe(const SRef keyframe) +FrameworkReturnCode KeyframeCollection::addKeyframe(const SRef keyframe, bool defineKeyframeId) { - keyframe->setId(m_id); - m_keyframes[m_id] = keyframe; - m_id++; + uint32_t id; + if (defineKeyframeId==true) + id = m_id++; + else + id = keyframe->getId(); + keyframe->setId(id); + m_keyframes[id] = keyframe; return FrameworkReturnCode::_SUCCESS; } -FrameworkReturnCode KeyframeCollection::addKeyframe(const Keyframe & keyframe) +FrameworkReturnCode KeyframeCollection::addKeyframe(const Keyframe & keyframe, bool defineKeyframeId) { SRef keyframe_ptr = xpcf::utils::make_shared(keyframe); - keyframe_ptr->setId(m_id); - m_keyframes[m_id] = keyframe_ptr; - m_id++; + uint32_t id; + if (defineKeyframeId==true) + id = m_id++; + else + id = keyframe.getId(); + + keyframe_ptr->setId(id); + m_keyframes[id] = keyframe_ptr; return FrameworkReturnCode::_SUCCESS; } diff --git a/src/datastructure/KeyframeRetrieval.cpp b/src/datastructure/KeyframeRetrieval.cpp index d76843e1..0df00c4d 100644 --- a/src/datastructure/KeyframeRetrieval.cpp +++ b/src/datastructure/KeyframeRetrieval.cpp @@ -88,6 +88,13 @@ FrameworkReturnCode KeyframeRetrieval::getInvertedIndex(uint32_t nodeId, std::se return FrameworkReturnCode::_ERROR_; } +void KeyframeRetrieval::reset() +{ + m_listBoWFeature.clear(); + m_listBoWLevelFeature.clear(); + m_invertedIndexKfs.clear(); +} + template void KeyframeRetrieval::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { diff --git a/src/datastructure/Map.cpp b/src/datastructure/Map.cpp index 2bc2e524..afd747cd 100644 --- a/src/datastructure/Map.cpp +++ b/src/datastructure/Map.cpp @@ -29,6 +29,7 @@ Map::Map() m_coordinateSystem = xpcf::utils::make_shared(); m_pointCloud = xpcf::utils::make_shared(); m_keyframeCollection = xpcf::utils::make_shared(); + m_cameraParametersCollection = xpcf::utils::make_shared(); m_covisibilityGraph = xpcf::utils::make_shared(); m_keyframeRetrieval = xpcf::utils::make_shared(); } @@ -133,6 +134,28 @@ void Map::setKeyframeRetrieval(const SRef keyframeRetrieval) m_keyframeRetrieval = keyframeRetrieval; } +const SRef& Map::getConstCameraParametersCollection() const +{ + return m_cameraParametersCollection; +} + +std::unique_lock Map::getCameraParametersCollection(SRef& cameraParametersCollection) +{ + cameraParametersCollection = m_cameraParametersCollection; + return m_cameraParametersCollection->acquireLock(); +} + +void Map::setCameraParametersCollection(const SRef cameraParametersCollection) +{ + m_mapSupportedTypes = m_mapSupportedTypes | MapType::_CameraParemeters; + m_cameraParametersCollection = cameraParametersCollection; +} + +TrackableType Map::getType() const +{ + return TrackableType::MAP; +} + template void Map::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { ar & m_mapSupportedTypes; @@ -142,6 +165,8 @@ void Map::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int vers ar & m_keyframeCollection; ar & m_covisibilityGraph; ar & m_keyframeRetrieval; + ar & m_cameraParametersCollection; + ar & m_transform3D; } IMPLEMENTSERIALIZE(Map); diff --git a/src/datastructure/Mesh.cpp b/src/datastructure/Mesh.cpp new file mode 100644 index 00000000..26b07199 --- /dev/null +++ b/src/datastructure/Mesh.cpp @@ -0,0 +1,53 @@ +/** + * @copyright Copyright (c) 2017 B-com http://www.b-com.com/ + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "datastructure/Mesh.h" + +#include "xpcf/core/helpers.h" + +#include //TO DO: remove with a complete implementation + +namespace SolAR { +namespace datastructure { + +Mesh::~Mesh(){ + +} + +Mesh::Mesh(const std::vector points, + const std::vector indexedFaceSets, + const std::vector normals, + const std::vector colors, + const std::vector texCoordinates): + m_points(points), m_indexedFaceSets(indexedFaceSets), m_normals(normals), m_colors(colors), m_texCoordinates(texCoordinates) +{ + +} + + +template +void Mesh::serialize(Archive &ar, ATTRIBUTE(maybe_unused) const unsigned int version) { + ar & m_points; + ar & m_indexedFaceSets; + ar & m_normals; + ar & m_colors; + ar & m_texCoordinates; +} + +IMPLEMENTSERIALIZE(Mesh); + +} +} diff --git a/src/datastructure/PointCloud.cpp b/src/datastructure/PointCloud.cpp index 95df1ad0..56fe885f 100644 --- a/src/datastructure/PointCloud.cpp +++ b/src/datastructure/PointCloud.cpp @@ -50,14 +50,26 @@ FrameworkReturnCode PointCloud::addPoint(const CloudPoint & point) return FrameworkReturnCode::_SUCCESS; } -FrameworkReturnCode PointCloud::addPoints(const std::vector& points) +FrameworkReturnCode PointCloud::addPoints(const std::vector& points, bool definePointId) { - for (auto &it : points) { + if (definePointId) + { + for (auto &it : points) + { SRef point_ptr = xpcf::utils::make_shared(it); point_ptr->setId(m_id); m_pointCloud[m_id] = point_ptr; m_id++; + } } + else + { + for (auto &it : points) + { + SRef point_ptr = xpcf::utils::make_shared(it); + m_pointCloud[point_ptr->getId()] = point_ptr; + } + } return FrameworkReturnCode::_SUCCESS; }