Function mola::vision::triangulatePoints

Function Documentation

void mola::vision::triangulatePoints(const std::vector<mrpt::math::TPoint2Df> &pts1, const std::vector<mrpt::math::TPoint2Df> &pts2, const Eigen::Matrix<float, 3, 4> &P1, const Eigen::Matrix<float, 3, 4> &P2, std::vector<mrpt::math::TPoint3Df> &out_pts3d, std::vector<bool> &out_valid)

Linear (DLT) triangulation from two views.

For each correspondence pair (pts1[i] in view1, pts2[i] in view2), solves for the 3D point X by SVD of the 4×4 design matrix derived from the two projection equations: pts1[i] × (P1 · X) = 0 pts2[i] × (P2 · X) = 0

pts1, pts2: normalized image coordinates (z=1 plane, already undistorted). P1, P2: 3×4 projection matrices [R|t] (NOT K·[R|t]; camera is assumed to be already in normalized coordinates). out_pts3d: output 3D points in world frame. out_valid: per-point validity flag (false if point is behind either camera or has too-high reprojection error).

Algorithm: Hartley & Zisserman §12.2 “Linear triangulation methods”. Implementation uses Eigen::JacobiSVD on the 4×4 system.