void operator()(Tsts... sts) const {
for (Tinit()(sts...); Tcond()(sts...); Tinc()(sts...)) {
try { Tstmt()(sts...); }
catch (break_stmt::exception&) { break; }
catch (continue_stmt::exception&) { continue; }
}
}
bool vm::scanner::cuda::ProjectiveICP::estimateTransform(Affine3f& affine, const Intr& intr, const PointsPyr& vcurr, const NormalsPyr ncurr, const PointsPyr vprev, const NormalsPyr nprev)
{
const int LEVELS = getUsedLevelsNum();
StreamHelper& sh = *shelp_;
device::ComputeIcpHelper helper(dist_thres_, angle_thres_);
affine = Affine3f::Identity();
for(int level_index = LEVELS - 1; level_index >= 0; --level_index)
{
const device::Normals& n = (const device::Normals& )nprev[level_index];
const device::Points& v = (const device::Points& )vprev[level_index];
helper.rows = (float)n.rows();
helper.cols = (float)n.cols();
helper.setLevelIntr(level_index, intr.fx, intr.fy, intr.cx, intr.cy);
helper.vcurr = vcurr[level_index];
helper.ncurr = ncurr[level_index];
for(int iter = 0; iter < iters_[level_index]; ++iter)
{
helper.aff = device_cast<device::Aff3f>(affine);
helper(v, n, buffer_, sh, sh);
StreamHelper::Vec6f b;
StreamHelper::Mat6f A = sh.get(b);
//checking nullspace
double det = cv::determinant(A);
if (fabs (det) < 1e-15 || cv::viz::isNan (det))
{
if (cv::viz::isNan (det)) std::cout << "qnan" << std::endl;
return false;
}
StreamHelper::Vec6f r;
cv::solve(A, b, r, cv::DECOMP_SVD);
Affine3f Tinc(Vec3f(r.val), Vec3f(r.val+3));
affine = Tinc * affine;
}
}
return true;
}