int main(int argC,char **argV)
{
ros::init(argC,argV,"startPointCloud");
ros::NodeHandle n;
std::string serverAddress;
n.getParam("/serverNameOrIP",serverAddress);
Socket mySocket(serverAddress.c_str(),"9005",streamSize);
ros::Publisher pub = n.advertise<PointCloud>("point_cloud",1);
PointCloud::Ptr pc (new PointCloud);
pc->header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/kinect_pcl";
while(ros::ok())
{
// TODO(Somhtr): change to ROS' logging API
cout << "Reading data..." << endl;
mySocket.readData();
// TODO(Somhtr): change to ROS' logging API
cout << "Copying data..." << endl;
float* pt_coords = reinterpret_cast<float*>(mySocket.mBuffer);
for(uint64_t idx=0; idx<numberOfPoints; idx+=3)
{
pc->push_back(pcl::PointXYZ(
pt_coords[idx], pt_coords[idx+1], pt_coords[idx+2]
));
}
double utcTime;
memcpy(&utcTime,&mySocket.mBuffer[pointBufferSize],sizeof(double));
pc->header.stamp = ros::Time(utcTime).toSec();
pub.publish(pc);
pc->clear();
ros::spinOnce();
}
return 0;
}
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud,
TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n";
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const pcl::PointXYZ & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n";
}
Eigen::Matrix4d calculate_transformation(Eigen::Vector3d x_or_y_axe_vector, Eigen::Vector3d z_axe_vector, Eigen::Vector4d xyz_centroid)
{
transformata_finala=Eigen::MatrixXd::Identity(4,4);
Eigen::Vector3d axa3;
axa3(0)=x_or_y_axe_vector(1)*z_axe_vector(2)-z_axe_vector(1)*x_or_y_axe_vector(2);// a_nou = b1*c2 - b2*c1;
axa3(1)=z_axe_vector(0)*x_or_y_axe_vector(2)-x_or_y_axe_vector(0)*z_axe_vector(2);// b_nou = a2*c1 - a1*c2;
axa3(2)=x_or_y_axe_vector(0)*z_axe_vector(1)-x_or_y_axe_vector(1)*z_axe_vector(0);// c_nou = a1*b2 - b1*a2;
for(int i=0;i<3;i++)
{
transformata_finala(i,0) =z_axe_vector(i);
transformata_finala(i,1) =axa3(i);
transformata_finala(i,2) =x_or_y_axe_vector(i);
}
for(int i=0;i<3;i++)
transformata_finala(i,3)=xyz_centroid(i);
std::cout<<std::endl<<"Transformation Matrix:"<<std::endl<<transformata_finala<<std::endl;
float x0,y0,z0;
x0=xyz_centroid[0];
y0=xyz_centroid[1];
z0=xyz_centroid[2];
float l = 0.005;
pcl::PointXYZRGB p_x,p_y,p_z;
p_x.r=255;
p_x.g=0;
p_x.b=0;
p_y.r=0;
p_y.g=255;
p_y.b=0;
p_z.r=0;
p_z.g=0;
p_z.b=255;
PointCloud::Ptr axe (new PointCloud);
for(int i=0;i<100;i++)
{
//axa x red
p_x.x =x0 + transformata_finala(0,0)*l;
p_x.y =y0 + transformata_finala(1,0)*l;
p_x.z =z0 + transformata_finala(2,0)*l;
//axa y green
p_y.x =x0 + transformata_finala(0,1)*l;
p_y.y =y0 + transformata_finala(1,1)*l;
p_y.z =z0 + transformata_finala(1,1)*l;
//axa z blue
p_z.x =x0 + transformata_finala(0,2)*l;
p_z.y =y0 + transformata_finala(1,2)*l;
p_z.z =z0 + transformata_finala(2,2)*l;
axe->push_back(p_x);
axe->push_back(p_y);
axe->push_back(p_z);
l+=0.005;
}
pcl::visualization::PCLVisualizer viewer ("ICP demo");
viewer.addPointCloud (cloud, "plane",0);
viewer.addPointCloud (axe, "a",0);
while (!viewer.wasStopped ()) {
viewer.spinOnce ();
}
}