Revision: 7202
http://playerstage.svn.sourceforge.net/playerstage/?rev=7202&view=rev
Author: natepak
Date: 2008-12-12 02:15:53 +0000 (Fri, 12 Dec 2008)
Log Message:
-----------
Fixed stereo camera depth computation. Fixed Pose3d subtraction
Modified Paths:
--------------
code/gazebo/trunk/server/Pose3d.cc
code/gazebo/trunk/server/Quatern.cc
code/gazebo/trunk/server/Quatern.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
Modified: code/gazebo/trunk/server/Pose3d.cc
===================================================================
--- code/gazebo/trunk/server/Pose3d.cc 2008-12-11 01:10:35 UTC (rev 7201)
+++ code/gazebo/trunk/server/Pose3d.cc 2008-12-12 02:15:53 UTC (rev 7202)
@@ -183,7 +183,9 @@
// Subtract one rotation from another: result = this->rot - rot
Quatern Pose3d::CoordRotationSub(const Quatern &rot) const
{
- return rot.GetInverse() * this->rot;
+ Quatern result(rot.GetInverse() * this->rot);
+ result.Normalize();
+ return result;
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/Quatern.cc
===================================================================
--- code/gazebo/trunk/server/Quatern.cc 2008-12-11 01:10:35 UTC (rev 7201)
+++ code/gazebo/trunk/server/Quatern.cc 2008-12-12 02:15:53 UTC (rev 7202)
@@ -102,6 +102,8 @@
q.y = -this->y;
q.z = -this->z;
+ //q.Normalize();
+
return q;
}
@@ -240,15 +242,33 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Addition operator
+Quatern Quatern::operator+( const Quatern &qt ) const
+{
+ Quatern result(this->u + qt.u, this->x + qt.x,
+ this->y + qt.y, this->z + qt.z);
+ return result;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Substraction operator
+Quatern Quatern::operator-( const Quatern &qt ) const
+{
+ Quatern result(this->u - qt.u, this->x - qt.x,
+ this->y - qt.y, this->z - qt.z);
+ return result;
+}
+
+////////////////////////////////////////////////////////////////////////////////
// Multiplication operator
Quatern Quatern::operator*( const Quatern &qt ) const
{
Quatern c;
- c.u = this->u * qt.u - this->x * qt.x - this->y * qt.y - this->z * qt.z;
c.x = this->u * qt.x + this->x * qt.u + this->y * qt.z - this->z * qt.y;
c.y = this->u * qt.y - this->x * qt.z + this->y * qt.u + this->z * qt.x;
c.z = this->u * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.u;
+ c.u = this->u * qt.u - this->x * qt.x - this->y * qt.y - this->z * qt.z;
return c;
}
Modified: code/gazebo/trunk/server/Quatern.hh
===================================================================
--- code/gazebo/trunk/server/Quatern.hh 2008-12-11 01:10:35 UTC (rev 7201)
+++ code/gazebo/trunk/server/Quatern.hh 2008-12-12 02:15:53 UTC (rev 7202)
@@ -110,6 +110,16 @@
/// \param scale Amount to scale this rotation
public: void Scale(double scale);
+ /// \brief Addition operator
+ /// \param qt Quatern for addition
+ /// \return This quatern + qt
+ public: Quatern operator+( const Quatern &qt ) const;
+
+ /// \brief Substraction operator
+ /// \param qt Quatern for substraction
+ /// \return This quatern - qt
+ public: Quatern operator-( const Quatern &qt ) const;
+
/// \brief Multiplication operator
/// \param qt Quatern for multiplication
/// \return This quatern multiplied by the parameter
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2008-12-11 01:10:35 UTC (rev
7201)
+++ code/gazebo/trunk/server/physics/Body.cc 2008-12-12 02:15:53 UTC (rev
7202)
@@ -287,7 +287,6 @@
// Set the pose of the body
void Body::SetPose(const Pose3d &pose)
{
-
this->pose = pose;
if (this->IsStatic())
@@ -298,17 +297,13 @@
std::map<std::string, Geom*>::iterator iter;
- this->SetPosition(this->staticPose.pos);
- this->SetRotation(this->staticPose.rot);
-
// This loop doesn't work properly when rotating objects
- /*for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++)
+ for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++)
{
- //newPose = (*iter)->GetPose() - this->staticPose;
newPose = iter->second->GetPose() - oldPose;
newPose += this->staticPose;
iter->second->SetPose(newPose);
- }*/
+ }
}
else
{
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2008-12-11 01:10:35 UTC (rev
7201)
+++ code/gazebo/trunk/server/physics/Geom.cc 2008-12-12 02:15:53 UTC (rev
7202)
@@ -287,7 +287,7 @@
////////////////////////////////////////////////////////////////////////////////
// Set the pose relative to the body
-void Geom::SetPose(const Pose3d &pose, bool updateCoM)
+void Geom::SetPose(const Pose3d &newPose, bool updateCoM)
{
if (this->placeable && this->geomId)
{
@@ -295,7 +295,7 @@
dQuaternion q;
// Transform into CoM relative Pose
- localPose = pose - this->body->GetCoMPose();
+ localPose = newPose - this->body->GetCoMPose();
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
@@ -307,7 +307,7 @@
dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y,
localPose.pos.z);
dGeomSetQuaternion(this->geomId, q);
- this->visualNode->SetPose(pose);
+ this->visualNode->SetPose(newPose);
if (updateCoM)
{
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-12-11 01:10:35 UTC
(rev 7201)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-12-12 02:15:53 UTC
(rev 7202)
@@ -138,7 +138,7 @@
// Update the dynamical model
dWorldStep( this->worldId, this->stepTimeP->GetValue() );
- //dWorldQuickStep(this->worldId, this->stepTime);
+ //dWorldQuickStep(this->worldId, this->stepTimeP->GetValue());
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
2008-12-11 01:10:35 UTC (rev 7201)
+++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
2008-12-12 02:15:53 UTC (rev 7202)
@@ -43,7 +43,7 @@
#include "CameraManager.hh"
#include "StereoCameraSensor.hh"
-#define PF_FLOAT Ogre::PF_FLOAT16_R
+#define PF_FLOAT Ogre::PF_FLOAT32_R
#define PF_RGB Ogre::PF_B8G8R8
using namespace gazebo;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
SF.Net email is Sponsored by MIX09, March 18-20, 2009 in Las Vegas, Nevada.
The future of the web can't happen without you. Join us at MIX09 to help
pave the way to the Next Web now. Learn more and register at
http://ad.doubleclick.net/clk;208669438;13503038;i?http://2009.visitmix.com/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
