Skip to content

Commit 84f4d04

Browse files
servantftransperfectservantftransperfect
authored
Merge pull request #1933 from alicevision/dev/cleanupBundle
Cleanup bundle code
2 parents 988d198 + 7ef8448 commit 84f4d04

File tree

10 files changed

+197
-219
lines changed

10 files changed

+197
-219
lines changed

‎src/aliceVision/geometry/CMakeLists.txt‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@ set(geometry_files_headers
55
lie.hpp
66
Pose3.hpp
77
rigidTransformation3D.hpp
8-
Similarity3.hpp
98
Intersection.hpp
109
)
1110

‎src/aliceVision/geometry/Geometry.i‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,5 +25,4 @@
2525
%include <aliceVision/geometry/Intersection.i>
2626
%include <aliceVision/geometry/Pose3.i>
2727
%include <aliceVision/geometry/RigidTransformation3D.i>
28-
%include <aliceVision/geometry/Similarity3.i>
29-
%include <aliceVision/geometry/lie.i>
28+
%include <aliceVision/geometry/lie.i>

‎src/aliceVision/geometry/Similarity3.hpp‎

Lines changed: 0 additions & 37 deletions
This file was deleted.

‎src/aliceVision/geometry/Similarity3.i‎

Lines changed: 0 additions & 11 deletions
This file was deleted.

‎src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp‎

Lines changed: 20 additions & 168 deletions
Original file line numberDiff line numberDiff line change
@@ -30,135 +30,6 @@ namespace sfm {
3030
using namespace aliceVision::camera;
3131
using namespace aliceVision::geometry;
3232

33-
/**
34-
* @brief Create the appropriate cost functor according the provided input camera intrinsic model
35-
* @param[in] intrinsicPtr The intrinsic pointer
36-
* @param[in] observation The corresponding observation
37-
* @return cost functor
38-
*/
39-
ceres::CostFunction* createCostFunctionFromIntrinsics(const std::shared_ptr<IntrinsicBase> intrinsic, const sfmData::Observation& observation)
40-
{
41-
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionSimpleErrorFunctor>(new ProjectionSimpleErrorFunctor(observation, intrinsic));
42-
43-
int distortionSize = 1;
44-
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
45-
if (isod)
46-
{
47-
auto distortion = isod->getDistortion();
48-
if (distortion)
49-
{
50-
distortionSize = distortion->getParameters().size();
51-
}
52-
}
53-
54-
costFunction->AddParameterBlock(intrinsic->getParameters().size());
55-
costFunction->AddParameterBlock(distortionSize);
56-
costFunction->AddParameterBlock(6);
57-
costFunction->AddParameterBlock(3);
58-
costFunction->SetNumResiduals(2);
59-
60-
return costFunction;
61-
}
62-
63-
ceres::CostFunction* createSurveyPointCostFunction(const std::shared_ptr<IntrinsicBase> intrinsic,
64-
const Vec3 & point,
65-
const sfmData::Observation& observation)
66-
{
67-
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionSurveyErrorFunctor>(new ProjectionSurveyErrorFunctor(point, observation, intrinsic));
68-
69-
int distortionSize = 1;
70-
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
71-
if (isod)
72-
{
73-
auto distortion = isod->getDistortion();
74-
if (distortion)
75-
{
76-
distortionSize = distortion->getParameters().size();
77-
}
78-
}
79-
80-
costFunction->AddParameterBlock(intrinsic->getParameters().size());
81-
costFunction->AddParameterBlock(distortionSize);
82-
costFunction->AddParameterBlock(6);
83-
costFunction->SetNumResiduals(2);
84-
85-
return costFunction;
86-
}
87-
88-
/**
89-
* @brief Create the appropriate cost functor according the provided input rig camera intrinsic model
90-
* @param[in] intrinsicPtr The intrinsic pointer
91-
* @param[in] observation The corresponding observation
92-
* @return cost functor
93-
*/
94-
ceres::CostFunction* createRigCostFunctionFromIntrinsics(std::shared_ptr<IntrinsicBase> intrinsic, const sfmData::Observation& observation)
95-
{
96-
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionErrorFunctor>(new ProjectionErrorFunctor(observation, intrinsic));
97-
98-
int distortionSize = 1;
99-
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
100-
if (isod)
101-
{
102-
auto distortion = isod->getDistortion();
103-
if (distortion)
104-
{
105-
distortionSize = distortion->getParameters().size();
106-
}
107-
}
108-
109-
costFunction->AddParameterBlock(intrinsic->getParameters().size());
110-
costFunction->AddParameterBlock(distortionSize);
111-
costFunction->AddParameterBlock(6);
112-
costFunction->AddParameterBlock(6);
113-
costFunction->AddParameterBlock(3);
114-
costFunction->SetNumResiduals(2);
115-
116-
return costFunction;
117-
}
118-
119-
/**
120-
* @brief Create the appropriate cost functor according the provided input camera intrinsic model
121-
* @param[in] intrinsicPtr The intrinsic pointer
122-
* @param[in] observation The corresponding observation
123-
* @return cost functor
124-
*/
125-
ceres::CostFunction* createConstraintsCostFunctionFromIntrinsics(std::shared_ptr<IntrinsicBase> intrinsic,
126-
const sfmData::Observation& observation_first,
127-
const sfmData::Observation& observation_second)
128-
{
129-
auto costFunction = new ceres::DynamicAutoDiffCostFunction<Constraint2dErrorFunctor>(new Constraint2dErrorFunctor(observation_first, observation_second, intrinsic));
130-
131-
int distortionSize = 1;
132-
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
133-
if (isod)
134-
{
135-
auto distortion = isod->getDistortion();
136-
if (distortion)
137-
{
138-
distortionSize = distortion->getParameters().size();
139-
}
140-
}
141-
142-
143-
costFunction->AddParameterBlock(intrinsic->getParameters().size());
144-
costFunction->AddParameterBlock(distortionSize);
145-
costFunction->AddParameterBlock(6);
146-
costFunction->AddParameterBlock(6);
147-
costFunction->SetNumResiduals(2);
148-
149-
return costFunction;
150-
}
151-
152-
ceres::CostFunction* createCostFunctionFromContraintPoint(const sfmData::Landmark & landmark, const Vec3 & normal)
153-
{
154-
const double weight = 100.0;
155-
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ConstraintPointErrorFunctor>(new ConstraintPointErrorFunctor(weight, normal, landmark.X));
156-
157-
costFunction->AddParameterBlock(3);
158-
costFunction->SetNumResiduals(1);
159-
160-
return costFunction;
161-
}
16233

16334
void BundleAdjustmentCeres::CeresOptions::setDenseBA()
16435
{
@@ -708,7 +579,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
708579

709580
if (view.isPartOfRig() && !view.isPoseIndependant())
710581
{
711-
ceres::CostFunction* costFunction = createRigCostFunctionFromIntrinsics(intrinsic, observation);
582+
ceres::CostFunction* costFunction = ProjectionErrorFunctor::createCostFunction(intrinsic, observation);
712583

713584
double* rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data();
714585
_linearSolverOrdering.AddElementToGroup(rigBlockPtr, 1);
@@ -724,7 +595,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
724595
}
725596
else
726597
{
727-
ceres::CostFunction* costFunction = createCostFunctionFromIntrinsics(intrinsic, observation);
598+
ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation);
728599

729600
std::vector<double*> params;
730601
params.push_back(intrinsicBlockPtr);
@@ -788,8 +659,8 @@ void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfm
788659
for (const auto & spoint: vspoints)
789660
{
790661
sfmData::Observation observation(spoint.survey, 0, 1.0);
791-
ceres::CostFunction* costFunction = createSurveyPointCostFunction(intrinsic, spoint.point3d, observation);
792-
662+
ceres::CostFunction* costFunction =
663+
ProjectionSurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, observation);
793664

794665
std::vector<double*> params;
795666
params.push_back(intrinsicBlockPtr);
@@ -844,7 +715,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf
844715
distortionBlockPtr_1 = _distortionsBlocks.at(intrinsicId_1).data();
845716
}
846717

847-
ceres::CostFunction* costFunction = createConstraintsCostFunctionFromIntrinsics(intrinsicObject1,
718+
ceres::CostFunction* costFunction = Constraint2dErrorFunctor::createCostFunction(intrinsicObject1,
848719
constraint.ObservationFirst,
849720
constraint.ObservationSecond);
850721

@@ -873,7 +744,7 @@ void BundleAdjustmentCeres::addConstraintsPointToProblem(const sfmData::SfMData&
873744
const sfmData::Landmark & l = sfmData.getLandmarks().at(landmarkId);
874745
double * ldata = _landmarksBlocks.at(landmarkId).data();
875746

876-
ceres::CostFunction* costFunction = createCostFunctionFromContraintPoint(l, constraint.normal);
747+
ceres::CostFunction* costFunction = ConstraintPointErrorFunctor::createCostFunction(l, constraint.normal);
877748

878749
problem.AddResidualBlock(costFunction, lossFunction, ldata);
879750
}
@@ -945,7 +816,8 @@ void BundleAdjustmentCeres::resetProblem()
945816
_intrinsicsBlocks.clear();
946817
_landmarksBlocks.clear();
947818
_rigBlocks.clear();
948-
819+
_intrinsicObjects.clear();
820+
_distortionsBlocks.clear();
949821
_linearSolverOrdering.Clear();
950822
}
951823

@@ -962,22 +834,15 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
962834
if (refinePoses)
963835
{
964836
// absolute poses
965-
for (auto& posePair : sfmData.getPoses())
837+
for (auto& [poseId, pose] : sfmData.getPoses())
966838
{
967-
const IndexT poseId = posePair.first;
968-
969-
// do not update a camera pose set as Ignored or Constant in the Local strategy
970-
if (posePair.second.getState() != EEstimatorParameterState::REFINED)
839+
if (pose.getState() != EEstimatorParameterState::REFINED)
840+
{
971841
continue;
842+
}
972843

973-
const std::array<double, 6>& poseBlock = _posesBlocks.at(poseId);
974-
975-
Mat3 R_refined;
976-
ceres::AngleAxisToRotationMatrix(poseBlock.data(), R_refined.data());
977-
const Vec3 t_refined(poseBlock.at(3), poseBlock.at(4), poseBlock.at(5));
978-
979-
// update the pose
980-
posePair.second.setTransform(poseFromRT(R_refined, t_refined));
844+
const std::array<double, 6> & poseBlock = _posesBlocks.at(poseId);
845+
pose.updateFromEstimator(poseBlock);
981846
}
982847

983848
// rig sub-poses
@@ -1003,19 +868,17 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
1003868
// update camera intrinsics with refined data
1004869
if (refineIntrinsics)
1005870
{
1006-
for (const auto& intrinsicBlockPair : _intrinsicsBlocks)
871+
for (const auto& [idIntrinsic, block] : _intrinsicsBlocks)
1007872
{
1008-
const IndexT intrinsicId = intrinsicBlockPair.first;
1009-
1010-
const auto& intrinsic = sfmData.getIntrinsicSharedPtr(intrinsicId);
873+
const auto& intrinsic = sfmData.getIntrinsicSharedPtr(idIntrinsic);
1011874

1012875
// do not update a camera pose set as Ignored or Constant in the Local strategy
1013876
if (intrinsic->getState() != EEstimatorParameterState::REFINED)
1014877
{
1015878
continue;
1016879
}
1017880

1018-
sfmData.getIntrinsics().at(intrinsicId)->updateFromParams(intrinsicBlockPair.second);
881+
sfmData.getIntrinsics().at(idIntrinsic)->updateFromParams(block);
1019882
}
1020883

1021884
for (const auto& [idIntrinsic, distortionBlock]: _distortionsBlocks)
@@ -1043,21 +906,10 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin
1043906
// update landmarks
1044907
if (refineStructure)
1045908
{
1046-
for (const auto& landmarksBlockPair : _landmarksBlocks)
909+
for (const auto& [idLandmark, block] : _landmarksBlocks)
1047910
{
1048-
const IndexT landmarkId = landmarksBlockPair.first;
1049-
sfmData::Landmark& landmark = sfmData.getLandmarks().at(landmarkId);
1050-
1051-
// do not update a camera pose set as Ignored or Constant in the Local strategy
1052-
if (landmark.state != EEstimatorParameterState::REFINED)
1053-
{
1054-
continue;
1055-
}
1056-
1057-
for (std::size_t i = 0; i < 3; ++i)
1058-
{
1059-
landmark.X(Eigen::Index(i)) = landmarksBlockPair.second.at(i);
1060-
}
911+
sfmData::Landmark& landmark = sfmData.getLandmarks().at(idLandmark);
912+
landmark.updateFromEstimator(block);
1061913
}
1062914
}
1063915
}

‎src/aliceVision/sfm/bundle/costfunctions/constraint2d.hpp‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,38 @@ struct Constraint2dErrorFunctor
5454
return _intrinsicProjectFunctor(projectParameters, residuals);
5555
}
5656

57+
/**
58+
* @brief Create the appropriate cost functor according the provided input camera intrinsic model
59+
* @param[in] intrinsicPtr The intrinsic pointer
60+
* @param[in] observation The corresponding observation
61+
* @return cost functor
62+
*/
63+
inline static ceres::CostFunction* createCostFunction(std::shared_ptr<camera::IntrinsicBase> intrinsic,
64+
const sfmData::Observation& observation_first,
65+
const sfmData::Observation& observation_second)
66+
{
67+
auto costFunction = new ceres::DynamicAutoDiffCostFunction<Constraint2dErrorFunctor>(new Constraint2dErrorFunctor(observation_first, observation_second, intrinsic));
68+
69+
int distortionSize = 1;
70+
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
71+
if (isod)
72+
{
73+
auto distortion = isod->getDistortion();
74+
if (distortion)
75+
{
76+
distortionSize = distortion->getParameters().size();
77+
}
78+
}
79+
80+
costFunction->AddParameterBlock(intrinsic->getParameters().size());
81+
costFunction->AddParameterBlock(distortionSize);
82+
costFunction->AddParameterBlock(6);
83+
costFunction->AddParameterBlock(6);
84+
costFunction->SetNumResiduals(2);
85+
86+
return costFunction;
87+
}
88+
5789
ceres::DynamicCostFunctionToFunctorTmp _intrinsicLiftFunctor;
5890
ceres::DynamicCostFunctionToFunctorTmp _intrinsicProjectFunctor;
5991
};

‎src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,23 @@ struct ConstraintPointErrorFunctor
3030
return true;
3131
}
3232

33+
/**
34+
* Create cost function for this class
35+
* @param landmark the point coordinates
36+
* @param normal the tangent surface to this point
37+
* @return a cost function
38+
*/
39+
inline static ceres::CostFunction* createCostFunction(const sfmData::Landmark & landmark, const Vec3 & normal)
40+
{
41+
const double weight = 100.0;
42+
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ConstraintPointErrorFunctor>(new ConstraintPointErrorFunctor(weight, normal, landmark.X));
43+
44+
costFunction->AddParameterBlock(3);
45+
costFunction->SetNumResiduals(1);
46+
47+
return costFunction;
48+
}
49+
3350
double _weight;
3451
Vec3 _normal;
3552
double _constraintDistance;

0 commit comments

Comments
 (0)