1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 */
35
36 #include <pcl/common/time.h>
37 #include <pcl/io/openni_camera/openni_driver.h>
38 #include <pcl/io/openni_grabber.h>
39 #include <pcl/surface/organized_fast_mesh.h>
40 #include <pcl/visualization/pcl_visualizer.h> // for PCLVisualizer
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
43
44 #include <mutex>
45 #include <thread>
46
47 using namespace pcl;
48 using namespace pcl::visualization;
49 using namespace std::chrono_literals;
50
51 // clang-format off
52 #define FPS_CALC(_WHAT_) \
53 do { \
54 static unsigned count = 0; \
55 static double last = pcl::getTime(); \
56 if (++count == 100) { \
57 double now = pcl::getTime(); \
58 std::cout << "Average framerate(" << _WHAT_ << "): " \
59 << double(count) / double(now - last) << " Hz" << std::endl; \
60 count = 0; \
61 last = now; \
62 } \
63 } while (false)
64 // clang-format on
65
66 template <typename PointType>
67 class OpenNIFastMesh {
68 public:
69 using Cloud = pcl::PointCloud<PointType>;
70 using CloudPtr = typename Cloud::Ptr;
71 using CloudConstPtr = typename Cloud::ConstPtr;
72
OpenNIFastMesh(const std::string & device_id="")73 OpenNIFastMesh(const std::string& device_id = "") : device_id_(device_id)
74 {
75 ofm.setTrianglePixelSize(3);
76 ofm.setTriangulationType(pcl::OrganizedFastMesh<PointType>::QUAD_MESH);
77 }
78
79 void
cloud_cb(const CloudConstPtr & cloud)80 cloud_cb(const CloudConstPtr& cloud)
81 {
82 // Computation goes here
83 FPS_CALC("computation");
84
85 // Prepare input
86 ofm.setInputCloud(cloud);
87
88 // Store the results in a temporary object
89 std::vector<pcl::Vertices> temp_verts;
90 ofm.reconstruct(temp_verts);
91
92 // Lock and copy
93 {
94 std::lock_guard<std::mutex> lock(mtx_);
95 vertices_ = std::move(temp_verts);
96 cloud_ = cloud; // reset (new Cloud (*cloud));
97 }
98 }
99
100 void
run(int argc,char ** argv)101 run(int argc, char** argv)
102 {
103 pcl::OpenNIGrabber interface{device_id_};
104
105 std::function<void(const CloudConstPtr&)> f = [this](const CloudConstPtr& cloud) {
106 cloud_cb(cloud);
107 };
108 boost::signals2::connection c = interface.registerCallback(f);
109
110 view.reset(
111 new pcl::visualization::PCLVisualizer(argc, argv, "PCL OpenNI Mesh Viewer"));
112
113 interface.start();
114
115 CloudConstPtr temp_cloud;
116 std::vector<pcl::Vertices> temp_verts;
117
118 while (!view->wasStopped()) {
119 if (!cloud_ || !mtx_.try_lock()) {
120 std::this_thread::sleep_for(1ms);
121 continue;
122 }
123
124 temp_cloud = cloud_;
125 temp_verts = std::move(vertices_);
126 mtx_.unlock();
127
128 if (!view->updatePolygonMesh<PointType>(temp_cloud, temp_verts, "surface")) {
129 view->addPolygonMesh<PointType>(temp_cloud, temp_verts, "surface");
130 view->resetCameraViewpoint("surface");
131 }
132
133 FPS_CALC("visualization");
134 view->spinOnce(1);
135 }
136
137 interface.stop();
138 }
139
140 pcl::OrganizedFastMesh<PointType> ofm;
141 std::string device_id_;
142 std::mutex mtx_;
143 // Data
144 CloudConstPtr cloud_;
145 std::vector<pcl::Vertices> vertices_;
146 pcl::PolygonMesh::Ptr mesh_;
147
148 pcl::visualization::PCLVisualizer::Ptr view;
149 };
150
151 void
usage(char ** argv)152 usage(char** argv)
153 {
154 std::cout << "usage: " << argv[0] << " <device_id> <options>\n\n";
155
156 openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance();
157 if (driver.getNumberDevices() > 0) {
158 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices(); ++deviceIdx) {
159 std::cout << "Device: " << deviceIdx + 1
160 << ", vendor: " << driver.getVendorName(deviceIdx)
161 << ", product: " << driver.getProductName(deviceIdx)
162 << ", connected: " << driver.getBus(deviceIdx) << " @ "
163 << driver.getAddress(deviceIdx) << ", serial number: \'"
164 << driver.getSerialNumber(deviceIdx) << "\'" << std::endl;
165 std::cout << "device_id may be #1, #2, ... for the first second etc device in "
166 "the list or"
167 << std::endl
168 << " bus@address for the device connected to a "
169 "specific usb-bus / address combination (works only in Linux) or"
170 << std::endl
171 << " <serial-number> (only in Linux and for devices "
172 "which provide serial numbers)"
173 << std::endl;
174 }
175 }
176 else
177 std::cout << "No devices connected." << std::endl;
178 }
179
180 int
main(int argc,char ** argv)181 main(int argc, char** argv)
182 {
183 std::string arg;
184 if (argc > 1)
185 arg = std::string(argv[1]);
186
187 if (arg == "--help" || arg == "-h") {
188 usage(argv);
189 return 1;
190 }
191
192 pcl::OpenNIGrabber grabber("");
193 if (grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba>()) {
194 PCL_INFO("PointXYZRGBA mode enabled.\n");
195 OpenNIFastMesh<pcl::PointXYZRGBA> v("");
196 v.run(argc, argv);
197 }
198 else {
199 PCL_INFO("PointXYZ mode enabled.\n");
200 OpenNIFastMesh<pcl::PointXYZ> v("");
201 v.run(argc, argv);
202 }
203 return 0;
204 }
205