Skip to content

Commit

Permalink
[sfm] Reduce duplication in sequantial sfm triangulation
Browse files Browse the repository at this point in the history 
The current code recomputes the same source data for both 2 and N
observation cases. Due to the code being structured differently it's
hard to see that in both cases the same operations are being performed.
By extracting the code into separate function we make this fact
explicit.
  • Loading branch information
@p12tic
p12tic committed Sep 29, 2022
1 parent f987354 commit e59df01
Showing 1 changed file with 52 additions and 37 deletions.
89 changes: 52 additions & 37 deletions src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1665,6 +1665,43 @@ void ReconstructionEngine_sequentialSfM::getTracksToTriangulate(const std::set<I
}
}

namespace {

struct ObservationData
{
std::shared_ptr<camera::Pinhole> cam;
Pose3 pose;
Mat34 P;
Vec2 x;
Vec2 xUd;
};

ObservationData getObservationData(const SfMData& scene,
feature::FeaturesPerView* featuresPerView,
IndexT viewId, const track::Track& track)
{
const View* view = scene.getViews().at(viewId).get();

std::shared_ptr<camera::IntrinsicBase> cam = scene.getIntrinsics().at(view->getIntrinsicId());
std::shared_ptr<camera::Pinhole> camPinHole = std::dynamic_pointer_cast<camera::Pinhole>(cam);

if (!camPinHole) {
ALICEVISION_LOG_ERROR("Camera is not pinhole in triangulate_multiViewsLORANSAC");
return {nullptr, nullptr, {}, {}, {}, {}};
}

Pose3 pose = scene.getPose(*view).getTransform();
Mat34 P = camPinHole->getProjectiveEquivalent(pose);

const auto& feature = featuresPerView->getFeatures(viewId, track.descType)[track.featPerView.at(viewId)];
Vec2 x = feature.coords().cast<double>();
Vec2 xUd = cam->get_ud_pixel(x); // undistorted 2D point

return {camPinHole, pose, P, x, xUd};
}

} // namespace

void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData& scene, const std::set<IndexT>& previousReconstructedViews, const std::set<IndexT>& newReconstructedViews)
{
ALICEVISION_LOG_DEBUG("Triangulating (mode: multi-view LO-RANSAC)... ");
Expand Down Expand Up @@ -1706,34 +1743,16 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData&
// -- Prepare:
IndexT I = *(observations.begin());
IndexT J = *(observations.rbegin());
const View* viewI = scene.getViews().at(I).get();
const View* viewJ = scene.getViews().at(J).get();

std::shared_ptr<camera::IntrinsicBase> camI = scene.getIntrinsics().at(viewI->getIntrinsicId());
std::shared_ptr<camera::Pinhole> camIPinHole = std::dynamic_pointer_cast<camera::Pinhole>(camI);
if (!camIPinHole) {
ALICEVISION_LOG_ERROR("Camera is not pinhole in triangulate_multiViewsLORANSAC");
continue;
}
const auto oi = getObservationData(scene, _featuresPerView, I, track);
const auto oj = getObservationData(scene, _featuresPerView, J, track);

std::shared_ptr<camera::IntrinsicBase> camJ = scene.getIntrinsics().at(viewJ->getIntrinsicId());
std::shared_ptr<camera::Pinhole> camJPinHole = std::dynamic_pointer_cast<camera::Pinhole>(camJ);
if (!camJPinHole) {
ALICEVISION_LOG_ERROR("Camera is not pinhole in triangulate_multiViewsLORANSAC");
if (!oi.cam || !oj.cam) {
continue;
}

const Pose3 poseI = scene.getPose(*viewI).getTransform();
const Pose3 poseJ = scene.getPose(*viewJ).getTransform();
const Vec2 xI = _featuresPerView->getFeatures(I, track.descType)[track.featPerView.at(I)].coords().cast<double>();
const Vec2 xJ = _featuresPerView->getFeatures(J, track.descType)[track.featPerView.at(J)].coords().cast<double>();

// -- Triangulate:
multiview::TriangulateDLT(camIPinHole->getProjectiveEquivalent(poseI),
camI->get_ud_pixel(xI),
camJPinHole->getProjectiveEquivalent(poseJ),
camI->get_ud_pixel(xJ),
&X_euclidean);
multiview::TriangulateDLT(oi.P, oi.xUd, oj.P, oj.xUd, &X_euclidean);

// -- Check:
// - angle (small angle leads imprecise triangulation)
Expand All @@ -1745,11 +1764,11 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData&
const double& acThresholdI = (acThresholdItI != _map_ACThreshold.end()) ? acThresholdItI->second : 4.0;
const double& acThresholdJ = (acThresholdItJ != _map_ACThreshold.end()) ? acThresholdItJ->second : 4.0;

if (angleBetweenRays(poseI, camI.get(), poseJ, camJ.get(), xI, xJ) < _params.minAngleForTriangulation ||
poseI.depth(X_euclidean) < 0 ||
poseJ.depth(X_euclidean) < 0 ||
camI->residual(poseI, X_euclidean.homogeneous(), xI).norm() > acThresholdI ||
camJ->residual(poseJ, X_euclidean.homogeneous(), xJ).norm() > acThresholdJ)
if (angleBetweenRays(oi.pose, oi.cam.get(), oj.pose, oj.cam.get(), oi.x, oj.x) < _params.minAngleForTriangulation ||
oi.pose.depth(X_euclidean) < 0 ||
oj.pose.depth(X_euclidean) < 0 ||
oi.cam->residual(oi.pose, X_euclidean.homogeneous(), oi.x).norm() > acThresholdI ||
oj.cam->residual(oj.pose, X_euclidean.homogeneous(), oj.x).norm() > acThresholdJ)
isValidTrack = false;
}
else
Expand All @@ -1767,19 +1786,15 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData&
int i = 0;
for (const IndexT& viewId : observations)
{
const View* view = scene.getViews().at(viewId).get();

std::shared_ptr<camera::IntrinsicBase> cam = scene.getIntrinsics().at(view->getIntrinsicId());
std::shared_ptr<camera::Pinhole> camPinHole = std::dynamic_pointer_cast<camera::Pinhole>(cam);
if (!camPinHole) {
ALICEVISION_LOG_ERROR("Camera is not pinhole in triangulate_multiViewsLORANSAC");
const auto o = getObservationData(scene, _featuresPerView, viewId, track);

if (!o.cam) {
continue;
}

const Vec2 x_ud = cam->get_ud_pixel(_featuresPerView->getFeatures(viewId, track.descType)[track.featPerView.at(viewId)].coords().cast<double>()); // undistorted 2D point
features(0,i) = x_ud(0);
features(1,i) = x_ud(1);
Ps.push_back(camPinHole->getProjectiveEquivalent(scene.getPose(*view).getTransform()));
features(0,i) = o.xUd(0);
features(1,i) = o.xUd(1);
Ps.push_back(o.P);
i++;
}
}
Expand Down

0 comments on commit e59df01

Please sign in to comment.