Merge pull request #1963 from alicevision/dev/cameraPosePtr

Moving poses to shared pointers

Author: cbentejac

meshroom/aliceVision/ImportKnownPoses.py

@@ -1303,7 +1303,7 @@ bool KeyframeSelector::writeSfMDataFromSfMData(const std::string& mediaPath)
             _selectedFrames[i] = '1';
 
             //Make sure to keep the pose
-            keyframesPoses.emplace(poseId, inputSfm.getPoses().at(poseId));
+            keyframesPoses.assign(poseId, inputSfm.getAbsolutePose(poseId));
         }
 
         if (_selectedFrames[i] == '1')

src/aliceVision/keyframe/KeyframeSelector.cpp

@@ -281,10 +281,9 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,
     if (b_BA_Status)
     {
         // get back the results and update the localization result with the refined pose
-        for (const auto& pose : tinyScene.getPoses())
+        for (const auto& [poseId, pose] : tinyScene.getPoses().valueRange())
         {
-            const IndexT idPose = pose.first;
-            vec_localizationResult[idPose].setPose(pose.second.getTransform());
+            vec_localizationResult[poseId].setPose(pose.getTransform());
         }
 
         if (!outputFilename.empty())

src/aliceVision/localization/optimization.cpp

@@ -95,10 +95,10 @@ void normalIntegration(const sfmData::SfMData& sfmData,
 
     if (sfmData.getPoses().size() > 0)
     {
-        for (auto& poseIt : sfmData.getPoses())
+        for (auto& [idPose, cameraPose] : sfmData.getPoses().valueRange())
         {
             // Read associated normal map :
-            image::readImage(inputPath + "/" + std::to_string(poseIt.first) + "_normals.png", normalsImPNG, image::EImageColorSpace::NO_CONVERSION);
+            image::readImage(inputPath + "/" + std::to_string(idPose) + "_normals.png", normalsImPNG, image::EImageColorSpace::NO_CONVERSION);
 
             int nbCols = normalsImPNG.cols();
             int nbRows = normalsImPNG.rows();
@@ -107,7 +107,7 @@ void normalIntegration(const sfmData::SfMData& sfmData,
             for (auto& viewIt : sfmData.getViews())
             {
                 poseId = viewIt.second->getPoseId();
-                if (poseId == poseIt.first)
+                if (poseId == idPose)
                 {
                     viewId = viewIt.first;
                     // Get intrinsics associated with this view :
@@ -184,10 +184,10 @@ void normalIntegration(const sfmData::SfMData& sfmData,
             // AliceVision uses distance-to-origin convention
             convertZtoDistance(depthMap, distanceMap, K);
 
-            std::string pathToDM = outputFolder + "/" + std::to_string(poseIt.first) + "_depthMap.exr";
+            std::string pathToDM = outputFolder + "/" + std::to_string(idPose) + "_depthMap.exr";
 
             // Create pose for metadata
-            const geometry::Pose3 pose = poseIt.second.getTransform();
+            const geometry::Pose3 pose = cameraPose.getTransform();
             std::shared_ptr<camera::IntrinsicBase> cam = sfmData.getIntrinsics().at(intrinsicId);
             std::shared_ptr<camera::Pinhole> camPinHole = std::dynamic_pointer_cast<camera::Pinhole>(cam);
             Mat34 P = camPinHole->getProjectiveEquivalent(pose);

src/aliceVision/photometricStereo/normalIntegration.cpp

@@ -60,8 +60,10 @@ void LocalBundleAdjustmentGraph::setAllParametersToRefine(const sfmData::SfMData
     _statePerLandmarkId.clear();
 
     // poses
-    for (sfmData::Poses::const_iterator itPose = sfmData.getPoses().begin(); itPose != sfmData.getPoses().end(); ++itPose)
-        _statePerPoseId[itPose->first] = EEstimatorParameterState::REFINED;
+    for (const auto & [poseId, _] : sfmData.getPoses())
+    {
+        _statePerPoseId[poseId] = EEstimatorParameterState::REFINED;
+    }
 
     // intrinsics
     for (const auto& itIntrinsic : sfmData.getIntrinsics())
@@ -383,13 +385,12 @@ void LocalBundleAdjustmentGraph::convertDistancesToStates(sfmData::SfMData& sfmD
     //    - Refined <=> its connected to a refined camera
 
     // poses
-    for (auto& posePair : sfmData.getPoses())
+    for (auto& [poseId, pose] : sfmData.getPoses().valueRange())
     {
-        const IndexT poseId = posePair.first;
         const int distance = getPoseDistance(poseId);
         const EEstimatorParameterState state = getStateFromDistance(distance);
 
-        posePair.second.setState(state);
+        pose.setState(state);
 
         _statePerPoseId[poseId] = state;
     }

src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp

@@ -278,11 +278,8 @@ void BundleAdjustmentCeres::addExtrinsicsToProblem(const sfmData::SfMData& sfmDa
     };
 
     // setup poses data
-    for (const auto& posePair : sfmData.getPoses())
+    for (const auto& [poseId, pose] : sfmData.getPoses().valueRange())
     {
-        const IndexT poseId = posePair.first;
-        const sfmData::CameraPose& pose = posePair.second;
-
         // skip camera pose set as Ignored in the Local strategy
         if (pose.getState() == EEstimatorParameterState::IGNORED)
         {
@@ -834,8 +831,9 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
     if (refinePoses)
     {
         // absolute poses
-        for (auto& [poseId, pose] : sfmData.getPoses())
+        for (auto & [poseId, pose] : sfmData.getPoses().valueRange())
         {
+            // do not update a camera pose set as Ignored or Constant in the Local strategy
             if (pose.getState() != EEstimatorParameterState::REFINED)
             {
                 continue;

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -98,7 +98,7 @@ void createScene(sfmData::SfMData& sfmData, const camera::IntrinsicBase& intrins
 
         geometry::Pose3 pose(T);
 
-        sfmData.getPoses().emplace(idview, sfmData::CameraPose(pose));
+        sfmData.getPoses().assign(idview, sfmData::CameraPose(pose));
         sfmData.getViews().emplace(idview, std::make_shared<sfmData::View>("", idview, 0, idview, intrinsic.w(), intrinsic.h()));
 
         Vec3 origin = pose(Vec3(0, 0, 0));
@@ -244,9 +244,9 @@ BOOST_AUTO_TEST_CASE(test_poses)
         createScene(sfmData, *pairIntrinsics.first);
 
         srand(0);
-        for (auto& lps : sfmData.getPoses())
+        for (auto& [_, pose] : sfmData.getPoses().valueRange())
         {
-            geometry::Pose3 pose3 = lps.second.getTransform();
+            geometry::Pose3 pose3 = pose.getTransform();
             Eigen::Matrix4d T = pose3.getHomogeneous();
 
             Eigen::Vector3d nt = Eigen::Vector3d::Random() * 0.1 + Eigen::Vector3d::UnitZ() * 0.1;
@@ -256,7 +256,7 @@ BOOST_AUTO_TEST_CASE(test_poses)
             U.block<3, 3>(0, 0) = nR;
             U.block<3, 1>(0, 3) = nt;
 
-            lps.second.setTransform(geometry::Pose3(U * T));
+            pose.setTransform(geometry::Pose3(U * T));
         }
 
         sfm::BundleAdjustmentCeres::CeresOptions options;

src/aliceVision/sfm/bundle/bundleAdjustment_Enhanced_test.cpp

@@ -233,7 +233,7 @@ void RigSequence::setupRelativePoses()
         SubPoseInfo& subPoseInfo = _rigInfoPerSubPose.at(subPoseId);
 
         const View& bestIndependantView = _sfmData.getView(_rigInfoPerFrame.at(subPoseInfo.maxFrameId).at(subPoseId).viewId);
-        const geometry::Pose3& bestIndependantPose = _sfmData.getPoses().at(bestIndependantView.getPoseId()).getTransform();
+        const geometry::Pose3& bestIndependantPose = _sfmData.getAbsolutePose(bestIndependantView.getPoseId()).getTransform();
 
         RigSubPose& newSubPose = _rig.getSubPose(subPoseId);
         newSubPose.status = ERigSubPoseStatus::ESTIMATED;
@@ -242,7 +242,7 @@ void RigSequence::setupRelativePoses()
 
         if (rigInitialized)
         {
-            const geometry::Pose3& rigPose = _sfmData.getPoses().at(getRigPoseId(_rigId, subPoseInfo.maxFrameId)).getTransform();
+            const geometry::Pose3& rigPose = _sfmData.getAbsolutePose(getRigPoseId(_rigId, subPoseInfo.maxFrameId)).getTransform();
             newSubPose.pose = bestIndependantPose * rigPose.inverse();
         }  // else pose is Identity
     }
@@ -287,13 +287,13 @@ void RigSequence::rigResection(std::set<IndexT>& updatedViews)
             const RigSubPose& subPose = _rig.getSubPose(bestSubPoseId);
             const View& view = _sfmData.getView(rigFrame.at(bestSubPoseId).viewId);
             const IndexT independantPoseId = view.getPoseId();
-            const CameraPose independantPose = _sfmData.getPoses().at(independantPoseId);
+            const CameraPose independantPose = _sfmData.getAbsolutePose(independantPoseId);
             const CameraPose rigPose(independantPose.getTransform() * subPose.pose.inverse());
 
             ALICEVISION_LOG_DEBUG("Add rig pose:"
                                   << "\n\t- rig pose id: " << rigPoseId << "\n\t- frame id: " << frameId << "\n\t- sub-pose id: " << bestSubPoseId);
 
-            _sfmData.getPoses()[rigPoseId] = rigPose;
+            _sfmData.getPoses().assign(rigPoseId, rigPose);
         }
 
         // remove independent poses and replace with rig pose

src/aliceVision/sfm/pipeline/RigSequence.cpp

@@ -28,8 +28,8 @@ bool bootstrapBase(sfmData::SfMData & sfmData,
     std::shared_ptr<camera::IntrinsicBase> nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId());
 
     sfmData::CameraPose cposeNext(otherTreference, false);
-    sfmData.getPoses()[refView.getPoseId()] = sfmData::CameraPose();
-    sfmData.getPoses()[nextView.getPoseId()] = cposeNext;
+    sfmData.getPoses().assign(refView.getPoseId(), sfmData::CameraPose());
+    sfmData.getPoses().assign(nextView.getPoseId(), cposeNext);
 
     const Mat4 T1 = Eigen::Matrix4d::Identity();
     Mat4 T2 = otherTreference.getHomogeneous();
@@ -122,7 +122,7 @@ bool bootstrapMesh(sfmData::SfMData & sfmData,
 
     geometry::Pose3 pose3(pose);
     sfmData::CameraPose cpose(pose3, false);
-    sfmData.getPoses()[viewId] = cpose;
+    sfmData.getPoses().assign(viewId, cpose);
 
     // Cleanup output
     sfmData::Landmarks & outLandmarks = sfmData.getLandmarks();

src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.cpp

@@ -32,27 +32,27 @@ void LbaPolicyConnexity::upgradeSfmData(sfmData::SfMData & sfmData, const Connex
     /**
      * Propagate states to views in sfmData
     */
-    for (auto & pp : sfmData.getPoses())
+    for (auto & [poseId, pose] : sfmData.getPoses().valueRange())
     {
-        int d = graph.getDistance(pp.first);
+        int d = graph.getDistance(poseId);
         if (d <= _distanceLimit)
         {
-            if (pp.second.isLocked())
+            if (pose.isLocked())
             {
-                pp.second.setState(EEstimatorParameterState::CONSTANT);
+                pose.setState(EEstimatorParameterState::CONSTANT);
             }
             else
             {
-                pp.second.setState(EEstimatorParameterState::REFINED);
+                pose.setState(EEstimatorParameterState::REFINED);
             }
         }
         else if (d == (_distanceLimit + 1))
         {
-            pp.second.setState(EEstimatorParameterState::CONSTANT);
+            pose.setState(EEstimatorParameterState::CONSTANT);
         }
         else 
         {
-            pp.second.setState(EEstimatorParameterState::IGNORED);
+            pose.setState(EEstimatorParameterState::IGNORED);
         }
     }