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);
}
}
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;
}