Revision: 6279
http://playerstage.svn.sourceforge.net/playerstage/?rev=6279&view=rev
Author: natepak
Date: 2008-04-07 11:04:20 -0700 (Mon, 07 Apr 2008)
Log Message:
-----------
Updates to stereo camera
Modified Paths:
--------------
code/gazebo/trunk/TODO
code/gazebo/trunk/libgazebo/Iface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/server/GazeboError.hh
code/gazebo/trunk/server/controllers/camera/SConscript
code/gazebo/trunk/server/rendering/OgreAdaptor.cc
code/gazebo/trunk/server/rendering/OgreCreator.cc
code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
code/gazebo/trunk/worlds/stereocamera.world
Added Paths:
-----------
code/gazebo/trunk/server/controllers/camera/stereo/
code/gazebo/trunk/server/controllers/camera/stereo/SConscript
code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh
Modified: code/gazebo/trunk/TODO
===================================================================
--- code/gazebo/trunk/TODO 2008-04-03 22:03:22 UTC (rev 6278)
+++ code/gazebo/trunk/TODO 2008-04-07 18:04:20 UTC (rev 6279)
@@ -1,14 +1,11 @@
Open:
-
-- Use the Contact information in the Collision Callback. See CVS version of
Gazebo.
+- Removing the sky results in a black screen
- Apply Linear and Angular Damping, see OGREODE sources.
- A static geom which is offset does not actually move the geom, so collision
detection does not work
- Force sensors
- Why order on hinge and hinge2 joints matters? Should we avoid this?
- Add spring force between bodies of a model to keep the together. See ODE
example test_joints.cpp
-
-- Try out code on AMD64 machine
- Python scripts to models, allow dynamic (movable) meshes
- Slider has to be the first in the XML file, when loading a bunch of joints
in a model.
- Add note in Controller tutorial about the Controller Factory
@@ -120,3 +117,4 @@
- Add in Pioneer blender models
- Add check for libxml2
- Add uninstall (scons -c automatically handles this!)
+- Use the Contact information in the Collision Callback. See CVS version of
Gazebo.
Modified: code/gazebo/trunk/libgazebo/Iface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Iface.cc 2008-04-03 22:03:22 UTC (rev
6278)
+++ code/gazebo/trunk/libgazebo/Iface.cc 2008-04-07 18:04:20 UTC (rev
6279)
@@ -56,6 +56,7 @@
GZ_REGISTER_IFACE("gripper", GripperIface);
GZ_REGISTER_IFACE("actarray", ActarrayIface);
GZ_REGISTER_IFACE("ptz", PTZIface);
+GZ_REGISTER_IFACE("stereocamera", StereoCameraIface);
//////////////////////////////////////////////////////////////////////////////
// Create an interface
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2008-04-03 22:03:22 UTC (rev
6278)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2008-04-07 18:04:20 UTC (rev
6279)
@@ -1313,7 +1313,88 @@
/// \}
+/***************************************************************************/
+/// \addtogroup libgazebo_iface
+/// \{
+/** \defgroup stereo_iface Stereo
+ \brief Stereo vision interface
+
+The stereo interface allows a client to read data from a stereo camera unit
+\{
+*/
+
+#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3
+#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480
+
+/// \brief Stereo data
+class StereoCameraData
+{
+ /// Data timestamp
+ public: double time;
+
+ /// Width of image in pixels
+ public: unsigned int width;
+
+ /// Height of image in pixels
+ public: unsigned int height;
+
+ /// Right image size
+ public: unsigned int right_rgb_size;
+
+ /// Right image (R8G8B8)
+ public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE];
+
+ /// Left image size
+ public: unsigned int left_rgb_size;
+
+ /// left image (R8G8B8)
+ public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE];
+
+ /// Left disparity size
+ public: unsigned int left_disparity_size;
+
+ /// Left disparity (float)
+ public: float left_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
+
+ /// Right Disparity size
+ public: unsigned int right_disparity_size;
+
+ /// Right disparity (float)
+ public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
+};
+
+
+/// \brief Stereo interface
+class StereoCameraIface : public Iface
+{
+ /// \brief Constructor
+ public: StereoCameraIface():Iface("stereo",
sizeof(StereoCameraIface)+sizeof(StereoCameraData)) {}
+
+ /// \brief Destructor
+ public: virtual ~StereoCameraIface() {this->data = NULL;}
+
+ /// \brief Create the server
+ public: virtual void Create(Server *server, std::string id)
+ {
+ Iface::Create(server,id);
+ this->data = (StereoCameraData*)this->mMap;
+ }
+
+ /// \brief Open the iface
+ public: virtual void Open(Client *client, std::string id)
+ {
+ Iface::Open(client,id);
+ this->data = (StereoCameraData*)this->mMap;
+ }
+
+ /// Pointer to the stereo data
+ public: StereoCameraData *data;
+};
+
+/** \} */
+/// \}
+
}
Modified: code/gazebo/trunk/server/GazeboError.hh
===================================================================
--- code/gazebo/trunk/server/GazeboError.hh 2008-04-03 22:03:22 UTC (rev
6278)
+++ code/gazebo/trunk/server/GazeboError.hh 2008-04-07 18:04:20 UTC (rev
6279)
@@ -41,8 +41,7 @@
//TODO: global variable, static in the class would be better, if only the
linker didn't oppose to it ...
static std::ostringstream throwStream;
/// Throw an error
- #define gzthrow(msg) throwStream << "Exception: " << msg << std::endl <<
std::flush;\
- throw
gazebo::GazeboError(__FILE__,__LINE__,throwStream.str())
+ #define gzthrow(msg) {throwStream << "Exception: " << msg << std::endl <<
std::flush; throw gazebo::GazeboError(__FILE__,__LINE__,throwStream.str()); }
/// \brief Class to handle errors
Modified: code/gazebo/trunk/server/controllers/camera/SConscript
===================================================================
--- code/gazebo/trunk/server/controllers/camera/SConscript 2008-04-03
22:03:22 UTC (rev 6278)
+++ code/gazebo/trunk/server/controllers/camera/SConscript 2008-04-07
18:04:20 UTC (rev 6279)
@@ -1,7 +1,7 @@
#Import variable
Import('env staticObjs sharedObjs')
-dirs = Split('generic')
+dirs = Split('generic stereo')
for subdir in dirs :
SConscript('%s/SConscript' % subdir)
Added: code/gazebo/trunk/server/controllers/camera/stereo/SConscript
===================================================================
--- code/gazebo/trunk/server/controllers/camera/stereo/SConscript
(rev 0)
+++ code/gazebo/trunk/server/controllers/camera/stereo/SConscript
2008-04-07 18:04:20 UTC (rev 6279)
@@ -0,0 +1,7 @@
+#Import variable
+Import('env staticObjs sharedObjs')
+
+sources = Split('Stereo_Camera.cc')
+
+staticObjs.append(env.StaticObject(sources))
+sharedObjs.append(env.SharedObject(sources))
Added: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
===================================================================
--- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
(rev 0)
+++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
2008-04-07 18:04:20 UTC (rev 6279)
@@ -0,0 +1,149 @@
+/*
+ * 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: Stereo camera controller.
+ * Author: Nathan Koenig
+ * Date: 06 April 2008
+ * SVN info: $Id: Stereo_Camera.cc 4436 2008-03-24 17:42:45Z robotos $
+ */
+
+#include <algorithm>
+#include <assert.h>
+
+#include "Sensor.hh"
+#include "Global.hh"
+#include "XMLConfig.hh"
+#include "Simulator.hh"
+#include "gazebo.h"
+#include "GazeboError.hh"
+#include "ControllerFactory.hh"
+#include "StereoCameraSensor.hh"
+#include "Stereo_Camera.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_CONTROLLER("stereo_camera", Stereo_Camera);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Stereo_Camera::Stereo_Camera(Entity *parent)
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<StereoCameraSensor*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("Stereo_Camera controller requires a Stereo Camera Sensor as its
parent");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Stereo_Camera::~Stereo_Camera()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Stereo_Camera::LoadChild(XMLConfigNode *node)
+{
+ this->cameraIface = dynamic_cast<StereoCameraIface*>(this->ifaces[0]);
+
+ if (!this->cameraIface)
+ gzthrow("Stereo_Camera controller requires a StereoCameraIface");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Stereo_Camera::InitChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Stereo_Camera::UpdateChild(UpdateParams ¶ms)
+{
+ this->PutCameraData();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Stereo_Camera::FiniChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Put laser data to the interface
+void Stereo_Camera::PutCameraData()
+{
+ StereoCameraData *data = this->cameraIface->data;
+ const unsigned char *rgb_src;
+ unsigned char *rgb_dst;
+
+ const float *disp_src;
+ float *disp_dst;
+
+ int i, j, k;
+
+ this->cameraIface->Lock(1);
+
+ // Data timestamp
+ data->time = Simulator::Instance()->GetSimTime();
+
+ data->width = this->myParent->GetImageWidth();
+ data->height = this->myParent->GetImageHeight();
+
+ data->right_rgb_size = data->width * data->height * 3;
+ data->left_rgb_size = data->width * data->height * 3;
+
+ data->right_disparity_size = data->width * data->height;
+ data->left_disparity_size = data->width * data->height;
+
+ // Make sure there is room to store the image
+ assert (data->right_rgb_size <= sizeof(data->right_rgb));
+ assert (data->left_rgb_size <= sizeof(data->left_rgb));
+
+ assert (data->right_disparity_size <= sizeof(data->right_disparity));
+ assert (data->left_disparity_size <= sizeof(data->left_disparity));
+
+ // Copy the left pixel data to the interface
+ rgb_src = this->myParent->GetImageData(0);
+ rgb_dst = data->left_rgb;
+ memcpy(rgb_dst, rgb_src, data->left_rgb_size);
+
+ // Copy the right pixel data to the interface
+ rgb_src = this->myParent->GetImageData(1);
+ rgb_dst = data->right_rgb;
+ memcpy(rgb_dst, rgb_src, data->right_rgb_size);
+
+ // Copy the left disparity data to the interface
+ disp_src = this->myParent->GetDisparityData(0);
+ disp_dst = data->left_disparity;
+ memcpy(disp_dst, disp_src, data->left_disparity_size);
+
+ // Copy the right disparity data to the interface
+ disp_src = this->myParent->GetDisparityData(0);
+ disp_dst = data->right_disparity;
+ memcpy(disp_dst, disp_src, data->right_disparity_size);
+
+ this->cameraIface->Unlock();
+
+ // New data is available
+ this->cameraIface->Post();
+}
Added: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh
===================================================================
--- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh
(rev 0)
+++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh
2008-04-07 18:04:20 UTC (rev 6279)
@@ -0,0 +1,109 @@
+/*
+ * 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 controller
+ * Author: Nathan Koenig
+ * Date: 06 April 2008
+ * SVN: $Id:$
+ */
+
+#ifndef STEREO_CAMERA_HH
+#define STEREO_CAMERA_HH
+
+#include "Controller.hh"
+
+namespace gazebo
+{
+ class CameraIface;
+ class CameraSensor;
+
+/// @addtogroup gazebo_controller
+/// @{
+/** \defgroup stereocamera stereo camera
+
+ \brief Stereo camera controller.
+
+ This is a controller that collects data from a Stereo Camera Sensor and
populates a libgazebo stereo camera interfaace. This controller should only be
used as a child of a stereo camera sensor
+
+ \verbatim
+ <model:physical name="camera_model">
+ <body:empty name="camera_body">
+ <sensor:stereocamera name="stereo_camera_sensor">
+
+ <controller:stereo_camera name="controller-name">
+ <interface:stereocamera name="iface-name"/>
+ </controller:stereo_camera>
+
+ </sensor:stereocamera>
+ </body:empty>
+ </model:phyiscal>
+ \endverbatim
+
+\{
+*/
+
+/// \brief Stereo camera controller.
+///
+/// This is a controller that simulates a stereo camera
+class Stereo_Camera : public Controller
+{
+ /// \brief Constructor
+ /// \param parent The parent entity, must be a Model or a Sensor
+ public: Stereo_Camera(Entity *parent);
+
+ /// \brief Destructor
+ public: virtual ~Stereo_Camera();
+
+ /// \brief Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// \brief Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// \brief Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild(UpdateParams ¶ms);
+
+ /// \brief Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// \brief Put camera data to the iface
+ private: void PutCameraData();
+
+ /// The camera interface
+ private: StereoCameraIface *cameraIface;
+
+ /// The parent sensor
+ private: StereoCameraSensor *myParent;
+
+};
+
+/** /} */
+/// @}
+
+}
+
+#endif
+
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-03 22:03:22 UTC
(rev 6278)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-07 18:04:20 UTC
(rev 6279)
@@ -187,13 +187,15 @@
// Settings for shadow mapping
std::string shadowTechnique = node->GetString("shadowTechnique",
"stencilAdditive");
+
if (shadowTechnique == std::string("stencilAdditive"))
this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_STENCIL_ADDITIVE );
else if (shadowTechnique == std::string("textureAdditive"))
this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_TEXTURE_ADDITIVE );
else if (shadowTechnique == std::string("none"))
this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_NONE );
- else gzthrow(std::string("Unsupported shadow technique: ") + shadowTechnique
+ "\n");
+ else
+ gzthrow(std::string("Unsupported shadow technique: ") + shadowTechnique +
"\n");
this->sceneMgr->setShadowTextureSelfShadow(true);
this->sceneMgr->setShadowTexturePixelFormat(Ogre::PF_FLOAT32_R);
@@ -476,7 +478,7 @@
{
OgreHUD::Instance()->Update();
- root->renderOneFrame();
+ this->root->renderOneFrame();
return 0;
}
Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-04-03 22:03:22 UTC
(rev 6278)
+++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-04-07 18:04:20 UTC
(rev 6279)
@@ -180,7 +180,7 @@
camera->setDirection(1,0,0);
camera->setNearClipDistance(nearClip);
- camera->setFarClipDistance(farClip+1000);
+ camera->setFarClipDistance(farClip);
// Setup the viewport to use the texture
cviewport = renderTarget->addViewport(camera);
Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
2008-04-03 22:03:22 UTC (rev 6278)
+++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
2008-04-07 18:04:20 UTC (rev 6279)
@@ -21,9 +21,9 @@
/* Desc: Stereo Camera Sensor
* Author: Nate Koenig
* Date: 25 March 2008
- * CVS: $Id:$
+ * SVN: $Id:$
*/
-
+#include <arpa/inet.h>
#include <sstream>
#include <OgreImageCodec.h>
#include <GL/gl.h>
@@ -51,6 +51,10 @@
StereoCameraSensor::StereoCameraSensor(Body *body)
: CameraSensor(body)
{
+ this->depthBuffer[0] = NULL;
+ this->depthBuffer[1] = NULL;
+ this->rgbBuffer[0] = NULL;
+ this->rgbBuffer[1] = NULL;
}
@@ -58,6 +62,14 @@
// Destructor
StereoCameraSensor::~StereoCameraSensor()
{
+ for (int i=0; i<2; i++)
+ {
+ if (this->depthBuffer[i])
+ delete [] this->depthBuffer[i];
+
+ if (this->rgbBuffer[i])
+ delete [] this->rgbBuffer[i];
+ }
}
//////////////////////////////////////////////////////////////////////////////
@@ -66,6 +78,8 @@
{
CameraSensor::LoadChild(node);
+ std::cout << "Image Size[" << this->imageWidth << " " << this->imageHeight
<< "]\n";
+
this->baseline = node->GetDouble("baseline",0,1);
}
@@ -73,149 +87,72 @@
// Initialize the camera
void StereoCameraSensor::InitChild()
{
- this->ogreTextureName[0] = this->GetName() + "_RttTex_Stereo_Left";
- //this->ogreTextureName[1] = this->GetName() + "_RttTex_Stereo_Right";
+ Ogre::Viewport *cviewport;
+ Ogre::MaterialPtr matPtr;
+ Ogre::TextureUnitState *texUnit;
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ int i;
- this->ogreDepthTextureName[0] = this->GetName() +"_RttTex_Stereo_Left_Depth";
- //this->ogreDepthTextureName[1] = this->GetName()
+"_RttTex_Stereo_Right_Depth";
+ this->textureName[LEFT] = this->GetName() + "_RttTex_Stereo_Left";
+ this->textureName[RIGHT] = this->GetName() + "_RttTex_Stereo_Right";
+ this->textureName[D_LEFT] = this->GetName() +"_RttTex_Stereo_Left_Depth";
+ this->textureName[D_RIGHT] = this->GetName() +"_RttTex_Stereo_Right_Depth";
- this->ogreMaterialName[0] = this->GetName() + "_RttMat_Stereo_Left";
- //this->ogreMaterialName[1] = this->GetName() + "_RttMat_Stereo_Right";
+ this->materialName[LEFT] = this->GetName() + "_RttMat_Stereo_Left";
+ this->materialName[RIGHT] = this->GetName() + "_RttMat_Stereo_Right";
+ this->materialName[D_LEFT] = this->GetName()+"_RttMat_Stereo_Left_Depth";
+ this->materialName[D_RIGHT] = this->GetName()+"_RttMat_Stereo_Right_Depth";
- this->ogreDepthMaterialName[0] = this->GetName()+"_RttMat_Stereo_Left_Depth";
- //this->ogreDepthMaterialName[1] =
this->GetName()+"_RttMat_Stereo_Right_Depth";
+ // Create the render textures for the color textures
+ for (i = 0; i<2; i++)
+ {
+ this->renderTexture[i] = this->CreateRTT(this->textureName[i], false);
+ this->renderTarget[i] =
this->renderTexture[i]->getBuffer()->getRenderTarget();
+ }
- // Create the left render texture
- this->renderTexture[0] = Ogre::TextureManager::getSingleton().createManual(
- this->ogreTextureName[0],
-
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- this->imageWidth, this->imageHeight, 0,
- Ogre::PF_R8G8B8,
- //Ogre::PF_L8,
- //Ogre::PF_FLOAT16_R,
- Ogre::TU_RENDERTARGET);
+ // Create the render texture for the depth textures
+ for (i = 2; i<4; i++)
+ {
+ this->renderTexture[i] = this->CreateRTT(this->textureName[i], true);
+ this->renderTarget[i] =
this->renderTexture[i]->getBuffer()->getRenderTarget();
+ }
- this->renderTarget[0] =
this->renderTexture[0]->getBuffer()->getRenderTarget();
-/*
- // Create the right render texture
- this->renderTexture[1] = Ogre::TextureManager::getSingleton().createManual(
- this->ogreTextureName[1],
-
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- this->imageWidth, this->imageHeight, 0,
- Ogre::PF_R8G8B8,
- Ogre::TU_RENDERTARGET);
-
- this->renderTarget[1] =
this->renderTexture[1]->getBuffer()->getRenderTarget();
- */
-
- // Create the left depth render texture
- /*this->depthRenderTexture[0] =
- Ogre::TextureManager::getSingleton().createManual(
- this->ogreDepthTextureName[0],
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- this->imageWidth, this->imageHeight, 0,
- Ogre::PF_FLOAT32_R,
- //Ogre::PF_DEPTH,
- //Ogre::PF_R8G8B8,
- Ogre::TU_RENDERTARGET);
-
- this->depthRenderTarget[0] =
this->depthRenderTexture[0]->getBuffer()->getRenderTarget();
- */
-
- // Create the right depth render texture
- /*this->depthRenderTexture[1] =
- Ogre::TextureManager::getSingleton().createManual(
- this->ogreDepthTextureName[1],
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- this->imageWidth, this->imageHeight, 0,
- Ogre::PF_FLOAT32_R,
- //Ogre::PF_DEPTH,
- //Ogre::PF_R8G8B8,
- Ogre::TU_RENDERTARGET);
-
- this->depthRenderTarget[1] =
this->depthRenderTexture[1]->getBuffer()->getRenderTarget();
- */
-
-
// Create the camera
this->camera = OgreCreator::CreateCamera(this->GetName(),
- this->nearClip, this->farClip, this->hfov,
this->renderTarget[0]);
+ this->nearClip, this->farClip, this->hfov,
+ this->renderTarget[0]);
+ //this->camera->setUseRenderingDistance(true);
- // Hack to make the camera use the right render target too
- /*{
- Ogre::Viewport *cviewport;
+ // Hack to make the camera use the right render target too.
+ for (i=0; i<4; i++)
+ {
+ this->renderTarget[i]->setAutoUpdated(false);
- // Setup the viewport to use the texture
- cviewport = this->renderTarget[1]->addViewport(camera);
- cviewport->setClearEveryFrame(true);
- cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor
);
- cviewport->setOverlaysEnabled(false);
+ if (i > 0)
+ {
+ // Setup the viewport to use the texture
+ cviewport = this->renderTarget[i]->addViewport(camera);
+ cviewport->setClearEveryFrame(true);
+ cviewport->setOverlaysEnabled(false);
+ cviewport->setBackgroundColour(
*OgreAdaptor::Instance()->backgroundColor );
+ }
- cviewport = this->depthRenderTarget[0]->addViewport(camera);
- cviewport->setClearEveryFrame(true);
- cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor
);
- cviewport->setOverlaysEnabled(false);
+ // Create materials for all the render textures.
+ matPtr = Ogre::MaterialManager::getSingleton().create(
+ this->materialName[i],
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
- cviewport = this->depthRenderTarget[1]->addViewport(camera);
- cviewport->setClearEveryFrame(true);
- cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor
);
- cviewport->setOverlaysEnabled(false);
- }*/
+ matPtr->getTechnique(0)->getPass(0)->createTextureUnitState(
+ this->textureName[i] );
+ }
- Ogre::MaterialPtr leftmat = Ogre::MaterialManager::getSingleton().create(
- this->ogreMaterialName[0],
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ Ogre::MaterialPtr tmpMat =
Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap",
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ // Get pointer to the depth map material
+ this->depthMaterial = tmpMat.get();
- Ogre::MaterialPtr depthMat =
Ogre::MaterialManager::getSingleton().getByName("Gazebo/DepthMap");
-
- /*Ogre::MaterialPtr rightmat = Ogre::MaterialManager::getSingleton().create(
- this->ogreMaterialName[1],
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
- */
-
-
//depthMat->getTechnique(0)->getPass(0)->getTextureUnitState(0)->setTextureName(this->renderTexture[0]->getName());
-
depthMat->getTechnique(0)->getPass(0)->createTextureUnitState(this->renderTexture[0]->getName());
- /*leftmat->getTechnique(0)->getPass(0)->removeAllTextureUnitStates();
- leftmat->getTechnique(0)->getPass(0)->setLightingEnabled(false);
- Ogre::TextureUnitState *texUnit =
leftmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[0]);
- texUnit->setTextureBorderColour(Ogre::ColourValue::White);
- texUnit->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP);
- */
-
-
//rightmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[1]);
-
- //Ogre::MaterialPtr leftdepthmat =
Ogre::MaterialManager::getSingleton().create(
- //this->ogreDepthMaterialName[0],
-
//Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-
- /*Ogre::MaterialPtr
rightdepthmat=Ogre::MaterialManager::getSingleton().create(
- this->ogreDepthMaterialName[1],
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
- */
-
- //Ogre::TextureUnitState *t =
leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[0]);
- //Ogre::TextureUnitState *t =
leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(OgreAdaptor::Instance()->sceneMgr->getShadowTexture(0)->getName());
- //leftdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false);
- //t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP);
-
- //leftdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true);
- //leftdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false);
-
-
//rightdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[1]);
- //rightdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false);
- //rightdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true);
- //rightdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false);
-
-
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
-
// Get access to the buffer and make an image and write it to file
mBuffer = this->renderTexture[0]->getBuffer(0, 0);
@@ -227,7 +164,20 @@
//this->renderTarget[0]->addListener(&this->leftCameraListener);
//this->renderTarget[1]->addListener(&this->rightCameraListener);
+
+ this->rgbBufferSize = this->imageWidth*this->imageHeight * 3;
+ this->depthBufferSize = this->imageWidth*this->imageHeight;
+ // Allocate buffers
+ if (!this->depthBuffer[0])
+ this->depthBuffer[0] = new float[this->depthBufferSize];
+ if (!this->depthBuffer[1])
+ this->depthBuffer[1] = new float[this->depthBufferSize];
+ if (!this->rgbBuffer[0])
+ this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize];
+ if (!this->rgbBuffer[1])
+ this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize];
+
CameraSensor::InitChild();
}
@@ -241,67 +191,177 @@
// Update the drawing
void StereoCameraSensor::UpdateChild(UpdateParams ¶ms)
{
+ OgreAdaptor *adapt = OgreAdaptor::Instance();
+ Ogre::RenderSystem *renderSys = adapt->root->getRenderSystem();
+ Ogre::Viewport *vp = NULL;
+ Ogre::SceneManager *sceneMgr = adapt->sceneMgr;
+ Ogre::AutoParamDataSource autoParamDataSource;
+ Ogre::Pass *pass;
+ int i;
+
CameraSensor::UpdateChild(params);
- this->renderTarget[0]->writeContentsToFile("texture.png");
+ sceneMgr->_suppressRenderStateChanges(true);
+
+ // Get pointer to the material pass
+ pass = this->depthMaterial->getBestTechnique()->getPass(0);
+
+ for (i=2; i<3; i++)
+ {
+ vp = this->renderTarget[i]->getViewport(0);
+
+ // Need this line to render the ground plane. No idea why it's necessary.
+ renderSys->_setViewport(vp);
+ sceneMgr->_setPass(pass, true, false);
+ autoParamDataSource.setCurrentPass(pass);
+ autoParamDataSource.setCurrentViewport(vp);
+ autoParamDataSource.setCurrentRenderTarget(this->renderTarget[i]);
+ autoParamDataSource.setCurrentSceneManager(sceneMgr);
+ autoParamDataSource.setCurrentCamera(this->camera);
+ pass->_updateAutoParamsNoLights(autoParamDataSource);
+
+ // These two lines don't seem to do anything useful
+ renderSys->_setProjectionMatrix(this->camera->getProjectionMatrixRS());
+ renderSys->_setViewMatrix(this->camera->getViewMatrix(true));
+
+ // NOTE: We MUST bind parameters AFTER updating the autos
+ if (pass->hasVertexProgram())
+ {
+ renderSys->bindGpuProgram(
pass->getVertexProgram()->_getBindingDelegate() );
+ renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
+ pass->getVertexProgramParameters());
+ }
+
+ if (pass->hasFragmentProgram())
+ {
+ renderSys->bindGpuProgram(
pass->getFragmentProgram()->_getBindingDelegate() );
+ renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM,
+ pass->getFragmentProgramParameters());
+ }
+
+ // Render the texture
+ this->renderTarget[i]->update();
+
+ // OgreSceneManager::_render function automatically sets farClip to 0.
+ // Which normally equates to infinite distance. We don't want this. So
+ // we have to set the distance every time.
+ this->camera->setFarClipDistance( this->farClip );
+ }
+
+ sceneMgr->_suppressRenderStateChanges(false);
+
+ this->FillBuffers();
}
////////////////////////////////////////////////////////////////////////////////
// Return the material the camera renders to
std::string StereoCameraSensor::GetMaterialName() const
{
- return "Gazebo/DepthMap";//this->ogreMaterialName[0];
+ return this->materialName[D_LEFT];
}
//////////////////////////////////////////////////////////////////////////////
/// Get a pointer to the image data
const unsigned char *StereoCameraSensor::GetImageData(unsigned int i)
{
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
- size_t size;
if (i > 1)
+ gzthrow("Index must be 0 for left, or 1 for right disparity image\n");
+
+ return this->rgbBuffer[i];
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Get a pointer to the disparity data
+const float *StereoCameraSensor::GetDisparityData(unsigned int i)
+{
+ if (i > 1)
+ gzthrow("Index must be 0 for left, or 1 for right disparity image\n");
+ return this->depthBuffer[i];
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Fill all RGB and Disparity buffers
+void StereoCameraSensor::FillBuffers()
+{
+ Ogre::HardwarePixelBufferSharedPtr hardwareBuffer;
+ int top, left, right, bottom, i;
+
+ top = (int)((hardwareBuffer->getHeight() - this->imageHeight) / 2.0);
+ left = (int)((hardwareBuffer->getWidth() - this->imageWidth) / 2.0);
+ right = left + this->imageWidth;
+ bottom = top + this->imageHeight;
+
+ Ogre::Box box(left, top, right, bottom);
+
+ for (i=0; i<4; i++)
{
- gzerr(0) << "Camera index must be 0=Left or 1=Right for stereo camera\n";
- i = 1;
+ // Get access to the buffer and make an image and write it to file
+ hardwareBuffer = this->renderTexture[i]->getBuffer(0, 0);
+
+ hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_DISCARD);
+
+ if (i < 2)
+ {
+ hardwareBuffer->blitToMemory ( box,
+ Ogre::PixelBox( this->imageWidth, this->imageHeight,
+ 1, Ogre::PF_R8G8B8, this->rgbBuffer[i])
+ );
+ }
+ else
+ {
+ hardwareBuffer->blitToMemory (box,
+ Ogre::PixelBox( this->imageWidth, this->imageHeight,
+ 1, Ogre::PF_FLOAT32_R, this->depthBuffer[i-2])
+ );
+ }
+
+ hardwareBuffer->unlock();
}
+}
- // Get access to the buffer and make an image and write it to file
- mBuffer = this->depthRenderTexture[i]->getBuffer(0, 0);
+////////////////////////////////////////////////////////////////////////////////
+// Save a single frame to disk
+void StereoCameraSensor::SaveFrame()
+{
+ char tmp[1024];
+ FILE *fp;
+
+ sprintf(tmp, "frame%04d.pgm", this->saveCount);
- size = this->imageWidth * this->imageHeight * 3;
+ fp = fopen( tmp, "wb" );
- // Allocate buffer
- if (!this->saveFrameBuffer)
- this->saveFrameBuffer = new unsigned char[size];
+ if (!fp)
+ {
+ printf( "unable to open file %s\n for writing", tmp );
+ return;
+ }
- mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
+ fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageWidth,
this->imageHeight);
- int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
- int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
- int right = left + this->imageWidth;
- int bottom = top + this->imageHeight;
+ for (int i = 0; i<this->imageHeight; i++)
+ {
+ for (int j =0; j<this->imageWidth; j++)
+ {
+ int index = i*this->imageWidth + j;
+ //float ptr = (float)(*(this->depthBuffer + i * this->imageWidth + j));
- // Get the center of the texture in RGB 24 bit format
- mBuffer->blitToMemory(
- Ogre::Box(left, top, right, bottom),
+ unsigned char value = this->saveFrameBuffer[i*this->imageWidth+j];
+ fwrite( &value, 1, 1, fp );
+ fwrite( &value, 1, 1, fp );
+ fwrite( &value, 1, 1, fp );
+ }
+ }
- Ogre::PixelBox(
- this->imageWidth,
- this->imageHeight,
- 1,
- Ogre::PF_B8G8R8,
- this->saveFrameBuffer)
- );
+ fclose( fp );
+ this->saveCount++;
- mBuffer->unlock();
-
- return this->saveFrameBuffer;
+ return;
}
//////////////////////////////////////////////////////////////////////////////
// Save the current frame to disk
-void StereoCameraSensor::SaveFrame()
+/*void StereoCameraSensor::SaveFrame()
{
Ogre::HardwarePixelBufferSharedPtr mBuffer;
std::ostringstream sstream;
@@ -309,12 +369,12 @@
Ogre::Codec * pCodec;
size_t size, pos;
-// for (int i=0; i<2; i++)
+ for (int i=0; i<4; i++)
{
- this->GetImageData(0);
+ this->GetImageData(i);
// Get access to the buffer and make an image and write it to file
- mBuffer = this->depthRenderTexture[0]->getBuffer(0, 0);
+ mBuffer = this->renderTexture[i]->getBuffer(0, 0);
// Create image data structure
imgData = new Ogre::ImageCodec::ImageData();
@@ -322,7 +382,11 @@
imgData->width = this->imageWidth;
imgData->height = this->imageHeight;
imgData->depth = 1;
- imgData->format = Ogre::PF_B8G8R8;
+ if (i<2)
+ imgData->format = Ogre::PF_B8G8R8;
+ else
+ imgData->format = Ogre::PF_FLOAT32_R;
+
size = this->GetImageByteSize();
// Wrap buffer in a chunk
@@ -364,16 +428,7 @@
this->saveCount++;
}
-
-//void StereCameraSensor::UpdateAllDependentRenderTargets()
-//{
-/* Ogre::RenderTargetList::iterator iter;
-
- for( iter = mRenderTargetList.begin(); iter != mRenderTargetList.end();
++iter )
- {
(*iter)->update();
- }
*/
-//}
////////////////////////////////////////////////////////////////////////////////
/// Get the baselien of the camera
@@ -383,7 +438,7 @@
}
-void StereoCameraSensor::StereoCameraListener::Init(
+/*void StereoCameraSensor::StereoCameraListener::Init(
StereoCameraSensor *cam, Ogre::RenderTarget *target, bool isLeft)
{
this->sensor = cam;
@@ -426,3 +481,23 @@
this->camera->setFrustumOffset(0,0);
this->camera->setPosition(this->pos);
}
+*/
+
+Ogre::TexturePtr StereoCameraSensor::CreateRTT(const std::string &name, bool
depth)
+{
+ Ogre::PixelFormat pf;
+
+ if (depth)
+ pf = Ogre::PF_FLOAT32_R;
+ else
+ pf = Ogre::PF_R8G8B8;
+
+ // Create the left render texture
+ return Ogre::TextureManager::getSingleton().createManual(
+ name,
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ this->imageWidth, this->imageHeight, 0,
+ pf,
+ Ogre::TU_RENDERTARGET);
+}
Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
2008-04-03 22:03:22 UTC (rev 6278)
+++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
2008-04-07 18:04:20 UTC (rev 6279)
@@ -40,6 +40,7 @@
class Camera;
class Viewport;
class SceneNode;
+ class Material;
}
namespace gazebo
@@ -58,6 +59,8 @@
class StereoCameraSensor : public CameraSensor
{
+ enum Sides {LEFT, RIGHT, D_LEFT, D_RIGHT};
+
/// \brief Constructor
public: StereoCameraSensor(Body *body);
@@ -81,34 +84,40 @@
public: virtual std::string GetMaterialName() const;
/// \brief Get a pointer to the image data
+ /// \param i 0=left, 1=right
public: virtual const unsigned char *GetImageData(unsigned int i=0);
+ /// \brief Get a point to the disparity data
+ /// \param i 0=left, 1=right
+ public: const float *GetDisparityData(unsigned int i=0);
+
/// \brief Get the baselien of the camera
public: double GetBaseline() const;
// Save the camera frame
protected: virtual void SaveFrame();
- private: void ReadDepthImage();
+ /// \brief Fill all the image buffers
+ private: void FillBuffers();
+ private: Ogre::TexturePtr CreateRTT( const std::string &name, bool depth);
+
//private: void UpdateAllDependentRenderTargets();
- private: Ogre::TexturePtr renderTexture[2];
- private: Ogre::TexturePtr depthRenderTexture[2];
+ private: Ogre::TexturePtr renderTexture[4];
+ private: Ogre::RenderTarget *renderTarget[4];
+ private: Ogre::Material *depthMaterial;
- private: Ogre::RenderTarget *renderTarget[2];
- private: Ogre::RenderTarget *depthRenderTarget[2];
+ private: std::string textureName[4];
+ private: std::string materialName[4];
-
- private: std::string ogreTextureName[2];
- private: std::string ogreMaterialName[2];
- private: std::string ogreDepthTextureName[2];
- private: std::string ogreDepthMaterialName[2];
-
-
+ private: unsigned int depthBufferSize;
+ private: unsigned int rgbBufferSize;
+ private: float *depthBuffer[2];
+ private: unsigned char *rgbBuffer[2];
private: double baseline;
- private:
+ /*private:
class StereoCameraListener : public Ogre::RenderTargetListener
{
public: StereoCameraListener() : Ogre::RenderTargetListener() {}
@@ -127,6 +136,7 @@
private: StereoCameraListener leftCameraListener;
private: StereoCameraListener rightCameraListener;
+ */
};
/// \}
Modified: code/gazebo/trunk/worlds/stereocamera.world
===================================================================
--- code/gazebo/trunk/worlds/stereocamera.world 2008-04-03 22:03:22 UTC (rev
6278)
+++ code/gazebo/trunk/worlds/stereocamera.world 2008-04-07 18:04:20 UTC (rev
6279)
@@ -29,14 +29,15 @@
<rendering:ogre>
<ambient>1.0 1.0 1.0 1.0</ambient>
- <sky>
+ <!--<sky>
<material>Gazebo/CloudySky</material>
</sky>
+ -->
</rendering:ogre>
<model:physical name="sphere1_model">
- <xyz>4 -1 1.5</xyz>
+ <xyz>90 -1 1.5</xyz>
<rpy>0.0 0.0 0.0</rpy>
<static>false</static>
@@ -103,6 +104,10 @@
<saveFrames>false</saveFrames>
<saveFramePath>frames</saveFramePath>
<baseline>0.2</baseline>
+
+ <controller:stereo_camera name="stereo_camera_controller">
+ <interface:stereocamera name="stereo_camera_interface" />
+ </controller:stereo_camera>
</sensor:stereocamera>
</body:empty>
</model:physical>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.net email is sponsored by the 2008 JavaOne(SM) Conference
Register now and save $200. Hurry, offer ends at 11:59 p.m.,
Monday, April 7! Use priority code J8TLD2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
