void DrawableTransformCovariance::updateCovarianceDrawList() { GLParameterTransformCovariance *covarianceParameter = dynamic_cast<GLParameterTransformCovariance*>(_parameter); glNewList(_covarianceDrawList, GL_COMPILE); if(_covariance != Eigen::Matrix3f::Zero() && covarianceParameter && covarianceParameter->show() && covarianceParameter->scale() > 0.0f) { float scale = covarianceParameter->scale(); Eigen::Vector4f color = covarianceParameter->color(); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; eigenSolver.computeDirect(_covariance, Eigen::ComputeEigenvectors); Eigen::Vector3f lambda = eigenSolver.eigenvalues(); Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); I.linear() = eigenSolver.eigenvectors(); I.translation() = Eigen::Vector3f(_mean.x(), _mean.y(), _mean.z()); float sx = sqrt(lambda[0]) * scale; float sy = sqrt(lambda[1]) * scale; float sz = sqrt(lambda[2]) * scale; glPushMatrix(); glMultMatrixf(I.data()); glColor4f(color[0], color[1], color[2], color[3]); glScalef(sx, sy, sz); glCallList(_sphereDrawList); glPopMatrix(); } glEndList(); }
void DrawableUncertainty::updateCovarianceDrawList() { GLParameterUncertainty *uncertaintyParameter = dynamic_cast<GLParameterUncertainty*>(_parameter); glNewList(_covarianceDrawList, GL_COMPILE); if(_covarianceDrawList && _covariances && uncertaintyParameter && uncertaintyParameter->ellipsoidScale() > 0.0f) { uncertaintyParameter->applyGLParameter(); Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver; float ellipsoidScale = uncertaintyParameter->ellipsoidScale(); for(size_t i = 0; i < _covariances->size(); i += uncertaintyParameter->step()) { Gaussian3f &gaussian3f = _covariances->at(i); Eigen::Matrix3f covariance = gaussian3f.covarianceMatrix(); Eigen::Vector3f mean = gaussian3f.mean(); eigenSolver.computeDirect(covariance, Eigen::ComputeEigenvectors); Eigen::Vector3f eigenValues = eigenSolver.eigenvalues(); Eigen::Isometry3f I = Eigen::Isometry3f::Identity(); I.linear() = eigenSolver.eigenvectors(); I.translation() = mean; float sx = sqrt(eigenValues[0]) * ellipsoidScale; float sy = sqrt(eigenValues[1]) * ellipsoidScale; float sz = sqrt(eigenValues[2]) * ellipsoidScale; glPushMatrix(); glMultMatrixf(I.data()); sx = sx; sy = sy; sz = sz; glScalef(sx, sy, sz); glCallList(_sphereDrawList); glPopMatrix(); } } glEndList(); }
inline void calcNormalsEigen(Mat &depth_img, Mat &points, Mat &normals, int k=11, float max_dist=0.02, bool dist_rel_z=true) { if (normals.rows != depth_img.rows || normals.cols != depth_img.cols || normals.channels() != 3) { normals = cv::Mat::zeros(depth_img.rows, depth_img.cols, CV_32FC3); } Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver; const float bad_point = std::numeric_limits<float>::quiet_NaN (); for (int y = 0; y < depth_img.rows; ++y) { for (int x = 0; x < depth_img.cols; ++x) { Eigen::Vector3f p_q = points.at<Eigen::Vector3f>(y,x); // depth-nan handle: bad point if (depth_img.at<float>(y, x) == 0 || p_q(0) != p_q(0)){ normals.at<Eigen::Vector3f>(y,x) = Eigen::Vector3f(bad_point, bad_point, bad_point); continue; } Eigen::Matrix3f A = Eigen::Matrix3f::Zero(); std::vector<Eigen::Vector3f> p_j_list; Eigen::Vector3f _p = Eigen::Vector3f::Zero(); float max_dist_rel = max_dist * ((dist_rel_z)? p_q[2]*1.5 : 1); for (int k_y = y-k/2; k_y <= y+k/2; ++k_y) { for (int k_x = x-k/2; k_x <= x+k/2; ++k_x) { if(k_y<0 || k_x<0 || k_y>=depth_img.rows || k_x >= depth_img.cols) continue; if (k_y == y && k_x == x) continue; if (depth_img.at<float>(k_y, k_x) == 0) continue; Eigen::Vector3f p_j = points.at<Eigen::Vector3f>(k_y,k_x); if( max_dist_rel <= 0 || ((p_q - p_j).norm() <= max_dist_rel) ) { p_j_list.push_back(p_j); _p += p_j; } } } _p /= p_j_list.size(); double weight_sum = 0; for (int i = 0; i < p_j_list.size(); ++i) { double w = 1.0/(p_j_list[i] - _p).squaredNorm(); A += w*((p_j_list[i] - _p)*((p_j_list[i] - _p).transpose())); weight_sum += w; } A /= weight_sum; solver.computeDirect(A); Eigen::Vector3f normal = solver.eigenvectors().col(0).normalized(); // flip to viewpoint (0,0,0) if(normal(2) > 0) normal *= -1; normals.at<Eigen::Vector3f>(y,x) = normal; } } }
IGL_INLINE void igl::polar_dec( const Eigen::PlainObjectBase<DerivedA> & A, Eigen::PlainObjectBase<DerivedR> & R, Eigen::PlainObjectBase<DerivedT> & T, Eigen::PlainObjectBase<DerivedU> & U, Eigen::PlainObjectBase<DerivedS> & S, Eigen::PlainObjectBase<DerivedV> & V) { using namespace std; using namespace Eigen; typedef typename DerivedA::Scalar Scalar; const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision()); Eigen::SelfAdjointEigenSolver<DerivedA> eig; feclearexcept(FE_UNDERFLOW); eig.computeDirect(A.transpose()*A); if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th) { cout<<"resorting to svd 1..."<<endl; return polar_svd(A,R,T,U,S,V); } S = eig.eigenvalues().cwiseSqrt(); V = eig.eigenvectors(); U = A * V; R = U * S.asDiagonal().inverse() * V.transpose(); T = V * S.asDiagonal() * V.transpose(); S = S.reverse().eval(); V = V.rowwise().reverse().eval(); U = U.rowwise().reverse().eval() * S.asDiagonal().inverse(); if(R.determinant() < 0) { // Annoyingly the .eval() is necessary auto W = V.eval(); const auto & SVT = S.asDiagonal() * V.adjoint(); W.col(V.cols()-1) *= -1.; R = U*W.transpose(); T = W*SVT; } if(std::fabs(R.squaredNorm()-3.) > th) { cout<<"resorting to svd 2..."<<endl; return polar_svd(A,R,T,U,S,V); } }
OBB::OBB(Mesh::const_iterator begin, Mesh::const_iterator end) { if (begin == end) { axes = -ZERO_SIZE * Matrix3f::Identity(); //make it inside out (i guess) origin = Vector3f::Zero(); return; } Vector3f centerOfMass = centroid(begin, end); Matrix3f inertiaTensor = Matrix3f::Zero(); auto addPt = [&](const Vector3f& pt, float mass) { Vector3f lpos = pt - centerOfMass; inertiaTensor(0, 0) += (lpos.y()*lpos.y() + lpos.z()*lpos.z()) * mass; inertiaTensor(1, 1) += (lpos.x()*lpos.x() + lpos.z()*lpos.z()) * mass; inertiaTensor(2, 2) += (lpos.x()*lpos.x() + lpos.y()*lpos.y()) * mass; inertiaTensor(1, 0) -= lpos.x()*lpos.y() * mass; inertiaTensor(2, 0) -= lpos.x()*lpos.z() * mass; inertiaTensor(2, 1) -= lpos.y()*lpos.z() * mass; }; for (const auto& tri : make_range(begin, end)) { float area = TriNormal(tri).norm() / 6.f; addPt(tri.col(0), area); addPt(tri.col(1), area); addPt(tri.col(2), area); } Eigen::SelfAdjointEigenSolver<Matrix3f> es; es.computeDirect(inertiaTensor); axes = es.eigenvectors(); float maxflt = std::numeric_limits<float>::max(); Eigen::Vector3f min{ maxflt, maxflt, maxflt }; Eigen::Vector3f max = -min; for (const auto& tri : make_range(begin, end)) { min = min.cwiseMin((axes.transpose() * tri).rowwise().minCoeff()); max = max.cwiseMax((axes.transpose() * tri).rowwise().maxCoeff()); } extent = (max - min).cwiseMax(ZERO_SIZE) / 2.f; origin = axes * (min + extent); }
inline void calcPC(Mat &normals, Mat &points, Mat &depth_img, Mat &pc, int k=5, float max_dist=0.02, bool dist_rel_z=true) { if (pc.rows != depth_img.rows || pc.cols != depth_img.cols || pc.channels() != 5) { pc = Mat::zeros(depth_img.rows, depth_img.cols, CV_32FC(5)); } Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver; Eigen::Matrix3f I = Eigen::Matrix3f::Identity(); int failed = 0; for (int y = 0; y < depth_img.rows; ++y) { for (int x = 0; x < depth_img.cols; ++x) { Eigen::Matrix3f A = Eigen::Matrix3f::Zero(); Eigen::Vector3f _m = Eigen::Vector3f::Zero(); Eigen::Vector3f n_q = normals.at<Eigen::Vector3f>(y,x); Eigen::Vector3f p_q = points.at<Eigen::Vector3f>(y,x); std::vector<Eigen::Vector3f> m_j_list; Eigen::Matrix3f M = (I - n_q*(n_q.transpose())); float max_dist_rel = max_dist * ((dist_rel_z)? p_q[2]*1.5 : 1); for (int k_y = y-k/2; k_y <= y+k/2; ++k_y) { for (int k_x = x-k/2; k_x <= x+k/2; ++k_x) { if(k_y<0 || k_x<0 || k_y>=depth_img.rows || k_x >= depth_img.cols) continue; if(depth_img.at<float>(k_y,k_x) == 0) continue; Eigen::Vector3f p_j = points.at<Eigen::Vector3f>(k_y,k_x); if( max_dist_rel <= 0 || ((p_q - p_j).norm() < max_dist_rel) ) { Eigen::Vector3f n_j = normals.at<Eigen::Vector3f>(k_y,k_x); Eigen::Vector3f m_j = M * n_j; m_j_list.push_back(m_j); _m += m_j; } } } if(m_j_list.size() >= k) { _m /= m_j_list.size(); for (int i = 0; i < m_j_list.size(); ++i) { A += (m_j_list[i] - _m)*((m_j_list[i] - _m).transpose()); } A /= m_j_list.size(); solver.computeDirect(A); float diff = solver.eigenvalues()(2) - solver.eigenvalues()(1); float mean = (solver.eigenvalues()(2) + solver.eigenvalues()(1)) / 2; float ratio = solver.eigenvalues()(1) / solver.eigenvalues()(2); Eigen::Vector3f evec = solver.eigenvectors().col(2); pc.at<Vector5f>(y,x) = Vector5f(); pc.at<Vector5f>(y,x) << solver.eigenvalues()(1), solver.eigenvalues()(2), evec; } else { failed++; pc.at<Vector5f>(y,x) = Vector5f::Zero(); pc.at<Vector5f>(y,x) << std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(); } } } }