CMyEllipsoid.cpp
Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                       http://www.mrpt.org/                                |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2012  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 
00029 #include <mrpt/opengl.h>  // Precompiled header
00030 
00031 
00032 #include "ndt_visualisation/CMyEllipsoid.h"
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/geometry.h>
00035 #include <mrpt/math/ops_matrices.h>
00036 
00037 #include "ndt_visualisation/opengl_internals.h"
00038 
00039 using namespace mrpt;
00040 using namespace mrpt::opengl;
00041 using namespace mrpt::utils;
00042 using namespace mrpt::math;
00043 using namespace std;
00044 
00045 IMPLEMENTS_SERIALIZABLE( CMyEllipsoid, CRenderizableDisplayList, mrpt::opengl )
00046 
00047 /*---------------------------------------------------------------
00048                                                         render
00049   ---------------------------------------------------------------*/
00050 void   CMyEllipsoid::render_dl() const
00051 {
00052 #if MRPT_HAS_OPENGL_GLUT
00053         MRPT_START
00054 
00055     const size_t dim = m_cov.getColCount();
00056 
00057         if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0)
00058         {
00059                 glEnable(GL_BLEND);
00060                 checkOpenGLError();
00061                 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00062                 checkOpenGLError();
00063                 glLineWidth(m_lineWidth);
00064                 checkOpenGLError();
00065 
00066                 if (dim==2)
00067                 {
00068                         // ---------------------
00069                         //     2D ellipse
00070                         // ---------------------
00071 
00072                         /* Equivalent MATLAB code: 
00073                          *
00074                          * q=1;
00075                          * [vec val]=eig(C); 
00076                          * M=(q*val*vec)';  
00077                          * R=M*[x;y];
00078                          * xx=R(1,:);yy=R(2,:);  
00079                          * plot(xx,yy), axis equal;
00080                          */
00081 
00082                         double                  ang;
00083                         unsigned int    i;
00084 
00085                         // Compute the new vectors for the ellipsoid:
00086                         CMatrixDouble   M;
00087                         M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();
00088 
00089                         glBegin( GL_LINE_LOOP );
00090 
00091                         // Compute the points of the 2D ellipse:
00092                         for (i=0,ang=0;i<m_2D_segments;i++,ang+= (M_2PI/m_2D_segments))
00093                         {
00094                                 double ccos = cos(ang);
00095                                 double ssin = sin(ang);
00096 
00097                                 const float x = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0);
00098                                 const float y = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1);
00099 
00100                                 glVertex2f( x,y );
00101                         } // end for points on ellipse
00102 
00103                         glEnd();
00104                 }
00105                 else
00106                 {
00107                         // ---------------------
00108                         //    3D ellipsoid
00109                         // ---------------------
00110                         GLfloat         mat[16];
00111 
00112                         //  A homogeneous transformation matrix, in this order:
00113                         //
00114                         //     0  4  8  12
00115                         //     1  5  9  13
00116                         //     2  6  10 14
00117                         //     3  7  11 15
00118                         //
00119                         mat[3] = mat[7] = mat[11] = 0;
00120                         mat[15] = 1;
00121                         mat[12] = mat[13] = mat[14] = 0;
00122 
00123                         mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0); // New X-axis
00124                         mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1); // New X-axis
00125                         mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2);        // New X-axis
00126 
00127                         glDisable(GL_LIGHTING);
00128                         //glEnable(GL_LIGHTING);
00129                         //glEnable(GL_LIGHT0);
00130                         glEnable(GL_COLOR_MATERIAL);
00131                         glShadeModel(GL_SMOOTH);
00132 
00133                         GLUquadricObj   *obj = gluNewQuadric();
00134                         checkOpenGLError();
00135 
00136                         gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);
00137 
00138                         glPushMatrix();
00139                         glMultMatrixf( mat );
00140                         glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles);
00141 
00142                         gluSphere( obj, 1,m_3D_segments,m_3D_segments);
00143                         checkOpenGLError();
00144 
00145                         glPopMatrix();
00146 
00147                         gluDeleteQuadric(obj);
00148                         checkOpenGLError();
00149 
00150                         glDisable(GL_LIGHTING);
00151                         glDisable(GL_LIGHT0);
00152 
00153 
00154                 }
00155 
00156 
00157                 glLineWidth(1.0f);
00158                 glDisable(GL_BLEND);
00159         }
00160         MRPT_END_WITH_CLEAN_UP( \
00161                 cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \
00162         );
00163 #endif
00164 }
00165 
00166 /*---------------------------------------------------------------
00167    Implements the writing to a CStream capability of
00168      CSerializable objects
00169   ---------------------------------------------------------------*/
00170 void  CMyEllipsoid::writeToStream(CStream &out,int *version) const
00171 {
00172         if (version)
00173                 *version = 1;
00174         else
00175         {
00176                 writeToStreamRender(out);
00177                 out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments << (uint32_t)m_3D_segments << m_lineWidth;
00178         }
00179 }
00180 
00181 /*---------------------------------------------------------------
00182         Implements the reading from a CStream capability of
00183                 CSerializable objects
00184   ---------------------------------------------------------------*/
00185 void  CMyEllipsoid::readFromStream(CStream &in,int version)
00186 {
00187         switch(version)
00188         {
00189         case 0:
00190         case 1:
00191                 {
00192                         uint32_t        i;
00193                         readFromStreamRender(in);
00194                         if (version==0)
00195                         {
00196                                 CMatrix c;
00197                                 in >> c; m_cov = c.cast<double>();
00198                         }
00199                         else
00200                         {
00201                                 in >> m_cov;
00202                         }
00203 
00204                         in >> m_drawSolid3D >> m_quantiles;
00205                         in >> i; m_2D_segments = i;
00206                         in >> i; m_3D_segments = i;
00207                         in >> m_lineWidth;
00208 
00209                         // Update cov. matrix cache:
00210                         m_prevComputedCov = m_cov;
00211                         m_cov.eigenVectors(m_eigVec,m_eigVal);
00212                         m_eigVal.Sqrt();
00213 
00214                 } break;
00215         default:
00216                 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
00217 
00218         };
00219 }
00220 
00221 bool quickSolveEqn(double a,double b_2,double c,double &t)      {
00222         double delta=square(b_2)-a*c;
00223         if (delta==0) return (t=-b_2/a)>=0;
00224         else if (delta>0)       {
00225                 delta=sqrt(delta);
00226                 if ((t=(-b_2-delta)/a)>=0) return true;
00227                 else return (t=(-b_2+delta)/a)>=0;
00228         }       else return false;
00229 }
00230 
00231 bool CMyEllipsoid::traceRay(const mrpt::poses::CPose3D &o,double &dist) const   {
00232         if (m_cov.getRowCount()!=3) return false;
00233         TLine3D lin,lin2;
00234         createFromPoseX(o-this->getPose(),lin);
00235         //createFromPoseX(o-this->m_pose,lin);
00236         lin.unitarize();        //By adding this line, distance from any point of the line to its base is exactly equal to the "t".
00237         for (size_t i=0;i<3;i++)        {
00238                 lin2.pBase[i]=0;
00239                 lin2.director[i]=0;
00240                 for (size_t j=0;j<3;j++)        {
00241                         double vji=m_eigVec(j,i);
00242                         lin2.pBase[i]+=vji*lin.pBase[j];
00243                         lin2.director[i]+=vji*lin.director[j];
00244                 }
00245         }
00246         double a=0,b_2=0,c=-square(m_quantiles);
00247         for (size_t i=0;i<3;i++)        {
00248                 double ev=m_eigVal(i,i);
00249                 a+=square(lin2.director[i]/ev);
00250                 b_2+=lin2.director[i]*lin2.pBase[i]/square(ev);
00251                 c+=square(lin2.pBase[i]/ev);
00252         }
00253         return quickSolveEqn(a,b_2,c,dist);
00254 }
00255 
00256 void CMyEllipsoid::setCovMatrix( const mrpt::math::CMatrixDouble &m, int resizeToSize)
00257 {
00258         MRPT_START
00259 
00260         ASSERT_( m.getColCount() == m.getRowCount() );
00261         ASSERT_( size(m,1)==2 || size(m,1)==3 || (resizeToSize>0 && (resizeToSize==2 || resizeToSize==3)));
00262 
00263         m_cov = m;
00264         if (resizeToSize>0 && resizeToSize<(int)size(m,1))
00265                 m_cov.setSize(resizeToSize,resizeToSize);
00266 
00267         if (m_cov==m_prevComputedCov)
00268                 return; // Done.
00269 
00270         CRenderizableDisplayList::notifyChange();
00271 
00272         // Handle the special case of an ellipsoid of volume = 0
00273         const double d=m_cov.det();
00274         if (d==0 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
00275         {
00276                 // All zeros:
00277                 m_prevComputedCov = m_cov;
00278                 m_eigVec.zeros(3,3);
00279                 m_eigVal.zeros(3,3);
00280         }
00281         else
00282         {
00283                 // Not null matrix: compute the eigen-vectors & values:
00284                 m_prevComputedCov = m_cov;
00285                 m_cov.eigenVectors(m_eigVec,m_eigVal);
00286                 m_eigVal.Sqrt();
00287                 // Do the scale at render to avoid recomputing the m_eigVal for different m_quantiles
00288         }
00289 
00290 
00291         MRPT_END
00292 }
00293 
00294 void CMyEllipsoid::setCovMatrix( const mrpt::math::CMatrixFloat &m, int resizeToSize)
00295 {
00296         CRenderizableDisplayList::notifyChange();
00297         setCovMatrix( CMatrixDouble(m), resizeToSize);
00298 }


ndt_visualisation
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:48