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;
}
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];
}
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;
}
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_);
}
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;
}
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);
}
}
}
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;
}
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;
}