Track<cv::Point2d> calibrateAndUndistortTrack(
const Track<cv::Point2d>& track,
const CameraProperties& intrinsics) {
// Calibrate each point, undo intrinsics.
Track<cv::Point2d> calibrated;
cv::Mat K_inv(intrinsics.matrix().inv());
std::transform(track.begin(), track.end(),
std::inserter(calibrated, calibrated.begin()),
boost::bind(calibrateIndexedPoint, _1, K_inv));
// Remove non-undistortable points.
Track<cv::Point2d> valid;
std::remove_copy_if(calibrated.begin(), calibrated.end(),
std::inserter(valid, valid.begin()),
boost::bind(indexedPointIsNotUndistortable, _1, intrinsics.distort_w));
DLOG(INFO) << valid.size() << " / " << calibrated.size() <<
" could be undistorted";
// Undistort undistortable points.
Track<cv::Point2d> undistorted;
std::transform(valid.begin(), valid.end(),
std::inserter(undistorted, undistorted.begin()),
boost::bind(undistortIndexedPoint, _1, intrinsics.distort_w));
return undistorted;
}
/*!
Return the inverted camera matrix \f$K^{-1}\f$ given by:
\f$ K^{-1} = \left[\begin{array}{ccc}
1/p_x & 0 & -u_0/p_x \\
0 & 1/p_y & -v_0/p_y \\
0 & 0 & 1
\end{array} \right] \f$
\sa get_K()
*/
vpMatrix
vpCameraParameters::get_K_inverse() const
{
vpMatrix K_inv(3, 3, 0.);
K_inv[0][0] = inv_px ;
K_inv[1][1] = inv_py ;
K_inv[0][2] = -u0*inv_px ;
K_inv[1][2] = -v0*inv_py ;
K_inv[2][2] = 1.0 ;
return K_inv;
}