1 ///
2 /// Copyright (c) 2012, Texas A&M University
3 /// All rights reserved.
4 ///
5 /// Redistribution and use in source and binary forms, with or without
6 /// modification, are permitted provided that the following conditions
7 /// are met:
8 ///
9 /// * Redistributions of source code must retain the above copyright
10 /// notice, this list of conditions and the following disclaimer.
11 /// * Redistributions in binary form must reproduce the above
12 /// copyright notice, this list of conditions and the following
13 /// disclaimer in the documentation and/or other materials provided
14 /// with the distribution.
15 /// * Neither the name of Texas A&M University nor the names of its
16 /// contributors may be used to endorse or promote products derived
17 /// from this software without specific prior written permission.
18 ///
19 /// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 /// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 /// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22 /// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23 /// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24 /// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25 /// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 /// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 /// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 /// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 /// POSSIBILITY OF SUCH DAMAGE.
31 ///
32 /// The following software was written as part of a collaboration with the
33 /// University of South Carolina, Interdisciplinary Mathematics Institute.
34 ///
35
36 /// @file cloud.cpp
37 /// @details Implementation of the class Cloud, see cloud.h for details.
38 /// @author Yue Li and Matthew Hielsberg
39
40 #include <algorithm>
41 #include <qgl.h>
42 #include <pcl/apps/point_cloud_editor/cloud.h>
43 #include <pcl/apps/point_cloud_editor/selection.h>
44 #include <pcl/apps/point_cloud_editor/localTypes.h>
45 #include <pcl/apps/point_cloud_editor/common.h>
46 #include <pcl/apps/point_cloud_editor/copyBuffer.h>
47
48 const float Cloud::DEFAULT_POINT_DISPLAY_SIZE_ = 2.0f;
49 const float Cloud::DEFAULT_POINT_HIGHLIGHT_SIZE_ = 4.0f;
50
51 const float Cloud::DEFAULT_POINT_DISPLAY_COLOR_RED_ = 1.0f;
52 const float Cloud::DEFAULT_POINT_DISPLAY_COLOR_GREEN_ = 1.0f;
53 const float Cloud::DEFAULT_POINT_DISPLAY_COLOR_BLUE_ = 1.0f;
54
55 const float Cloud::DEFAULT_POINT_HIGHLIGHT_COLOR_RED_ = 0.0f;
56 const float Cloud::DEFAULT_POINT_HIGHLIGHT_COLOR_GREEN_ = 1.0f;
57 const float Cloud::DEFAULT_POINT_HIGHLIGHT_COLOR_BLUE_ = 0.0f;
58
59
Cloud()60 Cloud::Cloud ()
61 : use_color_ramp_(true), color_ramp_axis_(Y),
62 display_scale_(1.0f),
63 point_size_(DEFAULT_POINT_DISPLAY_SIZE_),
64 selected_point_size_(DEFAULT_POINT_HIGHLIGHT_SIZE_),
65 select_translate_x_(0), select_translate_y_(0), select_translate_z_(0)
66 {
67 std::fill_n(center_xyz_, XYZ_SIZE, 0.0f);
68 setIdentity(cloud_matrix_);
69 setIdentity(select_matrix_);
70 color_[RED] = DEFAULT_POINT_DISPLAY_COLOR_RED_;
71 color_[GREEN] = DEFAULT_POINT_DISPLAY_COLOR_GREEN_;
72 color_[BLUE] = DEFAULT_POINT_DISPLAY_COLOR_BLUE_;
73 highlight_color_[RED] = DEFAULT_POINT_HIGHLIGHT_COLOR_RED_;
74 highlight_color_[GREEN] = DEFAULT_POINT_HIGHLIGHT_COLOR_GREEN_;
75 highlight_color_[BLUE] = DEFAULT_POINT_HIGHLIGHT_COLOR_BLUE_;
76 }
77
Cloud(const Cloud3D & cloud,bool register_stats)78 Cloud::Cloud (const Cloud3D &cloud, bool register_stats)
79 : cloud_(cloud),
80 use_color_ramp_(true), color_ramp_axis_(Y),
81 display_scale_(1.0f),
82 point_size_(DEFAULT_POINT_DISPLAY_SIZE_),
83 selected_point_size_(DEFAULT_POINT_HIGHLIGHT_SIZE_),
84 select_translate_x_(0), select_translate_y_(0), select_translate_z_(0)
85 {
86 std::fill_n(center_xyz_, XYZ_SIZE, 0.0f);
87 setIdentity(cloud_matrix_);
88 setIdentity(select_matrix_);
89 color_[RED] = DEFAULT_POINT_DISPLAY_COLOR_RED_;
90 color_[GREEN] = DEFAULT_POINT_DISPLAY_COLOR_GREEN_;
91 color_[BLUE] = DEFAULT_POINT_DISPLAY_COLOR_BLUE_;
92 highlight_color_[RED] = DEFAULT_POINT_HIGHLIGHT_COLOR_RED_;
93 highlight_color_[GREEN] = DEFAULT_POINT_HIGHLIGHT_COLOR_GREEN_;
94 highlight_color_[BLUE] = DEFAULT_POINT_HIGHLIGHT_COLOR_BLUE_;
95 updateCloudMembers();
96 if (register_stats)
97 registerStats();
98 }
99
Cloud(const Cloud & copy)100 Cloud::Cloud (const Cloud ©)
101 : Statistics (copy), cloud_(copy.cloud_), selection_wk_ptr_(copy.selection_wk_ptr_),
102 use_color_ramp_(copy.use_color_ramp_),
103 color_ramp_axis_(copy.color_ramp_axis_),
104 display_scale_(copy.display_scale_),
105 partitioned_indices_(copy.partitioned_indices_),
106 point_size_(copy.point_size_),
107 selected_point_size_(copy.selected_point_size_),
108 select_translate_x_(copy.select_translate_x_),
109 select_translate_y_(copy.select_translate_y_),
110 select_translate_z_(copy.select_translate_z_)
111 {
112 std::copy(copy.center_xyz_, copy.center_xyz_+XYZ_SIZE, center_xyz_);
113 std::copy(copy.cloud_matrix_, copy.cloud_matrix_+MATRIX_SIZE, cloud_matrix_);
114 std::copy(copy.select_matrix_, copy.select_matrix_+MATRIX_SIZE,
115 select_matrix_);
116 std::copy(copy.color_, copy.color_+RGB, color_);
117 std::copy(copy.highlight_color_, copy.highlight_color_+RGB, highlight_color_);
118 }
119
120 Cloud&
operator =(const Cloud & cloud)121 Cloud::operator= (const Cloud &cloud)
122 {
123 cloud_ = cloud.cloud_;
124 selection_wk_ptr_ = cloud.selection_wk_ptr_;
125 use_color_ramp_ = cloud.use_color_ramp_;
126 color_ramp_axis_ = cloud.color_ramp_axis_;
127 display_scale_ = cloud.display_scale_;
128 point_size_ = cloud.point_size_;
129 selected_point_size_ = cloud.selected_point_size_;
130 std::copy(cloud.center_xyz_, cloud.center_xyz_+XYZ_SIZE, center_xyz_);
131 std::copy(cloud.cloud_matrix_, cloud.cloud_matrix_+MATRIX_SIZE,
132 cloud_matrix_);
133 std::copy(cloud.select_matrix_, cloud.select_matrix_+MATRIX_SIZE,
134 select_matrix_);
135 partitioned_indices_ = cloud.partitioned_indices_;
136 std::copy(cloud.color_, cloud.color_+RGB, color_);
137 std::copy(cloud.highlight_color_,cloud.highlight_color_+RGB,highlight_color_);
138 select_translate_x_ = cloud.select_translate_x_;
139 select_translate_y_ = cloud.select_translate_y_;
140 select_translate_z_ = cloud.select_translate_z_;
141 return (*this);
142 }
143
144 Point3D&
operator [](unsigned int index)145 Cloud::operator[] (unsigned int index)
146 {
147 assert(index < cloud_.size());
148 return (cloud_[index]);
149 }
150
151 const Point3D&
operator [](unsigned int index) const152 Cloud::operator[] (unsigned int index) const
153 {
154 assert(index < cloud_.size());
155 return (cloud_[index]);
156 }
157
158 void
loadMatrix(const float * matrix)159 Cloud::loadMatrix (const float *matrix)
160 {
161 std::copy(matrix, matrix+MATRIX_SIZE, cloud_matrix_);
162 }
163
164 void
multMatrix(const float * matrix)165 Cloud::multMatrix (const float *matrix)
166 {
167 ::multMatrix(cloud_matrix_, matrix, cloud_matrix_);
168 }
169
170 void
setSelectionRotation(const float * matrix)171 Cloud::setSelectionRotation (const float* matrix)
172 {
173 std::copy(matrix, matrix+MATRIX_SIZE, select_matrix_);
174 }
175
176 void
setSelectionTranslation(float dx,float dy,float dz)177 Cloud::setSelectionTranslation (float dx, float dy, float dz)
178 {
179 select_translate_x_ = dx;
180 select_translate_y_ = dy;
181 select_translate_z_ = dz;
182 }
183
184 void
setSelection(const SelectionPtr & selection_ptr)185 Cloud::setSelection (const SelectionPtr& selection_ptr)
186 {
187 selection_wk_ptr_ = selection_ptr;
188 if (!selection_ptr || selection_ptr->empty())
189 return;
190 IncIndex inc;
191 partitioned_indices_.resize(cloud_.size());
192 std::generate(partitioned_indices_.begin(), partitioned_indices_.end(), inc);
193 unsigned int pos = 0;
194 // assumes selection is sorted small to large
195 for (auto it = selection_ptr->begin(); it != selection_ptr->end(); ++it, ++pos)
196 {
197 std::swap(partitioned_indices_[pos], partitioned_indices_[*it]);
198 }
199 }
200
201 void
setRGB(float r,float g,float b)202 Cloud::setRGB (float r, float g, float b)
203 {
204 color_[RED] = r;
205 color_[GREEN] = g;
206 color_[BLUE] = b;
207 }
208
209 void
setHighlightColor(float r,float g,float b)210 Cloud::setHighlightColor (float r, float g, float b)
211 {
212 highlight_color_[RED] = r;
213 highlight_color_[GREEN] = g;
214 highlight_color_[BLUE] = b;
215 }
216
217 void
drawWithTexture() const218 Cloud::drawWithTexture () const
219 {
220 enableTexture();
221 draw();
222 disableTexture();
223 }
224
225 void
drawWithRGB() const226 Cloud::drawWithRGB () const
227 {
228 glEnableClientState(GL_COLOR_ARRAY);
229 glColorPointer(3, GL_UNSIGNED_BYTE, sizeof(Point3D),
230 &(cloud_[0].b));
231 draw();
232 }
233
234 void
drawWithPureColor() const235 Cloud::drawWithPureColor () const
236 {
237 glDisableClientState(GL_COLOR_ARRAY);
238 glColor3fv(color_);
239 draw();
240 }
241
242 void
drawWithHighlightColor() const243 Cloud::drawWithHighlightColor () const
244 {
245 glDisableClientState(GL_COLOR_ARRAY);
246 glDisable(GL_TEXTURE_1D);
247 glColor3fv(highlight_color_);
248 draw();
249 }
250
251 void
draw(bool disable_highlight) const252 Cloud::draw (bool disable_highlight) const
253 {
254 SelectionPtr selection_ptr = selection_wk_ptr_.lock();
255
256 glPushAttrib(GL_CURRENT_BIT | GL_POINT_BIT | GL_COLOR_BUFFER_BIT);
257 {
258 glPointSize(point_size_);
259 glPushClientAttrib(GL_CLIENT_VERTEX_ARRAY_BIT);
260 {
261 glPushMatrix();
262 {
263 glLoadIdentity();
264 glTranslatef(0.0f, 0.0f, DISPLAY_Z_TRANSLATION);
265 glScalef(display_scale_, display_scale_, display_scale_);
266 glMultMatrixf(cloud_matrix_);
267 glTranslatef(-center_xyz_[0], -center_xyz_[1], -center_xyz_[2]);
268
269 glEnableClientState(GL_VERTEX_ARRAY);
270 glVertexPointer(3, GL_FLOAT, sizeof(Point3D), &(cloud_[0].x));
271
272 if (disable_highlight || (!selection_ptr) || selection_ptr->empty())
273 {
274 // draw the entire cloud
275 glDrawArrays(GL_POINTS, 0, cloud_.size());
276 }
277 else
278 {
279 // draw the unselected points
280 glDrawElements(GL_POINTS, cloud_.size()-selection_ptr->size(),
281 GL_UNSIGNED_INT,
282 (&(partitioned_indices_[selection_ptr->size()-1]))+1);
283
284 // handle selection transformation
285 glLoadIdentity();
286 glTranslatef(0.0f, 0.0f, DISPLAY_Z_TRANSLATION);
287 glScalef(display_scale_, display_scale_, display_scale_);
288 glTranslatef(select_translate_x_,
289 select_translate_y_, select_translate_z_);
290 glMultMatrixf(select_matrix_);
291 glMultMatrixf(cloud_matrix_);
292 glTranslatef(-center_xyz_[0], -center_xyz_[1], -center_xyz_[2]);
293
294
295 // set up highlight display
296 glDisable(GL_TEXTURE_1D);
297 glDisableClientState(GL_COLOR_ARRAY);
298 glColor3fv(highlight_color_);
299 glPointSize(selected_point_size_);
300 glBlendFunc( GL_SRC_ALPHA, GL_ZERO );
301
302 // draw the selected points
303 glDrawElements(GL_POINTS, selection_ptr->size(), GL_UNSIGNED_INT,
304 &(partitioned_indices_[0]));
305 }
306 }
307 glPopMatrix();
308 }
309 glPopClientAttrib();
310 }
311 glPopAttrib();
312 }
313
314 void
append(const Point3D & pt)315 Cloud::append (const Point3D &pt)
316 {
317 cloud_.push_back(pt);
318 }
319
320 void
append(const Cloud & cloud)321 Cloud::append (const Cloud & cloud)
322 {
323 cloud_ += cloud.cloud_;
324 }
325
326 void
remove(const Selection & selection)327 Cloud::remove(const Selection& selection)
328 {
329 unsigned int pos = cloud_.size();
330 for (auto rit = selection.rbegin(); rit != selection.rend(); ++rit)
331 std::swap(cloud_[--pos], cloud_[*rit]);
332 resize(cloud_.size()-selection.size());
333 }
334
335 void
resize(unsigned int new_size)336 Cloud::resize(unsigned int new_size)
337 {
338 cloud_.resize(new_size);
339 cloud_.width = new_size;
340 cloud_.height = 1;
341 }
342
343 void
clear()344 Cloud::clear ()
345 {
346 cloud_.clear();
347 }
348
349 void
setPointSize(int size)350 Cloud::setPointSize (int size)
351 {
352 point_size_ = size;
353 }
354
355 void
setHighlightPointSize(int size)356 Cloud::setHighlightPointSize (int size)
357 {
358 selected_point_size_ = size;
359 }
360
361 Point3D
getObjectSpacePoint(unsigned int index) const362 Cloud::getObjectSpacePoint (unsigned int index) const
363 {
364 Point3D pt = cloud_[index];
365 float x, y, z;
366 pt.x -= center_xyz_[0];
367 pt.y -= center_xyz_[1];
368 pt.z -= center_xyz_[2];
369 x = cloud_matrix_[0] * pt.x +
370 cloud_matrix_[4] * pt.y +
371 cloud_matrix_[8] * pt.z +
372 cloud_matrix_[12];
373 y = cloud_matrix_[1] * pt.x +
374 cloud_matrix_[5] * pt.y +
375 cloud_matrix_[9] * pt.z +
376 cloud_matrix_[13];
377 z = cloud_matrix_[2] * pt.x +
378 cloud_matrix_[6] * pt.y +
379 cloud_matrix_[10] * pt.z +
380 cloud_matrix_[14];
381 pt.x = x;
382 pt.y = y;
383 pt.z = z;
384
385 return (pt);
386 }
387
388 Point3D
getDisplaySpacePoint(unsigned int index) const389 Cloud::getDisplaySpacePoint (unsigned int index) const
390 {
391 Point3D pt = cloud_[index];
392 float x, y, z;
393 pt.x -= center_xyz_[0];
394 pt.y -= center_xyz_[1];
395 pt.z -= center_xyz_[2];
396 x = cloud_matrix_[0] * pt.x +
397 cloud_matrix_[4] * pt.y +
398 cloud_matrix_[8] * pt.z +
399 cloud_matrix_[12];
400 y = cloud_matrix_[1] * pt.x +
401 cloud_matrix_[5] * pt.y +
402 cloud_matrix_[9] * pt.z +
403 cloud_matrix_[13];
404 z = cloud_matrix_[2] * pt.x +
405 cloud_matrix_[6] * pt.y +
406 cloud_matrix_[10] * pt.z +
407 cloud_matrix_[14];
408 pt.x = x * display_scale_;
409 pt.y = y * display_scale_;
410 pt.z = z * display_scale_;
411 pt.z += DISPLAY_Z_TRANSLATION;
412
413 return (pt);
414 }
415
416 void
getDisplaySpacePoints(Point3DVector & pts) const417 Cloud::getDisplaySpacePoints (Point3DVector& pts) const
418 {
419 for(std::size_t i = 0; i < cloud_.size(); ++i)
420 pts.push_back(getDisplaySpacePoint(i));
421 }
422
423 const Cloud3D&
getInternalCloud() const424 Cloud::getInternalCloud () const
425 {
426 return (cloud_);
427 }
428
429 void
restore(const CopyBuffer & copy_buffer,const Selection & selection)430 Cloud::restore (const CopyBuffer& copy_buffer, const Selection& selection)
431 {
432
433 if (selection.empty())
434 return;
435 const Cloud& copied_cloud = copy_buffer.get();
436 if (copied_cloud.size() != selection.size())
437 return;
438
439 append(copied_cloud);
440 unsigned int pos = cloud_.size();
441 for (auto rit = selection.rbegin(); rit != selection.rend(); ++rit)
442 std::swap(cloud_[--pos], cloud_[*rit]);
443 }
444
445 std::string
getStat() const446 Cloud::getStat () const
447 {
448 const std::string title = "Total number of points: ";
449 const std::string num_str = std::to_string(cloud_.size());
450 return title + num_str;
451 }
452
453 void
updateCloudMembers()454 Cloud::updateCloudMembers ()
455 {
456 if (cloud_.empty())
457 return;
458
459 std::fill_n(min_xyz_, XYZ_SIZE, 0.0f);
460 std::fill_n(max_xyz_, XYZ_SIZE, 0.0f);
461 float *pt = &(cloud_[0].data[X]);
462 std::copy(pt, pt+XYZ_SIZE, max_xyz_);
463 std::copy(max_xyz_, max_xyz_+XYZ_SIZE, min_xyz_);
464 for (std::size_t i = 1; i < cloud_.size(); ++i)
465 {
466 for (unsigned int j = 0; j < XYZ_SIZE; ++j)
467 {
468 min_xyz_[j] = std::min(min_xyz_[j], cloud_[i].data[j]);
469 max_xyz_[j] = std::max(max_xyz_[j], cloud_[i].data[j]);
470 }
471 }
472 float range = 0.0f;
473 for (unsigned int j = 0; j < XYZ_SIZE; ++j)
474 {
475 range = std::max(range, max_xyz_[j] - min_xyz_[j]);
476 center_xyz_[j] = 0.5f * (max_xyz_[j] + min_xyz_[j]);
477 }
478 display_scale_ = 1.0f / range;
479 }
480
481 void
enableTexture() const482 Cloud::enableTexture () const
483 {
484 if (!use_color_ramp_)
485 return;
486 float ranges[3] ={max_xyz_[0] - min_xyz_[0],
487 max_xyz_[1] - min_xyz_[1],
488 max_xyz_[2] - min_xyz_[2]};
489 float transvals[3] = {-min_xyz_[0], -min_xyz_[1], -min_xyz_[2]};
490 float range = ranges[color_ramp_axis_];
491 float transval = transvals[color_ramp_axis_];
492 glEnable(GL_TEXTURE_1D);
493 glEnableClientState(GL_TEXTURE_COORD_ARRAY);
494 glTexCoordPointer(1, GL_FLOAT, sizeof(Point3D),
495 &(cloud_[0].data[color_ramp_axis_]));
496 glMatrixMode(GL_TEXTURE);
497 glPushMatrix();
498 glLoadIdentity();
499 if (range <= 0.0f)
500 range = 1.0f;
501 glScalef(1.0f/range, 1.0f, 1.0f);
502 glTranslatef(transval, 0.0f, 0.0f);
503 glMatrixMode(GL_MODELVIEW);
504 }
505
506 void
disableTexture() const507 Cloud::disableTexture () const
508 {
509 if (!use_color_ramp_)
510 return;
511 glMatrixMode(GL_TEXTURE);
512 glPopMatrix();
513 glDisable(GL_TEXTURE_1D);
514 glMatrixMode(GL_MODELVIEW);
515 }
516