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();
}
void Foam::mosDMDEigenBase::realSymmEigenSolver(const Eigen::MatrixXd& M, Eigen::DiagonalMatrix<scalar, Eigen::Dynamic>& S, Eigen::MatrixXd& U)
{
// Solve eigenvalues and eigenvectors
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver;
eigenSolver.compute(M);
// Sort eigenvalues and corresponding eigenvectors
// in descending order
SortableList<scalar> sortedList(M.rows());
forAll (sortedList, i)
{
sortedList[i] = eigenSolver.eigenvalues()[i];
}
// Do sort
sortedList.sort();
label n = 0;
forAllReverse(sortedList, i)
{
S.diagonal()[n] = sortedList[i];
U.col(n) = eigenSolver.eigenvectors().col(sortedList.indices()[i]);
n++;
}
double
RotationAverage::chordal(Sophus::SO3d *avg)
{
double max_angle=0;
Eigen::Matrix4d Q;
Q.setZero();
auto rest = rotations.begin();
rest++;
for(auto && t: zip_range(weights, rotations))
{
double w=t.get<0>();
Sophus::SO3d& q = t.get<1>();
Q += w * q.unit_quaternion().coeffs() * q.unit_quaternion().coeffs().transpose();
max_angle = std::accumulate(rest, rotations.end(), max_angle,
[&q](double max, const Sophus::SO3d& other)
{
return std::max(max,
q.unit_quaternion().angularDistance(other.unit_quaternion()));
});
}
Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver;
solver.compute(Q);
Eigen::Vector4d::Map(avg->data()) = solver.eigenvectors().col(3);
return max_angle;
}
void
NurbsTools::pca (const vector_vec2d &data, Eigen::Vector2d &mean, Eigen::Matrix2d &eigenvectors,
Eigen::Vector2d &eigenvalues)
{
if (data.empty ())
{
printf ("[NurbsTools::pca] Error, data is empty\n");
abort ();
}
mean = computeMean (data);
unsigned s = unsigned (data.size ());
Eigen::MatrixXd Q (2, s);
for (unsigned i = 0; i < s; i++)
Q.col (i) << (data[i] - mean);
Eigen::Matrix2d C = Q * Q.transpose ();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver (C);
if (eigensolver.info () != Success)
{
printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
abort ();
}
for (int i = 0; i < 2; ++i)
{
eigenvalues (i) = eigensolver.eigenvalues () (1 - i);
eigenvectors.col (i) = eigensolver.eigenvectors ().col (1 - i);
}
}
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;
}
}
}
/// Draw ellipsoid
void QGLVisualizer::drawEllipsoid(const Vec3& pos, const Mat33& covariance) const{
Eigen::SelfAdjointEigenSolver<Mat33> es;
es.compute(covariance);
Mat33 V(es.eigenvectors());
double GLmat[16]={V(0,0), V(1,0), V(2,0), 0, V(0,1), V(1,1), V(2,1), 0, V(0,2), V(1,2), V(2,2), 0, pos.x(), pos.y(), pos.z(), 1};
glPushMatrix();
glMultMatrixd(GLmat);
drawEllipsoid(10,10,sqrt(es.eigenvalues()(0))*config.ellipsoidScale, sqrt(es.eigenvalues()(1))*config.ellipsoidScale, sqrt(es.eigenvalues()(2))*config.ellipsoidScale);
glPopMatrix();
}
Eigen::Vector3f FitNormal(const std::vector<T>& points, F f)
{
// return Eigen::Vector3f(0.0f, 0.0f, 1.0f);
// compute covariance matrix
Eigen::Matrix3f A = PointCovariance(points, f);
// compute eigenvalues/-vectors
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
solver.compute(A);
// take eigenvector (first eigenvalue is smallest!)
return solver.eigenvectors().col(0).normalized();
}
void
NurbsTools::pca (const vector_vec3d &data, ON_3dVector &mean, Eigen::Matrix3d &eigenvectors,
Eigen::Vector3d &eigenvalues)
{
if (data.empty ())
{
printf ("[NurbsTools::pca] Error, data is empty\n");
abort ();
}
mean = computeMean (data);
unsigned s = data.size ();
ON_Matrix Q(3, s);
for (unsigned i = 0; i < s; i++) {
Q[0][i] = data[i].x - mean.x;
Q[1][i] = data[i].y - mean.y;
Q[2][i] = data[i].z - mean.z;
}
ON_Matrix Qt = Q;
Qt.Transpose();
ON_Matrix oC;
oC.Multiply(Q,Qt);
Eigen::Matrix3d C(3,3);
for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
C(i,j) = oC[i][j];
}
}
Eigen::SelfAdjointEigenSolver < Eigen::Matrix3d > eigensolver (C);
if (eigensolver.info () != Eigen::Success)
{
printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
abort ();
}
for (int i = 0; i < 3; ++i)
{
eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
if (i == 2)
eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
else
eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
}
}
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);
}
void computeEigenReferenceSolution
(
size_t nr_problems,
const Vcl::Core::InterleavedArray<float, 3, 3, -1>& ATA,
Vcl::Core::InterleavedArray<float, 3, 3, -1>& U,
Vcl::Core::InterleavedArray<float, 3, 1, -1>& S
)
{
// Compute reference using Eigen
for (int i = 0; i < static_cast<int>(nr_problems); i++)
{
Vcl::Matrix3f A = ATA.at<float>(i);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
solver.compute(A, Eigen::ComputeEigenvectors);
U.at<float>(i) = solver.eigenvectors();
S.at<float>(i) = solver.eigenvalues();
}
}
void
drawBoundingBox(PointCloudT::Ptr cloud,
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
int z)
{
//Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);
//Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
//Eigen::ComputeEigenvectors);
eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
// eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
// (covariance,Eigen::ComputeEigenvectors);
eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));
//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
//pcl::PointCloud<PointT> cPoints;
pcl::transformPointCloud(*cloud, cPoints, p2w);
//PointT min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
//viewer->addPointCloud(cloud);
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));
}
std::list< LieGroup > confidence_region_contours(const NormalDistributionOn<LieGroup>& N, int dim, typename LieGroup::Scalar confidence) {
using namespace std;
list< typename LieGroup::Tangent > sphere = sample_sphere< typename LieGroup::Scalar, LieGroup::DoF >(50, dim);
boost::math::chi_squared_distribution<typename LieGroup::Scalar> chi2( LieGroup::DoF );
double scale = sqrt( boost::math::quantile(chi2, confidence) ) ;
Eigen::SelfAdjointEigenSolver< typename NormalDistributionOn< LieGroup >::Covariance > eigs;
eigs.compute(N.covariance());
typename NormalDistributionOn<SE2d>::Covariance sqrt_cov = eigs.eigenvectors() * eigs.eigenvalues().array().sqrt().matrix().asDiagonal();
std::list< LieGroup > out;
for( typename list< typename LieGroup::Tangent >::iterator it = sphere.begin(); it != sphere.end(); it++ ) {
out.push_back( N.mean() * LieGroup::exp( scale* sqrt_cov * (*it) ) );
}
return out;
}
template<typename PointT> bool
pcl::PCA<PointT>::initCompute ()
{
if(!Base::initCompute ())
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
return (false);
}
if(indices_->size () < 3)
{
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
return (false);
}
// Compute mean
mean_ = Eigen::Vector4f::Zero ();
compute3DCentroid (*input_, *indices_, mean_);
// Compute demeanished cloud
Eigen::MatrixXf cloud_demean;
demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
assert (cloud_demean.cols () == int (indices_->size ()));
// Compute the product cloud_demean * cloud_demean^T
Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
// Compute eigen vectors and values
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
// Organize eigenvectors and eigenvalues in ascendent order
for (int i = 0; i < 3; ++i)
{
eigenvalues_[i] = evd.eigenvalues () [2-i];
eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
}
// If not basis only then compute the coefficients
if (!basis_only_)
coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
compute_done_ = true;
return (true);
}
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);
}
}
static void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
// Inertia tensor should by symmetric
assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
// Inertia tensor should have positive determinant
assert( I.determinant() > 0.0 );
// Compute the eigenvectors and eigenvalues of the input matrix
const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };
// Check for errors
if( es.info() == Eigen::NumericalIssue )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
}
else if( es.info() == Eigen::NoConvergence )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
}
else if( es.info() == Eigen::InvalidInput )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
}
assert( es.info() == Eigen::Success );
// Save the eigenvectors and eigenvalues
I0 = es.eigenvalues();
assert( ( I0.array() > 0.0 ).all() );
assert( I0.x() <= I0.y() );
assert( I0.y() <= I0.z() );
R0 = es.eigenvectors();
assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );
// Ensure that we have an orientation preserving transform
if( R0.determinant() < 0.0 )
{
R0.col( 0 ) *= -1.0;
}
}
void AbsoluteOrientation::compute( std::vector<Eigen::Vector3d> &left, std::vector<Eigen::Vector3d> &right, Eigen::Quaterniond &result )
{
int i, pairNum = left.size();
Eigen::MatrixXd muLmuR = Eigen::MatrixXd::Zero(3,3), M = Eigen::MatrixXd::Zero(3,3),
curMat = Eigen::MatrixXd::Zero(3,3), N = Eigen::MatrixXd::Zero(4,4);
Eigen::Vector3d meanFirst(0,0,0), meanSecond(0,0,0); //assume points set to zero by constructor
//compute the mean of both point sets
for (i=0; i<pairNum; i++) {
meanFirst[0] += left[i][0]; meanFirst[1] += left[i][1]; meanFirst[2] += left[i][2];
meanSecond[0] += right[i][0]; meanSecond[1] += right[i][1]; meanSecond[2] += right[i][2];
}
meanFirst[0]/=pairNum; meanFirst[1]/=pairNum; meanFirst[2]/=pairNum;
meanSecond[0]/=pairNum; meanSecond[1]/=pairNum; meanSecond[2]/=pairNum;
//compute the matrix muLmuR
muLmuR(0,0) = meanFirst[0]*meanSecond[0];
muLmuR(0,1) = meanFirst[0]*meanSecond[1];
muLmuR(0,2) = meanFirst[0]*meanSecond[2];
muLmuR(1,0) = meanFirst[1]*meanSecond[0];
muLmuR(1,1) = meanFirst[1]*meanSecond[1];
muLmuR(1,2) = meanFirst[1]*meanSecond[2];
muLmuR(2,0) = meanFirst[2]*meanSecond[0];
muLmuR(2,1) = meanFirst[2]*meanSecond[1];
muLmuR(2,2) = meanFirst[2]*meanSecond[2];
//compute the matrix M
for (i=0; i<pairNum; i++) {
Eigen::Vector3d &leftPoint = left[i];
Eigen::Vector3d &rightPoint = right[i];
curMat(0,0) = leftPoint[0]*rightPoint[0];
curMat(0,1) = leftPoint[0]*rightPoint[1];
curMat(0,2) = leftPoint[0]*rightPoint[2];
curMat(1,0) = leftPoint[1]*rightPoint[0];
curMat(1,1) = leftPoint[1]*rightPoint[1];
curMat(1,2) = leftPoint[1]*rightPoint[2];
curMat(2,0) = leftPoint[2]*rightPoint[0];
curMat(2,1) = leftPoint[2]*rightPoint[1];
curMat(2,2) = leftPoint[2]*rightPoint[2];
M+=curMat;
}
M+= (muLmuR *(-pairNum));
//compute the matrix N
Eigen::MatrixXd tmpMat = Eigen::MatrixXd::Zero(3,3);
double A12, A20, A01;
double traceM = 0.0;
for(i=0; i<3; i++) traceM += M(i,i);
tmpMat.diagonal() = Eigen::VectorXd::Constant(3, -traceM); //tmpMat.fill_diagonal(-traceM);
tmpMat += (M + M.transpose());
A12 = M(1,2) - M(2,1);
A20 = M(2,0) - M(0,2);
A01 = M(0,1) - M(1,0);
N(0,0)=traceM; N(0,1)=A12; N(0,2)=A20; N(0,3)=A01;
N(1,0)=A12;
N(2,0)=A20;
N(3,0)=A01;
N.bottomRightCorner(3,3) = tmpMat; //N.update(tmpMat,1,1);
////find the eigenvector that belongs to the maximal
////eigenvalue of N, eigenvalues are sorted from smallest to largest
//vnl_symmetric_eigensystem<double> eigenSystem(N);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
es.compute(N);
Eigen::MatrixXd V = es.eigenvectors();
//std::stringstream ss;ss << V;
//qDebug() << qPrintable(ss.str().c_str());
//setRotationQuaternion(eigenSystem.V(0,3),eigenSystem.V(1,3),eigenSystem.V(2,3),eigenSystem.V(3,3), true);
result = Eigen::Quaterniond( V(0,3),V(1,3),V(2,3),V(3,3) ).normalized();
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" vector
template <typename PointInT, typename PointNT, typename PointOutT> float
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::getSHOTLocalRF (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
const int index, const std::vector<int> &indices, const std::vector<float> &dists,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
if (rf.size () != 3)
rf.resize (3);
Eigen::Vector4f central_point = cloud.points[index].getVector4fMap ();
// Allocate enough space
Eigen::Vector4d *vij = new Eigen::Vector4d[indices.size ()];
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
if (indices[i_idx] == index)
continue;
Eigen::Vector4f pt = cloud.points[indices[i_idx]].getVector4fMap ();
// Difference between current point and origin
vij[valid_nn_points] = (pt - central_point).cast<double> ();
distance = search_radius_ - sqrt (dists[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij[valid_nn_points].head<3> () * vij[valid_nn_points].head<3> ().transpose ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
PCL_ERROR ("[pcl::%s::getSHOTLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point with index %d\n", getClassName ().c_str (), index);
rf[0].setZero ();
rf[1].setZero ();
rf[2].setZero ();
rf[0][0] = 1;
rf[1][1] = 1;
rf[2][2] = 1;
delete [] vij;
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
// Disambiguation
int plusNormal = 0, plusTangentDirection1=0;
Eigen::Vector3d v1c = solver.eigenvectors ().col (0);
Eigen::Vector3d v2c = solver.eigenvectors ().col (1);
Eigen::Vector3d v3c = solver.eigenvectors ().col (2);
double e1c = solver.eigenvalues ()[0];
double e2c = solver.eigenvalues ()[1];
double e3c = solver.eigenvalues ()[2];
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
if (e1c > e2c)
{
if (e1c > e3c) // v1c > max(v2c,v3c)
{
v1.head<3> () = v1c;
if (e2c > e3c) // v1c > v2c > v3c
v3.head<3> () = v3c;
else // v1c > v3c > v2c
v3.head<3> () = v2c;
}
else // v3c > v1c > v2c
{
v1.head<3> () = v3c;
v3.head<3> () = v2c;
}
}
else
{
if (e2c > e3c) // v2c > max(v1c,v3c)
{
v1.head<3> () = v2c;
if (e1c > e3c) // v2c > v1c > v3c
v3.head<3> () = v3c;
else // v2c > v3c > v1c
v3.head<3> () = v1c;
}
else // v3c > v2c > v1c
{
v1.head<3> () = v3c;
v3.head<3> () = v1c;
}
}
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij[ne].dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij[ne].dot (v3);
if (dp >= 0)
plusNormal++;
}
if (plusTangentDirection1 < valid_nn_points - plusTangentDirection1)
v1 *= - 1;
if (plusNormal < valid_nn_points - plusNormal)
v3 *= - 1;
rf[0] = v1.cast<float>();
rf[2] = v3.cast<float>();
rf[1] = rf[2].cross3 (rf[0]);
rf[0][3] = 0; rf[1][3] = 0; rf[2][3] = 0;
delete [] vij;
return (0.0f);
}
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();
}
}
}
}
typename GaussianProcess<TScalarType>::MatrixType GaussianProcess<TScalarType>::InvertKernelMatrix(const typename GaussianProcess<TScalarType>::MatrixType &K,
typename GaussianProcess<TScalarType>::InversionMethod inv_method = GaussianProcess<TScalarType>::FullPivotLU,
bool stable) const{
// compute core matrix
if(debug){
std::cout << "GaussianProcess::InvertKernelMatrix: inverting kernel matrix... ";
std::cout.flush();
}
typename GaussianProcess<TScalarType>::MatrixType core;
switch(inv_method){
// standard method: fast but not that accurate
// Uses the LU decomposition with full pivoting for the inversion
case FullPivotLU:{
if(debug) std::cout << " (inversion method: FullPivotLU) " << std::flush;
try{
if(stable){
core = K.inverse();
}
else{
if(debug) std::cout << " (using lapack) " << std::flush;
core = lapack::lu_invert<TScalarType>(K);
}
}
catch(lapack::LAPACKException& e){
core = K.inverse();
}
}
break;
// very accurate and very slow method, use it for small problems
// Uses the two-sided Jacobi SVD decomposition
case JacobiSVD:{
if(debug) std::cout << " (inversion method: JacobiSVD) " << std::flush;
Eigen::JacobiSVD<MatrixType> jacobisvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
if((jacobisvd.singularValues().real().array() < 0).any() && debug){
std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
std::cout.flush();
}
core = jacobisvd.matrixV() * VectorType(1/jacobisvd.singularValues().array()).asDiagonal() * jacobisvd.matrixU().transpose();
}
break;
// accurate method and faster than Jacobi SVD.
// Uses the bidiagonal divide and conquer SVD
case BDCSVD:{
if(debug) std::cout << " (inversion method: BDCSVD) " << std::flush;
#ifdef EIGEN_BDCSVD_H
Eigen::BDCSVD<MatrixType> bdcsvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
if((bdcsvd.singularValues().real().array() < 0).any() && debug){
std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
std::cout.flush();
}
core = bdcsvd.matrixV() * VectorType(1/bdcsvd.singularValues().array()).asDiagonal() * bdcsvd.matrixU().transpose();
#else
// this is checked, since BDCSVD is currently not in the newest release
throw std::string("GaussianProcess::InvertKernelMatrix: BDCSVD is not supported by the provided Eigen library.");
#endif
}
break;
// faster than the SVD method but less stable
// computes the eigenvalues/eigenvectors of selfadjoint matrices
case SelfAdjointEigenSolver:{
if(debug) std::cout << " (inversion method: SelfAdjointEigenSolver) " << std::flush;
try{
core = lapack::chol_invert<TScalarType>(K);
}
catch(lapack::LAPACKException& e){
Eigen::SelfAdjointEigenSolver<MatrixType> es;
es.compute(K);
VectorType eigenValues = es.eigenvalues().reverse();
MatrixType eigenVectors = es.eigenvectors().rowwise().reverse();
if((eigenValues.real().array() < 0).any() && debug){
std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
std::cout.flush();
}
core = eigenVectors * VectorType(1/eigenValues.array()).asDiagonal() * eigenVectors.transpose();
}
}
break;
}
if(debug) std::cout << "[done]" << std::endl;
return core;
}
int Fit(Vector& res_G, // residual under NULL -- may change when permuting
Vector& v_G, // variance under NULL -- may change when permuting
Matrix& X_G, // covariance
Matrix& G_G, // genotype
Vector& w_G) // weight
{
this->nPeople = X_G.rows;
this->nMarker = G_G.cols;
this->nCovariate = X_G.cols;
// calculation w_sqrt
G_to_Eigen(w_G, &this->w_sqrt);
w_sqrt = w_sqrt.cwiseSqrt();
// calculate K = G * W * G'
Eigen::MatrixXf G;
G_to_Eigen(G_G, &G);
this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();
// calculate Q = ||res * K||
Eigen::VectorXf res;
G_to_Eigen(res_G, &res);
this->Q = (this->K_sqrt * res).squaredNorm();
// calculate P0 = V - V X (X' V X)^(-1) X' V
Eigen::VectorXf v;
G_to_Eigen(v_G, &v);
if (this->nCovariate == 1) {
P0 = -v * v.transpose() / v.sum();
// printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
// printf("dim(v) = %d\n", v.size());
P0.diagonal() += v;
// printf("dim(v) = %d\n", v.size());
} else {
Eigen::MatrixXf X;
G_to_Eigen(X_G, &X);
Eigen::MatrixXf XtV; // X^t V
XtV.noalias() = X.transpose() * v.asDiagonal();
P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
P0.diagonal() += v;
}
// dump();
// Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
// dumpToFile(tmp, "out.tmp");
// eigen decomposition
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
es.compute(K_sqrt * P0 * K_sqrt.transpose());
#ifdef DEBUG
std::ofstream k("K");
k << K_sqrt;
k.close();
#endif
// std::ofstream p("P0");
// p << P0;
// p.close();
this->mixChiSq.reset();
int r_ub = std::min(nPeople, nMarker);
int r = 0; // es.eigenvalues().size();
int eigen_len = es.eigenvalues().size();
for (int i = eigen_len - 1; i >= 0; i--) {
if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
this->mixChiSq.addLambda(es.eigenvalues()[i]);
r++;
} else
break;
}
// calculate p-value
this->pValue = this->mixChiSq.getPvalue(this->Q);
if (this->pValue == 0.0 || this->pValue == 1.0) {
this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
}
return 0;
};
bool TraceStoreCol::PcaLossyCompressChunk(int row_start, int row_end,
int col_start, int col_end,
int num_rows, int num_cols, int num_frames,
int frame_stride,
int flow_ix, int flow_frame_stride,
short *data, bool replace,
float *ssq, char *filters,
int row_step, int col_step,
int num_pca) {
Eigen::MatrixXf Ysub, Y, S, Basis;
int loc_num_wells = (col_end - col_start) * (row_end - row_start);
int loc_num_cols = col_end - col_start;
// Take a sample of the data at rate step, avoiding flagged wells
// Count the good rows
int sample_wells = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
char *filt_start = filters + row_ix * num_cols + col_start;
char *filt_end = filt_start + loc_num_cols;
while (filt_start < filt_end) {
if (*filt_start == 0) {
sample_wells++;
}
filt_start += col_step;
}
}
// try backing off to every well rather than just sampled if we didn't get enough
if (sample_wells < MIN_SAMPLE_WELL) {
row_step = 1;
col_step = 1;
int sample_wells = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+= row_step) {
char *filt_start = filters + row_ix * num_cols + col_start;
char *filt_end = filt_start + loc_num_cols;
while (filt_start < filt_end) {
if (*filt_start == 0) {
sample_wells++;
}
filt_start += col_step;
}
}
}
if (sample_wells < MIN_SAMPLE_WELL) {
return false; // just give up
}
// Got enough data to work with, zero out the ssq array for accumulation
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
float *ssq_start = ssq + row_ix * num_cols + col_start;
float *ssq_end = ssq_start + loc_num_cols;
while (ssq_start != ssq_end) {
*ssq_start++ = 0;
}
}
// Copy the sampled data in Matrix, frame major
Ysub.resize(sample_wells, num_frames);
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
int sample_offset = 0;
for (int row_ix = row_start; row_ix < row_end; row_ix+=row_step) {
size_t store_offset = row_ix * num_cols + col_start;
char *filt_start = filters + store_offset;
char *filt_end = filt_start + loc_num_cols;
int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
float *ysub_start = Ysub.data() + sample_wells * frame_ix + sample_offset;
while (filt_start < filt_end) {
if (*filt_start == 0) {
*ysub_start = *trace_start;
ysub_start++;
sample_offset++;
}
trace_start += col_step;
filt_start += col_step;
}
}
}
// Copy in all the data into working matrix
Y.resize(loc_num_wells, (int)num_frames);
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
int16_t *trace_start = data + (flow_frame_stride * frame_ix) + (flow_ix * frame_stride) + store_offset;
int16_t *trace_end = trace_start + loc_num_cols;
float * y_start = Y.data() + loc_num_wells * frame_ix + (row_ix - row_start) * loc_num_cols;
while( trace_start != trace_end ) {
*y_start++ = *trace_start++;
}
}
}
Eigen::VectorXf col_mean = Y.colwise().sum();
col_mean /= Y.rows();
for (int i = 0; i < Y.cols(); i++) {
Y.col(i).array() -= col_mean.coeff(i);
}
// Create scatter matrix
S = Ysub.transpose() * Ysub;
// Compute the eigenvectors
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
es.compute(S);
Eigen::MatrixXf Pca_Basis = es.eigenvectors();
Eigen::VectorXf Pca_Values = es.eigenvalues();
// Copy top eigen vectors into basis for projection
Basis.resize(num_frames, num_pca);
for (int i = 0; i < Basis.cols(); i++) {
// Basis.col(i) = es.eigenvectors().col(es.eigenvectors().cols() - i -1);
Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
}
// Create solver matrix, often not a good way of solving things but eigen vectors should be stable and fast
Eigen::MatrixXf SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
// Get coefficients to solve
Eigen::MatrixXf B = Y * SX.transpose();
// Uncompress data into yhat matrix
Eigen::MatrixXf Yhat = B * Basis.transpose();
for (int i = 0; i < Yhat.cols(); i++) {
Yhat.col(i).array() += col_mean.coeff(i);
Y.col(i).array() += col_mean.coeff(i);
}
// H5File h5("pca_lossy.h5");
// h5.Open();
// char buff[256];
// snprintf(buff, sizeof(buff), "/Y_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Y);
// snprintf(buff, sizeof(buff), "/Yhat_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Yhat);
// snprintf(buff, sizeof(buff), "/Basis_%d_%d_%d", flow_ix, row_start, col_start);
// H5Eigen::WriteMatrix(h5, buff, Basis);
// h5.Close();
// Copy data out of yhat matrix into original data structure, keeping track of residuals
for (int frame_ix = 0; frame_ix < (int)num_frames; frame_ix++) {
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
int16_t *trace_start = data + flow_frame_stride * frame_ix + flow_ix * frame_stride + store_offset;
int16_t *trace_end = trace_start + loc_num_cols;
float * ssq_start = ssq + store_offset;
size_t loc_offset = (row_ix - row_start) * loc_num_cols;
float * y_start = Y.data() + loc_num_wells * frame_ix + loc_offset;
float * yhat_start = Yhat.data() + loc_num_wells * frame_ix + loc_offset;
while( trace_start != trace_end ) {
if (replace) {
*trace_start = (int16_t)(*yhat_start + .5);
}
float val = *y_start - *yhat_start;
*ssq_start += val * val;
y_start++;
yhat_start++;
trace_start++;
ssq_start++;
}
}
}
// divide ssq data out for per frame avg
for (int row_ix = row_start; row_ix < row_end; row_ix++) {
size_t store_offset = row_ix * num_cols + col_start;
float * ssq_start = ssq + store_offset;
float * ssq_end = ssq_start + loc_num_cols;
while (ssq_start != ssq_end) {
*ssq_start /= num_frames;
ssq_start++;
}
}
return true;
}
template<typename PointInT, typename PointOutT, typename NormalT> void
pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
// Make sure the output cloud is empty
output.points.clear ();
if (border_radius_ > 0.0)
edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
bool* borders = new bool [input_->size()];
int index;
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
borders[index] = false;
PointInT current_point = input_->points[index];
if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
{
std::vector<int> nn_indices;
std::vector<float> nn_distances;
this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
for (size_t j = 0 ; j < nn_indices.size (); j++)
{
if (edge_points_[nn_indices[j]])
{
borders[index] = true;
break;
}
}
}
}
#ifdef _OPENMP
Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
for (size_t i = 0; i < threads_; i++)
omp_mem[i].setZero (3);
#else
Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1];
omp_mem[0].setZero (3);
#endif
double *prg_local_mem = new double[input_->size () * 3];
double **prg_mem = new double * [input_->size ()];
for (size_t i = 0; i < input_->size (); i++)
prg_mem[i] = prg_local_mem + 3 * i;
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (index = 0; index < static_cast<int> (input_->size ()); index++)
{
#ifdef _OPENMP
int tid = omp_get_thread_num ();
#else
int tid = 0;
#endif
PointInT current_point = input_->points[index];
if ((!borders[index]) && pcl::isFinite(current_point))
{
//if the considered point is not a border point and the point is "finite", then compute the scatter matrix
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
getScatterMatrix (static_cast<int> (index), cov_m);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[2];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[0];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
continue;
if (e3c < 0)
{
PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
name_.c_str (), index);
continue;
}
omp_mem[tid][0] = e2c / e1c;
omp_mem[tid][1] = e3c / e2c;;
omp_mem[tid][2] = e3c;
}
for (int d = 0; d < omp_mem[tid].size (); d++)
prg_mem[index][d] = omp_mem[tid][d];
}
for (index = 0; index < int (input_->size ()); index++)
{
if (!borders[index])
{
if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
third_eigen_value_[index] = prg_mem[index][2];
}
}
bool* feat_max = new bool [input_->size()];
bool is_max;
#ifdef _OPENMP
#pragma omp parallel for private(is_max) num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
feat_max [index] = false;
PointInT current_point = input_->points[index];
if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
{
std::vector<int> nn_indices;
std::vector<float> nn_distances;
int n_neighbors;
this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
n_neighbors = static_cast<int> (nn_indices.size ());
if (n_neighbors >= min_neighbors_)
{
is_max = true;
for (int j = 0 ; j < n_neighbors; j++)
if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
is_max = false;
if (is_max)
feat_max[index] = true;
}
}
}
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
if (feat_max[index])
#ifdef _OPENMP
#pragma omp critical
#endif
{
PointOutT p;
p.getVector3fMap () = input_->points[index].getVector3fMap ();
output.points.push_back(p);
keypoints_indices_->indices.push_back (index);
}
}
output.header = input_->header;
output.width = static_cast<uint32_t> (output.points.size ());
output.height = 1;
// Clear the contents of variables and arrays before the beginning of the next computation.
if (border_radius_ > 0.0)
normals_.reset (new pcl::PointCloud<NormalT>);
delete[] borders;
delete[] prg_mem;
delete[] prg_local_mem;
delete[] feat_max;
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
{
Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
if (pt.head<3> () == central_point.head<3> ())
continue;
// Difference between current point and origin
vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
vij (valid_nn_points, 3) = 0;
distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[0];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[2];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
// Disambiguation
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
v1.head<3> () = solver.eigenvectors ().col (2);
v3.head<3> () = solver.eigenvectors ().col (0);
int plusNormal = 0, plusTangentDirection1=0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij.row (ne).dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij.row (ne).dot (v3);
if (dp >= 0)
plusNormal++;
}
//TANGENT
plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
if (plusTangentDirection1 == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v1) > 0)
plusTangentDirection1 ++;
if (plusTangentDirection1 < points/2+1)
v1 *= - 1;
} else if (plusTangentDirection1 < 0)
v1 *= - 1;
//Normal
plusNormal = 2*plusNormal - valid_nn_points;
if (plusNormal == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v3) > 0)
plusNormal ++;
if (plusNormal < points/2+1)
v3 *= - 1;
} else if (plusNormal < 0)
v3 *= - 1;
rf.row (0) = v1.head<3> ().cast<float> ();
rf.row (2) = v3.head<3> ().cast<float> ();
rf.row (1) = rf.row (2).cross (rf.row (0));
return (0.0f);
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" vector
template <typename PointInT> float
pcl::getLocalRF (const pcl::PointCloud<PointInT> &cloud,
const double search_radius,
const Eigen::Vector4f & central_point,
const std::vector<int> &indices,
const std::vector<float> &dists,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
if (rf.size () != 3)
rf.resize (3);
// Allocate enough space
Eigen::Vector4d *vij = new Eigen::Vector4d[indices.size ()];
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
/*if (indices[i_idx] == index)
continue;*/
Eigen::Vector4f pt = cloud.points[indices[i_idx]].getVector4fMap ();
pt[3] = 0;
if (pt == central_point)
continue;
// Difference between current point and origin
vij[valid_nn_points] = (pt - central_point).cast<double> ();
vij[valid_nn_points][3] = 0;
distance = search_radius - sqrt (dists[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij[valid_nn_points].head<3> () * vij[valid_nn_points].head<3> ().transpose ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
PCL_ERROR ("[pcl::%s::getSHOTLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOT", central_point[0], central_point[1], central_point[2]);
rf[0].setZero ();
rf[1].setZero ();
rf[2].setZero ();
rf[0][0] = 1;
rf[1][1] = 1;
rf[2][2] = 1;
delete [] vij;
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
// Disambiguation
Eigen::Vector3d v1c = solver.eigenvectors ().col (0);
Eigen::Vector3d v2c = solver.eigenvectors ().col (1);
Eigen::Vector3d v3c = solver.eigenvectors ().col (2);
double e1c = solver.eigenvalues ()[0];
double e2c = solver.eigenvalues ()[1];
double e3c = solver.eigenvalues ()[2];
if(!pcl_isfinite(e1c) || !pcl_isfinite(e2c) || !pcl_isfinite(e3c)){
PCL_ERROR ("[pcl::%s::getSHOTLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOT", central_point[0], central_point[1], central_point[2]);
rf[0].setZero ();
rf[1].setZero ();
rf[2].setZero ();
rf[0][0] = 1;
rf[1][1] = 1;
rf[2][2] = 1;
delete [] vij;
return (std::numeric_limits<float>::max ());
}
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
if (e1c > e2c)
{
if (e1c > e3c) // v1c > max(v2c,v3c)
{
v1.head<3> () = v1c;
if (e2c > e3c) // v1c > v2c > v3c
v3.head<3> () = v3c;
else // v1c > v3c > v2c
v3.head<3> () = v2c;
}
else // v3c > v1c > v2c
{
v1.head<3> () = v3c;
v3.head<3> () = v2c;
}
}
else
{
if (e2c > e3c) // v2c > max(v1c,v3c)
{
v1.head<3> () = v2c;
if (e1c > e3c) // v2c > v1c > v3c
v3.head<3> () = v3c;
else // v2c > v3c > v1c
v3.head<3> () = v1c;
}
else // v3c > v2c > v1c
{
v1.head<3> () = v3c;
v3.head<3> () = v1c;
}
}
int plusNormal = 0, plusTangentDirection1=0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij[ne].dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij[ne].dot (v3);
if (dp >= 0)
plusNormal++;
}
//TANGENT
if( abs ( plusTangentDirection1 - valid_nn_points + plusTangentDirection1 ) > 0 )
{
if (plusTangentDirection1 < valid_nn_points - plusTangentDirection1)
v1 *= - 1;
}
else
{
plusTangentDirection1=0;
int points = 5; ///std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij[medianIndex- i ].dot (v1) > 0)
plusTangentDirection1 ++;
if (plusTangentDirection1 < points/2+1)
v1 *= - 1;
}
//Normal
if( abs ( plusNormal - valid_nn_points + plusNormal ) > 0 )
{
if (plusNormal < valid_nn_points - plusNormal)
v3 *= - 1;
}
else
{
plusNormal = 0;
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
//std::cout << points << std::endl;
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij[medianIndex- i ].dot (v3) > 0)
plusNormal ++;
if (plusNormal < points/2+1)
v3 *= - 1;
}
rf[0] = v1.cast<float>();
rf[2] = v3.cast<float>();
rf[1] = rf[2].cross3 (rf[0]);
rf[0][3] = 0; rf[1][3] = 0; rf[2][3] = 0;
delete [] vij;
return (0.0f);
}
void operator() (Image<value_type>& dt_img)
{
/* check mask */
if (mask_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (mask_img);
if (!mask_img.value())
return;
}
/* input dt */
Eigen::Matrix<double, 6, 1> dt;
for (auto l = Loop (3) (dt_img); l; ++l)
dt[dt_img.index(3)] = dt_img.value();
/* output adc */
if (adc_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (adc_img);
adc_img.value() = DWI::tensor2ADC(dt);
}
double fa = 0.0;
if (fa_img.valid() || (vector_img.valid() && (modulate == 1)))
fa = DWI::tensor2FA(dt);
/* output fa */
if (fa_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (fa_img);
fa_img.value() = fa;
}
bool need_eigenvalues = value_img.valid() || (vector_img.valid() && (modulate == 2)) || ad_img.valid() || rd_img.valid() || cl_img.valid() || cp_img.valid() || cs_img.valid();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
if (need_eigenvalues || vector_img.valid()) {
Eigen::Matrix3d M;
M (0,0) = dt[0];
M (1,1) = dt[1];
M (2,2) = dt[2];
M (0,1) = M (1,0) = dt[3];
M (0,2) = M (2,0) = dt[4];
M (1,2) = M (2,1) = dt[5];
es = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(M, vector_img.valid() ? Eigen::ComputeEigenvectors : Eigen::EigenvaluesOnly);
}
Eigen::Vector3d eigval;
if (need_eigenvalues)
eigval = es.eigenvalues();
/* output value */
if (value_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (value_img);
if (vals.size() > 1) {
auto l = Loop(3)(value_img);
for (size_t i = 0; i < vals.size(); i++) {
value_img.value() = eigval(3-vals[i]); l++;
}
} else {
value_img.value() = eigval(3-vals[0]);
}
}
/* output ad */
if (ad_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (ad_img);
ad_img.value() = eigval(2);
}
/* output rd */
if (rd_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (rd_img);
rd_img.value() = (eigval(1) + eigval(0)) / 2;
}
/* output shape measures */
if (cl_img.valid() || cp_img.valid() || cs_img.valid()) {
double eigsum = eigval.sum();
if (eigsum != 0.0) {
if (cl_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cl_img);
cl_img.value() = (eigval(2) - eigval(1)) / eigsum;
}
if (cp_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cp_img);
cp_img.value() = 2.0 * (eigval(1) - eigval(0)) / eigsum;
}
if (cs_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cs_img);
cs_img.value() = 3.0 * eigval(0) / eigsum;
}
}
}
/* output vector */
if (vector_img.valid()) {
Eigen::Matrix3d eigvec = es.eigenvectors();
assign_pos_of (dt_img, 0, 3).to (vector_img);
auto l = Loop(3)(vector_img);
for (size_t i = 0; i < vals.size(); i++) {
double fact = 1.0;
if (modulate == 1)
fact = fa;
else if (modulate == 2)
fact = eigval(3-vals[i]);
vector_img.value() = eigvec(0,3-vals[i])*fact; l++;
vector_img.value() = eigvec(1,3-vals[i])*fact; l++;
vector_img.value() = eigvec(2,3-vals[i])*fact; l++;
}
}
}
void PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices,
const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){
_integralImage.compute(indices,points);
int q=0;
int outerStep = _numThreads * _step;
PixelMapper pixelMapper;
pixelMapper.setCameraMatrix(cameraMatrix);
pixelMapper.setTransform(transform);
Eigen::Isometry3f inverseTransform = transform.inverse();
#pragma omp parallel
{
#ifdef _PWN_USE_OPENMP_
int threadNum = omp_get_thread_num();
#else // _PWN_USE_OPENMP_
int threadNum = 0;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int c=threadNum; c<indices.cols(); c+=outerStep) {
#ifdef _PWN_USE_OPENMP_
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int r=0; r<indices.rows(); r+=_step, q++){
int index = indices(r,c);
//cerr << "index(" << r <<"," << c << ")=" << index << endl;
if (index<0)
continue;
// determine the region
PointWithNormal& point = points[index];
PointWithNormalSVD& originalSVD = svds[index];
PointWithNormalSVD svd;
Eigen::Vector3f normal = point.normal();
Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0));
svd._z=point(2);
coord.head<2>()*=(1./coord(2));
int dx = abs(c - coord[0]);
int dy = abs(r - coord[1]);
if (dx>_imageRadius)
dx = _imageRadius;
if (dy>_imageRadius)
dy = _imageRadius;
PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy);
svd._mean=point.point();
if (acc.n()>_minPoints){
Eigen::Vector3f mean = acc.mean();
svd._mean = mean;
svd._n = acc.n();
Eigen::Matrix3f cov = acc.covariance();
eigenSolver.compute(cov);
svd._U=eigenSolver.eigenvectors();
svd._singularValues=eigenSolver.eigenvalues();
if (svd._singularValues(0) <0)
svd._singularValues(0) = 0;
/*
cerr << "\t svd.singularValues():" << svd.singularValues() << endl;
cerr << "\t svd.U():" << endl << svd.U() << endl;
//cerr << "\t svd.curvature():" << svd.curvature() << endl;
*/
normal = eigenSolver.eigenvectors().col(0).normalized();
if (normal.dot(inverseTransform * mean) > 0.0f)
normal =-normal;
svd.updateCurvature();
//cerr << "n(" << index << ") c:" << svd.curvature() << endl << point.tail<3>() << endl;
if (svd.curvature()>_maxCurvature){
//cerr << "region: " << c-dx << " " << c+dx << " " << r-dx << " " << r+dx << " points: " << acc.n() << endl;
normal.setZero();
}
} else {
normal.setZero();
svd = PointWithNormalSVD();
}
if (svd.n() > originalSVD.n()){
originalSVD = svd;
point.setNormal(normal);
}
}
}
}
}
