Esempio n. 1
0
		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;
}