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.

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] Performance improvements: optimize RemoveOutliers_AngleError #841

Merged
merged 5 commits into from
Sep 2, 2020
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
26 changes: 20 additions & 6 deletions src/aliceVision/camera/IntrinsicBase.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -463,6 +463,24 @@ class IntrinsicBase
std::string _serialNumber;
};

/**
* @brief Return the angle (degree) between two bearing vector rays
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
* @param[in] pose Extrinsic pose
* @param[in] intrinsic Intrinsic camera paremeters
* @param[in] x Point in image
* @return The unit vector in 3D space pointing out from the camera to the point
*/
inline Vec3 applyIntrinsicExtrinsic(const geometry::Pose3& pose,
const IntrinsicBase* intrinsic,
const Vec2& x) {

// x = (u, v, 1.0) // image coordinates
// X = R.t() * K.inv() * x + C // Camera world point
// getting the ray:
// ray = X - C = R.t() * K.inv() * x
return (pose.rotation().transpose() * intrinsic->toUnitSphere(intrinsic->removeDistortion(intrinsic->ima2cam(x)))).normalized();
}

/**
* @brief Return the angle (degree) between two bearing vector rays
* @param[in] ray1 First bearing vector ray
Expand Down Expand Up @@ -493,12 +511,8 @@ inline double angleBetweenRays(const geometry::Pose3& pose1,
const Vec2& x1,
const Vec2& x2)
{
// x = (u, v, 1.0) // image coordinates
// X = R.t() * K.inv() * x + C // Camera world point
// getting the ray:
// ray = X - C = R.t() * K.inv() * x
const Vec3 ray1 = (pose1.rotation().transpose() * intrinsic1->toUnitSphere(intrinsic1->removeDistortion(intrinsic1->ima2cam(x1)))).normalized();
const Vec3 ray2 = (pose2.rotation().transpose() * intrinsic2->toUnitSphere(intrinsic2->removeDistortion(intrinsic2->ima2cam(x2)))).normalized();
const Vec3 ray1 = applyIntrinsicExtrinsic(pose1, intrinsic1, x1);
const Vec3 ray2 = applyIntrinsicExtrinsic(pose2, intrinsic2, x2);
return angleBetweenRays(ray1, ray2);
}

Expand Down
95 changes: 74 additions & 21 deletions src/aliceVision/sfm/sfmFilters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,41 +63,94 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData,

IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcceptedAngle)
{
// note that smallest accepted angle => largest accepted cos(angle)
const double dMaxAcceptedCosAngle = std::cos(degreeToRadian(dMinAcceptedAngle));
IndexT removedTrack_count = 0;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
sfmData::Landmarks::iterator iterTracks = sfmData.structure.begin();
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved

while(iterTracks != sfmData.structure.end())
typedef std::vector<sfmData::Landmarks::key_type> LandmarksKeysVec;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
LandmarksKeysVec v_keys; v_keys.reserve(sfmData.structure.size());
for (const sfmData::Landmarks::value_type &kv : sfmData.structure) v_keys.push_back(kv.first);
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved

std::vector<sfmData::Landmarks::key_type> toErase;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved

#pragma omp parallel for
for (auto it = v_keys.begin(); it < v_keys.end(); it ++)
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
{
sfmData::Observations & observations = iterTracks->second.observations;
double max_angle = 0.0;
for(sfmData::Observations::const_iterator itObs1 = observations.begin(); itObs1 != observations.end(); ++itObs1)
const sfmData::Observations &observations = sfmData.structure[*it].observations;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved

// create matrix for observation directions from camera to point
Eigen::Matrix<double, 3, Eigen::Dynamic> viewDirections(3, observations.size());
Eigen::Matrix<double, 3, Eigen::Dynamic>::Index i;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
sfmData::Observations::const_iterator itObs;

// Greedy algorithm almost always finds an acceptable angle in 1-5 iterations (if it exists).
// It works by greedily chasing the first larger view angle found from the current greedy index.
// View angles have a spartial distribution, so greedily jumping over larger and larger angles
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
// forces the greedy index towards the outside of the distribution.
double dGreedyCos = 1.1;
Eigen::Matrix<double, 3, Eigen::Dynamic>::Index greedyI = 0;
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved


// fill matrix, optimistically checking each new entry against col(greedyI)
for(itObs = observations.begin(), i = 0; itObs != observations.end(); ++itObs, ++i)
{
const sfmData::View * view1 = sfmData.views.at(itObs1->first).get();
const geometry::Pose3 pose1 = sfmData.getPose(*view1).getTransform();
const camera::IntrinsicBase * intrinsic1 = sfmData.intrinsics.at(view1->getIntrinsicId()).get();
const sfmData::View * view = sfmData.views.at(itObs->first).get();
const geometry::Pose3 pose = sfmData.getPose(*view).getTransform();
const camera::IntrinsicBase * intrinsic = sfmData.intrinsics.at(view->getIntrinsicId()).get();

sfmData::Observations::const_iterator itObs2 = itObs1;
++itObs2;
viewDirections.col(i) = applyIntrinsicExtrinsic(pose, intrinsic, itObs->second.x);

for(; itObs2 != observations.end(); ++itObs2)
double dCosAngle = viewDirections.col(i).transpose() * viewDirections.col(greedyI);
if (dCosAngle < dMaxAcceptedCosAngle)
{
break;
}
else if (dCosAngle < dGreedyCos)
{
const sfmData::View * view2 = sfmData.views.at(itObs2->first).get();
const geometry::Pose3 pose2 = sfmData.getPose(*view2).getTransform();
const camera::IntrinsicBase * intrinsic2 = sfmData.intrinsics.at(view2->getIntrinsicId()).get();
dGreedyCos = dCosAngle;
greedyI = i;
}
}

const double angle = angleBetweenRays(pose1, intrinsic1, pose2, intrinsic2, itObs1->second.x, itObs2->second.x);
max_angle = std::max(angle, max_angle);
// early exit, acceptable angle found
if (itObs != observations.end())
{
continue;
}

// Switch to O(n^2) exhaustive search.
// Although this is an O(n^2) loop, in practice it will almost always break very early.
//
// - Default value of dMinAcceptedAngle is 2 degrees. Any larger angle breaks.
// - For landmarks with small number of views, n^2 is negligible.
// - For landmarks with large number of views, backwards iteration means
// all view directions as considered as early as possible,
// making it difficult for a small angle to hide between views.
//
for(i = viewDirections.cols() - 1; i > 0; i -= 1)
{
// Compute and find minimum cosAngle between viewDirections[i] and all viewDirections[0:i].
// Single statement can allow Eigen optimizations
double dMinCosAngle = (viewDirections.col(i).transpose() * viewDirections.leftCols(i)).minCoeff();
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
if (dMinCosAngle < dMaxAcceptedCosAngle) {
break;
}
}
if (max_angle < dMinAcceptedAngle)

// acceptable angle not found
if (i == 0)
{
iterTracks = sfmData.structure.erase(iterTracks);
++removedTrack_count;
#pragma omp critical
toErase.push_back(*it);
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
}
else
++iterTracks;
}
return removedTrack_count;

for (IndexT key : toErase) {
sfmData.structure.erase(key);
fabiencastan marked this conversation as resolved.
Show resolved Hide resolved
}

return toErase.size();
}

bool eraseUnstablePoses(sfmData::SfMData& sfmData, const IndexT min_points_per_pose, std::set<IndexT>* outRemovedViewsId)
Expand Down