Cleanup bundle

Author: Fabien Servant

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -816,7 +816,8 @@ void BundleAdjustmentCeres::resetProblem()
     _intrinsicsBlocks.clear();
     _landmarksBlocks.clear();
     _rigBlocks.clear();
-
+    _intrinsicObjects.clear();
+    _distortionsBlocks.clear();
     _linearSolverOrdering.Clear();
 }
 
@@ -833,22 +834,15 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
     if (refinePoses)
     {
         // absolute poses
-        for (auto& posePair : sfmData.getPoses())
+        for (auto& [poseId, pose] : sfmData.getPoses())
         {
-            const IndexT poseId = posePair.first;
-
-            // do not update a camera pose set as Ignored or Constant in the Local strategy
-            if (posePair.second.getState() != EEstimatorParameterState::REFINED)
+            if (pose.getState() != EEstimatorParameterState::REFINED)
+            {
                 continue;
+            }
 
-            const std::array<double, 6>& poseBlock = _posesBlocks.at(poseId);
-
-            Mat3 R_refined;
-            ceres::AngleAxisToRotationMatrix(poseBlock.data(), R_refined.data());
-            const Vec3 t_refined(poseBlock.at(3), poseBlock.at(4), poseBlock.at(5));
-
-            // update the pose
-            posePair.second.setTransform(poseFromRT(R_refined, t_refined));
+            const std::array<double, 6> & poseBlock = _posesBlocks.at(poseId);
+            pose.updateFromEstimator(poseBlock);
         }
 
         // rig sub-poses
@@ -874,19 +868,17 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
     // update camera intrinsics with refined data
     if (refineIntrinsics)
     {
-        for (const auto& intrinsicBlockPair : _intrinsicsBlocks)
+        for (const auto& [idIntrinsic, block] : _intrinsicsBlocks)
         {
-            const IndexT intrinsicId = intrinsicBlockPair.first;
-
-            const auto& intrinsic = sfmData.getIntrinsicSharedPtr(intrinsicId);
+            const auto& intrinsic = sfmData.getIntrinsicSharedPtr(idIntrinsic);
 
             // do not update a camera pose set as Ignored or Constant in the Local strategy
             if (intrinsic->getState() != EEstimatorParameterState::REFINED)
             {
                 continue;
             }
 
-            sfmData.getIntrinsics().at(intrinsicId)->updateFromParams(intrinsicBlockPair.second);
+            sfmData.getIntrinsics().at(idIntrinsic)->updateFromParams(block);
         }
 
         for (const auto& [idIntrinsic, distortionBlock]: _distortionsBlocks)
@@ -914,21 +906,10 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
     // update landmarks
     if (refineStructure)
     {
-        for (const auto& landmarksBlockPair : _landmarksBlocks)
+        for (const auto& [idLandmark, block] : _landmarksBlocks)
         {
-            const IndexT landmarkId = landmarksBlockPair.first;
-            sfmData::Landmark& landmark = sfmData.getLandmarks().at(landmarkId);
-
-            // do not update a camera pose set as Ignored or Constant in the Local strategy
-            if (landmark.state != EEstimatorParameterState::REFINED)
-            {
-                continue;
-            }
-
-            for (std::size_t i = 0; i < 3; ++i)
-            {
-                landmark.X(Eigen::Index(i)) = landmarksBlockPair.second.at(i);
-            }
+            sfmData::Landmark& landmark = sfmData.getLandmarks().at(idLandmark);
+            landmark.updateFromEstimator(block);
         }
     }
 }

src/aliceVision/sfmData/CameraPose.hpp

@@ -111,6 +111,23 @@ class CameraPose
         return _removable;
     }
 
+    inline void updateFromEstimator(const std::array<double, 6> & data) 
+    {
+        // do not update a camera pose set as Ignored or Constant in the Local strategy
+        if (getState() != EEstimatorParameterState::REFINED)
+        {
+            return;
+        }
+        
+        const Vec3 r_refined(data.at(0), data.at(1), data.at(2));
+        const Vec3 t_refined(data.at(3), data.at(4), data.at(5));
+
+        const Mat3 R_refined = SO3::expm(r_refined);
+
+        // update the pose
+        setTransform(geometry::poseFromRT(R_refined, t_refined));
+    }
+
   private:
     /// camera 3d transformation
     geometry::Pose3 _transform;

src/aliceVision/sfmData/Landmark.hpp

@@ -58,6 +58,20 @@ class Landmark
     { 
         return MapObservations(_observations.begin(), _observations.end());;
     }
+    
+    inline void updateFromEstimator(const std::array<double, 3> & data)
+    {
+        // do not update a landmark set as Ignored or Constant in the Local strategy
+        if (state != EEstimatorParameterState::REFINED)
+        {
+            return;
+        }
+
+        for (std::size_t i = 0; i < 3; ++i)
+        {
+            X(Eigen::Index(i)) = data.at(i);
+        }
+    }
 
   private:
     Observations _observations;