Skip to content

Commit 5faee5f

Browse files
codebydantcodebydant
committed
refactored
1 parent 0c5f39b commit 5faee5f

1 file changed

Lines changed: 51 additions & 115 deletions

File tree

‎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)