Skip to content
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

[SfM] Major bug fix on BlockOrder in BACeres #849

Merged
merged 1 commit into from
Aug 19, 2020
Merged

[SfM] Major bug fix on BlockOrder in BACeres #849

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
28 changes: 21 additions & 7 deletions src/aliceVision/sfm/BundleAdjustmentCeres.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -292,10 +292,8 @@ void BundleAdjustmentCeres::setSolverOptions(ceres::Solver::Options& solverOptio

if(_ceresOptions.useParametersOrdering)
{
solverOptions.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering);

// copy ParameterBlockOrdering
*(solverOptions.linear_solver_ordering) = _ceresOptions.linearSolverOrdering;
solverOptions.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering(_linearSolverOrdering));
}
}

Expand Down Expand Up @@ -577,20 +575,23 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat
// apply a specific parameter ordering:
if(_ceresOptions.useParametersOrdering)
{
_ceresOptions.linearSolverOrdering.AddElementToGroup(landmarkBlockPtr, 0);
_ceresOptions.linearSolverOrdering.AddElementToGroup(poseBlockPtr, 1);
_ceresOptions.linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
_linearSolverOrdering.AddElementToGroup(landmarkBlockPtr, 0);
_linearSolverOrdering.AddElementToGroup(poseBlockPtr, 1);
_linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
}

if(view.isPartOfRig() && !view.isPoseIndependant())
{
ceres::CostFunction* costFunction = createRigCostFunctionFromIntrinsics(sfmData.getIntrinsicPtr(view.getIntrinsicId()), observation);

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

problem.AddResidualBlock(costFunction,
lossFunction,
intrinsicBlockPtr,
poseBlockPtr,
_rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data(), // subpose of the cameras rig
rigBlockPtr, // subpose of the cameras rig
landmarkBlockPtr); // do we need to copy 3D point to avoid false motion, if failure ?
}
else
Expand Down Expand Up @@ -697,6 +698,19 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData,
addRotationPriorsToProblem(sfmData, refineOptions, problem);
}

void BundleAdjustmentCeres::resetProblem()
{
_statistics = Statistics();

_allParametersBlocks.clear();
_posesBlocks.clear();
_intrinsicsBlocks.clear();
_landmarksBlocks.clear();
_rigBlocks.clear();

_linearSolverOrdering.Clear();
}

void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const
{
const bool refinePoses = (refineOptions & REFINE_ROTATION) || (refineOptions & REFINE_TRANSLATION);
Expand Down
16 changes: 5 additions & 11 deletions src/aliceVision/sfm/BundleAdjustmentCeres.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ class BundleAdjustmentCeres : public BundleAdjustment
ceres::LinearSolverType linearSolverType;
ceres::PreconditionerType preconditionerType;
ceres::SparseLinearAlgebraLibraryType sparseLinearAlgebraLibraryType;
ceres::ParameterBlockOrdering linearSolverOrdering;
std::shared_ptr<ceres::LossFunction> lossFunction;
unsigned int nbThreads;
bool useParametersOrdering = true;
Expand Down Expand Up @@ -171,16 +170,7 @@ class BundleAdjustmentCeres : public BundleAdjustment
/**
* @brief Clear structures for a new problem
*/
inline void resetProblem()
{
_statistics = Statistics();

_allParametersBlocks.clear();
_posesBlocks.clear();
_intrinsicsBlocks.clear();
_landmarksBlocks.clear();
_rigBlocks.clear();
}
void resetProblem();

/**
* @brief Set user Ceres options to the solver
Expand Down Expand Up @@ -306,6 +296,10 @@ class BundleAdjustmentCeres : public BundleAdjustment
/// block: ceres angleAxis(3) + translation(3)
HashMap<IndexT, HashMap<IndexT, std::array<double,6>>> _rigBlocks;

/// hinted order for ceres to eliminate blocks when solving.
/// note: this ceres parameter is built internally and must be reset on each call to the solver.
ceres::ParameterBlockOrdering _linearSolverOrdering;

};

} // namespace sfm
Expand Down
11 changes: 6 additions & 5 deletions src/aliceVision/sfm/ResidualErrorFunctor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -408,13 +408,14 @@ struct ResidualErrorFunctor_PinholeRadialK3
{
const T * cam_R = subpose_Rt;
const T * cam_t = &subpose_Rt[3];
// Rotate the point according to the camera rotation
ceres::AngleAxisRotatePoint(cam_R, pos_proj, pos_proj);
// Rotate the point according to the camera rotation. In-place rotation not supported by Ceres
T pos_proj_tmp[3];
ceres::AngleAxisRotatePoint(cam_R, pos_proj, pos_proj_tmp);

// Apply the camera translation
pos_proj[0] += cam_t[0];
pos_proj[1] += cam_t[1];
pos_proj[2] += cam_t[2];
pos_proj[0] = pos_proj_tmp[0] + cam_t[0];
pos_proj[1] = pos_proj_tmp[1] + cam_t[1];
pos_proj[2] = pos_proj_tmp[2] + cam_t[2];
}

// Transform the point from homogeneous to euclidean (undistorted point)
Expand Down