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