stereohead.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  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: Generic stereo camera model
 * Author: Andrew Howard
 * Date: 8 Oct 2004
 * CVS: $Id: StereoHead.cc,v 1.21 2006/02/22 15:29:24 natepak Exp $
 */

/// @addtogroup models 
/// @{
/** @defgroup StereoHead Stereo Head

@htmlinclude StereoHead_view.html

The StereoHead model simulates a generic stereo camera head.  It
generates the left/right images pairs and the corresponding disparity
images.

<center>
<table>
<tr>
<td> @image html left_image.gif "Left camera image"
<td> @image html right_image.gif "Right camera image"
<tr>
<td> @image html left_depth.gif "Left disparity image"
</table>
</center>


A few points to note:

- Disparity is returned as a floating point image, with each pixel giving
the disparity (in pixels) in the rectified images.

- The disparity calculation includes camera occlusion effects (points
must be visible by both cameras), so disparity images may be
incomplete.

- The disparity calculation does not consider surface textures (any
non-occluded surface will generate a depth value), so disparity images
will be more complete that their real-world counterparts.  This may be
changed in the future.

- There is no noise model for disparity calculations, although users
can expect some artifacts from the OpenGL rendering process.


@par libgazebo interfaces

This model supports the @ref stereo interface.

@par Player drivers

Left/Right images and disparity maps are available through the %gz_stereo driver.

@par Attributes

The following attributes are supported.

@htmlinclude default_attr_include.html

- updateRate (float, Hz)
  - Frame rate.
  - Default: 10

- imageSize (float tuple, pixel size)
  - Image width and height, in pixels.
  - Default: 320 240

- hfov (float, degrees)
  - Horizontal field of view for a perspective lens.
  - Default: 60


- baseline (float, meters)
  - Distance between camera axes.
  - Default: 0.12

- lensDiam (float, meters)
  - Lens diameter; purely for asthetic purposes.
  - Default: 0.035

- nearClip, farClip (float, meters)
  - Near and far clipping planes.
  - Default: 0.5 100.0

- renderMethod (string)
  - OpenGL rendering method: SGIX, GLX, XLIB, or AUTO.
  - See @ref gazebo_opengl
  - Default: AUTO

@par Bodies

The following bodies are created by this model.

@htmlinclude default_body_include.html


@par Example

@verbatim
<model:StereoHead>
  <id>stereo0</id>
  <xyz>0 0 0.30</xyz>
  <rpy>0 0 0</rpy>
  <imageSize>320 240</imageSize>
  <baseline>0.15</baseline>
</model:StereoHead>
@endverbatim

@par Views

@htmlinclude StereoHead_more_views.html

@par Authors

Andrew Howard

*/
/// @}

#include <assert.h>

#include "gazebo.h"
#include "Body.hh"
#include "World.hh"
#include "WorldFile.hh"
#include "ModelFactory.hh"
#include "BoxGeom.hh"
#include "CylinderGeom.hh"
#include "FrustrumGeom.hh"

#include "Camera/StereoCamera.hh"
#include "StereoHead.hh"



//////////////////////////////////////////////////////////////////////////////
// Register this model
GZ_REGISTER_STATIC("StereoHead", StereoHead);


//////////////////////////////////////////////////////////////////////////////
// Constructory
StereoHead::StereoHead( World *world )
  : Model(world)
{
  this->body = NULL;
  this->stereoSensor = new StereoCamera(world);
  this->stereo = gz_stereo_alloc();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Destructor
StereoHead::~StereoHead()
{
  delete this->stereoSensor;
  delete this->stereo;

  if (this->body)
    delete this->body;

  if (this->frustrum)
    delete this->frustrum;

  this->stereoSensor = NULL;
  this->stereo = NULL;
  this->body = NULL;
  this->frustrum = NULL;

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Load the model
int StereoHead::Load( WorldFile *file, WorldFileNode *node )
{
  // Get the time between updates
  this->updatePeriod = 1.0 / node->GetTime("updateRate", 10);
  this->updateTime = -this->updatePeriod;
  
  // Get image dimensions
  int imgWidth = node->GetTupleInt("imageSize", 0, 320);
  int imgHeight = node->GetTupleInt("imageSize", 1, 240);

  // Field of view (horizontal)
  double hfov = node->GetAngle("hfov", 60.0 * M_PI / 180);
    
  // Get the stereo baseline (distance between cameras)
  double baseline = node->GetLength("baseline", 0.120);

  // Clip planes
  double nearClip = node->GetLength("nearClip", 0.50);
  double farClip = node->GetLength("farClip", 10.0);

  // Get the lens diameter
  double lensDiam = node->GetLength("lensDiam", 0.035);
  const char *method = node->GetString("renderMethod", "auto");

  // Initialize the camera settings
  if (this->stereoSensor->Init(imgWidth, imgHeight, hfov, baseline,
                               nearClip, farClip, method) != 0)
    return -1;

  // Enable/disable images
  this->stereoSensor->EnableImage(0, node->GetBool("enableImageLeft", true));
  this->stereoSensor->EnableImage(1, node->GetBool("enableImageRight", true));
  this->stereoSensor->EnableDisparity(0, node->GetBool("enableDisparityLeft", true));
  this->stereoSensor->EnableDisparity(1, node->GetBool("enableDisparityRight", false));

  // Save frames?
  this->stereoSensor->SetSavePath(node->GetString("savePath", "."));
  this->stereoSensor->EnableSaveFrame(node->GetString("saveFrames", false));

  // Create a body for this model
  this->body = new Body(this->world);
  this->AddBody(this->body, true);

  double mass;
  Geom *geom;
  GzVector size;
  GzPose pose;

  mass = 0.300;
  size = GzVectorSet(0.030, 2 * lensDiam + baseline, 1.5 * lensDiam);

  // Create the head structure
  geom = new BoxGeom(this->body, this->modelSpaceId, size.x, size.y, size.z);
  geom->SetColor(GzColor(1.0, 1.0, 0.0));
  geom->SetMass(mass);

  // Create the left lens
  geom = new CylinderGeom(this->body, this->modelSpaceId, lensDiam / 2, lensDiam);
  pose.pos = GzVectorSet(size.x / 2, +baseline / 2, 0);
  pose.rot = GzQuaternFromAxis(0, 1, 0, M_PI / 2);
  geom->SetRelativePose(pose);
  geom->SetColor(GzColor(0.0, 0.0, 0.0));  

  // Create the right lens
  geom = new CylinderGeom(this->body, this->modelSpaceId, lensDiam / 2, lensDiam);
  pose.pos = GzVectorSet(size.x / 2, -baseline / 2, 0);
  pose.rot = GzQuaternFromAxis(0, 1, 0, M_PI / 2);
  geom->SetRelativePose(pose);
  geom->SetColor(GzColor(0.0, 0.0, 0.0));  

  // Create a frustrum for user feedback
  if (node->GetBool("useFrustrum", true))
  {
    this->frustrum = new FrustrumGeom(this->body, this->modelSpaceId,
                                      (double) imgWidth / imgHeight, nearClip, farClip);
    pose.pos = GzVectorSet(0, +baseline / 2, 0);
    pose.rot = GzQuaternIdent();
    this->frustrum->SetRelativePose(pose);
  }

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int StereoHead::Init( WorldFile *file, WorldFileNode *node )
{
  int width, height;
  gz_stereo_data_t *data;
  
  // Initialize the interface
  if (gz_stereo_create(this->stereo, this->world->gz_server, this->GetId(),
                       "StereoHead", this->GetIntId(), this->GetParentIntId()) != 0)
    return -1;
  
  // Set image dimensions (needed by GUI)
  gz_stereo_lock(this->stereo, 1);
  data = this->stereo->data;
  this->stereoSensor->GetSize(&width, &height);
  data->width = width;
  data->height = height;
  gz_stereo_unlock(this->stereo);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int StereoHead::Fini()
{
  gz_stereo_destroy(this->stereo);
  this->stereoSensor->Fini();

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update the model state
void StereoHead::Update(double step)
{
  GzPose pose;
  
  if (this->world->GetSimTime() - this->updateTime >= this->updatePeriod)
  {
    this->updateTime = this->world->GetSimTime();

    // Figure out the camera sensor pose
    pose = this->GetPose();

    // Update camera pose
    this->stereoSensor->SetPose(pose);
    
    // Update the camera    
    this->stereoSensor->Update();

    // Update the GUI
    if (this->frustrum)
    {
      int width, height;
      const uint8_t *data;
      this->stereoSensor->GetSize(&width, &height);
      data = this->stereoSensor->GetImageData(0);
      this->frustrum->SetFOV(this->stereoSensor->GetFOV());      
      this->frustrum->SetTexture2D(width, height, data);
    }

    // Update the interface
    this->PutCameraData();
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Send camera data
void StereoHead::PutCameraData()
{
  int i;
  int len;
  int width, height;
  gz_stereo_data_t *data;

  gz_stereo_lock(this->stereo, 1);

  data = this->stereo->data;

  // Get image dimensions
  this->stereoSensor->GetSize(&width, &height);
  
  data->time = this->world->GetSimTime();
  data->width = width;
  data->height = height;

  data->left_image_size = width * height * 3;
  data->right_image_size = width * height * 3;
  assert(data->left_image_size <= GAZEBO_STEREO_MAX_RGB_SIZE);
  assert(data->right_image_size <= GAZEBO_STEREO_MAX_RGB_SIZE);
  
  data->left_disparity_size = width * height;
  data->right_disparity_size = width * height;
  assert(data->left_disparity_size <= GAZEBO_STEREO_MAX_DISPARITY_SIZE);
  assert(data->right_disparity_size <= GAZEBO_STEREO_MAX_DISPARITY_SIZE);

  {
    const unsigned char *src;
    unsigned char *dst;

    src = this->stereoSensor->GetImageData(0);
    if (src != NULL)
    {
      // Copy the pixel data to the interface, but flip the y axis
      len = width * 3;
      src = src + (height - 1) * len;
      dst = data->left_image;    
      for (i = 0; i < height; i++, src -= len, dst += len)
        memcpy(dst, src, len * sizeof(unsigned char));
    }

    src = this->stereoSensor->GetImageData(1);
    if (src != NULL)
    {
      // Copy the pixel data to the interface, but flip the y axis
      len = width * 3;
      src = src + (height - 1) * len;
      dst = data->right_image;    
      for (i = 0; i < height; i++, src -= len, dst += len)
        memcpy(dst, src, len * sizeof(unsigned char));
    }
  }

  {
    const float *src;
    float *dst;

    src = this->stereoSensor->GetDisparityData(0);
    if (src != NULL)
    {
      // Copy the pixel data to the interface, but flip the y axis
      src = src + (height - 1) * width;
      dst = data->left_disparity;    
      len = width;
      for (i = 0; i < height; i++, src -= len, dst += len)
        memcpy(dst, src, len * sizeof(float));
    }

    src = this->stereoSensor->GetDisparityData(1);
    if (src != NULL)
    {
      // Copy the pixel data to the interface, but flip the y axis
      src = src + (height - 1) * width;
      dst = data->right_disparity;    
      len = width;
      for (i = 0; i < height; i++, src -= len, dst += len)
        memcpy(dst, src, len * sizeof(float));
    }
  }
  
  gz_stereo_unlock(this->stereo);
  gz_stereo_post(this->stereo);
  
  return;
}