Skip to content

Commit 4b82475

Browse files
miquelmassotmiquelmassot
committed
added pointcloud_to_webgl from fuerte branch
1 parent 5afd9a8 commit 4b82475

File tree

2 files changed

+177
-0
lines changed

2 files changed

+177
-0
lines changed

‎pointcloud_tools/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,11 @@ add_executable(pointcloud_mapper_for_slam src/pointcloud_mapper_for_slam.cpp)
1616
add_executable(pointcloud_filtering src/pointcloud_filtering.cpp)
1717
add_executable(pcd_publisher src/pcd_publisher.cpp)
1818
add_executable(pointcloud_viewer src/pointcloud_viewer.cpp)
19+
add_executable(pointcloud_to_webgl src/pointcloud_to_webgl.cpp)
1920

2021
target_link_libraries(pointcloud_viewer ${Boost_LIBRARIES} ${PCL_LIBRARIES} ${catkin_LIBRARIES} libvtkCommon.so libvtkFiltering.so libvtkRendering.so)
2122
target_link_libraries(pcd_publisher ${PCL_LIBRARIES} ${catkin_LIBRARIES})
2223
target_link_libraries(pointcloud_mapper ${PCL_LIBRARIES} ${catkin_LIBRARIES})
2324
target_link_libraries(pointcloud_mapper_for_slam ${PCL_LIBRARIES} ${catkin_LIBRARIES})
2425
target_link_libraries(pointcloud_filtering ${PCL_LIBRARIES} ${catkin_LIBRARIES})
26+
target_link_libraries(pointcloud_to_webgl ${PCL_LIBRARIES} ${catkin_LIBRARIES})

‎pointcloud_tools/src/pointcloud_to_webgl.cpp

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
#include <ros/ros.h>
2+
3+
#include <std_msgs/String.h>
4+
5+
#include <pcl/point_types.h>
6+
#include <pcl_ros/point_cloud.h>
7+
#include <pcl/io/pcd_io.h>
8+
#include <pcl/filters/voxel_grid.h>
9+
#include <pcl/common/centroid.h>
10+
11+
class PointCloudToWebgl {
12+
13+
// ROS properties
14+
ros::NodeHandle nh_;
15+
ros::NodeHandle nh_private_;
16+
17+
std::string pcd_filename_;
18+
double max_ascii_file_size_;
19+
int pcd_type_;
20+
21+
public:
22+
23+
/**
24+
* Class constructor
25+
*/
26+
PointCloudToWebgl() : nh_private_("~")
27+
{
28+
// Read the parameters from the parameter server (set defaults)
29+
nh_private_.param("pcd_filename", pcd_filename_, std::string("pointcloud_file.pcd"));
30+
nh_private_.param("max_ascii_file_size", max_ascii_file_size_, 4.0); // In MBytes
31+
nh_private_.param("pcd_type", pcd_type_, 0); // 0 -> XYZ | 1 -> XYZRGB
32+
33+
ROS_INFO_STREAM("[PointCloudToWebgl:] Opening file " << pcd_filename_);
34+
35+
// Read point cloud
36+
if (pcd_type_ == 0)
37+
{
38+
// NO RGB
39+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
40+
if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_filename_, *cloud_ptr) == -1) //* load the file
41+
{
42+
ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << pcd_filename_);
43+
}
44+
else
45+
{
46+
// Convert the cloud
47+
pcl::PointCloud<pcl::PointXYZ> cloud = *cloud_ptr;
48+
int file_point_size = 27;
49+
int max_bytes = (int)round(max_ascii_file_size_ * 1024 * 1024);
50+
int desired_points = max_bytes / file_point_size;
51+
double voxel_size = 0.001;
52+
double offset = 0.0002;
53+
while ((int)cloud.size() > desired_points)
54+
{
55+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled = filter(cloud.makeShared(), voxel_size);
56+
cloud = *cloud_downsampled;
57+
voxel_size = voxel_size + offset;
58+
}
59+
60+
// Compute the cloud centroid
61+
Eigen::Vector4f centroid;
62+
pcl::compute3DCentroid(cloud, centroid);
63+
64+
// Save int file
65+
int lastindex = pcd_filename_.find_last_of(".");
66+
std::string filename = pcd_filename_.substr(0, lastindex);
67+
filename = filename + ".txt";
68+
ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << filename);
69+
std::fstream f_webgl(filename.c_str(), std::ios::out);
70+
for (unsigned int i=0; i<cloud.size(); i++)
71+
{
72+
f_webgl << cloud[i].x - centroid[0] << "," <<
73+
cloud[i].y - centroid[1] << "," <<
74+
cloud[i].z - centroid[2] << "," <<
75+
(int)(224) << "," <<
76+
(int)(224) << "," <<
77+
(int)(224) << std::endl;
78+
}
79+
f_webgl.close();
80+
ROS_INFO("[PointCloudToWebgl:] Saved!");
81+
}
82+
}
83+
else
84+
{
85+
// RGB
86+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
87+
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (pcd_filename_, *cloud_ptr) == -1) //* load the file
88+
{
89+
ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << pcd_filename_);
90+
}
91+
else
92+
{
93+
// Convert the cloud
94+
pcl::PointCloud<pcl::PointXYZRGB> cloud = *cloud_ptr;
95+
int file_point_size = 37;
96+
int max_bytes = (int)round(max_ascii_file_size_ * 1024 * 1024);
97+
int desired_points = max_bytes / file_point_size;
98+
double voxel_size = 0.001;
99+
double offset = 0.0002;
100+
while ((int)cloud.size() > desired_points)
101+
{
102+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_downsampled = filter(cloud.makeShared(), voxel_size);
103+
cloud = *cloud_downsampled;
104+
voxel_size = voxel_size + offset;
105+
}
106+
107+
// Compute the cloud centroid
108+
Eigen::Vector4f centroid;
109+
pcl::compute3DCentroid(cloud, centroid);
110+
111+
// Save int file
112+
int lastindex = pcd_filename_.find_last_of(".");
113+
std::string filename = pcd_filename_.substr(0, lastindex);
114+
filename = filename + ".txt";
115+
ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << filename);
116+
std::fstream f_webgl(filename.c_str(), std::ios::out);
117+
for (unsigned int i=0; i<cloud.size(); i++)
118+
{
119+
f_webgl << cloud[i].x - centroid[0] << "," <<
120+
cloud[i].y - centroid[1] << "," <<
121+
cloud[i].z - centroid[2] << "," <<
122+
(int)cloud[i].r << "," <<
123+
(int)cloud[i].g << "," <<
124+
(int)cloud[i].b << std::endl;
125+
}
126+
f_webgl.close();
127+
ROS_INFO("[PointCloudToWebgl:] Saved!");
128+
}
129+
}
130+
}
131+
132+
pcl::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double voxel_size)
133+
{
134+
// Downsampling using voxel grid
135+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZ>);
136+
pcl::VoxelGrid<pcl::PointXYZ> grid_;
137+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled_ptr(new pcl::PointCloud<pcl::PointXYZ>);
138+
grid_.setLeafSize(voxel_size,
139+
voxel_size,
140+
voxel_size);
141+
grid_.setDownsampleAllData(true);
142+
grid_.setInputCloud(cloud);
143+
grid_.filter(*cloud_downsampled_ptr);
144+
return cloud_downsampled_ptr;
145+
}
146+
147+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, double voxel_size)
148+
{
149+
// Downsampling using voxel grid
150+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
151+
pcl::VoxelGrid<pcl::PointXYZRGB> grid_;
152+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_downsampled_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
153+
154+
grid_.setLeafSize(voxel_size,
155+
voxel_size,
156+
voxel_size);
157+
grid_.setDownsampleAllData(true);
158+
grid_.setInputCloud(cloud);
159+
grid_.filter(*cloud_downsampled_ptr);
160+
return cloud_downsampled_ptr;
161+
}
162+
163+
};
164+
165+
/**
166+
* Main entry point of the code
167+
*/
168+
int main(int argc, char **argv)
169+
{
170+
ros::init(argc, argv, "pointcloud_to_webgl");
171+
PointCloudToWebgl node;
172+
ros::spin();
173+
return 0;
174+
}
175+

0 commit comments

Comments
 (0)