Skip to content

Commit 1a907ec

Browse files
author
Daniel
authored
Merge pull request #4 from danielTobon43/refactor-code
Refactor code
2 parents 32da278 + e979af2 commit 1a907ec

File tree

3 files changed

+137
-115
lines changed

3 files changed

+137
-115
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ add_executable(${PROJECT_NAME} ${MAIN_SOURCE})
6161
# #############################################################################
6262
target_include_directories(${PROJECT_NAME} PRIVATE
6363
${PCL_INCLUDE_DIRS}
64+
include
6465
)
6566

6667
# #############################################################################

‎include/visualizer.hpp‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include <pcl/point_cloud.h>
2+
#include <pcl/point_types.h>
3+
#include <pcl/visualization/pcl_visualizer.h>
4+
5+
#include <sstream>
6+
#include <string>
7+
8+
void append_points_size_to_display(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
9+
pcl::visualization::PCLVisualizer::Ptr &viewer, int &PORT, std::string &name) {
10+
// add points label to visualizer
11+
std::string str = "Points: ";
12+
std::stringstream ss;
13+
ss << cloud->points.size();
14+
str += ss.str();
15+
int xpos = 10;
16+
int ypos = 25;
17+
int fontSize = 13;
18+
double r = 1.0;
19+
double g = 1.0;
20+
double b = 1.0;
21+
viewer->addText(str, xpos, ypos, fontSize, r, g, b, name, PORT);
22+
}
23+
24+
void display_upsampling(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input_cloud,
25+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr &output_cloud) {
26+
vtkObject::GlobalWarningDisplayOff(); // Disable vtk render warning
27+
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("PCL VISUALIZER"));
28+
29+
int PORT1 = 0;
30+
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, PORT1);
31+
viewer->setBackgroundColor(0, 0, 0, PORT1);
32+
viewer->addText("ORIGINAL", 10, 10, "PORT1", PORT1);
33+
34+
int PORT2 = 0;
35+
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, PORT2);
36+
viewer->setBackgroundColor(0, 0, 0, PORT2);
37+
viewer->addText("UPSAMPLING", 10, 10, "PORT2", PORT2);
38+
39+
std::string name1 = "points_cloud_1";
40+
std::string name2 = "points_cloud_2";
41+
42+
append_points_size_to_display(input_cloud, viewer, PORT1, name1);
43+
append_points_size_to_display(output_cloud, viewer, PORT2, name2);
44+
45+
viewer->removeAllPointClouds(0);
46+
47+
if (input_cloud->points[0].r <= 0 and input_cloud->points[0].g <= 0 and input_cloud->points[0].b <= 0) {
48+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(input_cloud, 255, 255, 0);
49+
viewer->addPointCloud(input_cloud, color_handler, "Original", PORT1);
50+
} else {
51+
viewer->addPointCloud(input_cloud, "Original", PORT1);
52+
}
53+
54+
if (output_cloud->points[0].r <= 0 and output_cloud->points[0].g <= 0 and output_cloud->points[0].b <= 0) {
55+
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(output_cloud, 255, 255, 0);
56+
viewer->addPointCloud(output_cloud, color_handler, "transform1 rvec", PORT2);
57+
} else {
58+
viewer->addPointCloud(output_cloud, "transform1 rvec", PORT2);
59+
}
60+
61+
pcl::PointXYZ p1, p2, p3;
62+
p1.getArray3fMap() << 1, 0, 0;
63+
p2.getArray3fMap() << 0, 1, 0;
64+
p3.getArray3fMap() << 0, 0.1, 1;
65+
66+
viewer->addCoordinateSystem(1, "original_usc", PORT1);
67+
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_", PORT1);
68+
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_", PORT1);
69+
viewer->addText3D("z", p3, 0.2, 0, 0, 1, "z_", PORT1);
70+
71+
viewer->addCoordinateSystem(1, "transform_ucs", PORT2);
72+
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_", PORT2);
73+
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_", PORT2);
74+
viewer->addText3D("z", p3, 0.2, 0, 0, 1, "z_", PORT2);
75+
76+
viewer->setPosition(0, 0);
77+
viewer->initCameraParameters();
78+
viewer->resetCamera();
79+
80+
std::cout << "\nPress [q] to exit" << std::endl;
81+
82+
while (!viewer->wasStopped()) {
83+
viewer->spin();
84+
}
85+
}

‎src/main.cpp‎

Lines changed: 51 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -1,81 +1,26 @@
1-
/*********************************
2-
HEADERS
3-
**********************************/
4-
#include <pcl/common/transforms.h>
5-
#include <pcl/console/parse.h>
61
#include <pcl/console/print.h>
7-
#include <pcl/console/time.h>
8-
#include <pcl/features/feature.h>
9-
#include <pcl/io/pcd_io.h>
10-
#include <pcl/io/ply_io.h>
11-
#include <pcl/io/vtk_lib_io.h>
12-
#include <pcl/kdtree/kdtree.h>
13-
#include <pcl/kdtree/kdtree_flann.h>
14-
#include <pcl/point_cloud.h>
15-
#include <pcl/point_types.h>
16-
#include <pcl/search/search.h>
17-
#include <pcl/segmentation/sac_segmentation.h>
18-
#include <pcl/surface/bilateral_upsampling.h>
2+
#include <pcl/search/kdtree.h>
193
#include <pcl/surface/mls.h>
20-
#include <pcl/visualization/pcl_visualizer.h>
21-
22-
#include <fstream>
23-
#include <iostream>
24-
#include <string>
254

265
#include "argparse/argparse.hpp"
276
#include "cloudparse/parser.hpp"
7+
#include "visualizer.hpp"
288

29-
// void printUsage(const char* progName) { std::cout << "\nUse: " << progName << " <file>" << std::endl << "support:
30-
// .pcd .ply .txt .xyz" << std::endl << "[q] to exit" << std::endl; }
31-
32-
int main(int argc, char** argv) {
33-
// -----------------Command line interface -----------------
34-
argparse::ArgumentParser arg_parser(argv[0]);
35-
36-
arg_parser.add_argument("--cloudfile").required().help("input cloud file");
37-
arg_parser.add_argument("--search-radius").default_value(double(0.03)).scan<'g', double>().help("epsilon value");
38-
arg_parser.add_argument("--sampling-radius").default_value(double(0.005)).scan<'g', double>().help("epsilon value");
39-
arg_parser.add_argument("--step-size").default_value(double(0.005)).scan<'g', double>().help("epsilon value");
40-
arg_parser.add_argument("-o", "--output-dir").default_value(std::string("-")).help("output dir to save clusters");
41-
arg_parser.add_argument("-d", "--display")
42-
.default_value(false)
43-
.implicit_value(true)
44-
.help("display clusters in the pcl visualizer");
45-
46-
try {
47-
arg_parser.parse_args(argc, argv);
48-
} catch (const std::runtime_error& err) {
49-
std::cerr << err.what() << std::endl;
50-
std::cerr << arg_parser;
51-
std::exit(0);
52-
}
53-
54-
// -----------------Read input cloud file -----------------
55-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
56-
57-
// cloud parser object
58-
CloudParserLibrary::ParserCloudFile cloud_parser;
59-
cloud_parser.load_cloudfile(arg_parser.get<std::string>("--cloudfile"), input_cloud);
60-
61-
// set cloud metadata
62-
input_cloud->width = (int)input_cloud->points.size();
63-
input_cloud->height = 1;
64-
input_cloud->is_dense = true;
9+
void upsampling(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& input_cloud, argparse::ArgumentParser& arg_parser,
10+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& output_cloud) {
11+
double search_radius = arg_parser.get<double>("--search-radius");
12+
double sampling_radius = arg_parser.get<double>("--sampling-radius");
13+
double step_size = arg_parser.get<double>("--step-size");
14+
double gauss_param = (double)std::pow(search_radius, 2);
15+
int pol_order = 2;
16+
unsigned int num_threats = 1;
6517

18+
// https://pointclouds.org/documentation/classpcl_1_1_moving_least_squares.html
19+
// check alternative https://pointclouds.org/documentation/classpcl_1_1_bilateral_upsampling.html
6620
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dense_points(new pcl::PointCloud<pcl::PointXYZRGB>());
67-
6821
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
69-
// pcl::PointCloud<pcl::PointNormal>::Ptr mls_points(new pcl::PointCloud<pcl::PointNormal>());
7022
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> mls;
7123

72-
double search_radius = arg_parser.get<double>("--search-radius"); // 0.03
73-
double sampling_radius = arg_parser.get<double>("--sampling-radius"); // 0.005
74-
double step_size = arg_parser.get<double>("--step-size"); // 0.005
75-
double gauss_param = (double)std::pow(search_radius, 2);
76-
int pol_order = 2;
77-
unsigned int num_threats = 1;
78-
7924
mls.setComputeNormals(true);
8025
mls.setInputCloud(input_cloud);
8126
mls.setSearchMethod(kd_tree);
@@ -92,72 +37,63 @@ int main(int argc, char** argv) {
9237
// mls.setPointDensity(15); //15
9338
mls.process(*dense_points);
9439

95-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
96-
9740
*output_cloud = *input_cloud;
9841
*output_cloud += *dense_points;
9942

43+
if (output_cloud->points.size() == input_cloud->points.size()) {
44+
pcl::console::print_warn("\ninput cloud could not be upsampled, change input parameters!");
45+
}
46+
10047
pcl::console::print_info("\nNew points: ");
10148
pcl::console::print_value("%d", dense_points->points.size());
10249

10350
pcl::console::print_info("\nOutput cloud points: ");
10451
pcl::console::print_value("%d", output_cloud->points.size());
10552
pcl::console::print_info("\n");
53+
}
10654

107-
vtkObject::GlobalWarningDisplayOff(); // Disable vtk render warning
108-
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("VISUALIZER"));
109-
110-
int PORT1 = 0;
111-
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, PORT1);
112-
viewer->setBackgroundColor(0, 0, 0, PORT1);
113-
viewer->addText("ORIGINAL", 10, 10, "PORT1", PORT1);
114-
115-
int PORT2 = 0;
116-
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, PORT2);
117-
viewer->setBackgroundColor(0, 0, 0, PORT2);
118-
viewer->addText("UPSAMPLING", 10, 10, "PORT2", PORT2);
119-
120-
viewer->removeAllPointClouds(0);
55+
int main(int argc, char** argv) {
56+
// -----------------Command line interface -----------------
57+
argparse::ArgumentParser arg_parser(argv[0]);
12158

122-
if (input_cloud->points[0].r <= 0 and input_cloud->points[0].g <= 0 and input_cloud->points[0].b <= 0) {
123-
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(input_cloud, 255, 255, 0);
124-
viewer->addPointCloud(input_cloud, color_handler, "Original", PORT1);
125-
} else {
126-
viewer->addPointCloud(input_cloud, "Original", PORT1);
127-
}
59+
arg_parser.add_argument("--cloudfile").required().help("input cloud file");
60+
arg_parser.add_argument("--search-radius").default_value(double(0.03)).scan<'g', double>().help("epsilon value");
61+
arg_parser.add_argument("--sampling-radius").default_value(double(0.005)).scan<'g', double>().help("epsilon value");
62+
arg_parser.add_argument("--step-size").default_value(double(0.005)).scan<'g', double>().help("epsilon value");
63+
arg_parser.add_argument("-o", "--output-dir").default_value(std::string("-")).help("output dir to save clusters");
64+
arg_parser.add_argument("-d", "--display")
65+
.default_value(false)
66+
.implicit_value(true)
67+
.help("display clusters in the pcl visualizer");
12868

129-
if (output_cloud->points[0].r <= 0 and output_cloud->points[0].g <= 0 and output_cloud->points[0].b <= 0) {
130-
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(output_cloud, 255, 255, 0);
131-
viewer->addPointCloud(output_cloud, color_handler, "transform1 rvec", PORT2);
132-
} else {
133-
viewer->addPointCloud(output_cloud, "transform1 rvec", PORT2);
69+
try {
70+
arg_parser.parse_args(argc, argv);
71+
} catch (const std::runtime_error& err) {
72+
std::cerr << err.what() << std::endl;
73+
std::cerr << arg_parser;
74+
std::exit(0);
13475
}
13576

136-
pcl::io::savePCDFile("upsampled_cloud.pcd", *output_cloud);
137-
138-
pcl::PointXYZ p1, p2, p3;
139-
p1.getArray3fMap() << 1, 0, 0;
140-
p2.getArray3fMap() << 0, 1, 0;
141-
p3.getArray3fMap() << 0, 0.1, 1;
142-
143-
viewer->addCoordinateSystem(1, "original_usc", PORT1);
144-
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_", PORT1);
145-
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_", PORT1);
146-
viewer->addText3D("z", p3, 0.2, 0, 0, 1, "z_", PORT1);
77+
// -----------------Read input cloud file -----------------
78+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
14779

148-
viewer->addCoordinateSystem(1, "transform_ucs", PORT2);
149-
viewer->addText3D("x", p1, 0.2, 1, 0, 0, "x_", PORT2);
150-
viewer->addText3D("y", p2, 0.2, 0, 1, 0, "y_", PORT2);
151-
viewer->addText3D("z", p3, 0.2, 0, 0, 1, "z_", PORT2);
80+
// cloud parser object
81+
CloudParserLibrary::ParserCloudFile cloud_parser;
82+
cloud_parser.load_cloudfile(arg_parser.get<std::string>("--cloudfile"), input_cloud);
15283

153-
viewer->setPosition(0, 0);
154-
viewer->initCameraParameters();
155-
viewer->resetCamera();
84+
// set cloud metadata
85+
input_cloud->width = (int)input_cloud->points.size();
86+
input_cloud->height = 1;
87+
input_cloud->is_dense = true;
15688

157-
std::cout << "\nPress [q] to exit" << std::endl;
89+
// -----------------Upsampling -----------------
90+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
91+
upsampling(input_cloud, arg_parser, output_cloud);
92+
pcl::io::savePCDFile("upsampled_cloud.pcd", *output_cloud);
15893

159-
while (!viewer->wasStopped()) {
160-
viewer->spin();
94+
// -----------------Visualize upsampling -----------------
95+
if (arg_parser["--display"] == true) {
96+
display_upsampling(input_cloud, output_cloud);
16197
}
16298

16399
return 0;

0 commit comments

Comments
 (0)