stereocamera.hh

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· HH 代码 · 共 182 行

/*
 *  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.hh,v 1.8 2004/11/13 22:23:23 inspectorg Exp $
 */

#ifndef STEREOCAMERA_HH
#define STEREOCAMERA_HH

#include <stdint.h>

#include "Error.hh"
#include "Sensor.hh"
#include "Vector.hh"
#include "RenderOptions.hh"
#include "GLContext.hh"

// Forward Declarations
class World;
class WorldFile;
class WorldFileNode;

/// @brief Stereo camera sensor
///
/// Generates images and disparity images for a pair of cameras.  The
/// disparity images a masked to simulated occlusion and camera field of
/// view.  The simulated cameras have parallel axis geometry, although
/// this could easily be extended.
class StereoCamera : public Sensor
{
  /// Constructor
  public: StereoCamera(World *world);

  /// Destructor
  public: virtual ~StereoCamera();

  /// @brief Initialize the sensor.
  /// @param width, height image width and height (pixels)
  /// @param hfov horizontal field of view (radians)
  /// @param baseline camera separation (m)
  /// @param nearClip, farClip near and far disparity clipping planes
  ///   (m); nearClip must be greater than zero; farClip must be
  ///   greater than nearClip.
  /// @param method Prefered rendering method: SGIX, GLX or XLIB.
  /// @returns Zero on success.
  public: int Init(int width, int height, double hfov, double baseline,
                   double nearClip, double farClip, const char *method);

  /// @brief Finalize the sensor
  public: int Fini();

  /// @brief Update the sensor information
  public: void Update();

  /// @brief Set the pose of the camera (global cs)
  public: void SetPose(GzPose pose);

  /// @brief Get the camera pose (global cs)
  public: GzPose GetPose();

  /// @brief Get the camera FOV (horizontal)  
  public: double GetFOV() const {return this->hfov;}

  /// @brief Get the image/disparity dimensions
  public: void GetSize(int *w, int *h);

  /// @brief Enable image rendering 
  /// @param camera Selected camera: 0 for left camera, 1 for right camera.
  /// @param enable Enable flag: 1 to enable rendering.
  // Render the scene from the camera perspective
  public: void EnableImage(int camera, bool enable);

  /// @brief Enable disparity rendering 
  /// @param camera Selected camera: 0 for left camera, 1 for right camera.
  /// @param enable Enable flag: 1 to enable rendering.
  // Render the scene from the camera perspective
  public: void EnableDisparity(int camera, bool enable);

  /// @brief Get a pointer to the image data
  /// @param camera Selected camera: 0 for left camera, 1 for right camera
  public: const unsigned char *GetImageData(int camera) const {return this->images[camera];}

  /// @brief Get a pointer to the disparity data
  /// @param camera 0 for left camera, 1 for right camera
  public: const float *GetDisparityData(int camera) const {return this->disparityImages[camera];}

  // Render image for given camera 
  private: void Render(int camera);

  // Render depth image for given camera
  private: void RenderDepth(int camera);

  // Render the models
  private: void RenderModels();

  // Post-process the disparity images to calculate occlusions, etc
  private: void ProcessDisparity();

  /// @brief Set the path for saved frames
  public: void SetSavePath(const char *pathname);

  /// @brief Enable or disable saving
  public: void EnableSaveFrame(bool enable);

  // Save the camera frame
  private: void SaveFrame();

  /// GL rendering contexts
  private: GLContext imageContext, depthContext;
  
  // Rendering options
  private: RenderOptions renderOpt;

  // Image dimensions
  private: int imgWidth, imgHeight;

  // Perspective lens options
  private: double hfov;

  // Clipping planes
  private: double nearClip, farClip;

  // Camera baseline (parallel projection)
  private: double baseline;

  // Camera model
  private: GzHomo P, iP;
  
  // Relative camera poses
  private: GzPose cameraPoses[2];

  // Current central pose (global cs)
  private: GzPose pose;

  /// Enable left/right images
  private: bool imageEnable[2];
  
  /// Buffer for storing left and right image data
  private: int imageSize;
  private: unsigned char *images[2];

  /// Buffer for storing depth image data
  private: int depthSize;
  private: float *depthImages[2];

  /// Enable left/right disparity images
  private: bool disparityEnable[2];

  /// Buffer for storing disparity image data
  private: int disparitySize;
  private: float *disparityImages[2];

  // Info for saving images
  private: bool saveEnable;
  private: const char *savePathname;
  private: unsigned int saveCount;
};


#endif