Update of /cvsroot/playerstage/code/gazebo/server/sensors/camera
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11622/server/sensors/camera
Added Files:
Tag: ogre
Camera.cc Camera.hh CameraManager.cc CameraManager.hh
SConscript
Log Message:
Added new gazebo files
--- NEW FILE: CameraManager.hh ---
#ifndef CAMERAMANAGER_HH
#define CAMERAMANAGER_HH
#include <deque>
class Camera;
class CameraManager
{
private: CameraManager();
private: ~CameraManager();
public: static CameraManager *Instance();
public: Camera *CreateCamera();
public: Camera *GetCamera(int index);
public: void SetActiveCamera(Camera *camera);
public: Camera *GetActiveCamera();
private: static CameraManager *myself;
private: std::deque<Camera*> cameras;
private: Camera *activeCamera;
};
#endif
--- NEW FILE: Camera.hh ---
/*
* 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 persepective X11 OpenGL Camera Sensor
* Author: Nate Koenig
* Date: 15 July 2003
* CVS: $Id: Camera.hh,v 1.1.2.1 2006/12/16 22:41:17 natepak Exp $
*/
#ifndef CAMERA_HH
#define CAMERA_HH
#include <OGRE/Ogre.h>
// Forward Declarations
namespace Ogre
{
class TexturePtr;
class RenderTarget;
class Camera;
class Viewport;
}
/// @brief Basic camera sensor
///
/// This sensor is used for simulating standard monocular cameras; is
/// is used by both camera models (e.g., SonyVID30) and user interface
/// models (e.g., ObserverCam).
class Camera
{
// Constructor
public: Camera();
// Destructor
public: virtual ~Camera();
/// @brief Initialize the sensor.
/// @param width, height image width and height (pixels)
/// @param hfov horizontal field of view (radians)
/// @param minDepth, maxDepth near and far depth clipping planes
/// (m); minDepth must be greater than zero; maxDepth must be
/// greater than minDepth.
/// @param method Prefered rendering method: SGIX, GLX or XLIB.
/// @param zBufferDepth Z buffer depth (in bits) used for rendering (useful
/// for some nvidia cards that do not support other depths than 24 bits)
/// @returns Zero on success.
public: int Init(int width, int height, double hfov,
double minDepth, double maxDepth, int zBufferDepth);
/// @brief Finalize the sensor
public: int Fini();
/// @brief Update the sensor information
public: void Update();
/// @brief Translate the camera
public: void Translate( const Ogre::Vector3 &direction );
/// @brief Rotate the camera around the yaw axis
public: void RotateYaw( float angle );
/// @brief Rotate the camera around the pitch axis
public: void RotatePitch( float angle );
/// @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 Set the camera FOV (horizontal)
public: void SetFOV(double fov);
/// @brief Get the camera FOV (horizontal)
public: double GetFOV() const;
/// @brief Get the image dimensions
public: void GetImageSize(int *w, int *h);
/// @brief Get a pointer to the image data
public: const unsigned char *GetImageData();
/// @brief Get the Z-buffer value at the given image coordinate.
///
/// @param x, y Image coordinate; (0, 0) specifies the top-left corner.
/// @returns Image z value; note that this is abitrarily scaled and
/// is @e not the same as the depth value.
public: double GetZValue(int x, int y);
/// @brief Compute the change in pose based on two image points.
///
/// This function provides natural feedback when using a mouse to
/// control the camera pose. The function computes a change in
/// camera pose, such that the initial image coordinate a and final
/// coordinate b map both to the same @e global coordinate. Thus,
/// it is possible to "grab" a piece of the terrain and "drag" it to
/// a new location.
///
/// Naturally, with only two image coordinates the solution is
/// under-determined (4 constraints and 6 DOF). We therefore
/// provide a mode argument specify what kind of transformation
/// should be affected; the supported modes are translating, zooming
/// and rotating.
///
/// @param mode Solution method: 0 = translate; 1 = zoom; 2 = rotate.
/// @param a, b Initial and final image points; the z value on a must be
/// specified (use GetZValue() for this).
/// @returns Change in pose (camera frame; post-multiply with
/// current global pose).
// public: GzPose CalcCameraDelta(int mode, GzVector a, GzVector b);
// Render the scene from the camera perspective
private: void Render();
/// @brief Set the path for saved frames
public: void SetSavePath(const char *pathname);
/// @brief Enable or disable saving
public: void EnableSaveFrame(bool enable);
// Camera pose
//private: GzPose cameraPose;
// Save the camera frame
private: void SaveFrame();
// Info for saving images
private: bool saveEnable;
private: const char *savePathname;
private: unsigned int saveCount;
private: double hfov;
private: double nearClip, farClip;
private: int imageWidth, imageHeight;
private: Ogre::TexturePtr renderTexture;
private: Ogre::RenderTarget *renderTarget;
private: Ogre::Viewport *viewport;
private: Ogre::Camera *camera;
private: Ogre::SceneNode *translateYawNode;
private: Ogre::SceneNode *pitchNode;
};
#endif
--- NEW FILE: SConscript ---
#Import variable
Import('env staticObjs sharedObjs')
sources = Split('Camera.cc CameraManager.cc')
staticObjs.append(env.StaticObject(sources))
sharedObjs.append(env.SharedObject(sources))
--- NEW FILE: CameraManager.cc ---
#include "Camera.hh"
#include "CameraManager.hh"
CameraManager *CameraManager::myself = NULL;
CameraManager::CameraManager()
{
this->activeCamera = NULL;
}
CameraManager::~CameraManager()
{}
CameraManager *CameraManager::Instance()
{
if (!myself)
{
myself = new CameraManager();
}
return myself;
}
Camera *CameraManager::CreateCamera()
{
Camera *newCamera = new Camera();
this->cameras.push_back(newCamera);
return newCamera;
}
Camera *CameraManager::GetCamera(int index)
{
return this->cameras[index];
}
void CameraManager::SetActiveCamera(Camera *camera)
{
this->activeCamera = camera;
}
Camera *CameraManager::GetActiveCamera()
{
return this->activeCamera;
}
--- NEW FILE: Camera.cc ---
/*
* 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 camera sensor using OpenGL
* Author: Nate Koenig
* Date: 15 July 2003
* CVS: $Id: Camera.cc,v 1.1.2.1 2006/12/16 22:41:17 natepak Exp $
*/
#if HAVE_CONFIG_H
#include <config.h>
#endif
#include <Ogre.h>
#include <OgreImageCodec.h>
#include "OgreAdaptor.hh"
#include "OgreFrameListener.hh"
#include "Camera.hh"
//////////////////////////////////////////////////////////////////////////////
// Constructor
Camera::Camera()
{
//this->cameraPose = GzPoseSet(GzVectorZero(), GzQuaternIdent());
this->imageWidth = this->imageHeight = 0;
this->saveEnable = false;
this->saveCount = 0;
this->savePathname = NULL;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Destructor
Camera::~Camera()
{
return;
}
//////////////////////////////////////////////////////////////////////////////
// Initialize the sensor
int Camera::Init(int width, int height, double hfov, double minDepth,
double maxDepth, int zBufferDepth)
{
this->imageWidth = width;
this->imageHeight = height;
this->nearClip = minDepth;
this->farClip = maxDepth;
this->hfov = hfov;
// Do some sanity checks
if (this->imageWidth == 0 || this->imageHeight == 0)
{
printf("image has zero size\n");
return -1;
}
if (this->hfov < 0.01 || this->hfov > M_PI)
{
printf("bad field of view\n");
return -1;
}
if (this->nearClip < 0.01)
{
printf("near clipping plane (min depth) is zero\n");
return -1;
}
// Create the render texture
/*this->renderTexture =
Ogre::TextureManager::getSingleton().createManual("Camera1",Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
Ogre::TEX_TYPE_2D, this->imageWidth, this->imageHeight, 0, Ogre::PF_R8G8B8,
Ogre::TU_RENDERTARGET);
this->renderTarget = this->renderTexture->getBuffer()->getRenderTarget();
this->renderTarget->setAutoUpdated(true);
*/
// Create the camera
this->camera = OgreAdaptor::Instance()->sceneMgr->createCamera("Camera1");
this->camera->setNearClipDistance(minDepth);
//this->camera->setFarClipDistance(maxDepth);
this->translateYawNode =
OgreAdaptor::Instance()->sceneMgr->getRootSceneNode()->createChildSceneNode("Camera1_TranslateYawSceneNode",
Ogre::Vector3(0,2,2));
this->pitchNode =
this->translateYawNode->createChildSceneNode("Camera1_PitchNode");
this->pitchNode->attachObject(this->camera);
this->pitchNode->pitch(Ogre::Degree(-30));
// Setup the viewport
this->viewport = OgreAdaptor::Instance()->window->addViewport(this->camera);
this->viewport->setBackgroundColour(Ogre::ColourValue::Black);
this->camera->setAspectRatio( Ogre::Real(this->viewport->getActualWidth()) /
Ogre::Real(this->viewport->getActualHeight()));
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Finalize the camera
int Camera::Fini()
{
return 0;
}
//////////////////////////////////////////////////////////////////////////////
// Update the drawing
void Camera::Update()
{
/*
//this->renderTexture->writeContentsToTimestampedFile("test",".jpg");
// Later on I update the render texture then get a textureptr
this->renderTarget->update();
// TESTING:
// this->renderTarget->writeContentsToFile("testFile.jpeg");
//Ogre::TexturePtr mRndrTexPtr =
Ogre::TextureManager::getSingleton().getByName(this->renderTexture.getName());
// Get access to the buffer and make an image and write it to file
Ogre::HardwarePixelBufferSharedPtr mBuffer =
this->renderTexture->getBuffer(0, 0);
// copyToMemory
Ogre::ImageCodec::ImageData *imgData = new Ogre::ImageCodec::ImageData();
imgData->width = this->imageWidth;
imgData->height = this->imageHeight;
imgData->depth = 1;
imgData->format = Ogre::PF_BYTE_RGBA;
size_t size = imgData->width * imgData->height * 4;
// Allocate buffer
uchar* pBuffer = new uchar[size];
// DELETE ME:
//mBuffer->blitToMemory( Ogre::Box(0, 0, this->imageWidth,
this->imageHeight), Ogre::PixelBox(this->imageWidth, this->imageHeight, 1,
imgData->format, pBuffer));
// Read pixels
mBuffer->blitToMemory( Ogre::Box((int)(this->imageWidth/2.0),
(int)(this->imageHeight/2.0), this->imageWidth, this->imageHeight),
Ogre::PixelBox(this->imageWidth, this->imageHeight, 1, imgData->format,
pBuffer));
// Wrap buffer in a chunk
Ogre::MemoryDataStreamPtr stream(new Ogre::MemoryDataStream( pBuffer, size,
false));
// Get codec
Ogre::String filename = "test.jpg";
size_t pos = filename.find_last_of(".");
Ogre::String extension;
while( pos != filename.length() - 1 )
extension += filename[++pos];
// Get the codec
Ogre::Codec * pCodec = Ogre::Codec::getCodec(extension);
// Write out
Ogre::Codec::CodecDataPtr codecDataPtr(imgData); pCodec->codeToFile(stream,
filename, codecDataPtr);
delete [] pBuffer;
*/
/* // Save image frames
if (this->saveEnable)
this->SaveFrame();
*/
return;
}
//////////////////////////////////////////////////////////////////////////////
// Render the scene from the camera perspective
void Camera::Render()
{
return;
}
const unsigned char *Camera::GetImageData()
{
return NULL;
}
//////////////////////////////////////////////////////////////////////////////
// Get the Z-buffer value at the given image coordinate
double Camera::GetZValue(int x, int y)
{
//GLfloat iz;
// Flip y axis
//y = this->imageHeight - 1 - y;
// Get image z value of first spot
// glReadPixels(x, y, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &iz);
return 0.0;// iz;
}
//////////////////////////////////////////////////////////////////////////////
// Determine change in camera pose based on two image points
/*GzPose Camera::CalcCameraDelta(int mode, GzVector a, GzVector b)
{
GzPose n;
GLfloat ix, iy, iz;
// Image coordates of the first spot
ix = a.x;
iy = this->imageHeight - 1 - a.y;
iz = a.z;
double dx, dy, dz;
// Convert to division coordinates
dx = 2 * ix / this->imageWidth - 1;
dy = 2 * iy / this->imageHeight - 1;
dz = (2 * iz - 1);
double px, py, pz, pw;
// Compute perspective divisor; assumes well-formed projection matrix
pw = this->cameraProjectionMatrix[14] / (dz +
this->cameraProjectionMatrix[10]);
// Convert to projected coordinate
px = dx * pw;
py = dy * pw;
pz = dz * pw;
double cx, cy, cz, cw;
// Convert to camera frame (inverse projection)
cx = 1 / this->cameraProjectionMatrix[0] * px;
cy = 1 / this->cameraProjectionMatrix[5] * py;
cz = -pw;
cw = 1 / this->cameraProjectionMatrix[14] * pz + 1 /
this->cameraProjectionMatrix[10] * pw;
double ix_, iy_;
double dx_, dy_;
// Image coordates of the second spot
ix_ = b.x;
iy_ = this->imageHeight - 1 - b.y;
// Convert to division coordinates
dx_ = 2 * ix_ / this->imageWidth - 1;
dy_ = 2 * iy_ / this->imageHeight - 1;
GzPose n;
n.pos = GzVectorZero();
n.rot = GzQuaternIdent();
double nx, ny, nz;
// Translate along x, y
if (mode == 0)
{
nz = 0.0;
nx = - (cz + nz) / this->cameraProjectionMatrix[0] * dx_ - cx;
ny = - (cz + nz) / this->cameraProjectionMatrix[5] * dy_ - cy;
}
// Translate (zoom) along z
else if (mode == 1)
{
ny = 0.0;
nz = - this->cameraProjectionMatrix[5] / (fabs(dy_) + 1e-16) * fabs(cy) -
cz;
nx = 0;
// Bound so we dont go too close
if (nz > -cz - 2 * this->nearClip)
nz = -cz - 2 * this->nearClip;
// Bound so we dont go too far away
else if (nz < -0.5 * this->farClip - cz)
nz = -0.5 * this->farClip - cz;
}
else
{
nx = 0;
ny = 0;
nz = 0;
}
// Convert to Gazebo coordinate system (camera axis along +x)
n.pos.x = +nz;
n.pos.y = +nx;
n.pos.z = -ny;
// Rotate (pitch and yaw)
if (mode == 2)
{
double rx, ry;
rx = atan((dx_ - dx) / this->cameraProjectionMatrix[0]);
ry = atan((dy_ - dy) / this->cameraProjectionMatrix[5]);
n.rot = GzQuaternFromEuler(0, ry, rx);
}
return n;
}*/
void Camera::Translate( const Ogre::Vector3 &direction )
{
this->translateYawNode->translate(this->translateYawNode->getOrientation() *
this->pitchNode->getOrientation() * direction);
}
//////////////////////////////////////////////////////////////////////////////
// Rotate the camera around the yaw axis
void Camera::RotateYaw( float angle )
{
this->translateYawNode->yaw(Ogre::Degree(angle));
}
//////////////////////////////////////////////////////////////////////////////
// Rotate the camera around the pitch axis
void Camera::RotatePitch( float angle )
{
this->pitchNode->pitch(Ogre::Degree(angle));
}
//////////////////////////////////////////////////////////////////////////////
// Set the pose of the camera
/*void Camera::SetPose(GzPose pose)
{
this->cameraPose = pose;
return;
}*/
//////////////////////////////////////////////////////////////////////////////
// Get the pose of the camera
/*GzPose Camera::GetPose()
{
return //this->cameraPose;
}*/
//////////////////////////////////////////////////////////////////////////////
// Set the camera FOV (horizontal)
void Camera::SetFOV(double fov)
{
this->hfov = fov;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Get the camera FOV (horizontal)
double Camera::GetFOV() const
{
return this->hfov;
}
//////////////////////////////////////////////////////////////////////////////
// Get the image dimensions
void Camera::GetImageSize(int *w, int *h)
{
*w = this->imageWidth;
*h = this->imageHeight;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Set the base filename for saved frames
void Camera::SetSavePath(const char *pathname)
{
/* char tmp[1024];
this->savePathname = pathname;
this->saveCount = 0;
sprintf(tmp, "mkdir %s 2>>/dev/null", this->savePathname);
system(tmp);
*/
return;
}
//////////////////////////////////////////////////////////////////////////////
// Enable or disable saving
void Camera::EnableSaveFrame(bool enable)
{
//this->saveEnable = enable;
return;
}
//////////////////////////////////////////////////////////////////////////////
// Save the current frame to disk
void Camera::SaveFrame()
{
/*char tmp[1024];
FILE *fp;
sprintf(tmp, "%s/%04d.pnm", this->savePathname, this->saveCount);
fp = fopen( tmp, "wb" );
if (!fp)
{
PRINT_ERR1( "unable to open file %s\n for writing", tmp );
return;
}
fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageWidth,
this->imageHeight);
for (int i = this->imageHeight-1; i >= 0; i--)
fwrite( this->rgbImage + i * this->imageWidth * 3, 1, this->imageWidth * 3,
fp );
fclose( fp );
this->saveCount++;
*/
return;
}
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys - and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
