GLcamera.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <fstream>
00003 #include <cstdio>
00004 #include <GL/glew.h>
00005 #ifdef __APPLE__
00006 #include <OpenGL/glu.h>
00007 #else
00008 #include <GL/glu.h>
00009 #endif
00010 #include <hrpUtil/Eigen3d.h>
00011 #include <hrpModel/Sensor.h>
00012 #include "GLutil.h"
00013 #include "GLsceneBase.h"
00014 #include "GLlink.h"
00015 #include "GLcamera.h"
00016 #include "GLshape.h"
00017 
00018 using namespace OpenHRP;
00019 using namespace hrp;
00020 
00021 GLcamera::GLcamera(int i_width, int i_height, 
00022                    double i_near, double i_far, double i_fovy,
00023                    GLlink *i_link, int i_id) : 
00024     m_link(i_link),
00025     m_near(i_near), m_far(i_far), 
00026     m_fovy(i_fovy), m_width(i_width), m_height(i_height), 
00027     m_frameBuffer(0), m_renderBuffer(0), m_texture(0),
00028     m_sensor(NULL), m_colorBuffer(NULL)
00029 {
00030     if (m_link) m_sensor = m_link->body->sensor<VisionSensor>(i_id);
00031 }
00032 
00033 GLcamera::~GLcamera()
00034 {
00035     for (size_t i=0; i<m_shapes.size(); i++){
00036         delete m_shapes[i];
00037     }
00038     if (m_colorBuffer) delete [] m_colorBuffer;
00039 }
00040 
00041 size_t GLcamera::draw(int i_mode)
00042 {
00043     size_t ntri = 0;
00044     glPushMatrix();
00045     glMultMatrixd(m_trans);
00046     for (size_t i=0; i<m_shapes.size(); i++){
00047         ntri += m_shapes[i]->draw(i_mode);
00048     }
00049     glPopMatrix();
00050     return ntri;
00051 }    
00052 
00053 
00054 
00055 const std::string& GLcamera::name() const {
00056     return m_name;
00057 }
00058 
00059 void GLcamera::name(const std::string &i_name){
00060     m_name = i_name;
00061 }
00062 
00063 void GLcamera::computeAbsTransform(double o_trans[16]){
00064     if (m_link){
00065         double trans[16];
00066         m_link->computeAbsTransform(trans);
00067         mulTrans(m_trans, trans, o_trans);
00068     }else{
00069         memcpy(o_trans, m_trans, sizeof(double)*16);
00070     }
00071 }
00072 
00073 void GLcamera::setView()
00074 {
00075     setView(width(), height());
00076 }
00077 void GLcamera::setView(int w, int h)
00078 {
00079     glMatrixMode(GL_PROJECTION);
00080     glLoadIdentity();
00081     gluPerspective(fovy()*180/M_PI, 
00082                    (double)w / (double)h, 
00083                    near(), far());
00084     if (m_link){
00085         computeAbsTransform(m_absTrans);
00086         gluLookAt(m_absTrans[12], m_absTrans[13], m_absTrans[14], 
00087                   m_absTrans[12]-m_absTrans[8], 
00088                   m_absTrans[13]-m_absTrans[9], 
00089                   m_absTrans[14]-m_absTrans[10],
00090                   m_absTrans[4], m_absTrans[5], m_absTrans[6]);
00091     }else{
00092         gluLookAt(m_viewPoint[0], m_viewPoint[1], m_viewPoint[2], 
00093                   m_viewTarget[0], m_viewTarget[1], m_viewTarget[2], 
00094                   0,0,1);
00095     }
00096 }
00097 
00098 void GLcamera::setViewPoint(double x, double y, double z)
00099 {
00100     m_viewPoint[0] = x; m_viewPoint[1] = y; m_viewPoint[2] = z;
00101 }
00102 
00103 void GLcamera::setViewTarget(double x, double y, double z)
00104 {
00105     m_viewTarget[0] = x; m_viewTarget[1] = y; m_viewTarget[2] = z;
00106 }
00107 
00108 double *GLcamera::getAbsTransform(){
00109     return m_absTrans;
00110 }
00111 
00112 GLlink *GLcamera::link()
00113 {
00114     return m_link;
00115 }
00116 
00117 void GLcamera::highlight(bool flag)
00118 {
00119     for (size_t i=0; i<m_shapes.size(); i++){
00120         m_shapes[i]->highlight(flag);
00121     }
00122 }
00123 
00124 void GLcamera::render(GLsceneBase *i_scene)
00125 {
00126     if (!m_frameBuffer){
00127         initTexture();
00128         initRenderbuffer();
00129         initFramebuffer();
00130     }
00131     /* switch to framebuffer object */
00132     glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, m_frameBuffer );
00133 
00134     glViewport( 0, 0, m_width, m_height );
00135 
00136     setView();
00137 
00138     glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
00139     glMatrixMode(GL_MODELVIEW);
00140     glLoadIdentity();
00141     int dm = GLlink::drawMode();
00142     GLlink::drawMode(GLlink::DM_SOLID);
00143     i_scene->drawObjects(false);
00144     GLlink::drawMode(dm);
00145 
00146     glFlush();
00147 
00148     glBindTexture( GL_TEXTURE_2D, m_texture );
00149     if (m_sensor->imageType != VisionSensor::NONE 
00150         && m_sensor->imageType != VisionSensor::DEPTH){
00151         if (!m_colorBuffer) {
00152             m_colorBuffer = new unsigned char[m_width*m_height*3];
00153         }
00154         glReadPixels(0,0, m_width, m_height, GL_RGB, GL_UNSIGNED_BYTE, m_colorBuffer);
00155 
00156         if (m_sensor->imageType == VisionSensor::COLOR
00157             || m_sensor->imageType == VisionSensor::COLOR_DEPTH){
00158             if (m_sensor->image.size() != m_width*m_height*3){
00159                 std::cerr << "invalid image length" << std::endl;
00160             }else{
00161                 unsigned char *src=m_colorBuffer;
00162                 unsigned char *dst=&m_sensor->image[m_width*(m_height-1)*3];
00163                 for (unsigned int i=0; i<m_height; i++){
00164                     memcpy(dst, src, m_width*3);
00165                     src += m_width*3;
00166                     dst -= m_width*3;
00167                 }
00168                 m_sensor->isUpdated = true;
00169             }
00170         }else if (m_sensor->imageType == VisionSensor::MONO
00171                   || m_sensor->imageType == VisionSensor::MONO_DEPTH){
00172             if (m_sensor->image.size() != m_width*m_height){
00173                 std::cerr << "invalid image length" << std::endl;
00174             }else{
00175                 unsigned char *src=m_colorBuffer;
00176                 unsigned char *dst=&m_sensor->image[m_width*(m_height-1)];
00177                 for (unsigned int i=0; i<m_height; i++){
00178                     for (unsigned int j=0; j<m_width; j++){
00179                         *dst = 0.299*src[0] + 0.587*src[1] + 0.114*src[2];
00180                         dst++;
00181                         src+=3;
00182                     }
00183                     dst -= m_width*2;
00184                 }
00185                 m_sensor->isUpdated = true;
00186             }
00187         }
00188     }
00189     if (m_sensor->imageType == VisionSensor::DEPTH
00190         || m_sensor->imageType == VisionSensor::COLOR_DEPTH
00191         || m_sensor->imageType == VisionSensor::MONO_DEPTH){
00192         float depth[m_width*m_height];
00193         glReadPixels(0,0,m_width, m_height, GL_DEPTH_COMPONENT, GL_FLOAT,
00194                      depth);
00195         // depth -> point cloud
00196         int w = m_sensor->width;
00197         int h = m_sensor->height;
00198         m_sensor->depth.resize(w*h*16);// will be shrinked later
00199         double far = m_sensor->far;
00200         double near = m_sensor->near;
00201         double fovx = 2*atan(w*tan(m_sensor->fovy/2)/h);
00202         double zs = w/(2*tan(fovx/2));
00203         unsigned int npoints=0;
00204         float *ptr = (float *)&m_sensor->depth[0];
00205         unsigned char *rgb = &m_sensor->image[0];
00206         bool colored = m_sensor->imageType == VisionSensor::COLOR_DEPTH;
00207         int step = 1;
00208         for (int i=0; i<h; i+=step){
00209             for (int j=0; j<w; j+=step){
00210                 float d = depth[i*w+j];
00211                 if (d == 1.0) {
00212                     continue;
00213                 }
00214                 ptr[2] = far*near/(d*(far-near)-far);
00215                 ptr[0] = -(j-w/2)*ptr[2]/zs;
00216                 ptr[1] = -(i-h/2)*ptr[2]/zs;
00217                 if (colored){
00218                     unsigned char *c = (unsigned char *)(ptr + 3);
00219                     int offset = ((h-1-i)*w+j)*3;
00220                     c[0] = rgb[offset];
00221                     c[1] = rgb[offset+1];
00222                     c[2] = rgb[offset+2];
00223                 }
00224                 ptr += 4;
00225                 npoints++;
00226             }
00227         }
00228         m_sensor->depth.resize(npoints*16);
00229         
00230         m_sensor->isUpdated = true;
00231     }
00232     /* switch to default buffer */
00233     glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, 0 );
00234 }
00235 
00236 void GLcamera::initTexture( void )
00237 {
00238     glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
00239     glGenTextures( 1, &m_texture );
00240     glBindTexture( GL_TEXTURE_2D, m_texture );
00241     glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT );
00242     glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT );
00243     glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
00244     glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
00245     glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
00246     glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, m_width, m_height,
00247                   0, GL_RGBA, GL_UNSIGNED_BYTE, 0 );
00248 }
00249 
00250 void GLcamera::initFramebuffer( void )
00251 {
00252     glGenFramebuffersEXT( 1, &m_frameBuffer );
00253     glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, m_frameBuffer );
00254 
00255     glFramebufferTexture2DEXT( GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT,
00256                                GL_TEXTURE_2D, m_texture, 0 );
00257     glFramebufferRenderbufferEXT( GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT,
00258                                   GL_RENDERBUFFER_EXT, m_renderBuffer );
00259 
00260     glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, 0 );
00261 }
00262 
00263 void GLcamera::initRenderbuffer( void )
00264 {
00265     glGenRenderbuffersEXT( 1, &m_renderBuffer );
00266     glBindRenderbufferEXT( GL_RENDERBUFFER_EXT, m_renderBuffer );
00267     glRenderbufferStorageEXT( GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT,
00268                               m_width, m_height );
00269 }
00270 
00271 VisionSensor *GLcamera::sensor()
00272 {
00273     return m_sensor;
00274 }
00275 
00276 void GLcamera::addShape(GLshape *i_shape)
00277 {
00278     m_shapes.push_back(i_shape);
00279 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55