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 &copy)
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