Skip to content

Commit 1737d5a

Browse files
author
Daniel
authored
Add files via upload
1 parent 914ed52 commit 1737d5a

2 files changed

Lines changed: 393 additions & 0 deletions

File tree

‎src/CMakeLists.txt‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
##############################################################################
2+
# CMAKE CONFIGURATION
3+
##############################################################################
4+
cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
5+
6+
# set project name
7+
project(upsampling_cloud VERSION 1.0.0 LANGUAGES CXX)
8+
9+
# set build type = Release mode
10+
set(CMAKE_BUILD_TYPE Release)
11+
12+
message("\n" "=========================================")
13+
message("Project: ${PROJECT_NAME} ")
14+
message("=========================================")
15+
16+
# set the include directive in the same project folder
17+
set(CMAKE_INCLUDE_CURRENT_DIR ON)
18+
19+
# set corresponding package directories
20+
set(PCL_DIR /opt/pcl-1.9.1/build)
21+
#set(VTK_DIR 8 )
22+
23+
#set the CMP0074 policy to old behavior (disable warnings) (CMake 3.12.0-rc1)
24+
if(${CMAKE_VERSION} MATCHES 3.12.0)
25+
cmake_policy(SET CMP0074 OLD)
26+
if(POLICY CMP0048)
27+
cmake_policy(SET CMP0048 NEW)
28+
endif(POLICY CMP0048)
29+
endif()
30+
31+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
32+
33+
# Include dependencies of pcl 1.8.1 in project directorie
34+
set(CMAKE_MODULE_PATH ${PCL_DIR}/../cmake/Modules)
35+
36+
# set cmake for use std c++11 and output executable folder to bin
37+
#set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin)
38+
39+
##############################################################################
40+
# PACKAGES
41+
##############################################################################
42+
message("***********************")
43+
message("point cloud library pkg")
44+
message("***********************")
45+
46+
find_package(PCL 1.8 PATHS ${PCL_DIR} QUIET)
47+
if(PCL_FOUND)
48+
message(STATUS "PCL status:")
49+
message(STATUS " version: ${PCL_VERSION}")
50+
message(STATUS " pcl directorie: ${PCL_DIR}")
51+
else()
52+
message(WARNING " PCL 1.8 not found, attempting 1.7...")
53+
find_package(PCL 1.7 REQUIRED)
54+
if(PCL_FOUND)
55+
message(STATUS "PCL status:")
56+
message(STATUS " version: ${PCL_VERSION}")
57+
message(STATUS " pcl directorie: ${PCL_DIR}")
58+
else()
59+
message(FATAL_ERROR " ERROR: PCL minimum required version 1.7. Not found")
60+
endif()
61+
endif()
62+
63+
##############################################################################
64+
# HEADERS
65+
##############################################################################
66+
include_directories(
67+
${PCL_INCLUDE_DIRS}
68+
69+
)
70+
71+
include(CheckFunctionExists)
72+
73+
# Use the compile definitions defined in PCL
74+
add_definitions(${PCL_DEFINITIONS})
75+
76+
##############################################################################
77+
# LIBRARIES PATH
78+
##############################################################################
79+
link_directories(
80+
${PCL_LIBRARY_DIRS}
81+
82+
)
83+
84+
##############################################################################
85+
# SOURCE CODE
86+
##############################################################################
87+
set(MAIN_SOURCE "main.cpp")
88+
89+
##############################################################################
90+
# EXECUTABLES
91+
##############################################################################
92+
add_executable(${PROJECT_NAME}
93+
${MAIN_SOURCE}
94+
)
95+
96+
##############################################################################
97+
# TARGET LIBRARIES
98+
##############################################################################
99+
target_link_libraries(${PROJECT_NAME}
100+
${PCL_LIBRARIES}
101+
102+
)
103+
104+
message("=========================================")
105+
message("Project: ${PROJECT_NAME} COMPILED WITH CMAKE " ${CMAKE_VERSION})
106+
message("=========================================")

‎src/main.cpp‎

Lines changed: 287 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,287 @@
1+
/*********************************
2+
HEADERS
3+
**********************************/
4+
#include <pcl/io/pcd_io.h>
5+
#include <pcl/io/ply_io.h>
6+
#include <pcl/io/vtk_lib_io.h>
7+
8+
#include <pcl/visualization/pcl_visualizer.h>
9+
10+
#include <pcl/surface/bilateral_upsampling.h>
11+
#include <pcl/surface/mls.h>
12+
13+
#include <pcl/point_cloud.h>
14+
#include <pcl/kdtree/kdtree_flann.h>
15+
#include <pcl/kdtree/kdtree.h>
16+
17+
#include <pcl/search/search.h>
18+
19+
#include <pcl/features/feature.h>
20+
#include <pcl/point_types.h>
21+
22+
#include <pcl/common/transforms.h>
23+
#include <pcl/segmentation/sac_segmentation.h>
24+
25+
#include <pcl/console/print.h>
26+
#include <pcl/console/parse.h>
27+
#include <pcl/console/time.h>
28+
29+
#include <iostream>
30+
#include <fstream>
31+
#include <string>
32+
33+
void printUsage (const char* progName){
34+
std::cout << "\nUse: " << progName << " <file>" << std::endl <<
35+
"support: .pcd .ply .txt .xyz" << std::endl <<
36+
"[q] to exit" << std::endl;
37+
}
38+
39+
40+
int main(int argc, char **argv){
41+
42+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
43+
44+
pcl::PolygonMesh cl;
45+
std::vector<int> filenames;
46+
bool file_is_pcd = false;
47+
bool file_is_ply = false;
48+
bool file_is_txt = false;
49+
bool file_is_xyz = false;
50+
51+
52+
if(argc < 2 or argc > 2){
53+
printUsage (argv[0]);
54+
return -1;
55+
}
56+
57+
pcl::console::TicToc tt;
58+
pcl::console::print_highlight ("Loading ");
59+
60+
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
61+
if(filenames.size()<=0){
62+
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
63+
if(filenames.size()<=0){
64+
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".txt");
65+
if(filenames.size()<=0){
66+
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".xyz");
67+
if(filenames.size()<=0){
68+
printUsage (argv[0]);
69+
return -1;
70+
}else if(filenames.size() == 1){
71+
file_is_xyz = true;
72+
}
73+
}else if(filenames.size() == 1){
74+
file_is_txt = true;
75+
}
76+
}else if(filenames.size() == 1){
77+
file_is_pcd = true;
78+
}
79+
}else if(filenames.size() == 1){
80+
file_is_ply = true;
81+
}else{
82+
printUsage (argv[0]);
83+
return -1;
84+
}
85+
86+
if(file_is_pcd){
87+
if(pcl::io::loadPCDFile(argv[filenames[0]], *input_cloud) < 0){
88+
std::cout << "Error loading point cloud " << argv[filenames[0]] << "\n";
89+
printUsage (argv[0]);
90+
return -1;
91+
}
92+
pcl::console::print_info("\nFound pcd file.\n");
93+
pcl::console::print_info ("[done, ");
94+
pcl::console::print_value ("%g", tt.toc ());
95+
pcl::console::print_info (" ms : ");
96+
pcl::console::print_value ("%d", input_cloud->size ());
97+
pcl::console::print_info (" points]\n");
98+
}else if(file_is_ply){
99+
pcl::io::loadPLYFile(argv[filenames[0]],*input_cloud);
100+
if(input_cloud->points.size()<=0 or input_cloud->points.at(0).x <=0 and input_cloud->points.at(0).y <=0 and input_cloud->points.at(0).z <=0){
101+
pcl::console::print_warn("\nloadPLYFile could not read the cloud, attempting to loadPolygonFile...\n");
102+
pcl::io::loadPolygonFile(argv[filenames[0]], cl);
103+
pcl::fromPCLPointCloud2(cl.cloud, *input_cloud);
104+
if(input_cloud->points.size()<=0 or input_cloud->points.at(0).x <=0 and input_cloud->points.at(0).y <=0 and input_cloud->points.at(0).z <=0){
105+
pcl::console::print_warn("\nloadPolygonFile could not read the cloud, attempting to PLYReader...\n");
106+
pcl::PLYReader plyRead;
107+
plyRead.read(argv[filenames[0]],*input_cloud);
108+
if(input_cloud->points.size()<=0 or input_cloud->points.at(0).x <=0 and input_cloud->points.at(0).y <=0 and input_cloud->points.at(0).z <=0){
109+
pcl::console::print_error("\nError. ply file is not compatible.\n");
110+
return -1;
111+
}
112+
}
113+
}
114+
115+
pcl::console::print_info("\nFound ply file.\n");
116+
pcl::console::print_info ("[done, ");
117+
pcl::console::print_value ("%g", tt.toc ());
118+
pcl::console::print_info (" ms : ");
119+
pcl::console::print_value ("%d", input_cloud->points.size ());
120+
pcl::console::print_info (" points]\n");
121+
122+
}else if(file_is_txt){
123+
std::ifstream file(argv[filenames[0]]);
124+
if(!file.is_open()){
125+
std::cout << "Error: Could not find "<< argv[filenames[0]] << std::endl;
126+
return -1;
127+
}
128+
129+
std::cout << "file opened." << std::endl;
130+
double x_,y_,z_;
131+
unsigned int r, g, b;
132+
133+
while(file >> x_ >> y_ >> z_ >> r >> g >> b){
134+
pcl::PointXYZRGB pt;
135+
pt.x = x_;
136+
pt.y = y_;
137+
pt.z= z_;
138+
139+
uint8_t r_, g_, b_;
140+
r_ = uint8_t(r);
141+
g_ = uint8_t(g);
142+
b_ = uint8_t(b);
143+
144+
uint32_t rgb_ = ((uint32_t)r_ << 16 | (uint32_t)g_ << 8 | (uint32_t)b_);
145+
pt.rgb = *reinterpret_cast<float*>(&rgb_);
146+
147+
input_cloud->points.push_back(pt);
148+
//std::cout << "pointXYZRGB:" << pt << std::endl;
149+
}
150+
151+
pcl::console::print_info("\nFound txt file.\n");
152+
pcl::console::print_info ("[done, ");
153+
pcl::console::print_value ("%g", tt.toc ());
154+
pcl::console::print_info (" ms : ");
155+
pcl::console::print_value ("%d", input_cloud->points.size ());
156+
pcl::console::print_info (" points]\n");
157+
158+
}else if(file_is_xyz){
159+
std::ifstream file(argv[filenames[0]]);
160+
if(!file.is_open()){
161+
std::cout << "Error: Could not find "<< argv[filenames[0]] << std::endl;
162+
return -1;
163+
}
164+
165+
std::cout << "file opened." << std::endl;
166+
double x_,y_,z_;
167+
168+
while(file >> x_ >> y_ >> z_){
169+
170+
pcl::PointXYZRGB pt;
171+
pt.x = x_;
172+
pt.y = y_;
173+
pt.z= z_;
174+
175+
input_cloud->points.push_back(pt);
176+
//std::cout << "pointXYZRGB:" << pt << std::endl;
177+
}
178+
179+
pcl::console::print_info("\nFound xyz file.\n");
180+
pcl::console::print_info ("[done, ");
181+
pcl::console::print_value ("%g", tt.toc ());
182+
pcl::console::print_info (" ms : ");
183+
pcl::console::print_value ("%d", input_cloud->points.size ());
184+
pcl::console::print_info (" points]\n");
185+
}
186+
187+
input_cloud->width = (int) input_cloud->points.size ();
188+
input_cloud->height = 1;
189+
input_cloud->is_dense = true;
190+
191+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
192+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dense_points (new pcl::PointCloud<pcl::PointXYZRGB> ());
193+
194+
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kd_tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
195+
pcl::PointCloud<pcl::PointNormal>::Ptr mls_points (new pcl::PointCloud<pcl::PointNormal>());
196+
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> mls;
197+
198+
double search_radius = 0.03; //0.03
199+
double sampling_radius = 0.005; //0.005
200+
double step_size = 0.005; //0.005
201+
double gauss_param = (double)std::pow(search_radius,2);
202+
int pol_order = 2;
203+
unsigned int num_threats = 1;
204+
205+
mls.setComputeNormals(true);
206+
mls.setInputCloud(input_cloud);
207+
mls.setSearchMethod(kd_tree);
208+
mls.setSearchRadius(search_radius);
209+
mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB>::UpsamplingMethod::SAMPLE_LOCAL_PLANE);
210+
mls.setUpsamplingRadius(sampling_radius);
211+
mls.setUpsamplingStepSize(step_size);
212+
mls.setPolynomialOrder(pol_order);
213+
mls.setSqrGaussParam(gauss_param);// (the square of the search radius works best in general)
214+
mls.setCacheMLSResults(true);//Set whether the mls results should be stored for each point in the input cloud.
215+
mls.setNumberOfThreads(num_threats);
216+
//mls.setDilationVoxelSize();//Used only in the VOXEL_GRID_DILATION upsampling method
217+
//mls.setPointDensity(15); //15
218+
mls.process(*dense_points);
219+
220+
*output_cloud = *input_cloud;
221+
*output_cloud += *dense_points;
222+
223+
pcl::console::print_info("\nNew points: ");
224+
pcl::console::print_value("%d", dense_points->points.size());
225+
226+
pcl::console::print_info("\nOutput cloud points: ");
227+
pcl::console::print_value("%d", output_cloud->points.size());
228+
pcl::console::print_info("\n");
229+
230+
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("VISUALIZER"));
231+
232+
int PORT1 = 0;
233+
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, PORT1);
234+
viewer->setBackgroundColor (0, 0, 0, PORT1);
235+
viewer->addText("ORIGINAL", 10, 10, "PORT1", PORT1);
236+
237+
int PORT2 = 0;
238+
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, PORT2);
239+
viewer->setBackgroundColor (0, 0, 0, PORT2);
240+
viewer->addText("UPSAMPLING", 10, 10, "PORT2", PORT2);
241+
242+
viewer->removeAllPointClouds(0);
243+
244+
if(input_cloud->points[0].r <= 0 and input_cloud->points[0].g <= 0 and input_cloud->points[0].b<= 0 ){
245+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(input_cloud,255,255,0);
246+
viewer->addPointCloud(input_cloud,color_handler,"Original",PORT1);
247+
}else{
248+
viewer->addPointCloud(input_cloud,"Original",PORT1);
249+
}
250+
251+
if(output_cloud->points[0].r <= 0 and output_cloud->points[0].g <= 0 and output_cloud->points[0].b<= 0 ){
252+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(output_cloud,255,255,0);
253+
viewer->addPointCloud(output_cloud,color_handler,"transform1 rvec",PORT2);
254+
}else{
255+
viewer->addPointCloud(output_cloud,"transform1 rvec",PORT2);
256+
}
257+
258+
pcl::io::savePCDFile ("upsampled_cloud.pcd", *output_cloud);
259+
260+
pcl::PointXYZ p1, p2, p3;
261+
p1.getArray3fMap() << 1, 0, 0;
262+
p2.getArray3fMap() << 0, 1, 0;
263+
p3.getArray3fMap() << 0,0.1,1;
264+
265+
viewer->addCoordinateSystem(1,"original_usc",PORT1);
266+
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_",PORT1);
267+
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_",PORT1);
268+
viewer->addText3D ("z", p3,0.2, 0, 0, 1, "z_",PORT1);
269+
270+
viewer->addCoordinateSystem(1,"transform_ucs",PORT2);
271+
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_",PORT2);
272+
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_",PORT2);
273+
viewer->addText3D ("z", p3,0.2, 0, 0, 1, "z_",PORT2);
274+
275+
viewer->setPosition(0,0);
276+
viewer->initCameraParameters();
277+
viewer->resetCamera();
278+
279+
std::cout << "\nPress [q] to exit" << std::endl;
280+
281+
while(!viewer->wasStopped ()) {
282+
viewer->spin();
283+
}
284+
285+
return 0;
286+
287+
}

0 commit comments

Comments
 (0)