common.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) 2010, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #include <pcl/point_types.h>
00039 #include <pcl/visualization/common/common.h>
00040 #include <stdlib.h>
00041 
00043 void
00044 pcl::visualization::getRandomColors (double &r, double &g, double &b, double min, double max)
00045 {
00046   double sum;
00047   static unsigned stepRGBA = 100;
00048   do
00049   {
00050     sum = 0;
00051     r = (rand () % stepRGBA) / static_cast<double> (stepRGBA);
00052     while ((g = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) {}
00053     while (((b = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) && (b == g)) {}
00054     sum = r + g + b;
00055   }
00056   while (sum <= min || sum >= max);
00057 }
00058 
00060 void
00061 pcl::visualization::getRandomColors (pcl::RGB &rgb, double min, double max)
00062 {
00063   double sum;
00064   static unsigned stepRGBA = 100;
00065   double r, g, b;
00066   do
00067   {
00068     sum = 0;
00069     r = (rand () % stepRGBA) / static_cast<double> (stepRGBA);
00070     while ((g = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) {}
00071     while (((b = (rand () % stepRGBA) / static_cast<double> (stepRGBA)) == r) && (b == g)) {}
00072     sum = r + g + b;
00073   }
00074   while (sum <= min || sum >= max);
00075   rgb.r = uint8_t (r * 255.0);
00076   rgb.g = uint8_t (g * 255.0);
00077   rgb.b = uint8_t (b * 255.0);
00078 }
00079 
00081 
00082 Eigen::Matrix4d
00083 pcl::visualization::vtkToEigen (vtkMatrix4x4* vtk_matrix)
00084 {
00085   Eigen::Matrix4d eigen_matrix = Eigen::Matrix4d::Identity ();
00086   for (int i=0; i < 4; i++)
00087   {
00088     for (int j=0; j < 4; j++)
00089     {
00090       // VTK
00091       eigen_matrix (i, j) = vtk_matrix->GetElement (i, j);
00092     }
00093   }
00094   return eigen_matrix;
00095 }
00096 
00098 Eigen::Vector2i
00099 pcl::visualization::worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
00100 {
00101   // Transform world to clipping coordinates
00102   Eigen::Vector4d world (view_projection_matrix * world_pt);
00103   // Normalize w-component
00104   world /= world.w ();
00105 
00106   // X/Y screen space coordinate
00107   int screen_x = int (floor (double (((world.x () + 1) / 2.0) * width) + 0.5));
00108   int screen_y = int (floor (double (((world.y () + 1) / 2.0) * height) + 0.5));
00109 
00110   // Calculate -world_pt.y () because the screen Y axis is oriented top->down, ie 0 is top-left
00111   //int winY = (int) floor ( (double) (((1 - world_pt.y ()) / 2.0) * height) + 0.5); // top left
00112 
00113   return (Eigen::Vector2i (screen_x, screen_y));
00114 }
00115 
00117 void
00118 pcl::visualization::getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24])
00119 {
00120   // Set up the normals
00121   Eigen::Vector4d normals[6];
00122   for (int i=0; i < 6; i++)
00123   {
00124     normals[i] = Eigen::Vector4d (0.0, 0.0, 0.0, 1.0);
00125 
00126     // if i is even set to -1, if odd set to +1
00127     normals[i] (i/2) = 1 - (i%2)*2;
00128   }
00129 
00130   // Transpose the matrix for use with normals
00131   Eigen::Matrix4d view_matrix = view_projection_matrix.transpose ();
00132 
00133   // Transform the normals to world coordinates
00134   for (int i=0; i < 6; i++)
00135   {
00136     normals[i] = view_matrix * normals[i];
00137 
00138     double f = 1.0/sqrt (normals[i].x () * normals[i].x () +
00139                          normals[i].y () * normals[i].y () +
00140                          normals[i].z () * normals[i].z ());
00141 
00142     planes[4*i + 0] = normals[i].x ()*f;
00143     planes[4*i + 1] = normals[i].y ()*f;
00144     planes[4*i + 2] = normals[i].z ()*f;
00145     planes[4*i + 3] = normals[i].w ()*f;
00146   }
00147 }
00148 
00149 int
00150 pcl::visualization::cullFrustum (double frustum[24], const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb)
00151 {
00152   int result = PCL_INSIDE_FRUSTUM;
00153 
00154   for(int i =0; i < 6; i++){
00155     double a = frustum[(i*4)];
00156     double b = frustum[(i*4)+1];
00157     double c = frustum[(i*4)+2];
00158     double d = frustum[(i*4)+3];
00159 
00160     //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl;
00161 
00162     //  Basic VFC algorithm
00163     Eigen::Vector3d center ((max_bb.x () - min_bb.x ()) / 2 + min_bb.x (),
00164                             (max_bb.y () - min_bb.y ()) / 2 + min_bb.y (),
00165                             (max_bb.z () - min_bb.z ()) / 2 + min_bb.z ());
00166 
00167     Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
00168                             fabs (static_cast<double> (max_bb.y () - center.y ())),
00169                             fabs (static_cast<double> (max_bb.z () - center.z ())));
00170 
00171     double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
00172     double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
00173 
00174     if (m + n < 0){
00175       result = PCL_OUTSIDE_FRUSTUM;
00176       break;
00177     }
00178 
00179     if (m - n < 0)
00180     {
00181       result = PCL_INTERSECT_FRUSTUM;
00182     }
00183   }
00184 
00185   return result;
00186 }
00187 
00188 //void
00189 //pcl::visualization::getModelViewPosition (Eigen::Matrix4d model_view_matrix, Eigen::Vector3d &position)
00190 //{
00191 //  //Compute eye or position from model view matrix
00192 //  Eigen::Matrix4d inverse_model_view_matrix = model_view_matrix.inverse();
00193 //  for (int i=0; i < 3; i++)
00194 //  {
00195 //    position(i) = inverse_model_view_matrix(i, 3);
00196 //  }
00197 //}
00198 
00199 // Lookup table of max 6 bounding box vertices, followed by number of vertices, ie {v0, v1, v2, v3, v4, v5, nv}
00200 //
00201 //      3--------2
00202 //     /|       /|       Y      0 = xmin, ymin, zmin
00203 //    / |      / |       |      6 = xmax, ymax. zmax
00204 //   7--------6  |       |
00205 //   |  |     |  |       |
00206 //   |  0-----|--1       +------X
00207 //   | /      | /       /
00208 //   |/       |/       /
00209 //   4--------5       Z
00210 
00211 int hull_vertex_table[43][7] = {
00212   { 0, 0, 0, 0, 0, 0, 0 }, // inside
00213   { 0, 4, 7, 3, 0, 0, 4 }, // left
00214   { 1, 2, 6, 5, 0, 0, 4 }, // right
00215   { 0, 0, 0, 0, 0, 0, 0 },
00216   { 0, 1, 5, 4, 0, 0, 4 }, // bottom
00217   { 0, 1, 5, 4, 7, 3, 6 }, // bottom, left
00218   { 0, 1, 2, 6, 5, 4, 6 }, // bottom, right
00219   { 0, 0, 0, 0, 0, 0, 0 },
00220   { 2, 3, 7, 6, 0, 0, 4 }, // top
00221   { 4, 7, 6, 2, 3, 0, 6 }, // top, left
00222   { 2, 3, 7, 6, 5, 1, 6 }, // top, right
00223   { 0, 0, 0, 0, 0, 0, 0 },
00224   { 0, 0, 0, 0, 0, 0, 0 },
00225   { 0, 0, 0, 0, 0, 0, 0 },
00226   { 0, 0, 0, 0, 0, 0, 0 },
00227   { 0, 0, 0, 0, 0, 0, 0 },
00228   { 0, 3, 2, 1, 0, 0, 4 }, // front
00229   { 0, 4, 7, 3, 2, 1, 6 }, // front, left
00230   { 0, 3, 2, 6, 5, 1, 6 }, // front, right
00231   { 0, 0, 0, 0, 0, 0, 0 },
00232   { 0, 3, 2, 1, 5, 4, 6 }, // front, bottom
00233   { 2, 1, 5, 4, 7, 3, 6 }, // front, bottom, left
00234   { 0, 3, 2, 6, 5, 4, 6 },
00235   { 0, 0, 0, 0, 0, 0, 0 },
00236   { 0, 3, 7, 6, 2, 1, 6 }, // front, top
00237   { 0, 4, 7, 6, 2, 1, 6 }, // front, top, left
00238   { 0, 3, 7, 6, 5, 1, 6 }, // front, top, right
00239   { 0, 0, 0, 0, 0, 0, 0 },
00240   { 0, 0, 0, 0, 0, 0, 0 },
00241   { 0, 0, 0, 0, 0, 0, 0 },
00242   { 0, 0, 0, 0, 0, 0, 0 },
00243   { 0, 0, 0, 0, 0, 0, 0 },
00244   { 4, 5, 6, 7, 0, 0, 4 }, // back
00245   { 4, 5, 6, 7, 3, 0, 6 }, // back, left
00246   { 1, 2, 6, 7, 4, 5, 6 }, // back, right
00247   { 0, 0, 0, 0, 0, 0, 0 },
00248   { 0, 1, 5, 6, 7, 4, 6 }, // back, bottom
00249   { 0, 1, 5, 6, 7, 3, 6 }, // back, bottom, left
00250   { 0, 1, 2, 6, 7, 4, 6 }, // back, bottom, right
00251   { 0, 0, 0, 0, 0, 0, 0 },
00252   { 2, 3, 7, 4, 5, 6, 6 }, // back, top
00253   { 0, 4, 5, 6, 2, 3, 6 }, // back, top, left
00254   { 1, 2, 3, 7, 4, 5, 6 }  // back, top, right
00255 };
00256 
00258 float
00259 pcl::visualization::viewScreenArea (
00260     const Eigen::Vector3d &eye, 
00261     const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, 
00262     const Eigen::Matrix4d &view_projection_matrix, int width, int height)
00263 {
00264   Eigen::Vector4d bounding_box[8];
00265   bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
00266   bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
00267   bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
00268   bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
00269   bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
00270   bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
00271   bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
00272   bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
00273 
00274   // Compute 6-bit code to classify eye with respect to the 6 defining planes
00275   int pos = ((eye.x () < bounding_box[0].x ()) )  // 1 = left
00276       + ((eye.x () > bounding_box[6].x ()) << 1)  // 2 = right
00277       + ((eye.y () < bounding_box[0].y ()) << 2)  // 4 = bottom
00278       + ((eye.y () > bounding_box[6].y ()) << 3)  // 8 = top
00279       + ((eye.z () < bounding_box[0].z ()) << 4)  // 16 = front
00280       + ((eye.z () > bounding_box[6].z ()) << 5); // 32 = back
00281 
00282   // Look up number of vertices
00283   int num = hull_vertex_table[pos][6];
00284   if (num == 0)
00285   {
00286     return (float (width * height));
00287   }
00288     //return 0.0;
00289 
00290 
00291 //  cout << "eye: " << eye.x() << " " << eye.y() << " " << eye.z() << endl;
00292 //  cout << "min: " << bounding_box[0].x() << " " << bounding_box[0].y() << " " << bounding_box[0].z() << endl;
00293 //
00294 //  cout << "pos: " << pos << " ";
00295 //  switch(pos){
00296 //    case 0:  cout << "inside" << endl; break;
00297 //    case 1:  cout << "left" << endl; break;
00298 //    case 2:  cout << "right" << endl; break;
00299 //    case 3:
00300 //    case 4:  cout << "bottom" << endl; break;
00301 //    case 5:  cout << "bottom, left" << endl; break;
00302 //    case 6:  cout << "bottom, right" << endl; break;
00303 //    case 7:
00304 //    case 8:  cout << "top" << endl; break;
00305 //    case 9:  cout << "top, left" << endl; break;
00306 //    case 10:  cout << "top, right" << endl; break;
00307 //    case 11:
00308 //    case 12:
00309 //    case 13:
00310 //    case 14:
00311 //    case 15:
00312 //    case 16:  cout << "front" << endl; break;
00313 //    case 17:  cout << "front, left" << endl; break;
00314 //    case 18:  cout << "front, right" << endl; break;
00315 //    case 19:
00316 //    case 20:  cout << "front, bottom" << endl; break;
00317 //    case 21:  cout << "front, bottom, left" << endl; break;
00318 //    case 22:
00319 //    case 23:
00320 //    case 24:  cout << "front, top" << endl; break;
00321 //    case 25:  cout << "front, top, left" << endl; break;
00322 //    case 26:  cout << "front, top, right" << endl; break;
00323 //    case 27:
00324 //    case 28:
00325 //    case 29:
00326 //    case 30:
00327 //    case 31:
00328 //    case 32:  cout << "back" << endl; break;
00329 //    case 33:  cout << "back, left" << endl; break;
00330 //    case 34:  cout << "back, right" << endl; break;
00331 //    case 35:
00332 //    case 36:  cout << "back, bottom" << endl; break;
00333 //    case 37:  cout << "back, bottom, left" << endl; break;
00334 //    case 38:  cout << "back, bottom, right" << endl; break;
00335 //    case 39:
00336 //    case 40:  cout << "back, top" << endl; break;
00337 //    case 41:  cout << "back, top, left" << endl; break;
00338 //    case 42:  cout << "back, top, right" << endl; break;
00339 //  }
00340 
00341   //return -1 if inside
00342   Eigen::Vector2d dst[8];
00343   for (int i = 0; i < num; i++)
00344   {
00345     Eigen::Vector4d world_pt = bounding_box[hull_vertex_table[pos][i]];
00346     Eigen::Vector2i screen_pt = pcl::visualization::worldToView(world_pt, view_projection_matrix, width, height);
00347 //    cout << "point[" << i << "]: " << screen_pt.x() << " " << screen_pt.y() << endl;
00348     dst[i] = Eigen::Vector2d(screen_pt.x (), screen_pt.y ());
00349   }
00350 
00351   double sum = 0.0;
00352   for (int i = 0; i < num; ++i)
00353   {
00354     sum += (dst[i].x () - dst[(i+1) % num].x ()) * (dst[i].y () + dst[(i+1) % num].y ());
00355   }
00356 
00357   return (fabsf (float (sum * 0.5f)));
00358 }
00359 
00361 void
00362 pcl::visualization::Camera::computeViewMatrix (Eigen::Matrix4d &view_mat) const
00363 {
00364 //constructs view matrix from camera pos, view up, and the point it is looking at
00365 //this code is based off of gluLookAt http://www.opengl.org/wiki/GluLookAt_code
00366         Eigen::Vector3d focal_point (focal[0], focal[1], focal[2]);
00367         Eigen::Vector3d posv        (pos[0]  , pos[1]  , pos[2]);
00368         Eigen::Vector3d up          (view[0] , view[1] , view[2]);
00369 
00370         Eigen::Vector3d zAxis = (focal_point - posv).normalized();
00371   Eigen::Vector3d xAxis = zAxis.cross(up).normalized();
00372   // make sure the y-axis is orthogonal to the other two
00373   Eigen::Vector3d yAxis = xAxis.cross (zAxis);
00374 
00375         view_mat.block <1, 3> (0, 0) = xAxis;
00376         view_mat.block <1, 3> (1, 0) = yAxis;
00377         view_mat.block <1, 3> (2, 0) = -zAxis;
00378         view_mat.row (3) << 0, 0, 0, 1;
00379 
00380         view_mat.block <3, 1> (0, 3) = view_mat.topLeftCorner<3, 3> () * (-posv);
00381 }
00382 
00384 void
00385 pcl::visualization::Camera::computeProjectionMatrix (Eigen::Matrix4d& proj) const
00386 {
00387   float top    = static_cast<float> (clip[0]) * tanf (0.5f * static_cast<float> (fovy));
00388   float left   = -top * static_cast<float> (window_size[0] / window_size[1]);
00389   float right  = -left;
00390   float bottom = -top;
00391 
00392   float temp1, temp2, temp3, temp4;
00393         temp1 = 2.0f * static_cast<float> (clip[0]);
00394         temp2 = 1.0f / (right - left);
00395         temp3 = 1.0f / (top - bottom);
00396         temp4 = 1.0f / static_cast<float> (clip[1] - clip[0]);
00397 
00398   proj.setZero ();
00399 
00400         proj(0,0) = temp1 * temp2;
00401         proj(1,1) = temp1 * temp3;
00402         proj(0,2) = (right + left) * temp2;
00403         proj(1,2) = (top + bottom) * temp3;
00404         proj(2,2) = (-clip[1] - clip[0]) * temp4;
00405         proj(3,2) = -1.0;
00406         proj(2,3) = (-temp1 * clip[1]) * temp4;
00407 }


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