opengl_viewer.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  * All rights reserved.
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions
00012  * are met:
00013  *
00014  *  * Redistributions of source code must retain the above copyright
00015  *    notice, this list of conditions and the following disclaimer.
00016  *  * Redistributions in binary form must reproduce the above
00017  *    copyright notice, this list of conditions and the following
00018  *    disclaimer in the documentation and/or other materials provided
00019  *    with the distribution.
00020  *  * Neither the name of the copyright holder(s) nor the names of its
00021  *    contributors may be used to endorse or promote products derived
00022  *    from this software without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #include <pcl/apps/in_hand_scanner/opengl_viewer.h>
00042 
00043 #include <cmath>
00044 #include <typeinfo>
00045 #include <cstdlib>
00046 
00047 #ifdef __APPLE__
00048 #  include <OpenGL/gl.h>
00049 #  include <OpenGL/glu.h>
00050 #else
00051 #  include <GL/gl.h>
00052 #  include <GL/glu.h>
00053 #endif
00054 
00055 #include <QtOpenGL>
00056 
00057 #include <pcl/common/centroid.h>
00058 #include <pcl/common/impl/centroid.hpp> // TODO: PointIHS is not registered
00059 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
00060 
00062 // FaceVertexMesh
00064 
00065 pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh ()
00066   : vertices       (),
00067     triangles      (),
00068     transformation (Eigen::Isometry3d::Identity ())
00069 {
00070 }
00071 
00073 
00074 pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh (const Mesh& mesh, const Eigen::Isometry3d& T)
00075   : vertices       (mesh.getVertexDataCloud ()),
00076     triangles      (),
00077     transformation (T)
00078 {
00079   if (typeid (Mesh::MeshTag) != typeid (pcl::geometry::TriangleMeshTag))
00080   {
00081     std::cerr << "In opengl_viewer.cpp: Only triangle meshes are currently supported!\n";
00082     exit (EXIT_FAILURE);
00083   }
00084 
00085   for (CloudIHS::iterator it=vertices.begin (); it!=vertices.end (); ++it)
00086   {
00087     std::swap (it->r, it->b);
00088   }
00089 
00090   triangles.reserve (mesh.sizeFaces ());
00091   pcl::ihs::detail::FaceVertexMesh::Triangle triangle;
00092 
00093   for (unsigned int i=0; i<mesh.sizeFaces (); ++i)
00094   {
00095     Mesh::VertexAroundFaceCirculator circ = mesh.getVertexAroundFaceCirculator (Mesh::FaceIndex (i));
00096     triangle.first  = (circ++).getTargetIndex ().get ();
00097     triangle.second = (circ++).getTargetIndex ().get ();
00098     triangle.third  = (circ  ).getTargetIndex ().get ();
00099 
00100     triangles.push_back (triangle);
00101   }
00102 }
00103 
00105 // OpenGLViewer
00107 
00108 pcl::ihs::OpenGLViewer::OpenGLViewer (QWidget* parent)
00109   : QGLWidget            (parent),
00110     mutex_vis_           (),
00111     timer_vis_           (new QTimer (this)),
00112     colormap_            (Colormap::Constant (255)),
00113     vis_conf_norm_       (1),
00114     drawn_meshes_        (),
00115     mesh_representation_ (MR_POINTS),
00116     coloring_            (COL_RGB),
00117     draw_box_            (false),
00118     box_coefficients_    (),
00119     scaling_factor_      (1.),
00120     R_cam_               (1., 0., 0., 0.),
00121     t_cam_               (0., 0., 0.),
00122     cam_pivot_           (0., 0., 0.),
00123     cam_pivot_id_        (""),
00124     mouse_pressed_begin_ (false),
00125     x_prev_              (0),
00126     y_prev_              (0)
00127 {
00128   // Timer: Defines the update rate for the visualization
00129   connect (timer_vis_.get (), SIGNAL (timeout ()), this, SLOT (timerCallback ()));
00130   timer_vis_->start (33);
00131 
00132   // http://doc.qt.digia.com/qt/opengl-overpainting.html
00133   QWidget::setAutoFillBackground (false);
00134 
00135   // http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent
00136   this->setFocusPolicy (Qt::StrongFocus);
00137 
00138   // http://doc.qt.digia.com/qt/qmetatype.html#qRegisterMetaType
00139   qRegisterMetaType <pcl::ihs::OpenGLViewer::MeshRepresentation> ("MeshRepresentation");
00140   qRegisterMetaType <pcl::ihs::OpenGLViewer::Coloring>           ("Coloring");
00141 
00143   // Code to generate the colormap (I don't want to link against vtk just for the colormap).
00145 
00146   //#include <cstdlib>
00147   //#include <iomanip>
00148 
00149   //#include <vtkColorTransferFunction.h>
00150   //#include <vtkSmartPointer.h>
00151 
00152   //int
00153   //main ()
00154   //{
00155   //  static const unsigned int n = 256;
00156   //  // double rgb_1 [] = { 59./255., 76./255., 192./255.};
00157   //  // double rgb_2 [] = {180./255.,  4./255.,  38./255.};
00158   //  double rgb_1 [] = {180./255.,   0./255.,  0./255.};
00159   //  double rgb_2 [] = {  0./255., 180./255.,  0./255.};
00160 
00161   //  vtkSmartPointer <vtkColorTransferFunction> ctf = vtkColorTransferFunction::New ();
00162   //  ctf->SetColorSpaceToDiverging ();
00163   //  ctf->AddRGBPoint (  0., rgb_1 [0], rgb_1 [1], rgb_1 [2]);
00164   //  ctf->AddRGBPoint (  1., rgb_2 [0], rgb_2 [1], rgb_2 [2]);
00165   //  ctf->Build ();
00166 
00167   //  const unsigned char* colormap = ctf->GetTable (0., 1., n);
00168 
00169   //  for (unsigned int i=0; i<n; ++i)
00170   //  {
00171   //    const unsigned int r = static_cast <unsigned int> (colormap [3 * i    ]);
00172   //    const unsigned int g = static_cast <unsigned int> (colormap [3 * i + 1]);
00173   //    const unsigned int b = static_cast <unsigned int> (colormap [3 * i + 2]);
00174 
00175   //    std::cerr << "colormap_.col ("
00176   //              << std::setw (3) << i << ") = Color ("
00177   //              << std::setw (3) << r << ", "
00178   //              << std::setw (3) << g << ", "
00179   //              << std::setw (3) << b << ");\n";
00180   //  }
00181 
00182   //  return (EXIT_SUCCESS);
00183   //}
00184 
00185   colormap_.col (  0) = Color (180,   0,   0);
00186   colormap_.col (  1) = Color (182,   9,   1);
00187   colormap_.col (  2) = Color (184,  17,   1);
00188   colormap_.col (  3) = Color (186,  24,   2);
00189   colormap_.col (  4) = Color (188,  29,   2);
00190   colormap_.col (  5) = Color (190,  33,   3);
00191   colormap_.col (  6) = Color (192,  38,   4);
00192   colormap_.col (  7) = Color (194,  42,   5);
00193   colormap_.col (  8) = Color (196,  46,   6);
00194   colormap_.col (  9) = Color (197,  49,   7);
00195   colormap_.col ( 10) = Color (199,  53,   9);
00196   colormap_.col ( 11) = Color (201,  56,  10);
00197   colormap_.col ( 12) = Color (203,  59,  12);
00198   colormap_.col ( 13) = Color (205,  63,  13);
00199   colormap_.col ( 14) = Color (207,  66,  15);
00200   colormap_.col ( 15) = Color (208,  69,  17);
00201   colormap_.col ( 16) = Color (210,  72,  18);
00202   colormap_.col ( 17) = Color (212,  75,  20);
00203   colormap_.col ( 18) = Color (214,  78,  21);
00204   colormap_.col ( 19) = Color (215,  81,  23);
00205   colormap_.col ( 20) = Color (217,  84,  25);
00206   colormap_.col ( 21) = Color (219,  87,  26);
00207   colormap_.col ( 22) = Color (221,  89,  28);
00208   colormap_.col ( 23) = Color (222,  92,  30);
00209   colormap_.col ( 24) = Color (224,  95,  32);
00210   colormap_.col ( 25) = Color (225,  98,  33);
00211   colormap_.col ( 26) = Color (227, 101,  35);
00212   colormap_.col ( 27) = Color (229, 103,  37);
00213   colormap_.col ( 28) = Color (230, 106,  39);
00214   colormap_.col ( 29) = Color (232, 109,  40);
00215   colormap_.col ( 30) = Color (233, 112,  42);
00216   colormap_.col ( 31) = Color (235, 114,  44);
00217   colormap_.col ( 32) = Color (236, 117,  46);
00218   colormap_.col ( 33) = Color (238, 120,  48);
00219   colormap_.col ( 34) = Color (239, 122,  50);
00220   colormap_.col ( 35) = Color (241, 125,  52);
00221   colormap_.col ( 36) = Color (242, 127,  54);
00222   colormap_.col ( 37) = Color (244, 130,  56);
00223   colormap_.col ( 38) = Color (245, 133,  58);
00224   colormap_.col ( 39) = Color (246, 135,  60);
00225   colormap_.col ( 40) = Color (248, 138,  62);
00226   colormap_.col ( 41) = Color (249, 140,  64);
00227   colormap_.col ( 42) = Color (250, 143,  66);
00228   colormap_.col ( 43) = Color (252, 145,  68);
00229   colormap_.col ( 44) = Color (253, 148,  70);
00230   colormap_.col ( 45) = Color (254, 150,  73);
00231   colormap_.col ( 46) = Color (255, 153,  75);
00232   colormap_.col ( 47) = Color (255, 154,  76);
00233   colormap_.col ( 48) = Color (255, 156,  78);
00234   colormap_.col ( 49) = Color (255, 158,  80);
00235   colormap_.col ( 50) = Color (255, 159,  82);
00236   colormap_.col ( 51) = Color (255, 161,  84);
00237   colormap_.col ( 52) = Color (255, 163,  86);
00238   colormap_.col ( 53) = Color (255, 164,  88);
00239   colormap_.col ( 54) = Color (255, 166,  90);
00240   colormap_.col ( 55) = Color (255, 168,  92);
00241   colormap_.col ( 56) = Color (255, 169,  93);
00242   colormap_.col ( 57) = Color (255, 171,  95);
00243   colormap_.col ( 58) = Color (255, 172,  97);
00244   colormap_.col ( 59) = Color (255, 174,  99);
00245   colormap_.col ( 60) = Color (255, 176, 101);
00246   colormap_.col ( 61) = Color (255, 177, 103);
00247   colormap_.col ( 62) = Color (255, 179, 105);
00248   colormap_.col ( 63) = Color (255, 180, 107);
00249   colormap_.col ( 64) = Color (255, 182, 109);
00250   colormap_.col ( 65) = Color (255, 183, 111);
00251   colormap_.col ( 66) = Color (255, 185, 113);
00252   colormap_.col ( 67) = Color (255, 186, 115);
00253   colormap_.col ( 68) = Color (255, 188, 117);
00254   colormap_.col ( 69) = Color (255, 189, 119);
00255   colormap_.col ( 70) = Color (255, 191, 122);
00256   colormap_.col ( 71) = Color (255, 192, 124);
00257   colormap_.col ( 72) = Color (255, 194, 126);
00258   colormap_.col ( 73) = Color (255, 195, 128);
00259   colormap_.col ( 74) = Color (255, 196, 130);
00260   colormap_.col ( 75) = Color (255, 198, 132);
00261   colormap_.col ( 76) = Color (255, 199, 134);
00262   colormap_.col ( 77) = Color (255, 201, 136);
00263   colormap_.col ( 78) = Color (255, 202, 139);
00264   colormap_.col ( 79) = Color (255, 203, 141);
00265   colormap_.col ( 80) = Color (255, 205, 143);
00266   colormap_.col ( 81) = Color (255, 206, 145);
00267   colormap_.col ( 82) = Color (255, 207, 147);
00268   colormap_.col ( 83) = Color (255, 209, 149);
00269   colormap_.col ( 84) = Color (255, 210, 152);
00270   colormap_.col ( 85) = Color (255, 211, 154);
00271   colormap_.col ( 86) = Color (255, 213, 156);
00272   colormap_.col ( 87) = Color (255, 214, 158);
00273   colormap_.col ( 88) = Color (255, 215, 161);
00274   colormap_.col ( 89) = Color (255, 216, 163);
00275   colormap_.col ( 90) = Color (255, 218, 165);
00276   colormap_.col ( 91) = Color (255, 219, 168);
00277   colormap_.col ( 92) = Color (255, 220, 170);
00278   colormap_.col ( 93) = Color (255, 221, 172);
00279   colormap_.col ( 94) = Color (255, 223, 175);
00280   colormap_.col ( 95) = Color (255, 224, 177);
00281   colormap_.col ( 96) = Color (255, 225, 179);
00282   colormap_.col ( 97) = Color (255, 226, 182);
00283   colormap_.col ( 98) = Color (255, 227, 184);
00284   colormap_.col ( 99) = Color (255, 228, 186);
00285   colormap_.col (100) = Color (255, 230, 189);
00286   colormap_.col (101) = Color (255, 231, 191);
00287   colormap_.col (102) = Color (255, 232, 193);
00288   colormap_.col (103) = Color (255, 233, 196);
00289   colormap_.col (104) = Color (255, 234, 198);
00290   colormap_.col (105) = Color (255, 235, 201);
00291   colormap_.col (106) = Color (255, 236, 203);
00292   colormap_.col (107) = Color (255, 237, 205);
00293   colormap_.col (108) = Color (255, 238, 208);
00294   colormap_.col (109) = Color (255, 239, 210);
00295   colormap_.col (110) = Color (255, 240, 213);
00296   colormap_.col (111) = Color (255, 241, 215);
00297   colormap_.col (112) = Color (255, 242, 218);
00298   colormap_.col (113) = Color (255, 243, 220);
00299   colormap_.col (114) = Color (255, 244, 222);
00300   colormap_.col (115) = Color (255, 245, 225);
00301   colormap_.col (116) = Color (255, 246, 227);
00302   colormap_.col (117) = Color (255, 247, 230);
00303   colormap_.col (118) = Color (255, 248, 232);
00304   colormap_.col (119) = Color (255, 249, 235);
00305   colormap_.col (120) = Color (255, 249, 237);
00306   colormap_.col (121) = Color (255, 250, 239);
00307   colormap_.col (122) = Color (255, 251, 242);
00308   colormap_.col (123) = Color (255, 252, 244);
00309   colormap_.col (124) = Color (255, 253, 247);
00310   colormap_.col (125) = Color (255, 253, 249);
00311   colormap_.col (126) = Color (255, 254, 251);
00312   colormap_.col (127) = Color (255, 255, 254);
00313   colormap_.col (128) = Color (255, 255, 254);
00314   colormap_.col (129) = Color (254, 255, 253);
00315   colormap_.col (130) = Color (253, 255, 252);
00316   colormap_.col (131) = Color (252, 255, 250);
00317   colormap_.col (132) = Color (251, 255, 249);
00318   colormap_.col (133) = Color (250, 255, 248);
00319   colormap_.col (134) = Color (249, 255, 246);
00320   colormap_.col (135) = Color (248, 255, 245);
00321   colormap_.col (136) = Color (247, 255, 244);
00322   colormap_.col (137) = Color (246, 255, 242);
00323   colormap_.col (138) = Color (245, 255, 241);
00324   colormap_.col (139) = Color (244, 255, 240);
00325   colormap_.col (140) = Color (243, 255, 238);
00326   colormap_.col (141) = Color (242, 255, 237);
00327   colormap_.col (142) = Color (241, 255, 236);
00328   colormap_.col (143) = Color (240, 255, 235);
00329   colormap_.col (144) = Color (239, 255, 233);
00330   colormap_.col (145) = Color (238, 255, 232);
00331   colormap_.col (146) = Color (237, 255, 231);
00332   colormap_.col (147) = Color (236, 255, 229);
00333   colormap_.col (148) = Color (235, 255, 228);
00334   colormap_.col (149) = Color (234, 255, 227);
00335   colormap_.col (150) = Color (234, 255, 225);
00336   colormap_.col (151) = Color (233, 255, 224);
00337   colormap_.col (152) = Color (232, 255, 223);
00338   colormap_.col (153) = Color (231, 255, 221);
00339   colormap_.col (154) = Color (230, 255, 220);
00340   colormap_.col (155) = Color (229, 255, 219);
00341   colormap_.col (156) = Color (228, 255, 218);
00342   colormap_.col (157) = Color (227, 255, 216);
00343   colormap_.col (158) = Color (226, 255, 215);
00344   colormap_.col (159) = Color (225, 255, 214);
00345   colormap_.col (160) = Color (224, 255, 212);
00346   colormap_.col (161) = Color (223, 255, 211);
00347   colormap_.col (162) = Color (222, 255, 210);
00348   colormap_.col (163) = Color (221, 255, 208);
00349   colormap_.col (164) = Color (220, 255, 207);
00350   colormap_.col (165) = Color (219, 255, 206);
00351   colormap_.col (166) = Color (218, 255, 204);
00352   colormap_.col (167) = Color (217, 255, 203);
00353   colormap_.col (168) = Color (216, 255, 202);
00354   colormap_.col (169) = Color (215, 255, 201);
00355   colormap_.col (170) = Color (214, 255, 199);
00356   colormap_.col (171) = Color (213, 255, 198);
00357   colormap_.col (172) = Color (211, 255, 197);
00358   colormap_.col (173) = Color (210, 255, 195);
00359   colormap_.col (174) = Color (209, 255, 194);
00360   colormap_.col (175) = Color (208, 255, 193);
00361   colormap_.col (176) = Color (207, 255, 191);
00362   colormap_.col (177) = Color (206, 255, 190);
00363   colormap_.col (178) = Color (205, 255, 188);
00364   colormap_.col (179) = Color (204, 255, 187);
00365   colormap_.col (180) = Color (203, 255, 186);
00366   colormap_.col (181) = Color (202, 255, 184);
00367   colormap_.col (182) = Color (201, 255, 183);
00368   colormap_.col (183) = Color (199, 255, 182);
00369   colormap_.col (184) = Color (198, 255, 180);
00370   colormap_.col (185) = Color (197, 255, 179);
00371   colormap_.col (186) = Color (196, 255, 177);
00372   colormap_.col (187) = Color (195, 255, 176);
00373   colormap_.col (188) = Color (194, 255, 174);
00374   colormap_.col (189) = Color (192, 255, 173);
00375   colormap_.col (190) = Color (191, 255, 172);
00376   colormap_.col (191) = Color (190, 255, 170);
00377   colormap_.col (192) = Color (189, 255, 169);
00378   colormap_.col (193) = Color (188, 255, 167);
00379   colormap_.col (194) = Color (186, 255, 166);
00380   colormap_.col (195) = Color (185, 255, 164);
00381   colormap_.col (196) = Color (184, 255, 163);
00382   colormap_.col (197) = Color (183, 255, 161);
00383   colormap_.col (198) = Color (181, 255, 160);
00384   colormap_.col (199) = Color (180, 255, 158);
00385   colormap_.col (200) = Color (179, 255, 157);
00386   colormap_.col (201) = Color (177, 255, 155);
00387   colormap_.col (202) = Color (176, 255, 154);
00388   colormap_.col (203) = Color (175, 255, 152);
00389   colormap_.col (204) = Color (173, 255, 150);
00390   colormap_.col (205) = Color (172, 255, 149);
00391   colormap_.col (206) = Color (170, 255, 147);
00392   colormap_.col (207) = Color (169, 255, 145);
00393   colormap_.col (208) = Color (166, 253, 143);
00394   colormap_.col (209) = Color (164, 252, 141);
00395   colormap_.col (210) = Color (162, 251, 138);
00396   colormap_.col (211) = Color (159, 250, 136);
00397   colormap_.col (212) = Color (157, 248, 134);
00398   colormap_.col (213) = Color (155, 247, 131);
00399   colormap_.col (214) = Color (152, 246, 129);
00400   colormap_.col (215) = Color (150, 245, 127);
00401   colormap_.col (216) = Color (148, 243, 124);
00402   colormap_.col (217) = Color (145, 242, 122);
00403   colormap_.col (218) = Color (143, 240, 119);
00404   colormap_.col (219) = Color (140, 239, 117);
00405   colormap_.col (220) = Color (138, 238, 114);
00406   colormap_.col (221) = Color (135, 236, 112);
00407   colormap_.col (222) = Color (133, 235, 110);
00408   colormap_.col (223) = Color (130, 233, 107);
00409   colormap_.col (224) = Color (128, 232, 105);
00410   colormap_.col (225) = Color (125, 230, 102);
00411   colormap_.col (226) = Color (122, 229, 100);
00412   colormap_.col (227) = Color (120, 227,  97);
00413   colormap_.col (228) = Color (117, 226,  94);
00414   colormap_.col (229) = Color (114, 224,  92);
00415   colormap_.col (230) = Color (111, 223,  89);
00416   colormap_.col (231) = Color (109, 221,  87);
00417   colormap_.col (232) = Color (106, 220,  84);
00418   colormap_.col (233) = Color (103, 218,  82);
00419   colormap_.col (234) = Color (100, 217,  79);
00420   colormap_.col (235) = Color ( 97, 215,  76);
00421   colormap_.col (236) = Color ( 94, 213,  73);
00422   colormap_.col (237) = Color ( 91, 212,  71);
00423   colormap_.col (238) = Color ( 88, 210,  68);
00424   colormap_.col (239) = Color ( 85, 208,  65);
00425   colormap_.col (240) = Color ( 82, 207,  62);
00426   colormap_.col (241) = Color ( 78, 205,  59);
00427   colormap_.col (242) = Color ( 75, 203,  57);
00428   colormap_.col (243) = Color ( 71, 201,  54);
00429   colormap_.col (244) = Color ( 68, 200,  50);
00430   colormap_.col (245) = Color ( 64, 198,  47);
00431   colormap_.col (246) = Color ( 60, 196,  44);
00432   colormap_.col (247) = Color ( 56, 195,  41);
00433   colormap_.col (248) = Color ( 52, 193,  37);
00434   colormap_.col (249) = Color ( 47, 191,  33);
00435   colormap_.col (250) = Color ( 42, 189,  29);
00436   colormap_.col (251) = Color ( 37, 187,  25);
00437   colormap_.col (252) = Color ( 31, 186,  20);
00438   colormap_.col (253) = Color ( 24, 184,  15);
00439   colormap_.col (254) = Color ( 14, 182,   7);
00440   colormap_.col (255) = Color (  0, 180,   0);
00441 }
00442 
00444 
00445 pcl::ihs::OpenGLViewer::~OpenGLViewer ()
00446 {
00447   this->stopTimer ();
00448 }
00449 
00451 
00452 bool
00453 pcl::ihs::OpenGLViewer::addMesh (const MeshConstPtr& mesh, const std::string& id, const Eigen::Isometry3d& T)
00454 {
00455   if (!mesh)
00456   {
00457     std::cerr << "ERROR in opengl_viewer.cpp: Input mesh is not valid.\n";
00458     return (false);
00459   }
00460 
00461   boost::mutex::scoped_lock lock (mutex_vis_);
00462 
00463   if (this->getMeshIsAdded (id))
00464     drawn_meshes_ [id] = FaceVertexMeshPtr (new FaceVertexMesh (*mesh, T));
00465   else
00466     drawn_meshes_.insert (std::make_pair (id, FaceVertexMeshPtr (new FaceVertexMesh (*mesh, T))));
00467 
00468   return (true);
00469 }
00470 
00472 
00473 bool
00474 pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const std::string& id, const Eigen::Isometry3d& T)
00475 {
00476   if (!cloud)
00477   {
00478     std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not valid.\n";
00479     return (false);
00480   }
00481   if (!cloud->isOrganized ())
00482   {
00483     std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not organized.\n";
00484     return (false);
00485   }
00486 
00487   // Convert the cloud to a mesh using the following pattern
00488   // 2 - 1 //
00489   // | / | //
00490   // 3 - 0 //
00491   const int w        = cloud->width;
00492   const int h        = cloud->height;
00493   const int offset_1 = -w;
00494   const int offset_2 = -w - 1;
00495   const int offset_3 =    - 1;
00496 
00497   FaceVertexMeshPtr mesh (new FaceVertexMesh ());
00498   mesh->transformation = T;
00499 
00500   std::vector <int> indices (w * h, -1); // Map the original indices to the vertex indices.
00501   CloudIHS& vertices = mesh->vertices;
00502   std::vector <FaceVertexMesh::Triangle>& triangles = mesh->triangles;
00503   vertices.reserve (cloud->size ());
00504   triangles.reserve (2 * (w-1) * (h-1));
00505 
00506   // Helper functor
00507   struct AddVertex
00508   {
00509     inline int operator () (const PointXYZRGBNormal& pt, CloudIHS& vertices, int& ind_o) const
00510     {
00511       if (ind_o == -1)
00512       {
00513         ind_o = vertices.size ();
00514         vertices.push_back (PointIHS (pt, -pt.normal_z));
00515         std::swap (vertices.back ().r, vertices.back ().b);
00516       }
00517       return (ind_o);
00518     }
00519   };
00520   AddVertex addVertex;
00521 
00522   int ind_o_0, ind_o_1, ind_o_2, ind_o_3; // Index into the organized cloud.
00523   int ind_v_0, ind_v_1, ind_v_2, ind_v_3; // Index to the new vertices.
00524 
00525   for (int r=1; r<h; ++r)
00526   {
00527     for (int c=1; c<w; ++c)
00528     {
00529       ind_o_0 = r*w + c;
00530       ind_o_1 = ind_o_0 + offset_1;
00531       ind_o_2 = ind_o_0 + offset_2;
00532       ind_o_3 = ind_o_0 + offset_3;
00533 
00534       const PointXYZRGBNormal& pt_0 = cloud->operator [] (ind_o_0);
00535       const PointXYZRGBNormal& pt_1 = cloud->operator [] (ind_o_1);
00536       const PointXYZRGBNormal& pt_2 = cloud->operator [] (ind_o_2);
00537       const PointXYZRGBNormal& pt_3 = cloud->operator [] (ind_o_3);
00538 
00539       if (!boost::math::isnan (pt_1.x) && !boost::math::isnan (pt_3.x))
00540       {
00541         if (!boost::math::isnan (pt_2.x)) // 1-2-3 is valid
00542         {
00543           if (std::abs (pt_1.z - pt_2.z) < 1 &&
00544               std::abs (pt_1.z - pt_3.z) < 1 &&
00545               std::abs (pt_2.z - pt_3.z) < 1) // distance threshold
00546           {
00547             ind_v_1 = addVertex (pt_1, vertices, indices [ind_o_1]);
00548             ind_v_2 = addVertex (pt_2, vertices, indices [ind_o_2]);
00549             ind_v_3 = addVertex (pt_3, vertices, indices [ind_o_3]);
00550 
00551             triangles.push_back (FaceVertexMesh::Triangle (ind_v_1, ind_v_2, ind_v_3));
00552           }
00553         }
00554         if (!boost::math::isnan (pt_0.x)) // 0-1-3 is valid
00555         {
00556           if (std::abs (pt_0.z - pt_1.z) < 1 &&
00557               std::abs (pt_0.z - pt_3.z) < 1 &&
00558               std::abs (pt_1.z - pt_3.z) < 1) // distance threshold
00559           {
00560             ind_v_1 = addVertex (pt_1, vertices, indices [ind_o_1]);
00561             ind_v_3 = addVertex (pt_3, vertices, indices [ind_o_3]);
00562             ind_v_0 = addVertex (pt_0, vertices, indices [ind_o_0]);
00563 
00564             triangles.push_back (FaceVertexMesh::Triangle (ind_v_1, ind_v_3, ind_v_0));
00565           }
00566         }
00567       }
00568     }
00569   }
00570 
00571   // Finally add the mesh.
00572   boost::mutex::scoped_lock lock (mutex_vis_);
00573 
00574   if (this->getMeshIsAdded (id))
00575     drawn_meshes_ [id] = mesh;
00576   else
00577     drawn_meshes_.insert (std::make_pair (id, mesh));
00578 
00579   return (true);
00580 }
00581 
00583 
00584 bool
00585 pcl::ihs::OpenGLViewer::removeMesh (const std::string& id)
00586 {
00587   boost::mutex::scoped_lock lock (mutex_vis_);
00588   if (!this->getMeshIsAdded (id)) return (false);
00589 
00590   drawn_meshes_.erase (id);
00591 
00592   return (true);
00593 }
00594 
00596 
00597 void
00598 pcl::ihs::OpenGLViewer::removeAllMeshes ()
00599 {
00600   boost::mutex::scoped_lock lock (mutex_vis_);
00601   drawn_meshes_.clear ();
00602 }
00603 
00605 
00606 void
00607 pcl::ihs::OpenGLViewer::setBoxCoefficients (const BoxCoefficients& coeffs)
00608 {
00609   boost::mutex::scoped_lock lock (mutex_vis_);
00610   box_coefficients_ = coeffs;
00611 }
00612 
00614 
00615 void
00616 pcl::ihs::OpenGLViewer::setDrawBox (const bool enabled)
00617 {
00618   boost::mutex::scoped_lock lock (mutex_vis_);
00619   draw_box_ = enabled;
00620 }
00621 
00623 
00624 bool
00625 pcl::ihs::OpenGLViewer::getDrawBox () const
00626 {
00627   return (draw_box_);
00628 }
00629 
00631 
00632 void
00633 pcl::ihs::OpenGLViewer::setPivot (const Eigen::Vector3d& pivot)
00634 {
00635   boost::mutex::scoped_lock lock (mutex_vis_);
00636   cam_pivot_ = pivot;
00637 }
00638 
00640 
00641 void
00642 pcl::ihs::OpenGLViewer::setPivot (const std::string& id)
00643 {
00644   boost::mutex::scoped_lock lock (mutex_vis_);
00645   cam_pivot_id_ = id;
00646 }
00647 
00649 
00650 void
00651 pcl::ihs::OpenGLViewer::stopTimer ()
00652 {
00653   boost::mutex::scoped_lock lock (mutex_vis_);
00654   if (timer_vis_)
00655   {
00656     timer_vis_->stop ();
00657   }
00658 }
00659 
00661 
00662 void
00663 pcl::ihs::OpenGLViewer::setVisibilityConfidenceNormalization (const float vis_conf_norm)
00664 {
00665   boost::mutex::scoped_lock lock (mutex_vis_);
00666 
00667   vis_conf_norm_ = vis_conf_norm < 1 ? 1 : vis_conf_norm;
00668 }
00669 
00671 
00672 QSize
00673 pcl::ihs::OpenGLViewer::minimumSizeHint () const
00674 {
00675   return (QSize (160, 120));
00676 }
00677 
00679 
00680 QSize
00681 pcl::ihs::OpenGLViewer::sizeHint () const
00682 {
00683   return (QSize (640, 480));
00684 }
00685 
00687 
00688 void
00689 pcl::ihs::OpenGLViewer::setScalingFactor (const double scale)
00690 {
00691   boost::mutex::scoped_lock lock (mutex_vis_);
00692   scaling_factor_ = scale;
00693 }
00694 
00696 
00697 void
00698 pcl::ihs::OpenGLViewer::timerCallback ()
00699 {
00700   QWidget::update ();
00701 }
00702 
00704 
00705 void
00706 pcl::ihs::OpenGLViewer::resetCamera ()
00707 {
00708   boost::mutex::scoped_lock lock (mutex_vis_);
00709 
00710   R_cam_ = Eigen::Quaterniond (1., 0., 0., 0.);
00711   t_cam_ = Eigen::Vector3d    (0., 0., 0.);
00712 }
00713 
00715 
00716 void
00717 pcl::ihs::OpenGLViewer::toggleMeshRepresentation ()
00718 {
00719   switch (mesh_representation_)
00720   {
00721     case MR_POINTS: this->setMeshRepresentation (MR_EDGES);  break;
00722     case MR_EDGES:  this->setMeshRepresentation (MR_FACES);  break;
00723     case MR_FACES:  this->setMeshRepresentation (MR_POINTS); break;
00724     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE);
00725   }
00726 }
00727 
00729 
00730 void
00731 pcl::ihs::OpenGLViewer::setMeshRepresentation (const MeshRepresentation& representation)
00732 {
00733   boost::mutex::scoped_lock lock (mutex_vis_);
00734 
00735   switch (mesh_representation_)
00736   {
00737     case MR_POINTS: std::cerr << "Drawing the points.\n";   break;
00738     case MR_EDGES:  std::cerr << "Drawing the edges.\n";  break;
00739     case MR_FACES:  std::cerr << "Drawing the faces.\n"; break;
00740     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE);
00741   }
00742 
00743   mesh_representation_ = representation;
00744 }
00745 
00747 
00748 void
00749 pcl::ihs::OpenGLViewer::toggleColoring ()
00750 {
00751   switch (coloring_)
00752   {
00753     case COL_RGB:       this->setColoring (COL_ONE_COLOR); break;
00754     case COL_ONE_COLOR: this->setColoring (COL_VISCONF);   break;
00755     case COL_VISCONF:   this->setColoring (COL_RGB);       break;
00756     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE);
00757   }
00758 }
00759 
00761 
00762 void
00763 pcl::ihs::OpenGLViewer::setColoring (const Coloring& coloring)
00764 {
00765   boost::mutex::scoped_lock lock (mutex_vis_);
00766 
00767   switch (coloring)
00768   {
00769     case COL_RGB:       std::cerr << "Coloring according to the rgb values.\n";            break;
00770     case COL_ONE_COLOR: std::cerr << "Use one color for all points.\n";                    break;
00771     case COL_VISCONF:   std::cerr << "Coloring according to the visibility confidence.\n"; break;
00772     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE);
00773   }
00774   coloring_ = coloring;
00775 }
00776 
00778 
00779 void
00780 pcl::ihs::OpenGLViewer::paintEvent (QPaintEvent* /*event*/)
00781 {
00782   this->calcPivot ();
00783   this->makeCurrent ();
00784 
00785   // Clear information from the last draw
00786   glClearColor (0.f, 0.f, 0.f, 0.f);
00787   glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00788 
00789   this->setupViewport (this->width (), this->height ());
00790 
00791   // Move light with camera (see example 5-7)
00792   // http://www.glprogramming.com/red/chapter05.html
00793   glMatrixMode(GL_MODELVIEW);
00794   glLoadIdentity();
00795 
00796   // light 0 (directional)
00797   glLightfv (GL_LIGHT0, GL_AMBIENT , Eigen::Vector4f (0.1f, 0.1f, 0.1f, 1.0f).eval ().data ());
00798   glLightfv (GL_LIGHT0, GL_DIFFUSE , Eigen::Vector4f (0.6f, 0.6f, 0.6f, 1.0f).eval ().data () );
00799   glLightfv (GL_LIGHT0, GL_SPECULAR, Eigen::Vector4f (0.2f, 0.2f, 0.2f, 1.0f).eval ().data ());
00800   glLightfv (GL_LIGHT0, GL_POSITION, Eigen::Vector4f (0.3f, 0.5f, 0.8f, 0.0f).normalized ().eval ().data ());
00801 
00802   // light 1 (directional)
00803   glLightfv (GL_LIGHT1, GL_AMBIENT , Eigen::Vector4f ( 0.0f, 0.0f, 0.0f, 1.0f).eval ().data ());
00804   glLightfv (GL_LIGHT1, GL_DIFFUSE , Eigen::Vector4f ( 0.3f, 0.3f, 0.3f, 1.0f).eval ().data () );
00805   glLightfv (GL_LIGHT1, GL_SPECULAR, Eigen::Vector4f ( 0.1f, 0.1f, 0.1f, 1.0f).eval ().data ());
00806   glLightfv (GL_LIGHT1, GL_POSITION, Eigen::Vector4f (-0.3f, 0.5f, 0.8f, 0.0f).normalized ().eval ().data ());
00807 
00808   // Material
00809   glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
00810   glMaterialfv    (GL_FRONT, GL_SPECULAR , Eigen::Vector4f (0.1f, 0.1f, 0.1f, 1.0f).eval ().data ());
00811   glMaterialf     (GL_FRONT, GL_SHININESS, 25.f);
00812 
00813   glEnable (GL_DEPTH_TEST);
00814   glEnable (GL_NORMALIZE);
00815   glEnable (GL_COLOR_MATERIAL);
00816   glEnable (GL_LIGHTING);
00817   glEnable (GL_LIGHT0);
00818   glEnable (GL_LIGHT1);
00819 
00820   // Projection matrix
00821   glMatrixMode   (GL_PROJECTION);
00822   glLoadIdentity ();
00823   gluPerspective (43., 4./3., 0.01 / scaling_factor_, 10. / scaling_factor_);
00824   glMatrixMode   (GL_MODELVIEW);
00825 
00826   // ModelView matrix
00827   Eigen::Quaterniond R_cam;
00828   Eigen::Vector3d    t_cam;
00829   {
00830     boost::mutex::scoped_lock lock (mutex_vis_);
00831     R_cam = R_cam_;
00832     t_cam = t_cam_;
00833   }
00834 
00835   const Eigen::Vector3d o  = Eigen::Vector3d::Zero  ();
00836   const Eigen::Vector3d ey = Eigen::Vector3d::UnitY ();
00837   const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ ();
00838 
00839   const Eigen::Vector3d eye    =  R_cam * o  + t_cam;
00840   const Eigen::Vector3d center =  R_cam * ez + t_cam;
00841   const Eigen::Vector3d up     = (R_cam * -ey).normalized ();
00842 
00843   glMatrixMode (GL_MODELVIEW);
00844   gluLookAt (eye.   x (), eye.   y (), eye.   z (),
00845              center.x (), center.y (), center.z (),
00846              up.    x (), up.    y (), up.    z ());
00847 
00848   // Draw everything
00849   this->drawMeshes ();
00850 
00851   glDisable (GL_LIGHTING); // This is needed so the color is right.
00852   this->drawBox ();
00853 }
00854 
00856 
00857 bool
00858 pcl::ihs::OpenGLViewer::getMeshIsAdded (const std::string& id)
00859 {
00860   // boost::mutex::scoped_lock lock (mutex_vis_);
00861   return (drawn_meshes_.find (id) != drawn_meshes_.end ());
00862 }
00863 
00865 
00866 void
00867 pcl::ihs::OpenGLViewer::calcPivot ()
00868 {
00869   boost::mutex::scoped_lock lock (mutex_vis_);
00870   if (this->getMeshIsAdded (cam_pivot_id_))
00871   {
00872     Eigen::Vector4f pivot;
00873     const FaceVertexMeshConstPtr mesh = drawn_meshes_ [cam_pivot_id_];
00874 
00875     if (pcl::compute3DCentroid (mesh->vertices, pivot))
00876     {
00877       const Eigen::Vector3d p = mesh->transformation * pivot.head <3> ().cast <double> ();
00878       lock.unlock ();
00879       this->setPivot (p);
00880     }
00881   }
00882   cam_pivot_id_.clear ();
00883 }
00884 
00886 
00887 void
00888 pcl::ihs::OpenGLViewer::drawMeshes ()
00889 {
00890   boost::mutex::scoped_lock lock (mutex_vis_);
00891 
00892   glEnableClientState (GL_VERTEX_ARRAY);
00893   glEnableClientState (GL_NORMAL_ARRAY);
00894   switch (coloring_)
00895   {
00896     case COL_RGB:       glEnableClientState  (GL_COLOR_ARRAY); break;
00897     case COL_ONE_COLOR: glDisableClientState (GL_COLOR_ARRAY); break;
00898     case COL_VISCONF:   glEnableClientState  (GL_COLOR_ARRAY); break;
00899     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE);
00900   }
00901   switch (mesh_representation_)
00902   {
00903     case MR_POINTS:                                             break;
00904     case MR_EDGES:  glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); break;
00905     case MR_FACES:  glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); break;
00906     default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE);
00907   }
00908 
00909   for (FaceVertexMeshMap::const_iterator it=drawn_meshes_.begin (); it!=drawn_meshes_.end (); ++it)
00910   {
00911     if (it->second && !it->second->vertices.empty ())
00912     {
00913       const FaceVertexMesh& mesh = *it->second;
00914 
00915       glVertexPointer (3, GL_FLOAT, sizeof (PointIHS), &(mesh.vertices [0].x       ));
00916       glNormalPointer (   GL_FLOAT, sizeof (PointIHS), &(mesh.vertices [0].normal_x));
00917 
00918       Colors colors (3, mesh.vertices.size ());
00919 
00920       switch (coloring_)
00921       {
00922         case COL_RGB:
00923         {
00924           glColorPointer (3, GL_UNSIGNED_BYTE, sizeof (PointIHS), &(mesh.vertices [0].b));
00925           break;
00926         }
00927         case COL_ONE_COLOR:
00928         {
00929           glColor3f (.7f, .7f, .7f);
00930           break;
00931         }
00932         case COL_VISCONF:
00933         {
00934           for (unsigned int i=0; i<mesh.vertices.size (); ++i)
00935           {
00936             const unsigned int n = pcl::ihs::countDirections (mesh.vertices [i].directions);
00937             const unsigned int index = static_cast <unsigned int> (
00938                                          static_cast <float> (colormap_.cols ()) *
00939                                          static_cast <float> (n) / vis_conf_norm_);
00940 
00941             colors.col (i) = colormap_.col (index < 256 ? index : 255);
00942           }
00943 
00944           glColorPointer (3, GL_UNSIGNED_BYTE, 0, colors.data ());
00945         }
00946       }
00947 
00948       glPushMatrix ();
00949       {
00950         glMultMatrixd (mesh.transformation.matrix ().data ());
00951 
00952         switch (mesh_representation_)
00953         {
00954           case MR_POINTS:
00955           {
00956             glDrawArrays (GL_POINTS, 0, mesh.vertices.size ());
00957             break;
00958           }
00959           case MR_EDGES: case MR_FACES:
00960           {
00961             glDrawElements (GL_TRIANGLES, 3*mesh.triangles.size (), GL_UNSIGNED_INT, &mesh.triangles [0]);
00962             break;
00963           }
00964         }
00965       }
00966       glPopMatrix ();
00967     }
00968   }
00969 
00970   glDisableClientState (GL_VERTEX_ARRAY);
00971   glDisableClientState (GL_NORMAL_ARRAY);
00972   glDisableClientState (GL_COLOR_ARRAY);
00973   glPolygonMode (GL_FRONT_AND_BACK, GL_FILL);
00974 }
00975 
00977 
00978 void
00979 pcl::ihs::OpenGLViewer::drawBox ()
00980 {
00981   BoxCoefficients coeffs;
00982   {
00983     boost::mutex::scoped_lock lock (mutex_vis_);
00984     if (draw_box_) coeffs = box_coefficients_;
00985     else           return;
00986   }
00987 
00988   glColor3f (1.f, 1.f, 1.f);
00989 
00990   glPushMatrix ();
00991   {
00992     glMultMatrixd (coeffs.transformation.matrix ().data ());
00993 
00994     // Front
00995     glBegin(GL_LINE_STRIP);
00996     {
00997       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min);
00998       glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_min);
00999       glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_min);
01000       glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_min);
01001       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min);
01002     }
01003     glEnd();
01004 
01005     // Back
01006     glBegin (GL_LINE_STRIP);
01007     {
01008       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max);
01009       glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_max);
01010       glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_max);
01011       glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_max);
01012       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max);
01013     }
01014     glEnd();
01015 
01016     // Sides
01017     glBegin (GL_LINES);
01018     {
01019       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min);
01020       glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max);
01021 
01022       glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_min);
01023       glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_max);
01024 
01025       glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_min);
01026       glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_max);
01027 
01028       glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_min);
01029       glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_max);
01030     }
01031     glEnd();
01032   }
01033   glPopMatrix ();
01034 }
01035 
01037 
01038 void
01039 pcl::ihs::OpenGLViewer::initializeGL ()
01040 {
01041 }
01042 
01044 
01045 void
01046 pcl::ihs::OpenGLViewer::setupViewport (const int w, const int h)
01047 {
01048   const float aspect_ratio = 4./3.;
01049 
01050   // Use the biggest possible area of the window to draw to
01051   //    case 1 (w < w_scaled):        case 2 (w >= w_scaled):
01052   //      w
01053   //    |---|         ^               |-------------|  ^
01054   //    |---| ^       |               |    |   |    |  | h
01055   //    |   | | h_sc  | h             |-------------|  v
01056   //    |---| v       |                    <---> w_sc
01057   //    |---|         v               <----- w ----->
01058   const float w_scaled = h * aspect_ratio;
01059   const float h_scaled = w / aspect_ratio;
01060 
01061   if (w < w_scaled)
01062   {
01063     glViewport (0, static_cast <GLint> ((h - h_scaled) / 2.f), w, static_cast <GLsizei> (h_scaled));
01064   }
01065   else
01066   {
01067     glViewport (static_cast <GLint> ((w - w_scaled) / 2.f), 0, static_cast <GLsizei> (w_scaled), h);
01068   }
01069 }
01070 
01072 
01073 void
01074 pcl::ihs::OpenGLViewer::resizeGL (int w, int h)
01075 {
01076   this->setupViewport (w, h);
01077 }
01078 
01080 
01081 void
01082 pcl::ihs::OpenGLViewer::mousePressEvent (QMouseEvent* /*event*/)
01083 {
01084   boost::mutex::scoped_lock lock (mutex_vis_);
01085   mouse_pressed_begin_ = true;
01086 }
01087 
01089 
01090 void
01091 pcl::ihs::OpenGLViewer::mouseMoveEvent (QMouseEvent* event)
01092 {
01093   boost::mutex::scoped_lock lock (mutex_vis_);
01094 
01095   if (mouse_pressed_begin_)
01096   {
01097     x_prev_ = event->pos ().x ();
01098     y_prev_ = event->pos ().y ();
01099     mouse_pressed_begin_ = false;
01100     return;
01101   }
01102   if (event->pos ().x () == x_prev_ && event->pos ().y () == y_prev_) return;
01103   if (this->width () == 0 || this->height () == 0)                    return;
01104 
01105   const double dx = static_cast <double> (event->pos ().x ()) - static_cast <double> (x_prev_);
01106   const double dy = static_cast <double> (event->pos ().y ()) - static_cast <double> (y_prev_);
01107   const double w  = static_cast <double> (this->width ());
01108   const double h  = static_cast <double> (this->height ());
01109   const double d  = std::sqrt (w*w + h*h);
01110 
01111   const Eigen::Vector3d o  = Eigen::Vector3d::Zero  ();
01112   const Eigen::Vector3d ex = Eigen::Vector3d::UnitX ();
01113   const Eigen::Vector3d ey = Eigen::Vector3d::UnitY ();
01114   const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ ();
01115 
01116   // Scale with the distance between the pivot and camera eye.
01117   const double scale = std::max ((cam_pivot_ - R_cam_ * o - t_cam_).norm (), .1 / scaling_factor_) / d;
01118 
01119   if (QApplication::mouseButtons () == Qt::LeftButton)
01120   {
01121     const double          rot_angle = -7. * std::atan (std::sqrt ((dx*dx + dy*dy)) / d);
01122     const Eigen::Vector3d rot_axis  = (R_cam_ * ex * dy - R_cam_ * ey * dx).normalized ();
01123 
01124     const Eigen::Quaterniond dR (Eigen::AngleAxisd (rot_angle, rot_axis));
01125     t_cam_ = dR * (t_cam_ - cam_pivot_) + cam_pivot_;
01126     R_cam_ = (dR * R_cam_).normalized ();
01127   }
01128   else if (QApplication::mouseButtons () == Qt::MiddleButton)
01129   {
01130     t_cam_ += 1.3 * scale * Eigen::Vector3d (R_cam_ * (ey * -dy + ex * -dx));
01131   }
01132   else if (QApplication::mouseButtons () == Qt::RightButton)
01133   {
01134     t_cam_ += 2.6 * scale * Eigen::Vector3d (R_cam_ * (ez * -dy));
01135   }
01136 
01137   x_prev_ = event->pos ().x ();
01138   y_prev_ = event->pos ().y ();
01139 }
01140 
01142 
01143 void
01144 pcl::ihs::OpenGLViewer::wheelEvent (QWheelEvent* event)
01145 {
01146   if (QApplication::mouseButtons () == Qt::NoButton)
01147   {
01148     boost::mutex::scoped_lock lock (mutex_vis_);
01149 
01150     // Scale with the distance between the pivot and camera eye.
01151     const Eigen::Vector3d o     = Eigen::Vector3d::Zero  ();
01152     const Eigen::Vector3d ez    = Eigen::Vector3d::UnitZ ();
01153     const double          w     = static_cast <double> (this->width ());
01154     const double          h     = static_cast <double> (this->height ());
01155     const double          d     = std::sqrt (w*w + h*h);
01156     const double          scale = std::max ((cam_pivot_ - R_cam_ * o - t_cam_).norm (), .1 / scaling_factor_) / d;
01157 
01158     // http://doc.qt.digia.com/qt/qwheelevent.html#delta
01159     t_cam_ += scale * Eigen::Vector3d (R_cam_ * (ez * static_cast <double> (event->delta ())));
01160   }
01161 }
01162 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:20