Skip to content

Relative landmarks Support #1859

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 6 commits into
base: develop
Choose a base branch
from
Draft

Relative landmarks Support #1859

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
0068864
Move cost function creation to classes
Apr 30, 2025
c232dc8
remove unused class
Apr 30, 2025
57386c9
Add relative landmarks concept
Apr 30, 2025
203f9ff
force depth cost function
Apr 30, 2025
3ce0a3e
clearing values in sfm
Apr 30, 2025
9386565
Cleanup sfm update of solution
Apr 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion src/aliceVision/geometry/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@ set(geometry_files_headers
lie.hpp
Pose3.hpp
rigidTransformation3D.hpp
Similarity3.hpp
Intersection.hpp
)

Expand Down
3 changes: 1 addition & 2 deletions src/aliceVision/geometry/Geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,4 @@
%include <aliceVision/geometry/HalfPlane.i>
%include <aliceVision/geometry/Intersection.i>
%include <aliceVision/geometry/Pose3.i>
%include <aliceVision/geometry/RigidTransformation3D.i>
%include <aliceVision/geometry/Similarity3.i>
%include <aliceVision/geometry/RigidTransformation3D.i>
37 changes: 0 additions & 37 deletions src/aliceVision/geometry/Similarity3.hpp
View file
Open in desktop

This file was deleted.

11 changes: 0 additions & 11 deletions src/aliceVision/geometry/Similarity3.i
View file
Open in desktop

This file was deleted.

221 changes: 50 additions & 171 deletions src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,135 +30,6 @@
using namespace aliceVision::camera;
using namespace aliceVision::geometry;

/**
* @brief Create the appropriate cost functor according the provided input camera intrinsic model
* @param[in] intrinsicPtr The intrinsic pointer
* @param[in] observation The corresponding observation
* @return cost functor
*/
ceres::CostFunction* createCostFunctionFromIntrinsics(const std::shared_ptr<IntrinsicBase> intrinsic, const sfmData::Observation& observation)
{
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionSimpleErrorFunctor>(new ProjectionSimpleErrorFunctor(observation, intrinsic));

int distortionSize = 1;
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
if (isod)
{
auto distortion = isod->getDistortion();
if (distortion)
{
distortionSize = distortion->getParameters().size();
}
}

costFunction->AddParameterBlock(intrinsic->getParameters().size());
costFunction->AddParameterBlock(distortionSize);
costFunction->AddParameterBlock(6);
costFunction->AddParameterBlock(3);
costFunction->SetNumResiduals(2);

return costFunction;
}

ceres::CostFunction* createSurveyPointCostFunction(const std::shared_ptr<IntrinsicBase> intrinsic,
const Vec3 & point,
const sfmData::Observation& observation)
{
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionSurveyErrorFunctor>(new ProjectionSurveyErrorFunctor(point, observation, intrinsic));

int distortionSize = 1;
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
if (isod)
{
auto distortion = isod->getDistortion();
if (distortion)
{
distortionSize = distortion->getParameters().size();
}
}

costFunction->AddParameterBlock(intrinsic->getParameters().size());
costFunction->AddParameterBlock(distortionSize);
costFunction->AddParameterBlock(6);
costFunction->SetNumResiduals(2);

return costFunction;
}

/**
* @brief Create the appropriate cost functor according the provided input rig camera intrinsic model
* @param[in] intrinsicPtr The intrinsic pointer
* @param[in] observation The corresponding observation
* @return cost functor
*/
ceres::CostFunction* createRigCostFunctionFromIntrinsics(std::shared_ptr<IntrinsicBase> intrinsic, const sfmData::Observation& observation)
{
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ProjectionErrorFunctor>(new ProjectionErrorFunctor(observation, intrinsic));

int distortionSize = 1;
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
if (isod)
{
auto distortion = isod->getDistortion();
if (distortion)
{
distortionSize = distortion->getParameters().size();
}
}

costFunction->AddParameterBlock(intrinsic->getParameters().size());
costFunction->AddParameterBlock(distortionSize);
costFunction->AddParameterBlock(6);
costFunction->AddParameterBlock(6);
costFunction->AddParameterBlock(3);
costFunction->SetNumResiduals(2);

return costFunction;
}

/**
* @brief Create the appropriate cost functor according the provided input camera intrinsic model
* @param[in] intrinsicPtr The intrinsic pointer
* @param[in] observation The corresponding observation
* @return cost functor
*/
ceres::CostFunction* createConstraintsCostFunctionFromIntrinsics(std::shared_ptr<IntrinsicBase> intrinsic,
const sfmData::Observation& observation_first,
const sfmData::Observation& observation_second)
{
auto costFunction = new ceres::DynamicAutoDiffCostFunction<Constraint2dErrorFunctor>(new Constraint2dErrorFunctor(observation_first, observation_second, intrinsic));

int distortionSize = 1;
auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic);
if (isod)
{
auto distortion = isod->getDistortion();
if (distortion)
{
distortionSize = distortion->getParameters().size();
}
}


costFunction->AddParameterBlock(intrinsic->getParameters().size());
costFunction->AddParameterBlock(distortionSize);
costFunction->AddParameterBlock(6);
costFunction->AddParameterBlock(6);
costFunction->SetNumResiduals(2);

return costFunction;
}

ceres::CostFunction* createCostFunctionFromContraintPoint(const sfmData::Landmark & landmark, const Vec3 & normal)
{
const double weight = 100.0;
auto costFunction = new ceres::DynamicAutoDiffCostFunction<ConstraintPointErrorFunctor>(new ConstraintPointErrorFunctor(weight, normal, landmark.X));

costFunction->AddParameterBlock(3);
costFunction->SetNumResiduals(1);

return costFunction;
}

void BundleAdjustmentCeres::CeresOptions::setDenseBA()
{
Expand Down Expand Up @@ -640,114 +511,141 @@
}
}

void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
{
const bool refineStructure = refineOptions & REFINE_STRUCTURE;

// set a LossFunction to be less penalized by false measurements.
// note: set it to NULL if you don't want use a lossFunction.
ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get();

// build the residual blocks corresponding to the track observations
for (const auto& landmarkPair : sfmData.getLandmarks())
{
const IndexT landmarkId = landmarkPair.first;
const sfmData::Landmark& landmark = landmarkPair.second;

// do not create a residual block if the landmark
// have been set as Ignored by the Local BA strategy
if (landmark.state == EEstimatorParameterState::IGNORED)
{
_statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::IGNORED);
continue;
}

std::array<double, 3>& landmarkBlock = _landmarksBlocks[landmarkId];
for (std::size_t i = 0; i < 3; ++i)
{
landmarkBlock.at(i) = landmark.X(Eigen::Index(i));
}

//If the landmark has a referenceViewIndex set, then retrieve the reference pose
double * referencePoseBlockPtr = nullptr;
if (landmark.referenceViewIndex != UndefinedIndexT)
{
const sfmData::View& refview = sfmData.getView(landmark.referenceViewIndex);
referencePoseBlockPtr = _posesBlocks.at(refview.getPoseId()).data();
}

double* landmarkBlockPtr = landmarkBlock.data();
problem.AddParameterBlock(landmarkBlockPtr, 3);

double* fakeDistortionBlockPtr = _fakeDistortionBlock.data();

// add landmark parameter to the all parameters blocks pointers list
_allParametersBlocks.push_back(landmarkBlockPtr);

// iterate over 2D observation associated to the 3D landmark
for (const auto& [viewId, observation] : landmark.getObservations())
{
const sfmData::View& view = sfmData.getView(viewId);
const IndexT intrinsicId = view.getIntrinsicId();

// each residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
const auto& pose = sfmData.getPose(view);

// needed parameters to create a residual block (K, pose)
double* poseBlockPtr = _posesBlocks.at(view.getPoseId()).data();
double* intrinsicBlockPtr = _intrinsicsBlocks.at(intrinsicId).data();
const std::shared_ptr<IntrinsicBase> intrinsic = _intrinsicObjects[intrinsicId];

double * distortionBlockPtr = fakeDistortionBlockPtr;
if (_distortionsBlocks.find(intrinsicId) != _distortionsBlocks.end())
{
distortionBlockPtr = _distortionsBlocks.at(intrinsicId).data();
}

// apply a specific parameter ordering:
if (_ceresOptions.useParametersOrdering)
{
_linearSolverOrdering.AddElementToGroup(landmarkBlockPtr, 0);
_linearSolverOrdering.AddElementToGroup(poseBlockPtr, 1);
_linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
_linearSolverOrdering.AddElementToGroup(distortionBlockPtr, 2);
}

if (view.isPartOfRig() && !view.isPoseIndependant())
{
ceres::CostFunction* costFunction = createRigCostFunctionFromIntrinsics(intrinsic, observation);
ceres::CostFunction* costFunction = ProjectionErrorFunctor::createCostFunction(intrinsic, observation);

double* rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data();
_linearSolverOrdering.AddElementToGroup(rigBlockPtr, 1);

std::vector<double*> params;
params.push_back(intrinsicBlockPtr);
params.push_back(distortionBlockPtr);
params.push_back(poseBlockPtr);
params.push_back(rigBlockPtr);
params.push_back(landmarkBlockPtr);

problem.AddResidualBlock(costFunction, lossFunction, params);
}
if (referencePoseBlockPtr != nullptr)
{
bool samePose = (referencePoseBlockPtr == poseBlockPtr);
ceres::CostFunction* costFunction = ProjectionRelativeErrorFunctor::createCostFunction(intrinsic, observation, samePose);

std::vector<double*> params;
params.push_back(intrinsicBlockPtr);
params.push_back(distortionBlockPtr);
if (!samePose)
{
params.push_back(poseBlockPtr);
params.push_back(referencePoseBlockPtr);
}
params.push_back(landmarkBlockPtr);

problem.AddResidualBlock(costFunction, lossFunction, params);
}
else
{
ceres::CostFunction* costFunction = createCostFunctionFromIntrinsics(intrinsic, observation);
ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation);

std::vector<double*> params;
params.push_back(intrinsicBlockPtr);
params.push_back(distortionBlockPtr);
params.push_back(poseBlockPtr);
params.push_back(landmarkBlockPtr);

problem.AddResidualBlock(costFunction, lossFunction, params);
}

if (!refineStructure || landmark.state == EEstimatorParameterState::CONSTANT)
{
// set the whole landmark parameter block as constant.
_statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::CONSTANT);
problem.SetParameterBlockConstant(landmarkBlockPtr);
}
else
{
_statistics.addState(EParameter::LANDMARK, EEstimatorParameterState::REFINED);
}
}
}
}

Check notice on line 648 in src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

View check run for this annotation

codefactor.io / CodeFactor

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp#L514-L648 

Complex Method
void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
{

Expand Down Expand Up @@ -787,8 +685,8 @@
for (const auto & spoint: vspoints)
{
sfmData::Observation observation(spoint.survey, 0, 1.0);
ceres::CostFunction* costFunction = createSurveyPointCostFunction(intrinsic, spoint.point3d, observation);

ceres::CostFunction* costFunction =
ProjectionSurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, observation);

std::vector<double*> params;
params.push_back(intrinsicBlockPtr);
Expand Down Expand Up @@ -843,7 +741,7 @@
distortionBlockPtr_1 = _distortionsBlocks.at(intrinsicId_1).data();
}

ceres::CostFunction* costFunction = createConstraintsCostFunctionFromIntrinsics(intrinsicObject1,
ceres::CostFunction* costFunction = Constraint2dErrorFunctor::createCostFunction(intrinsicObject1,
constraint.ObservationFirst,
constraint.ObservationSecond);

Expand Down Expand Up @@ -872,7 +770,7 @@
const sfmData::Landmark & l = sfmData.getLandmarks().at(landmarkId);
double * ldata = _landmarksBlocks.at(landmarkId).data();

ceres::CostFunction* costFunction = createCostFunctionFromContraintPoint(l, constraint.normal);
ceres::CostFunction* costFunction = ConstraintPointErrorFunctor::createCostFunction(l, constraint.normal);

problem.AddResidualBlock(costFunction, lossFunction, ldata);
}
Expand Down Expand Up @@ -944,8 +842,9 @@
_intrinsicsBlocks.clear();
_landmarksBlocks.clear();
_rigBlocks.clear();

_linearSolverOrdering.Clear();
_fakeDistortionBlock.clear();
_intrinsicObjects.clear();
_distortionsBlocks.clear();
}

void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const
Expand All @@ -961,22 +860,10 @@
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)
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.updateFromExternal(poseBlock);
}

// rig sub-poses
Expand All @@ -1002,19 +889,17 @@
// 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)
Expand Down Expand Up @@ -1042,21 +927,10 @@
// 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.updateFromExternal(block);
}
}
}
Expand Down Expand Up @@ -1150,6 +1024,11 @@
}
}
}

for (const auto & [idLandmark, landmarkBlock]: _landmarksBlocks)
{

}
}
}

Expand Down
Loading
Loading