qglv_opengl: pose.hpp Source File

Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef qglv_opengl_POSE_HPP_
00009 #define qglv_opengl_POSE_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <sophus/se3.hpp>
00016 #include "../gl/headers.hpp"
00017 #include "../gl/colours.hpp"
00018 #include "../gl/helpers.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace qglv {
00025 
00026 /*****************************************************************************
00027 ** Interfaces
00028 *****************************************************************************/
00029 
00030 template<typename T>
00031 void pose( const Sophus::SE3Group<T> & pose, const float &size, const float &width = 1.0, const AxisColourScheme colour_scheme = AxisColourRGB )
00032 {
00033   glPushMatrix();
00034   const Eigen::Matrix<T,3,1> & center = pose.translation();
00035 
00036   qglv::translate(center);
00037 
00038   Eigen::Matrix<T,3,1> axis_angle = pose.so3().log();
00039   double angle = axis_angle.norm();
00040   if(angle != 0.)
00041   {
00042     glRotatef(angle * 180.0 / M_PI,
00043                axis_angle[0], axis_angle[1], axis_angle[2]);
00044   }
00045 
00046   float cached_line_width;
00047   glGetFloatv(GL_LINE_WIDTH, &cached_line_width);
00048   glLineWidth(width);
00049   switch (colour_scheme) {
00050     case AxisColourWhite:
00051       glColor4f(1.0f, 1.0f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00052       glColor4f(1.0f, 1.0f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00053       glColor4f(1.0f, 1.0f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00054       break;
00055     case AxisColourGray:
00056       glColor4f(0.6f, 0.6f, 0.6f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00057       glColor4f(0.4f, 0.4f, 0.4f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00058       glColor4f(0.2f, 0.2f, 0.2f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00059       break;
00060     case AxisColourFadedBlue:
00061       glColor4f(0.9f, 0.9f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00062       glColor4f(0.7f, 0.7f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00063       glColor4f(0.4f, 0.4f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00064       break;
00065     case AxisColourGolden:
00066       glColor4f(1.0f, 0.8f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00067       glColor4f(0.6f, 0.4f, 0.1f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00068       glColor4f(0.5f, 0.3f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00069       break;
00070     case AxisColourGreen:
00071       glColor4f(0.0f, 1.0f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00072       glColor4f(0.0f, 0.7f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00073       glColor4f(0.0f, 0.4f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00074       break;
00075     case AxisColourBlue:
00076       glColor4f(0.0f, 0.0f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00077       glColor4f(0.0f, 0.0f, 0.7f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00078       glColor4f(0.0f, 0.0f, 0.4f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00079       break;
00080     case AxisColourRed:
00081       glColor4f(1.0f, 0.0f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00082       glColor4f(0.7f, 0.0f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00083       glColor4f(0.4f, 0.0f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00084       break;
00085     case AxisColourMagenta:
00086       glColor4f(1.0f, 0.0f, 1.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00087       glColor4f(0.8f, 0.0f, 0.8f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00088       glColor4f(0.4f, 0.0f, 0.4f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00089       break;
00090     default : // AxisColourRGB
00091       glColor3f(1.0f, 0.0f, 0.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(size, 0, 0));
00092       glColor3f(0.0f, 1.0f, 0.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, size, 0));
00093       glColor3f(0.0f, 0.0f, 1.0f);  line(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0, 0, size));
00094       break;
00095   }
00096   glLineWidth(cached_line_width);
00097   glPopMatrix();
00098 }
00099 
00100 /*****************************************************************************
00101 ** Trailers
00102 *****************************************************************************/
00103 
00104 } // namespace qglv
00105 
00106 #endif /* qglv_opengl_POSE_HPP_ */