void eigenTest()
{
    e::Matrix<double, 3, 3> Q;
    e::Matrix<double, 3, 3> corrMatrix;
    corrMatrix(0, 0) = 17.25905;
    corrMatrix(0, 1) = -23.88775;
    corrMatrix(0, 2) = 6.448684;
    corrMatrix(1, 0) = -23.88775;
    corrMatrix(1, 1) = 37.74094;
    corrMatrix(1, 2) = -0.7966833;
    corrMatrix(2, 0) = 6.448684;
    corrMatrix(2, 1) = -0.7966833;
    corrMatrix(2, 2) = 17.4851;
    std::cout << "UNITTEST" << std::endl;
    std::cout << "CorrMatrix: " << std::endl;
    std::cout << corrMatrix << std::endl;
    std::cout << "******" << std::endl;

    std::vector<double> eigenValues(3);
    //jacobi_sweep(corrMatrix, Q, eigenValues);
    eigenSolver(corrMatrix, Q, eigenValues);


    std::cout << "EigenValues: " << eigenValues[0] << " " << eigenValues[1] << " " << eigenValues[2] << std::endl;
    std::cout << "EigenVec1:" << Q(0, 0) << " " << Q(1, 0) << " " << Q(2, 0) << std::endl;
    std::cout << "EigenVec2:" << Q(0, 1) << " " << Q(1, 1) << " " << Q(2, 1) << std::endl;
    std::cout << "EigenVec3:" << Q(0, 2) << " " << Q(1, 2) << " " << Q(2, 2) << std::endl;
}
Esempio n. 2
0
void Molecule::moveToPAF()
{
    Eigen::Matrix3d inertia = inertiaTensor();
    // Diagonalize inertia tensor V^t * I * V
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
    if (eigenSolver.info() != Eigen::Success) abort();
    // Rotate to Principal Axes Frame
    this->rotate(eigenSolver.eigenvectors());
    std::cout << eigenSolver.eigenvalues() << std::endl;
}
void umdFitLine(int lineCount, e::Vector2d **points, Line &l)
{
    e::Vector2d mean(0, 0);
    for(int i = 0; i < lineCount; ++i)
    {
        mean += *(points[i]);
    }
    mean = mean*(1.0/lineCount);

    //move it to mean;
    for(int i = 0; i < lineCount; ++i)
    {
        *(points[i]) -= mean;
    }

    //first compute the correlation matrix
    //should be row major
    e::Matrix<double, 2, 2> corrMatrix;
    for(int row = 0; row < 2; row++)
    {
        for(int col = 0; col < 2; col++)
        {
            //normal matrix multiplication m * m^T, this could be probably optimized since it's symmetrical
            double sum = 0;
            for(int i = 0; i < lineCount; i++)
            {
                sum += (*(points[i]))[row]* (*(points[i]))[col];
            }
            corrMatrix(row, col) = sum;
        }
    }


    e::Matrix<double, 2, 2> V;

    std::vector<double> eigenValues(2);
    eigenSolver(corrMatrix, V, eigenValues);

    //we want the normal
    l.a = V(1, 0);
    l.b = V(1, 1); //the second eigenvector - it is sorted for 2x2
    //compute mean

    l.c = -l.a*mean[0] - l.b*mean[1];
}
Esempio n. 4
0
rotorType Molecule::findRotorType()
{
    rotorType type;
    if (nAtoms_ == 1) {
        type = rtAtom;
    } else {
        // Get inertia tensor
        Eigen::Matrix3d inertia = inertiaTensor();
        // Diagonalize inertia tensor V^t * I * V
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
        if (eigenSolver.info() != Eigen::Success) abort();
        // Determine the degeneracy of the eigenvalues.
        int deg = 0;
        double tmp, abs, rel;
        for (int i = 0; i < 2; ++i) {
            for (int j = i + 1; j < 3 && deg < 2; ++j) { // Check i and j != i
                abs = fabs(eigenSolver.eigenvalues()[i] - eigenSolver.eigenvalues()[j]);
                tmp = eigenSolver.eigenvalues()[j]; // Because the eigenvalues are already in ascending order.
                if (abs > 1.0e-14) {
                    rel = abs/tmp;
                } else {
                    rel = 0.0;
                }
                if (rel < 1.0e-8) {
                    ++deg;
                }
            }
        }
        // Get the rotor type based on the degeneracy.
        if (eigenSolver.eigenvalues()[0] == 0.0) {
            type = rtLinear;
        } else if (deg == 2) {
            type = rtSpherical;
        } else if (deg == 1) { // We do not distinguish between prolate and oblate.
            type = rtSymmetric;
        } else {
            type = rtAsymmetric;
        }
    }

    return type;
}
Esempio n. 5
0
	Eigen::VectorXf ZMeshFilterManifold::computeMaxEigenVector(const Eigen::MatrixXf& inputMat, const std::vector<bool>& clusterK)
	{
		//Eigen::VectorXf ret(rangeDim_);
		Eigen::VectorXf ret(3);
		ret.fill(0);
		int count = 0;
		for (int i=0; i<clusterK.size(); i++) if (clusterK[i]) count++;
		Eigen::MatrixXf realInput(count, rangeDim_);
		//Eigen::MatrixXf realInput(count, 3);
		count = 0;
		for (int i=0; i<clusterK.size(); i++)
		{
			if (clusterK[i])
			{
				realInput.row(count) = inputMat.row(i);
				//realInput.row(count) = MATH::spherical2Cartesian(inputMat.row(i));
				count++;
			}
		}
		Eigen::MatrixXf mat = realInput.transpose()*realInput;
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenSolver(mat);
		if (eigenSolver.info()!=Eigen::Success) {
			std::cerr << "Error in SVD decomposition!\n";
			return ret;
		}
		Eigen::VectorXf eigenValues = eigenSolver.eigenvalues();
		Eigen::MatrixXf eigenVectors = eigenSolver.eigenvectors();
		int maxIdx = 0;
		float maxValue = eigenValues(maxIdx);
		for (int i=1; i<eigenValues.size(); i++)
		{
			if (eigenValues(i)>maxValue)
			{
				maxIdx = i;
				maxValue = eigenValues(maxIdx);
			}
		}
		ret = eigenVectors.col(maxIdx);
		return ret;
// 		Eigen::VectorXf retSph = MATH::cartesian2Spherical(ret, 1);
// 		return retSph.head(rangeDim_);
	}
Esempio n. 6
0
bool Functions::selfAdjointMatrixDecomposition(RefArrayXXd const covarianceMatrix, RefArrayXd eigenvalues, 
                                               RefArrayXXd eigenvectorsMatrix)
{
    assert(covarianceMatrix.cols() == covarianceMatrix.rows());
    assert(eigenvalues.size() == covarianceMatrix.cols());
    assert(eigenvectorsMatrix.cols() == eigenvectorsMatrix.rows());
    assert(eigenvectorsMatrix.cols() == eigenvalues.size());

    SelfAdjointEigenSolver<MatrixXd> eigenSolver(covarianceMatrix.matrix());

    if (eigenSolver.info() != Success)
    {
        cout << "Covariance Matrix decomposition failed." << endl;
        cout << "Quitting program" << endl;
        return false;
    }

    eigenvalues = eigenSolver.eigenvalues();
    eigenvectorsMatrix = eigenSolver.eigenvectors();

    return true;
}
Esempio n. 7
0
bool Ellipsoid::overlapsWith(Ellipsoid ellipsoid, bool &ellipsoidMatrixDecompositionIsSuccessful)
{
    // Construct translation matrix

    MatrixXd T1 = MatrixXd::Identity(Ndimensions+1,Ndimensions+1);
    MatrixXd T2 = MatrixXd::Identity(Ndimensions+1,Ndimensions+1);
    
    T1.bottomLeftCorner(1,Ndimensions) = (-1.0) * centerCoordinates.transpose();
    T2.bottomLeftCorner(1,Ndimensions) = (-1.0) * ellipsoid.getCenterCoordinates().transpose();


    // Construct ellipsoid matrix in homogeneous coordinates

    MatrixXd A = MatrixXd::Zero(Ndimensions+1,Ndimensions+1);
    MatrixXd B = A;

    A(Ndimensions,Ndimensions) = -1;
    B(Ndimensions,Ndimensions) = -1;

    A.topLeftCorner(Ndimensions,Ndimensions) = covarianceMatrix.matrix().inverse();
    B.topLeftCorner(Ndimensions,Ndimensions) = ellipsoid.getCovarianceMatrix().matrix().inverse();

    MatrixXd AT = T1*A*T1.transpose();        // Translating to ellipsoid center
    MatrixXd BT = T2*B*T2.transpose();        // Translating to ellipsoid center


    // Compute Hyper Quadric Matrix generating from the two ellipsoids 
    // and derive its eigenvalues decomposition

    MatrixXd C = AT.inverse() * BT;
    MatrixXcd CC(Ndimensions+1,Ndimensions+1);

    CC.imag() = MatrixXd::Zero(Ndimensions+1,Ndimensions+1); 
    CC.real() = C;
    
    ComplexEigenSolver<MatrixXcd> eigenSolver(CC);


    // If eigenvalue decomposition fails, set control flag to false 
    // to stop the nested sampling and print the results 

    if (eigenSolver.info() != Success)
    {
        ellipsoidMatrixDecompositionIsSuccessful = false;
    }
    
    MatrixXcd E = eigenSolver.eigenvalues();
    MatrixXcd V = eigenSolver.eigenvectors();

    bool ellipsoidsDoOverlap = false;       // Assume no overlap in the beginning
    double pointA;                          // Point laying in this ellipsoid
    double pointB;                          // Point laying in the other ellipsoid


    // Loop over all eigenvectors

    for (int i = 0; i < Ndimensions+1; i++) 
    {
        // Skip inadmissible eigenvectors

        if (V(Ndimensions,i).real() == 0)
        {
            continue;                   
        }
        else if (E(i).imag() != 0)
            {
                V.col(i) = V.col(i).array() * (V.conjugate())(Ndimensions,i);      // Multiply eigenvector by complex conjugate of last element
                V.col(i) = V.col(i).array() / V(Ndimensions,i).real();             // Normalize eigenvector to last component value
                pointA = V.col(i).transpose().real() * AT * V.col(i).real();       // Evaluate point from this ellipsoid
                pointB = V.col(i).transpose().real() * BT * V.col(i).real();       // Evaluate point from the other ellipsoid


                // Accept only if point belongs to both ellipsoids

                if ((pointA <= 0) && (pointB <= 0))  
                {
                    ellipsoidsDoOverlap = true;            // Exit if ellipsoidsDoOverlap is found
                    break;
                }
            }
    }

    return ellipsoidsDoOverlap;
}
void umdFitHyperplane(int lineCount, e::Vector3d **lines, e::Hyperplane<double, 3> &hyperplane)
{
	//first compute the correlation matrix
	//should be row major
    e::Matrix<double, 3, 3> corrMatrix;
    //std::cout << "CORRELATION*******************" << std::endl;
	for(int row = 0; row < 3; row++)
	{
		for(int col = 0; col < 3; col++)
		{
			//normal matrix multiplication m * m^T, this could be probably optimized since it's symmetrical
            double sum = 0;
			for(int i = 0; i < lineCount; i++)
			{
				sum += (*(lines[i]))[row]* (*(lines[i]))[col];
			}
			corrMatrix(row, col) = sum;
            //std::cout << sum << " ";
		}
       //std::cout << std::endl;
	}
    //std::cout << "CORRELATION*******************" << std::endl;



    e::Matrix<double, 3, 3> D;
    e::Matrix<double, 3, 3> Q;
	//std::cout << "correlation matrix: " << corrMatrix << std::endl;
	//std::cout << "**********" << std::endl;
	/*
	diagonalizer(corrMatrix, Q, D); //this does basically the eigen decomposition, the quaternions rot matrix should give the eigenVectors

	//get the eigenvalues A = Q * D * QT
    std::vector<double> eigenValues(3);
    eigenValues[0] = (const double)(D(0,0));
    eigenValues[1] = (const double)(D(1,1));
    eigenValues[2] = (const double)(D(2,2));

	//find the smallest
	int k = (int)(std::min_element(eigenValues.begin(), eigenValues.end()) - eigenValues.begin());

	//get the k's row for our hyperplane normal
	hyperplane.coeffs().data()[0] = Q(k , 0);
	hyperplane.coeffs().data()[1] = Q(k, 1);
	hyperplane.coeffs().data()[2] = Q(k, 2);
	hyperplane.coeffs().data()[3] = 0; //non linear part - should go through 0s
	std::cout << "Coeffs: " << Q(k, 0) << " " << Q(k, 1) << " " << Q(k, 2) << std::endl;
	std::cout << "Q: " << Q << std::endl;
	std::cout << "******" << std::endl;
	std::cout << "D: " << D << std::endl;
	std::cout << "******" << std::endl;
	*/

    std::vector<double> eigenValues(3);
    //jacobi_sweep(corrMatrix, Q, eigenValues);
    eigenSolver(corrMatrix, Q, eigenValues);

    //eigenTest();

    //e::SelfAdjointEigenSolver< e::Matrix<double, 3, 3> > slvr(corrMatrix);
    //e::Vector3d ev = slvr.eigenvalues();
    //Q = slvr.eigenvectors();
    //eigenValues[0] = ev[0];
    //eigenValues[1] = ev[1];
    //eigenValues[2] = ev[2];

	//std::cout << "Q: " << Q << std::endl;
	//std::cout << "******" << std::endl;


	//find the smallest
	int k = (int)(std::min_element(eigenValues.begin(), eigenValues.end()) - eigenValues.begin());

	hyperplane.coeffs().data()[0] = Q(0, k);
	hyperplane.coeffs().data()[1] = Q(1, k);
	hyperplane.coeffs().data()[2] = Q(2, k);
	hyperplane.coeffs().data()[3] = 0; //non linear part - should go through 0s

	e::Vector3d hyperplaneNormal = hyperplane.normal();
	//std::cout << "Normal: " << hyperplaneNormal << std::endl;
}
void locallyLinearEmbeddings(const Mat_<float> &samples, int outDim, Mat_<float> &embeddings, int k) {
	assert(outDim < samples.cols);
	assert(k >= 1);
	Mat_<int> nearestNeighbors(samples.rows, k);

	// determining k nearest neighbors for each sample
	flann::Index flannIndex(samples, flann::LinearIndexParams());

	for (int i = 0; i < samples.rows; i++) {
		Mat_<int> nearest;
		Mat dists;

		flannIndex.knnSearch(samples.row(i), nearest, dists, k + 1);
		nearest.colRange(1, nearest.cols).copyTo(nearestNeighbors.row(i));
	}

	// determining weights for each sample
	vector<Triplet<double> > tripletList;
	tripletList.reserve(samples.rows * k);

	for (int i = 0; i < samples.rows; i++) {
		Mat_<double> C(k,k);

		for (int u = 0; u < k; u++) {
			for (int v = u; v < k; v++) {
				C(u,v) = (samples.row(i) - samples.row(nearestNeighbors(i,u))).dot(samples.row(i) - samples.row(nearestNeighbors(i,v)));
				C(v,u) = C(u,v);
			}
		}

		// regularize when the number of neighbors is greater than the input
		// dimension
		if (k > samples.cols) {
			C = C + Mat_<double>::eye(k,k) * 10E-3 * trace(C)[0];
		}

		Map<MatrixXd,RowMajor> eigC((double*)C.data, C.rows, C.cols);

		LDLT<MatrixXd> solver(eigC);

		Mat_<double> weights(k,1);
		Map<MatrixXd,RowMajor> eigWeights((double*)weights.data, weights.rows, weights.cols);

		eigWeights = solver.solve(MatrixXd::Ones(k,1));	

		Mat_<double> normWeights;

		double weightsSum = sum(weights)[0];

		if (weightsSum == 0) {
			cout<<"error: cannot reconstruct point "<<i<<" from its neighbors"<<endl;
			exit(EXIT_FAILURE);
		}

		normWeights = weights / weightsSum;

		for (int j = 0; j < k; j++) {
			tripletList.push_back(Triplet<double>(nearestNeighbors(i,j), i, normWeights(j)));
		}
	}

	SparseMatrix<double> W(samples.rows, samples.rows);

	W.setFromTriplets(tripletList.begin(), tripletList.end());

	// constructing vectors in lower dimensional space from the weights
	VectorXd eigenvalues;
	MatrixXd eigenvectors;

	LLEMult mult(&W);

	symmetricSparseEigenSolver(samples.rows, "SM", outDim + 1, samples.rows, eigenvalues, eigenvectors, mult);
	embeddings = Mat_<double>(samples.rows, outDim);

	if (DEBUG_LLE) {
		cout<<"actual eigenvalues : "<<eigenvalues<<endl;
		cout<<"actual : "<<endl<<eigenvectors<<endl;

		MatrixXd denseW(W);
		MatrixXd tmp = MatrixXd::Identity(W.rows(), W.cols()) - denseW;
		MatrixXd M = tmp.transpose() * tmp;
		SelfAdjointEigenSolver<MatrixXd> eigenSolver(M);
		MatrixXd expectedEigenvectors = eigenSolver.eigenvectors();

		cout<<"expected eigenvalues : "<<eigenSolver.eigenvalues()<<endl;
		cout<<"expected : "<<endl<<expectedEigenvectors<<endl;
	}

	for (int i = 0; i < samples.rows; i++) {
		for (int j = 0; j < outDim; j++) {
			embeddings(i,j) = eigenvectors(i, j + 1);
		}
	}
}
Esempio n. 10
0
visualization_msgs::MarkerArray RosVSLAM::ActualCameraRepr() {

	visualization_msgs::MarkerArray camera;

	visualization_msgs::Marker camera_marker;
	camera_marker.header.frame_id = "/world";
	camera_marker.header.stamp = ros::Time::now();
	camera_marker.ns  = "camera_marker";
	camera_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
	camera_marker.mesh_resource = "package://vslam/mesh/test.dae";
    camera_marker.action = visualization_msgs::Marker::ADD;
    camera_marker.pose.orientation.w = 1.0;
	camera_marker.id = 0;
    camera_marker.scale.x = 0.1;
    camera_marker.scale.y = 0.1;
	camera_marker.scale.z = 0.1;
	camera_marker.color.b = 1.0f;
    camera_marker.color.a = 1.0;

	VectorXf state = getState();

	std::cout << map_scale << std::endl;
	camera_marker.pose.position.x = state(0)*map_scale;
	camera_marker.pose.position.y = state(1)*map_scale;
	camera_marker.pose.position.z = state(2)*map_scale;
	camera_marker.pose.orientation.x = state(4);
	camera_marker.pose.orientation.y = state(5);
	camera_marker.pose.orientation.z = state(6);
	camera_marker.pose.orientation.w = state(3);


	Matrix3f Cov = Sigma.block<3,3>(0,0);
    SelfAdjointEigenSolver<MatrixXf> eigenSolver(Cov);
    Vector3f eigs = eigenSolver.eigenvalues();
    Matrix3f vecs = eigenSolver.eigenvectors();
    Quaternionf q(vecs);

	visualization_msgs::Marker CameraCov;
	CameraCov.header.frame_id = "/world";
	CameraCov.header.stamp = ros::Time::now();

	CameraCov.ns  = "CameraCov";
	CameraCov.action = visualization_msgs::Marker::ADD;
   // pointsXYZ.pose.orientation.w = 1.0;
	CameraCov.id = 0;
	CameraCov.type = visualization_msgs::Marker::SPHERE;
	CameraCov.scale.x = 9*eigs(0)*map_scale;
	CameraCov.scale.y = 9*eigs(1)*map_scale;
	CameraCov.scale.z = 9*eigs(2)*map_scale;
	CameraCov.pose.orientation.w = q.w();
	CameraCov.pose.orientation.x = q.x();
	CameraCov.pose.orientation.y = q.y();
	CameraCov.pose.orientation.z = q.z();
	CameraCov.pose.position.x = state(0)*map_scale;
	CameraCov.pose.position.y = state(1)*map_scale;
	CameraCov.pose.position.z = state(2)*map_scale;
	CameraCov.color.b = 1.0f;
	CameraCov.color.a = 0.3;
	CameraCov.lifetime.sec = 1;

	camera.markers.push_back(CameraCov);
	camera.markers.push_back(camera_marker);



	return camera;
}
Esempio n. 11
0
visualization_msgs::MarkerArray RosVSLAM::getFeatures() {
	visualization_msgs::Marker pointsINV;
	pointsINV.header.frame_id = "/world";
	pointsINV.header.stamp = ros::Time::now();
	pointsINV.ns  = "points_and_lines";
	pointsINV.action = visualization_msgs::Marker::ADD;
	pointsINV.pose.orientation.w = 1.0;
	pointsINV.id = 0;
	pointsINV.type = visualization_msgs::Marker::SPHERE_LIST;
	pointsINV.scale.x = 0.1;
	pointsINV.scale.y = 0.1;
	pointsINV.scale.z = 0.1;
	pointsINV.color.r = 0.0f;
	pointsINV.color.g = 0.0f;
	pointsINV.color.b = 0.0f;
	pointsINV.color.a = 1.0;


	visualization_msgs::MarkerArray points;

	for (int i = 0; i < this->patches.size(); ++i) {
		int pos = this->patches[i].position_in_state;
		Vector3f d;
		Matrix3f Cov;
		if (!patches[i].isXYZ()) {
			MatrixXf Jf;
			d = depth2XYZ(mu.segment<6>(pos), Jf);
			geometry_msgs::Point p;
			p.x = d(0)*map_scale;
			p.y = d(1)*map_scale;
			p.z = d(2)*map_scale;
			pointsINV.points.push_back(p);
			Cov = Jf*Sigma.block<6,6>(pos,pos)*Jf.transpose();

	        SelfAdjointEigenSolver<MatrixXf> eigenSolver(Cov);
	        Vector3f eigs = eigenSolver.eigenvalues();
	        Matrix3f vecs = eigenSolver.eigenvectors();
	        Quaternionf q(vecs);

	    	visualization_msgs::Marker pointsINV;
	    	pointsINV.header.frame_id = "/world";
	    	pointsINV.header.stamp = ros::Time::now();
	    	char name[20];
	    	sprintf(name, "F%d",i);
	    	std::stringstream ss;
	    	ss << i;

	    	pointsINV.ns  = name;
	    	pointsINV.action = visualization_msgs::Marker::ADD;
	       // pointsXYZ.pose.orientation.w = 1.0;
	    	pointsINV.id = i;
	        pointsINV.type = visualization_msgs::Marker::SPHERE;
	        pointsINV.scale.x = eigs(0)*map_scale;
	        pointsINV.scale.y = eigs(1)*map_scale;
	        pointsINV.scale.z = eigs(2)*map_scale;
	        pointsINV.pose.orientation.w = q.w();
	        pointsINV.pose.orientation.x = q.x();
	        pointsINV.pose.orientation.y = q.y();
	        pointsINV.pose.orientation.z = q.z();
	        pointsINV.pose.position.x = d(0)*map_scale;
	        pointsINV.pose.position.y = d(1)*map_scale;
	        pointsINV.pose.position.z = d(2)*map_scale;
	        pointsINV.color.g = 1.0f;
	        pointsINV.color.a = 0.5;
	        pointsINV.lifetime.sec = 1;

	    	// points.markers.push_back(pointsINV);

		} else {
			d = mu.segment<3>(pos);
			Cov = Sigma.block<3,3>(pos,pos);
			Cov.eigenvalues();
	        SelfAdjointEigenSolver<MatrixXf> eigenSolver(Cov);
	        Vector3f eigs = eigenSolver.eigenvalues();
	        Matrix3f vecs = eigenSolver.eigenvectors();
	        Quaternion<float> q(vecs);

	        visualization_msgs::Marker pointsXYZ;
	    	pointsXYZ.header.frame_id = "/world";
	    	pointsXYZ.header.stamp = ros::Time::now();
	    	char name[20];
	    	sprintf(name, "F%d",i);
	    	std::stringstream ss;
	    	ss << i;
	    	pointsXYZ.ns = name;
	    	pointsXYZ.action = visualization_msgs::Marker::ADD;
	        pointsXYZ.id = i;
	        pointsXYZ.type = visualization_msgs::Marker::SPHERE;
	        pointsXYZ.scale.x = eigs(0)*map_scale;
	        pointsXYZ.scale.y = eigs(1)*map_scale;
	        pointsXYZ.scale.z = eigs(2)*map_scale;
	        pointsXYZ.pose.orientation.w = q.w();
	        pointsXYZ.pose.orientation.x = q.x();
	        pointsXYZ.pose.orientation.y = q.y();
	        pointsXYZ.pose.orientation.z = q.z();
	        pointsXYZ.pose.position.x = d(0)*map_scale;
	        pointsXYZ.pose.position.y = d(1)*map_scale;
	        pointsXYZ.pose.position.z = d(2)*map_scale;
	        pointsXYZ.color.r = 1.0f;
	        pointsXYZ.color.a = 0.5;
	        pointsXYZ.lifetime.sec = 1;
	    	//points.markers.push_back(pointsXYZ);

	        visualization_msgs::Marker textpointsXYZ;
	        textpointsXYZ.header.frame_id = "/world";
	        textpointsXYZ.header.stamp = ros::Time::now();
	    	sprintf(name, "name%d",i);
	    	textpointsXYZ.ns = name;
	    	textpointsXYZ.text = ss.str();
	    	textpointsXYZ.action = visualization_msgs::Marker::ADD;
	       // pointsXYZ.pose.orientation.w = 1.0;
	    	textpointsXYZ.id = 0;
	    	textpointsXYZ.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
	    	textpointsXYZ.scale.x = 1;
	        textpointsXYZ.scale.y = 1;
	        textpointsXYZ.scale.z = 1;
	        textpointsXYZ.pose.position.x = d(0)*map_scale;
	        textpointsXYZ.pose.position.y = d(1)*map_scale;
	        textpointsXYZ.pose.position.z = d(2)*map_scale;
	        textpointsXYZ.color.r = 0.0f;
	        textpointsXYZ.color.g = 0.0f;
	        textpointsXYZ.color.b = 0.0f;
	        textpointsXYZ.color.a = 1;
	        textpointsXYZ.lifetime.sec = 1;
	    	// points.markers.push_back(textpointsXYZ);







			geometry_msgs::Point p;
			p.x = d(0)*map_scale;
			p.y = d(1)*map_scale;
			p.z = d(2)*map_scale;
			pointsINV.points.push_back(p);


		}

    }

	points.markers.push_back(pointsINV);




	static int count_OLD = 0;
	char name[100];
	visualization_msgs::Marker pointsOLD;
	pointsOLD.header.frame_id = "/world";
	pointsOLD.header.stamp = ros::Time::now();
	pointsOLD.action = visualization_msgs::Marker::ADD;
	pointsOLD.pose.orientation.w = 1.0;
	pointsOLD.id = 0;
	pointsOLD.type = visualization_msgs::Marker::SPHERE_LIST;
	pointsOLD.scale.x = 0.1;
	pointsOLD.scale.y = 0.1;
	pointsOLD.scale.z = 0.1;
	pointsOLD.color.r = 0.0f;
	pointsOLD.color.g = 0.0f;
	pointsOLD.color.b = 0.0f;
	pointsOLD.color.a = 1.0;



	std::cout << "size = " <<deleted_patches.size() << std::endl;
	if (deleted_patches.size() > 1000) {
		for (int i = 0; i < deleted_patches.size(); i++) {
			sprintf(name, "OLD_points%d", count_OLD++);
			pointsOLD.ns  = name;
			geometry_msgs::Point p;
			p.x = deleted_patches[i].XYZ_pos(0)*map_scale;
			p.y = deleted_patches[i].XYZ_pos(1)*map_scale;
			p.z = deleted_patches[i].XYZ_pos(2)*map_scale;
			pointsOLD.points.push_back(p);
		}
		deleted_patches.clear();
	} else {
		for (int i = 0; i < deleted_patches.size(); i++) {
			pointsOLD.ns  = "OLD_points";
			geometry_msgs::Point p;
			p.x = deleted_patches[i].XYZ_pos(0)*map_scale;
			p.y = deleted_patches[i].XYZ_pos(1)*map_scale;
			p.z = deleted_patches[i].XYZ_pos(2)*map_scale;
			pointsOLD.points.push_back(p);
		}
	}

	points.markers.push_back(pointsOLD);


    return points;
}