GLcamera.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <fstream>
3 #include <cstdio>
4 #include <GL/glew.h>
5 #ifdef __APPLE__
6 #include <OpenGL/glu.h>
7 #else
8 #include <GL/glu.h>
9 #endif
10 #include <hrpUtil/Eigen3d.h>
11 #include <hrpModel/Sensor.h>
12 #include "GLutil.h"
13 #include "GLsceneBase.h"
14 #include "GLlink.h"
15 #include "GLcamera.h"
16 #include "GLshape.h"
17 
18 using namespace OpenHRP;
19 using namespace hrp;
20 
21 GLcamera::GLcamera(int i_width, int i_height,
22  double i_near, double i_far, double i_fovy,
23  GLlink *i_link, int i_id) :
24  m_link(i_link),
25  m_near(i_near), m_far(i_far),
26  m_fovy(i_fovy), m_width(i_width), m_height(i_height),
27  m_frameBuffer(0), m_renderBuffer(0), m_texture(0),
28  m_sensor(NULL), m_colorBuffer(NULL)
29 {
30  if (m_link) m_sensor = m_link->body->sensor<VisionSensor>(i_id);
31 }
32 
34 {
35  for (size_t i=0; i<m_shapes.size(); i++){
36  delete m_shapes[i];
37  }
38  if (m_colorBuffer) delete [] m_colorBuffer;
39 }
40 
41 size_t GLcamera::draw(int i_mode)
42 {
43  size_t ntri = 0;
44  glPushMatrix();
45  glMultMatrixd(m_trans);
46  for (size_t i=0; i<m_shapes.size(); i++){
47  ntri += m_shapes[i]->draw(i_mode);
48  }
49  glPopMatrix();
50  return ntri;
51 }
52 
53 
54 
55 const std::string& GLcamera::name() const {
56  return m_name;
57 }
58 
59 void GLcamera::name(const std::string &i_name){
60  m_name = i_name;
61 }
62 
63 void GLcamera::computeAbsTransform(double o_trans[16]){
64  if (m_link){
65  double trans[16];
67  mulTrans(m_trans, trans, o_trans);
68  }else{
69  memcpy(o_trans, m_trans, sizeof(double)*16);
70  }
71 }
72 
74 {
75  setView(width(), height());
76 }
77 void GLcamera::setView(int w, int h)
78 {
79  glMatrixMode(GL_PROJECTION);
80  glLoadIdentity();
81  gluPerspective(fovy()*180/M_PI,
82  (double)w / (double)h,
83  near(), far());
84  if (m_link){
86  gluLookAt(m_absTrans[12], m_absTrans[13], m_absTrans[14],
87  m_absTrans[12]-m_absTrans[8],
88  m_absTrans[13]-m_absTrans[9],
89  m_absTrans[14]-m_absTrans[10],
90  m_absTrans[4], m_absTrans[5], m_absTrans[6]);
91  }else{
92  gluLookAt(m_viewPoint[0], m_viewPoint[1], m_viewPoint[2],
94  0,0,1);
95  }
96 }
97 
98 void GLcamera::setViewPoint(double x, double y, double z)
99 {
100  m_viewPoint[0] = x; m_viewPoint[1] = y; m_viewPoint[2] = z;
101 }
102 
103 void GLcamera::setViewTarget(double x, double y, double z)
104 {
105  m_viewTarget[0] = x; m_viewTarget[1] = y; m_viewTarget[2] = z;
106 }
107 
109  return m_absTrans;
110 }
111 
113 {
114  return m_link;
115 }
116 
117 void GLcamera::highlight(bool flag)
118 {
119  for (size_t i=0; i<m_shapes.size(); i++){
120  m_shapes[i]->highlight(flag);
121  }
122 }
123 
125 {
126  if (!m_frameBuffer){
127  initTexture();
129  initFramebuffer();
130  }
131  /* switch to framebuffer object */
132  glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, m_frameBuffer );
133 
134  glViewport( 0, 0, m_width, m_height );
135 
136  setView();
137 
138  glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
139  glMatrixMode(GL_MODELVIEW);
140  glLoadIdentity();
141  int dm = GLlink::drawMode();
143  i_scene->drawObjects(false);
144  GLlink::drawMode(dm);
145 
146  glFlush();
147 
148  glBindTexture( GL_TEXTURE_2D, m_texture );
149  if (m_sensor->imageType != VisionSensor::NONE
150  && m_sensor->imageType != VisionSensor::DEPTH){
151  if (!m_colorBuffer) {
152  m_colorBuffer = new unsigned char[m_width*m_height*3];
153  }
154  glReadPixels(0,0, m_width, m_height, GL_RGB, GL_UNSIGNED_BYTE, m_colorBuffer);
155 
156  if (m_sensor->imageType == VisionSensor::COLOR
157  || m_sensor->imageType == VisionSensor::COLOR_DEPTH){
158  if (m_sensor->image.size() != m_width*m_height*3){
159  std::cerr << "invalid image length" << std::endl;
160  }else{
161  unsigned char *src=m_colorBuffer;
162  unsigned char *dst=&m_sensor->image[m_width*(m_height-1)*3];
163  for (unsigned int i=0; i<m_height; i++){
164  memcpy(dst, src, m_width*3);
165  src += m_width*3;
166  dst -= m_width*3;
167  }
168  m_sensor->isUpdated = true;
169  }
170  }else if (m_sensor->imageType == VisionSensor::MONO
171  || m_sensor->imageType == VisionSensor::MONO_DEPTH){
172  if (m_sensor->image.size() != m_width*m_height){
173  std::cerr << "invalid image length" << std::endl;
174  }else{
175  unsigned char *src=m_colorBuffer;
176  unsigned char *dst=&m_sensor->image[m_width*(m_height-1)];
177  for (unsigned int i=0; i<m_height; i++){
178  for (unsigned int j=0; j<m_width; j++){
179  *dst = 0.299*src[0] + 0.587*src[1] + 0.114*src[2];
180  dst++;
181  src+=3;
182  }
183  dst -= m_width*2;
184  }
185  m_sensor->isUpdated = true;
186  }
187  }
188  }
189  if (m_sensor->imageType == VisionSensor::DEPTH
190  || m_sensor->imageType == VisionSensor::COLOR_DEPTH
191  || m_sensor->imageType == VisionSensor::MONO_DEPTH){
192  float depth[m_width*m_height];
193  glReadPixels(0,0,m_width, m_height, GL_DEPTH_COMPONENT, GL_FLOAT,
194  depth);
195  // depth -> point cloud
196  int w = m_sensor->width;
197  int h = m_sensor->height;
198  m_sensor->depth.resize(w*h*16);// will be shrinked later
199  double far = m_sensor->far;
200  double near = m_sensor->near;
201  double fovx = 2*atan(w*tan(m_sensor->fovy/2)/h);
202  double zs = w/(2*tan(fovx/2));
203  unsigned int npoints=0;
204  float *ptr = (float *)&m_sensor->depth[0];
205  unsigned char *rgb = &m_sensor->image[0];
206  bool colored = m_sensor->imageType == VisionSensor::COLOR_DEPTH;
207  int step = 1;
208  for (int i=0; i<h; i+=step){
209  for (int j=0; j<w; j+=step){
210  float d = depth[i*w+j];
211  if (d == 1.0) {
212  continue;
213  }
214  ptr[2] = far*near/(d*(far-near)-far);
215  ptr[0] = -(j-w/2)*ptr[2]/zs;
216  ptr[1] = -(i-h/2)*ptr[2]/zs;
217  if (colored){
218  unsigned char *c = (unsigned char *)(ptr + 3);
219  int offset = ((h-1-i)*w+j)*3;
220  c[0] = rgb[offset];
221  c[1] = rgb[offset+1];
222  c[2] = rgb[offset+2];
223  }
224  ptr += 4;
225  npoints++;
226  }
227  }
228  m_sensor->depth.resize(npoints*16);
229 
230  m_sensor->isUpdated = true;
231  }
232  /* switch to default buffer */
233  glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, 0 );
234 }
235 
237 {
238  glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
239  glGenTextures( 1, &m_texture );
240  glBindTexture( GL_TEXTURE_2D, m_texture );
241  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT );
242  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT );
243  glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
244  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
245  glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
246  glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, m_width, m_height,
247  0, GL_RGBA, GL_UNSIGNED_BYTE, 0 );
248 }
249 
251 {
252  glGenFramebuffersEXT( 1, &m_frameBuffer );
253  glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, m_frameBuffer );
254 
255  glFramebufferTexture2DEXT( GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT,
256  GL_TEXTURE_2D, m_texture, 0 );
257  glFramebufferRenderbufferEXT( GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT,
258  GL_RENDERBUFFER_EXT, m_renderBuffer );
259 
260  glBindFramebufferEXT( GL_FRAMEBUFFER_EXT, 0 );
261 }
262 
264 {
265  glGenRenderbuffersEXT( 1, &m_renderBuffer );
266  glBindRenderbufferEXT( GL_RENDERBUFFER_EXT, m_renderBuffer );
267  glRenderbufferStorageEXT( GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT,
268  m_width, m_height );
269 }
270 
272 {
273  return m_sensor;
274 }
275 
277 {
278  m_shapes.push_back(i_shape);
279 }
double m_viewTarget[3]
Definition: GLcamera.h:50
d
unsigned int height()
Definition: GLcamera.h:31
hrp::VisionSensor * sensor()
Definition: GLcamera.cpp:271
double * getAbsTransform()
Definition: GLcamera.cpp:108
unsigned int m_height
Definition: GLcamera.h:49
double m_trans[16]
Definition: GLmodel.h:25
void initTexture(void)
Definition: GLcamera.cpp:236
double near()
Definition: GLcamera.h:27
png_voidp ptr
std::vector< unsigned char > image
void addShape(GLshape *i_shape)
Definition: GLcamera.cpp:276
GLuint m_texture
Definition: GLcamera.h:52
void setView()
Definition: GLcamera.cpp:73
std::vector< GLshape * > m_shapes
Definition: GLcamera.h:51
const std::string & name() const
Definition: GLcamera.cpp:55
w
void setViewTarget(double x, double y, double z)
Definition: GLcamera.cpp:103
#define for
png_uint_32 i
double fovy()
Definition: GLcamera.h:29
GLlink * m_link
Definition: GLcamera.h:47
png_infop png_bytep * trans
GLuint m_renderBuffer
Definition: GLcamera.h:52
GLlink * link()
Definition: GLcamera.cpp:112
size_t drawObjects(bool showSensors=true)
~GLcamera()
Definition: GLcamera.cpp:33
void setViewPoint(double x, double y, double z)
Definition: GLcamera.cpp:98
double m_viewPoint[3]
Definition: GLcamera.h:50
unsigned int width()
Definition: GLcamera.h:30
void render(GLsceneBase *i_scene)
Definition: GLcamera.cpp:124
Sensor * sensor(int sensorType, int sensorId) const
unsigned int m_width
Definition: GLcamera.h:49
void initRenderbuffer(void)
Definition: GLcamera.cpp:263
GLuint m_frameBuffer
Definition: GLcamera.h:52
void initFramebuffer(void)
Definition: GLcamera.cpp:250
GLcamera(int i_width, int i_height, double i_near, double i_far, double i_fovy, GLlink *i_link=NULL, int i_id=-1)
Definition: GLcamera.cpp:21
void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
Definition: GLutil.cpp:58
double m_absTrans[16]
Definition: GLcamera.h:46
#define M_PI
std::vector< unsigned char > depth
hrp::VisionSensor * m_sensor
Definition: GLcamera.h:53
size_t draw(int i_mode)
Definition: GLcamera.cpp:41
void computeAbsTransform(double o_trans[16])
Definition: GLcamera.cpp:63
std::string m_name
Definition: GLcamera.h:45
void highlight(bool flag)
Definition: GLcamera.cpp:117
unsigned char * m_colorBuffer
Definition: GLcamera.h:54
ImageType imageType
double far()
Definition: GLcamera.h:28


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20