Skip to content

Commit

Permalink
[sfm] Remove undistortion from estimation
Browse files Browse the repository at this point in the history
  • Loading branch information
@servantftechnicolor
servantftechnicolor committed Nov 28, 2022
1 parent b8874d3 commit 5000404
Show file tree
Hide file tree
Showing 6 changed files with 30 additions and 192 deletions.
1 change: 0 additions & 1 deletion src/aliceVision/sfm/BundleAdjustment.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,6 @@ class BundleAdjustment
REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS = 16, //< refine the optical offset from the center
REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA = 32, //< refine the optical offset only if we have a minimum number of cameras
REFINE_INTRINSICS_DISTORTION = 64, //< refine the distortion parameters
REFINE_INTRINSICS_UNDISTORTION = 128, //< refine the undistortion parameters
/// Refine all intrinsics parameters
REFINE_INTRINSICS_ALL = REFINE_INTRINSICS_FOCAL | REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA | REFINE_INTRINSICS_DISTORTION,
/// Refine all parameters
Expand Down
187 changes: 16 additions & 171 deletions src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,20 +225,14 @@ class IntrinsicsSymbolicManifold : public ceres::Manifold {

class CostProjection : public ceres::CostFunction {
public:
CostProjection(const sfmData::Observation& measured, const std::shared_ptr<camera::IntrinsicBase> & intrinsics, const std::shared_ptr<camera::Undistortion> & undistortion, bool withRig) : _measured(measured), _intrinsics(intrinsics), _undistortion(undistortion), _withRig(withRig)
CostProjection(const sfmData::Observation& measured, const std::shared_ptr<camera::IntrinsicBase> & intrinsics, bool withRig) : _measured(measured), _intrinsics(intrinsics), _withRig(withRig)
{
set_num_residuals(2);

mutable_parameter_block_sizes()->push_back(16);
mutable_parameter_block_sizes()->push_back(16);
mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size());
mutable_parameter_block_sizes()->push_back(3);

if (undistortion)
{
mutable_parameter_block_sizes()->push_back(2);
mutable_parameter_block_sizes()->push_back(undistortion->getParameters().size());
}
}

bool Evaluate(double const * const * parameters, double * residuals, double ** jacobians) const override
Expand All @@ -260,37 +254,16 @@ class CostProjection : public ceres::CostFunction {
}
_intrinsics->updateFromParams(params);

if (_undistortion)
{
const double* parameter_undistortionOffset = parameters[4];
const double* parameter_undistortionParams = parameters[5];

_undistortion->setOffset({ parameter_undistortionOffset[0], parameter_undistortionOffset[1] });

size_t params_size = _undistortion->getParameters().size();
std::vector<double> paramsUndist;
for (size_t param_id = 0; param_id < params_size; param_id++) {
paramsUndist.push_back(parameter_undistortionParams[param_id]);
}

_undistortion->setParameters(paramsUndist);
}

const SE3::Matrix T = cTr * rTo;
const geometry::Pose3 T_pose3(T.block<3, 4>(0, 0));

const Vec4 pth = pt.homogeneous();
const Vec2 pt_est = _intrinsics->project(T_pose3, pth, true);
const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0;

Vec2 measured_undistorted = _measured.x;
if (_undistortion)
{
measured_undistorted = _undistortion->undistort(_measured.x);
}

residuals[0] = (pt_est(0) - measured_undistorted(0)) / scale;
residuals[1] = (pt_est(1) - measured_undistorted(1)) / scale;
residuals[0] = (pt_est(0) - _measured.x(0)) / scale;
residuals[1] = (pt_est(1) - _measured.x(1)) / scale;

if (jacobians == nullptr) {
return true;
Expand Down Expand Up @@ -323,31 +296,12 @@ class CostProjection : public ceres::CostFunction {
J = d_res_d_pt_est * _intrinsics->getDerivativeProjectWrtPoint(T_pose3, pth) * Eigen::Matrix<double, 4, 3>::Identity();
}

if (_undistortion)
{
size_t undistortion_params_size = _undistortion->getParameters().size();

if (jacobians[4] != nullptr) {
Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor>> J(jacobians[4]);

J = -d_res_d_pt_est * _undistortion->getDerivativeUndistortWrtOffset(_measured.x);
J.fill(0);
}

if (jacobians[5] != nullptr) {
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> J(jacobians[5], 2, undistortion_params_size);

J = -d_res_d_pt_est * _undistortion->getDerivativeUndistortWrtParameters(_measured.x);
}
}

return true;
}

private:
const sfmData::Observation & _measured;
const sfmData::Observation _measured;
const std::shared_ptr<camera::IntrinsicBase> _intrinsics;
std::shared_ptr<camera::Undistortion> _undistortion;
bool _withRig;
};

Expand Down Expand Up @@ -634,7 +588,6 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat
const bool refineIntrinsicsOpticalCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA);
const bool refineIntrinsicsFocalLength = refineOptions & REFINE_INTRINSICS_FOCAL;
const bool refineIntrinsicsDistortion = refineOptions & REFINE_INTRINSICS_DISTORTION;
const bool refineIntrinsicsUndistortion = refineOptions & REFINE_INTRINSICS_UNDISTORTION;
const bool refineIntrinsics = refineIntrinsicsDistortion || refineIntrinsicsFocalLength || refineIntrinsicsOpticalCenter;
const bool fixFocalRatio = true;

Expand Down Expand Up @@ -770,75 +723,6 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat
}
}

void BundleAdjustmentSymbolicCeres::addUndistortionsToProblem(const sfmData::SfMData& sfmData, BundleAdjustment::ERefineOptions refineOptions, ceres::Problem& problem)
{
const bool refineIntrinsicsUndistortion = refineOptions & REFINE_INTRINSICS_UNDISTORTION;

/*Sort undistortions by frame id*/
std::map<IndexT, IndexT> sorted_undistortions;
for (auto v : sfmData.getViews())
{
IndexT uid = v.second->getUndistortionId();
if (uid == UndefinedIndexT)
{
continue;
}


sorted_undistortions[v.second->getFrameId()] = uid;
}

for (auto p : sfmData.getUndistortions())
{
std::shared_ptr<camera::Undistortion> undistortionObject = p.second;

_undistortionOffsetBlocks[p.first] = undistortionObject->getOffset();
_undistortionParametersBlocks[p.first] = undistortionObject->getParameters();


Vec2& undistOffset = _undistortionOffsetBlocks[p.first];
std::vector<double>& undistParameters = _undistortionParametersBlocks[p.first];

problem.AddParameterBlock(undistOffset.data(), 2);
problem.AddParameterBlock(undistParameters.data(), undistParameters.size());

if (!refineIntrinsicsUndistortion)
{
problem.SetParameterBlockConstant(undistOffset.data());
problem.SetParameterBlockConstant(undistParameters.data());
}

_allParametersBlocks.push_back(undistOffset.data());
_allParametersBlocks.push_back(undistParameters.data());
}

for (auto it = sorted_undistortions.begin(); it != sorted_undistortions.end(); it++)
{
auto next_it = std::next(it);
if (next_it == sorted_undistortions.end())
{
break;
}

//std::shared_ptr<camera::Undistortion> undistortion = sfmData.getUndistortions().at(uid);
Vec2& undistOffset_1 = _undistortionOffsetBlocks[it->second];
Vec2& undistOffset_2 = _undistortionOffsetBlocks[next_it->second];
std::vector<double>& undistParameters_1 = _undistortionParametersBlocks[it->second];
std::vector<double>& undistParameters_2 = _undistortionParametersBlocks[next_it->second];

//problem.AddResidualBlock(new CostUndistortionSimilar(undistParameters_1.size()), nullptr, undistOffset_1.data(), undistOffset_2.data(), undistParameters_1.data(), undistParameters_2.data());

auto next_next_it = std::next(next_it);
if (next_next_it == sorted_undistortions.end())
{
continue;
}

std::vector<double>& undistParameters_3 = _undistortionParametersBlocks[next_next_it->second];
//problem.AddResidualBlock(new CostUndistortionGradient(undistParameters_1.size()), nullptr, undistParameters_1.data(), undistParameters_2.data(), undistParameters_3.data());
}
}

void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem)
{
const bool refineStructure = refineOptions & REFINE_STRUCTURE;
Expand Down Expand Up @@ -870,21 +754,22 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData
// 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& observationPair: landmark.observations)
{
const sfmData::View& view = sfmData.getView(observationPair.first);
const sfmData::Observation& observation = observationPair.second;
const std::shared_ptr<IntrinsicBase> intrinsic = sfmData.getIntrinsicsharedPtr(view.getIntrinsicId());
std::shared_ptr<camera::Undistortion> undistortion;
IndexT undistortionId = view.getUndistortionId();
if (undistortionId != UndefinedIndexT)

sfmData::Observation observation_copy = observationPair.second;
IndexT udid = view.getUndistortionId();
if (udid != UndefinedIndexT)
{
undistortion = sfmData.getUndistortions().at(undistortionId);
std::shared_ptr<camera::Undistortion> undisto = sfmData.getUndistortions().at(udid);
if (undisto)
{
observation_copy.x = undisto->undistort(observation_copy.x);
}
}

// each residual block takes a point and a camera as input and outputs a 2
Expand Down Expand Up @@ -916,24 +801,13 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData
_linearSolverOrdering.AddElementToGroup(intrinsicBlockPtr, 2);
}

ceres::CostFunction* costFunction = new CostProjection(observation, intrinsic, undistortion, withRig);
ceres::CostFunction* costFunction = new CostProjection(observation_copy, intrinsic, withRig);

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


if (_undistortionParametersBlocks.find(view.getUndistortionId()) != _undistortionParametersBlocks.end())
{
double* undistortionParameterBlockPtr = _undistortionParametersBlocks.at(view.getUndistortionId()).data();
double* undistortionOffsetBlockPtr = _undistortionOffsetBlocks.at(view.getUndistortionId()).data();
params.push_back(undistortionOffsetBlockPtr);
params.push_back(undistortionParameterBlockPtr);
}


params.push_back(landmarkBlockPtr);

problem.AddResidualBlock(costFunction, lossFunction, params);

Expand Down Expand Up @@ -969,9 +843,6 @@ void BundleAdjustmentSymbolicCeres::createProblem(const sfmData::SfMData& sfmDat
// add SfM intrinsics to the Ceres problem
addIntrinsicsToProblem(sfmData, refineOptions, problem);

// add SfM undistortions to the Ceres problem
addUndistortionsToProblem(sfmData, refineOptions, problem);

// add SfM landmarks to the Ceres problem
addLandmarksToProblem(sfmData, refineOptions, problem);
}
Expand All @@ -980,9 +851,8 @@ void BundleAdjustmentSymbolicCeres::updateFromSolution(sfmData::SfMData& sfmData
{
const bool refinePoses = (refineOptions & REFINE_ROTATION) || (refineOptions & REFINE_TRANSLATION);
const bool refineIntrinsicsOpticalCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA);
const bool refineIntrinsics = (refineOptions & REFINE_INTRINSICS_FOCAL) || (refineOptions & REFINE_INTRINSICS_DISTORTION) || refineIntrinsicsOpticalCenter || (refineOptions & REFINE_INTRINSICS_UNDISTORTION);
const bool refineIntrinsics = (refineOptions & REFINE_INTRINSICS_FOCAL) || (refineOptions & REFINE_INTRINSICS_DISTORTION) || refineIntrinsicsOpticalCenter;
const bool refineStructure = refineOptions & REFINE_STRUCTURE;
const bool refineUndistortion = refineOptions & REFINE_INTRINSICS_UNDISTORTION;

// update camera poses with refined data
if(refinePoses)
Expand Down Expand Up @@ -1035,31 +905,6 @@ void BundleAdjustmentSymbolicCeres::updateFromSolution(sfmData::SfMData& sfmData
}
}

if (refineUndistortion)
{
for (const auto& undistoBlockPair : _undistortionOffsetBlocks)
{
const IndexT undistortionId = undistoBlockPair.first;

std::shared_ptr<camera::Undistortion> undistortionObject = sfmData.getUndistortions().at(undistortionId);
if (undistortionObject)
{
undistortionObject->setOffset(undistoBlockPair.second);
}
}

for (const auto& undistoBlockPair : _undistortionParametersBlocks)
{
const IndexT undistortionId = undistoBlockPair.first;

std::shared_ptr<camera::Undistortion> undistortionObject = sfmData.getUndistortions().at(undistortionId);
if (undistortionObject)
{
undistortionObject->setParameters(undistoBlockPair.second);
}
}
}

// update landmarks
if(refineStructure)
{
Expand Down
8 changes: 0 additions & 8 deletions src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,14 +211,6 @@ class BundleAdjustmentSymbolicCeres : public BundleAdjustment
*/
void addIntrinsicsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);

/**
* @brief Create a parameter block for each undistortions according to the Ceres format
* @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the undistortions
* @param[in] refineOptions The chosen refine flag
* @param[out] problem The Ceres bundle adjustement problem
*/
void addUndistortionsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem);

/**
* @brief Create a residual block for each landmarks according to the Ceres format
* @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics
Expand Down
16 changes: 8 additions & 8 deletions src/aliceVision/sfm/ResidualErrorFunctor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ struct ResidualErrorFunctor_Pinhole
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -323,7 +323,7 @@ struct ResidualErrorFunctor_PinholeRadialK1
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -480,7 +480,7 @@ struct ResidualErrorFunctor_PinholeRadialK3
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -650,7 +650,7 @@ struct ResidualErrorFunctor_PinholeBrownT2
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -822,7 +822,7 @@ struct ResidualErrorFunctor_PinholeFisheye
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -980,7 +980,7 @@ struct ResidualErrorFunctor_PinholeFisheye1
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -1160,7 +1160,7 @@ struct ResidualErrorFunctor_Pinhole3DEClassicLD
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down Expand Up @@ -1335,7 +1335,7 @@ struct ResidualErrorFunctor_Pinhole3DERadial4
return true;
}

const sfmData::Observation& _obs; // The 2D observation
const sfmData::Observation _obs; // The 2D observation
const Vec2 _center;
};

Expand Down
Loading

0 comments on commit 5000404

Please sign in to comment.