stereocamera.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 695 行 · 第 1/2 页

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* Desc: A stereo camera sensor using OpenGL
 * Author: Andrew Howard
 * Date: 7 Oct 2004
 * CVS: $Id: StereoCamera.cc,v 1.11 2005/09/30 20:50:09 natepak Exp $
 */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include "World.hh"
#include "WorldFile.hh"
#include "Body.hh"
#include "StereoCamera.hh"


//////////////////////////////////////////////////////////////////////////////
// Constructor 
StereoCamera::StereoCamera(World *world)
    : Sensor(world)
{
  this->imgWidth = this->imgHeight = 0;  

  this->pose = GzPoseSet(GzVectorZero(), GzQuaternIdent());
  this->cameraPoses[0] = GzPoseSet(GzVectorZero(), GzQuaternIdent());
  this->cameraPoses[0] = GzPoseSet(GzVectorZero(), GzQuaternIdent());
  this->baseline = 0;

  this->imageSize = 0;
  this->images[0] = this->images[1] = NULL;
  this->depthSize = 0;
  this->depthImages[0] = this->depthImages[1] = NULL;
  this->disparitySize = 0;
  this->disparityImages[0] = this->disparityImages[1] = NULL;

  this->saveEnable = false;
  this->saveCount = 0;
  this->savePathname = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
StereoCamera::~StereoCamera()
{
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the sensor
int StereoCamera::Init(int width, int height, double hfov, double baseline,
                       double nearClip, double farClip, const char *method)
{
  double focal;
    
  this->imgWidth = width;
  this->imgHeight = height;

  this->hfov = hfov;
  this->baseline = baseline;
  this->nearClip = nearClip;
  this->farClip = farClip;
  
  // Do some sanity checks
  if (this->imgWidth == 0 || this->imgHeight == 0)
  {
    PRINT_ERR("image has zero size");
    return -1;
  }
  if (this->hfov < 0.01 || this->hfov > M_PI)
  {
    PRINT_ERR("bad field of view");
    return -1;
  }
  if (this->baseline < 0.01)
  {
    PRINT_ERR("zero baseline");
    return -1;
  }
  if (this->nearClip < 0.01)
  {
    PRINT_ERR("near clipping plane (min depth) is zero");
    return -1;
  }

  // Set the relative camera poses
  this->cameraPoses[0] = GzPoseSet(GzVectorSet(0, +baseline / 2, 0), GzQuaternIdent());
  this->cameraPoses[1] = GzPoseSet(GzVectorSet(0, -baseline / 2, 0), GzQuaternIdent());

  // Initialize the GL rendering context for regular images
  if (this->imageContext.Init(this->imgWidth, this->imgHeight, 5, 0, 16, method) != 0)
  {
    PRINT_ERR("camera initialization failed: no rendering context");
    return -1;
  }

  // Initialize the GL rendering context for disparity images
  if (this->depthContext.Init(this->imgWidth, this->imgHeight, 5, 0, 16, method) != 0)
  {
    PRINT_ERR("camera initialization failed: no rendering context");
    return -1;
  }

  // Initialize camera model: standard perspective projection
  focal = this->imgWidth / 2 / tan(this->hfov / 2);
  this->P.m[0][0] = focal;
  this->P.m[0][1] = 0;
  this->P.m[0][2] = this->imgWidth / 2;
  this->P.m[1][0] = 0;
  this->P.m[1][1] = focal;
  this->P.m[1][2] = this->imgHeight / 2;
  this->P.m[2][0] = 0;
  this->P.m[2][1] = 0;
  this->P.m[2][2] = 1;

  // Inverse projection
  this->iP = GzHomoInverse(P);

  // Initialize the image buffers
  this->imageSize = this->imgWidth * this->imgHeight * 3;
  this->images[0] = new unsigned char[this->imageSize];
  this->images[1] = new unsigned char[this->imageSize];

  this->depthSize = this->imgWidth * this->imgHeight;
  this->depthImages[0] = new float[this->depthSize];
  this->depthImages[1] = new float[this->depthSize];

  this->disparitySize = this->imgWidth * this->imgHeight;
  this->disparityImages[0] = new float[this->disparitySize];
  this->disparityImages[1] = new float[this->disparitySize];

  this->imageEnable[0] = false;
  this->imageEnable[1] = false;
  this->disparityEnable[0] = false;
  this->disparityEnable[1] = false;

  // Initialize display options
  this->renderOpt.displayBBox = false;
  this->renderOpt.displayAxes = false;
  this->renderOpt.displayCoM = false;
  this->renderOpt.displaySkins = false;  
  this->renderOpt.displayRays = false;
  this->renderOpt.displayFrustrums = false;
  this->renderOpt.displayMaterials = true;
  this->renderOpt.displayTextures = true;
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the camera
int StereoCamera::Fini()
{
  if (this->disparityImages[0])
    delete [] this->disparityImages[0];
  if (this->disparityImages[1])
    delete [] this->disparityImages[1];

  if (this->depthImages[0])
    delete [] this->depthImages[0];
  if (this->depthImages[1])
    delete [] this->depthImages[1];

  if (this->images[0])
    delete [] this->images[0];
  if (this->images[1])
    delete [] this->images[1];
    
  this->imageContext.Fini();
  this->depthContext.Fini();
  
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Set the pose of the camera
void StereoCamera::SetPose(GzPose pose)
{
  this->pose = pose;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the pose of the camera
GzPose StereoCamera::GetPose()
{
  return this->pose;
}


//////////////////////////////////////////////////////////////////////////////
// Get the image dimensions
void StereoCamera::GetSize(int *w, int *h)
{
  *w = this->imgWidth;
  *h = this->imgHeight;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Enable image rendering 
void StereoCamera::EnableImage(int camera, bool enable)
{
  assert(camera == 0 || camera == 1);
  this->imageEnable[camera] = enable;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Enable disparity rendering 
void StereoCamera::EnableDisparity(int camera, bool enable)
{
  assert(camera == 0 || camera == 1);
  this->disparityEnable[camera] = enable;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the drawing
void StereoCamera::Update()
{  
  // Render the camera views
  if (this->imageEnable[0])
    this->Render(0);
  if (this->imageEnable[1])
    this->Render(1);

  // Render the disparity images
  if (this->disparityEnable[0] || this->disparityEnable[1])
  {
    this->RenderDepth(0);
    this->RenderDepth(1);

    // Post process to correct disparity values and compute occulsions
    this->ProcessDisparity();
  }

  // Save image frames
  if (this->saveEnable)
    this->SaveFrame();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Render the scene from the camera perspective
void StereoCamera::Render(int camera)
{
  // Select the GL context
  if (this->imageContext.MakeCurrent() != 0)
    return;

  // Clear the window
  glClearColor(this->world->skyColor.r, this->world->skyColor.g,
               this->world->skyColor.b, this->world->skyColor.a);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Use disparity buffering for hidden surface elimination
  glEnable(GL_DEPTH_TEST);

  // Enable back face culling
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);

  // Set the camera lens propeties (assumes square pixels)
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(this->hfov * 180 / M_PI, (double) this->imgWidth / this->imgHeight,
                 this->nearClip, this->farClip);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  GzPose pose;
  GzVector a, b;

  // Set the camera pose; compute the point to look at, and the
  // orientation vector
  pose = GzCoordPoseAdd(this->cameraPoses[camera], this->pose);
  a = GzVectorSet(1, 0, 0);
  b = GzVectorSet(1, 0, 1);
  a = GzCoordPositionAdd(a, pose.pos, pose.rot);
  b = GzCoordPositionAdd(b, pose.pos, pose.rot);
  b = GzVectorSub(b, a); 
  gluLookAt(pose.pos.x, pose.pos.y, pose.pos.z, a.x, a.y, a.z, b.x, b.y, b.z);

  // Enable lighting
  glEnable(GL_LIGHTING);

  // Set the ambient light; this should be stored in the world
  // somewhere
  GLfloat col[4];
  GZ_COLOR_COPY(col, GzColor(1.0, 1.0, 1.0, 1.0));
  glLightModelfv(GL_LIGHT_MODEL_AMBIENT, col);

  // Ligth model (two-sided)
  glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);

  // Set rendering options
  glShadeModel(GL_SMOOTH);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  // Enable blending for transparency
  //glEnable(GL_BLEND);
  //glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  this->renderOpt.displayMaterials = true;
  this->renderOpt.displayTextures = true;
  this->renderOpt.cameraIndex = this->imageContext.GetContextIndex();
  this->renderOpt.cameraPose = pose;