inline void igl::PlanarizerShapeUp<DerivedV, DerivedF>::assembleP()
{
  P.setZero(3*ni*numF);
  for (int fi = 0; fi< numF; fi++)
  {
    // todo: this can be made faster by omitting the selector matrix
    Eigen::SparseMatrix<typename DerivedV::Scalar > Sfi;
    assembleSelector(fi, Sfi);
    Eigen::SparseMatrix<typename DerivedV::Scalar > NSi = Ni*Sfi;
    
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> Vi = NSi*Vv;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> CC(3,ni);
    for (int i = 0; i <ni; ++i)
      CC.col(i) = Vi.segment(3*i, 3);
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> C = CC*CC.transpose();
    
    // Alec: Doesn't compile
    Eigen::EigenSolver<Eigen::Matrix<typename DerivedV::Scalar, 3, 3>> es(C);
    // the real() is for compilation purposes
    Eigen::Matrix<typename DerivedV::Scalar, 3, 1> lambda = es.eigenvalues().real();
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> U = es.eigenvectors().real();
    int min_i;
    lambda.cwiseAbs().minCoeff(&min_i);
    U.col(min_i).setZero();
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> PP = U*U.transpose()*CC;
    for (int i = 0; i <ni; ++i)
     P.segment(3*ni*fi+3*i, 3) =  weightsSqrt[fi]*PP.col(i);
    
  }
}
Exemple #2
0
 inline
 Eigen::Matrix<T,Eigen::Dynamic,1>
 segment(const Eigen::Matrix<T,Eigen::Dynamic,1>& v,
         size_t i, size_t n) {
   stan::math::check_greater("segment(%1%)",i,0.0,"n",(double*)0);
   stan::math::check_less_or_equal("segment(%1%)",i,static_cast<size_t>(v.rows()),"n",(double*)0);
   if (n != 0) {
     stan::math::check_greater("segment(%1%)",i+n-1,0.0,"n",(double*)0);
     stan::math::check_less_or_equal("segment(%1%)",i+n-1,static_cast<size_t>(v.rows()),"n",(double*)0);
   } 
   return v.segment(i-1,n);
 }
/*void CameraPoseFinderICP::findCorresSet(unsigned level,const Mat44& cur_transform,const Mat44& last_transform_inv)
{
	 cudaProjectionMapFindCorrs(level,cur_transform,last_transform_inv,
			 _camera_params_pyramid[level],
			AppParams::instance()->_icp_params.fDistThres,
			AppParams::instance()->_icp_params.fNormSinThres);

}*/
bool CameraPoseFinderICP::vector6ToTransformMatrix(const Eigen::Matrix<float, 6, 1, 0, 6, 1>& x,  Eigen::Matrix4f& output)
{
	Eigen::Matrix3f R = Eigen::AngleAxisf(x[0], Eigen::Vector3f::UnitX()).toRotationMatrix()
		*Eigen::AngleAxisf(x[1], Eigen::Vector3f::UnitY()).toRotationMatrix()
		*Eigen::AngleAxisf(x[2], Eigen::Vector3f::UnitZ()).toRotationMatrix();
	Eigen::Vector3f t = x.segment(3, 3);
	Eigen::AngleAxisf aa(R);
	float angle = aa.angle();
	float d = t.norm();
	if (angle > AppParams::instance()->_icp_params.fAngleShake|| d> AppParams::instance()->_icp_params.fDistShake)
	{
		return false;
	}
	output.block(0, 0, 3, 3) = R;
	output.block(0, 3, 3, 1) = t;
	return true;
}
    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  Eigen::Dynamic, 1>
    csr_matrix_times_vector(const int& m,
                            const int& n,
                            const Eigen::Matrix<T1, Eigen::Dynamic, 1>& w,
                            const std::vector<int>& v,
                            const std::vector<int>& u,
                            const Eigen::Matrix<T2, Eigen::Dynamic, 1>& b) {
      typedef typename boost::math::tools::promote_args<T1, T2>::type
        result_t;

      check_positive("csr_matrix_times_vector", "m", m);
      check_positive("csr_matrix_times_vector", "n", n);
      check_size_match("csr_matrix_times_vector", "n", n, "b", b.size());
      check_size_match("csr_matrix_times_vector", "m", m, "u", u.size() - 1);
      check_size_match("csr_matrix_times_vector", "w", w.size(), "v", v.size());
      check_size_match("csr_matrix_times_vector", "u/z",
                       u[m - 1] + csr_u_to_z(u, m - 1) - 1, "v", v.size());
      for (unsigned int i = 0; i < v.size(); ++i)
        check_range("csr_matrix_times_vector", "v[]", n, v[i]);

      Eigen::Matrix<result_t, Eigen::Dynamic, 1>  result(m);
      result.setZero();
      for (int row = 0; row < m; ++row) {
        int idx = csr_u_to_z(u, row);
        int row_end_in_w = (u[row] - stan::error_index::value) + idx;
        int i = 0;  // index into dot-product segment entries.
        Eigen::Matrix<result_t, Eigen::Dynamic, 1> b_sub(idx);
        b_sub.setZero();
        for (int nze = u[row] - stan::error_index::value;
             nze < row_end_in_w; ++nze, ++i) {
          check_range("csr_matrix_times_vector", "j", n, v[nze]);
          b_sub.coeffRef(i) = b.coeffRef(v[nze] - stan::error_index::value);
        }  // loop skipped when z is zero.
        Eigen::Matrix<T1, Eigen::Dynamic, 1>
          w_sub(w.segment(u[row] - stan::error_index::value, idx));
        result.coeffRef(row) = dot_product(w_sub, b_sub);
      }
      return result;
    }