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;
 }
Exemple #5
0
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);
  }
}
Exemple #6
0
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;
		}
	}
}
Exemple #7
0
/// 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();
}
Exemple #8
0
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();
}
Exemple #9
0
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);
  }
}
Exemple #10
0
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);
}
Exemple #11
0
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;
	
}
Exemple #14
0
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);
}
Exemple #15
0
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);
  }
}
Exemple #16
0
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();
}
Exemple #18
0
//////////////////////////////////////////////////////////////////////////////////////////////
// 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);
}
Exemple #19
0
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();
			}
		}
	}
}
Exemple #20
0
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;
}
Exemple #21
0
  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;
  };
Exemple #22
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;
}
Exemple #23
0
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;
}
Exemple #24
0
//////////////////////////////////////////////////////////////////////////////////////////////
// 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);
}
Exemple #25
0
//////////////////////////////////////////////////////////////////////////////////////////////
// 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);
}
Exemple #26
0
 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);
	}
      } 
    }
  }
}