diff --git a/VirtualRobot/Workspace/WorkspaceData.h b/VirtualRobot/Workspace/WorkspaceData.h
index fbab964174162eac99969284e4dd18afe494ff02..3556b776043f79b0a2b58538514d09ae35bf002f 100644
--- a/VirtualRobot/Workspace/WorkspaceData.h
+++ b/VirtualRobot/Workspace/WorkspaceData.h
@@ -24,6 +24,7 @@
#define _VirtualRobot_WorkspaceData_h_
#include "../VirtualRobotImportExport.h"
+#include "WorkspaceRepresentation.h"
#include <boost/enable_shared_from_this.hpp>
#include <boost/type_traits/is_base_of.hpp>
@@ -41,6 +42,8 @@ namespace VirtualRobot
Stores a 6-dimensional array for the vertex data of a workspace representation.
Internally unsigned char data types are used (0...255)
*/
+class WorkspaceRepresentation;
+
class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceData : public boost::enable_shared_from_this<WorkspaceData>
{
public:
@@ -49,12 +52,6 @@ public:
virtual unsigned int getSizeTr() const = 0;
virtual unsigned int getSizeRot() const = 0;
- virtual void getPos( unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5 ,
- unsigned int &storePosTr, unsigned int &storePosRot) const = 0;
-
- virtual void getPos( unsigned int x[6], unsigned int &storePosTr, unsigned int &storePosRot ) const = 0;
-
virtual void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value) = 0;
@@ -62,10 +59,15 @@ public:
virtual void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels) = 0;
+ virtual void increaseDatum(float x[], WorkspaceRepresentation
+ *workspace) = 0;
+
+ /*!
virtual void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
unsigned int x3, unsigned int x4, unsigned int x5) = 0;
virtual void increaseDatum( unsigned int x[6] ) = 0;
+ */
/*!
Set rotation data for given x,y,z position.
*/
@@ -84,7 +86,7 @@ public:
virtual bool hasEntry(unsigned int x, unsigned int y, unsigned int z) = 0;
- // Set all entries to 0
+ // clear all entrys
virtual void clear() = 0;
virtual unsigned char getMaxEntry() const = 0;
@@ -104,6 +106,7 @@ public:
virtual unsigned char** getRawData() = 0;
virtual WorkspaceData* clone() = 0;
+
};
diff --git a/VirtualRobot/Workspace/WorkspaceDataArray.cpp b/VirtualRobot/Workspace/WorkspaceDataArray.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0800b3dea1597dd8a96ebc9dc0fcf69bea3ee8f
--- /dev/null
+++ b/VirtualRobot/Workspace/WorkspaceDataArray.cpp
@@ -0,0 +1,303 @@
+#include "WorkspaceDataArray.h"
+
+#include <fstream>
+#include <cmath>
+#include <float.h>
+#include <limits.h>
+
+namespace VirtualRobot
+{
+
+WorkspaceDataArray::WorkspaceDataArray(unsigned int size1, unsigned int size2, unsigned int size3,
+ unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow)
+{
+ unsigned long long sizeTr = (unsigned long long)size1 * (unsigned long long)size2 * (unsigned long long)size3;
+ unsigned long long sizeRot = (unsigned long long)size4 * (unsigned long long)size5 * (unsigned long long)size6;
+ sizes[0] = size1;
+ sizes[1] = size2;
+ sizes[2] = size3;
+ sizes[3] = size4;
+ sizes[4] = size5;
+ sizes[5] = size6;
+ sizeTr0 = sizes[1]*sizes[2];
+ sizeTr1 = sizes[2];
+ sizeRot0 = sizes[4]*sizes[5];
+ sizeRot1 = sizes[5];
+
+ if (sizeRot>UINT_MAX || sizeTr>UINT_MAX )
+ {
+ VR_ERROR << "Could not assign " << sizeRot << " bytes of memory (>UINT_MAX). Reduce size of reachability space..." << endl;
+ }
+ try
+ {
+ data = new unsigned char*[(unsigned int)sizeTr];
+ for (unsigned int x=0;x<size1;x++)
+ {
+ for (unsigned int y=0;y<size2;y++)
+ {
+ for (unsigned int z=0;z<size3;z++)
+ {
+ data[x*sizeTr0+y*sizeTr1+z] = NULL;
+ }
+ }
+ }
+ } catch (const std::exception &e)
+ {
+ VR_ERROR << "Exception: " << e.what() << endl << "Could not assign " << sizeRot << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ } catch (...)
+ {
+ VR_ERROR << "Could not assign " << sizeRot << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ }
+
+ minValidValue = 1;
+ maxEntry = 0;
+ voxelFilledCount = 0;
+ this->adjustOnOverflow = adjustOnOverflow;
+}
+
+WorkspaceDataArray::WorkspaceDataArray(WorkspaceDataArray* other)
+{
+ VR_ASSERT(other);
+ for (int i=0;i<6;i++)
+ this->sizes[i] = other->sizes[i];
+ unsigned long long sizeTr = (unsigned long long)sizes[0] * (unsigned long long)sizes[1] * (unsigned long long)sizes[2];
+ unsigned long long sizeRot = (unsigned long long)sizes[3] * (unsigned long long)sizes[4] * (unsigned long long)sizes[5];
+ sizeTr0 = sizes[1]*sizes[2];
+ sizeTr1 = sizes[2];
+ sizeRot0 = sizes[4]*sizes[5];
+ sizeRot1 = sizes[5];
+
+ if (sizeRot>UINT_MAX || sizeTr>UINT_MAX )
+ {
+ VR_ERROR << "Could not assign " << sizeRot << " bytes of memory (>UINT_MAX). Reduce size of reachability space..." << endl;
+ }
+ try
+ {
+ data = new unsigned char*[(unsigned int)sizeTr];
+ for (unsigned int x=0;x<sizes[0];x++)
+ {
+ for (unsigned int y=0;y<sizes[1];y++)
+ {
+ for (unsigned int z=0;z<sizes[2];z++)
+ {
+ int pos = x*sizeTr0+y*sizeTr1+z;
+ if (other->data[pos] != NULL)
+ {
+ data[pos] = new unsigned char[(unsigned int)sizeRot];
+ memcpy(data[pos],other->data[pos],(unsigned int)sizeRot*sizeof(unsigned char));
+ }
+ else
+ {
+ data[pos] = NULL;
+ }
+ }
+ }
+ }
+ } catch (const std::exception &e)
+ {
+ VR_ERROR << "Exception: " << e.what() << endl << "Could not assign " << sizeRot << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ } catch (...)
+ {
+ VR_ERROR << "Could not assign " << sizeRot << " bytes of memory. Reduce size of reachability space..." << endl;
+ throw;
+ }
+
+ minValidValue = other->minValidValue;
+ maxEntry = other->maxEntry;
+ voxelFilledCount = other->voxelFilledCount;
+ this->adjustOnOverflow = other->adjustOnOverflow;
+}
+
+WorkspaceDataArray::~WorkspaceDataArray()
+{
+ for (unsigned int x=0;x<sizes[0];x++)
+ {
+ for (unsigned int y=0;y<sizes[1];y++)
+ {
+ for (unsigned int z=0;z<sizes[2];z++)
+ {
+ delete[] data[x*sizeTr0+y*sizeTr1+z];
+ }
+ }
+ }
+ delete[] data;
+}
+
+unsigned int WorkspaceDataArray::getSizeTr() const
+{
+ return sizes[0]*sizes[1]*sizes[2];
+}
+
+unsigned int WorkspaceDataArray::getSizeRot() const
+{
+ return sizes[3]*sizes[4]*sizes[5];
+}
+
+void WorkspaceDataArray::ensureData(unsigned int x, unsigned int y, unsigned int z)
+{
+ if (data[x*sizeTr0+y*sizeTr1+z])
+ return;
+ unsigned long long sizeRot = (unsigned long long)sizes[3] * (unsigned long long)sizes[4] * (unsigned long long)sizes[5];
+ data[x*sizeTr0+y*sizeTr1+z] = new unsigned char[(unsigned int)sizeRot];
+ memset(data[x*sizeTr0+y*sizeTr1+z],0,(unsigned int)sizeRot*sizeof(unsigned char));
+}
+
+void WorkspaceDataArray::setDataRot(unsigned char *data, unsigned int x, unsigned int y, unsigned int z)
+{
+ ensureData(x,y,z);
+ memcpy(this->data[x*sizeTr0+y*sizeTr1+z], data, getSizeRot()*sizeof(unsigned char));
+}
+
+const unsigned char *WorkspaceDataArray::getDataRot(unsigned int x, unsigned int y, unsigned int z)
+{
+ ensureData(x,y,z);
+ return data[x*sizeTr0+y*sizeTr1+z];
+}
+
+unsigned char WorkspaceDataArray::getMaxEntry() const
+{
+ return maxEntry;
+}
+
+unsigned int WorkspaceDataArray::getVoxelFilledCount() const
+{
+ return voxelFilledCount;
+}
+
+void WorkspaceDataArray::binarize()
+{
+ unsigned int posTr = 0, posRot = 0;
+ for (unsigned int a=0;a<sizes[0];a++)
+ {
+ for (unsigned int b=0;b<sizes[1];b++)
+ {
+ for (unsigned int c=0;c<sizes[2];c++)
+ {
+ for (unsigned int d=0;d<sizes[3];d++)
+ {
+ for (unsigned int e=0;e<sizes[4];e++)
+ {
+ for (unsigned int f=0;f<sizes[5];f++)
+ {
+ getPos(a,b,c,d,e,f, posTr, posRot);
+ if (data[posTr])
+ if (data[posTr][posRot]>minValidValue)
+ data[posTr][posRot] = minValidValue;
+ }
+ }
+ }
+ }
+ }
+ }
+ maxEntry = minValidValue;
+}
+
+void WorkspaceDataArray::bisectData()
+{
+ unsigned int posTr = 0, posRot = 0;
+ for (unsigned int x0=0;x0<sizes[0];x0++)
+ for (unsigned int x1=0;x1<sizes[1];x1++)
+ for (unsigned int x2=0;x2<sizes[2];x2++)
+ for (unsigned int x3=0;x3<sizes[3];x3++)
+ for (unsigned int x4=0;x4<sizes[4];x4++)
+ for (unsigned int x5=0;x5<sizes[5];x5++)
+ {
+ getPos(x0,x1,x2,x3,x4,x5, posTr, posRot);
+ if (data[posTr])
+ if (data[posTr][posRot]>minValidValue)
+ {
+ data[posTr][posRot] /= 2;
+ if (data[posTr][posRot]<minValidValue)
+ data[posTr][posRot] = minValidValue;
+ }
+ }
+ if (maxEntry>minValidValue)
+ {
+ maxEntry = maxEntry / 2;
+ if (maxEntry<minValidValue)
+ maxEntry = minValidValue;
+ }
+}
+
+void WorkspaceDataArray::setDatumCheckNeighbors( unsigned int x[6], unsigned char value, unsigned int neighborVoxels )
+{
+ setDatum(x,value);
+ if (neighborVoxels==0)
+ return;
+ int minX[6];
+ int maxX[6];
+ for (int i=0;i<6;i++)
+ {
+ minX[i] = x[i] - neighborVoxels;
+ maxX[i] = x[i] + neighborVoxels;
+ if (minX[i]<0)
+ minX[i] = 0;
+ if (maxX[i]>=(int)sizes[i])
+ maxX[i] = sizes[i]-1;
+ }
+
+ for (int a=minX[0]; a<=maxX[0]; a++)
+ for (int b=minX[1]; b<=maxX[1]; b++)
+ for (int c=minX[2]; c<=maxX[2]; c++)
+ for (int d=minX[3]; d<=maxX[3]; d++)
+ for (int e=minX[4]; e<=maxX[4]; e++)
+ for (int f=minX[5]; f<=maxX[5]; f++)
+ {
+ if (get(a,b,c,d,e,f)<value)
+ {
+ setDatum((unsigned int)a,(unsigned int)b,(unsigned int)c,(unsigned int)d,(unsigned int)e,(unsigned int)f,value);
+ }
+ }
+}
+
+void WorkspaceDataArray::increaseDatum(float x[6], WorkspaceRepresentation *workspace)
+{
+ // get voxels
+ unsigned int v[6];
+ if (workspace->getVoxelFromPose(x,v))
+ {
+ increaseDatum(v);
+ }
+}
+
+void WorkspaceDataArray::clear()
+{
+ for (unsigned int x=0;x<sizes[0];x++)
+ {
+ for (unsigned int y=0;y<sizes[1];y++)
+ {
+ for (unsigned int z=0;z<sizes[2];z++)
+ {
+ if (data[x*sizeTr0+y*sizeTr1+z])
+ {
+ delete [] data[x*sizeTr0+y*sizeTr1+z];
+ data[x*sizeTr0+y*sizeTr1+z] = NULL;
+ }
+ }
+ }
+ }
+ maxEntry = 0;
+ voxelFilledCount = 0;
+}
+
+bool WorkspaceDataArray::hasEntry( unsigned int x, unsigned int y, unsigned int z )
+{
+ if (x<0 || y<0 || z<0 || x>=sizes[0] || y>=sizes[1] || z>=sizes[2])
+ return false;
+ return (data[x*sizeTr0+y*sizeTr1+z]!=NULL);
+}
+
+void WorkspaceDataArray::setMinValidValue( unsigned char v )
+{
+ minValidValue = v;
+}
+
+WorkspaceData *WorkspaceDataArray::clone()
+{
+ return new WorkspaceDataArray(this);
+}
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/WorkspaceDataArray.h b/VirtualRobot/Workspace/WorkspaceDataArray.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f63df8cb510e5c13b1cc0105211f49e7a2ed830
--- /dev/null
+++ b/VirtualRobot/Workspace/WorkspaceDataArray.h
@@ -0,0 +1,214 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Peter Kaiser, Nikolaus Vahrenkamp
+* @copyright 2011 Peter Kaiser, Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_WorkspaceDataArray_h_
+#define _VirtualRobot_WorkspaceDataArray_h_
+
+#include "../VirtualRobotImportExport.h"
+
+#include <boost/enable_shared_from_this.hpp>
+#include <boost/type_traits/is_base_of.hpp>
+#include <boost/mpl/assert.hpp>
+
+#include <vector>
+#include "WorkspaceData.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
+namespace VirtualRobot
+{
+/*!
+ Stores a 6-dimensional array for the vertex data of a workspace representation.
+ Internally unsigned char data types are used (0...255)
+*/
+class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceDataArray : public WorkspaceData ,boost::enable_shared_from_this<WorkspaceDataArray>
+{
+public:
+ /*!
+ Constructor, fills the data with 0
+ */
+ WorkspaceDataArray(unsigned int size1, unsigned int size2, unsigned int size3,
+ unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow);
+
+ //! Clone other data structure
+ WorkspaceDataArray(WorkspaceDataArray *other);
+
+ ~WorkspaceDataArray();
+
+ //! Return the amount of data in bytes
+ unsigned int getSizeTr() const;
+ unsigned int getSizeRot() const;
+
+ inline void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value)
+ {
+ ensureData(x0,x1,x2);
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
+ if (data[posTr][posRot]==0)
+ voxelFilledCount++;
+ data[posTr][posRot] = value;
+ if (value >= maxEntry)
+ maxEntry = value;
+ }
+
+ inline void setDatum(unsigned int x[6], unsigned char value)
+ {
+ ensureData(x[0],x[1],x[2]);
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x,posTr,posRot);
+ if (data[posTr][posRot]==0)
+ voxelFilledCount++;
+ data[posTr][posRot] = value;
+ if (value >= maxEntry)
+ maxEntry = value;
+ }
+
+ void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels);
+
+ void increaseDatum(float x[], WorkspaceRepresentation *workspace);
+
+ inline void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5)
+ {
+ ensureData(x0,x1,x2);
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
+ unsigned char e = data[posTr][posRot];
+ if (e==0)
+ voxelFilledCount++;
+ if (e<UCHAR_MAX)
+ {
+ data[posTr][posRot]++;
+ if (e >= maxEntry)
+ maxEntry = e+1;
+ } else if (adjustOnOverflow)
+ bisectData();
+ }
+ inline void increaseDatum( unsigned int x[6] )
+ {
+ ensureData(x[0],x[1],x[2]);
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x,posTr,posRot);
+ unsigned char e = data[posTr][posRot];
+ if (e==0)
+ voxelFilledCount++;
+ if (e<UCHAR_MAX)
+ {
+ data[posTr][posRot]++;
+ if (e >= maxEntry)
+ maxEntry = e+1;
+ } else if (adjustOnOverflow)
+ bisectData();
+ }
+ /*!
+ Set rotation data for given x,y,z position.
+ */
+ void setDataRot(unsigned char *data, unsigned int x, unsigned int y, unsigned int z);
+ /*!
+ Get rotation data for given x,y,z position.
+ */
+ const unsigned char *getDataRot(unsigned int x, unsigned int y, unsigned int z);
+
+ //! Simulates a multi-dimensional array access
+ inline unsigned char get(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5) const
+ {
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
+ if (data[posTr])
+ return data[posTr][posRot];
+ else
+ return 0;
+ }
+
+ //! Simulates a multi-dimensional array access
+ inline unsigned char get( unsigned int x[6] ) const
+ {
+ unsigned int posTr = 0, posRot = 0;
+ getPos(x,posTr,posRot);
+ if (data[posTr])
+ return data[posTr][posRot];
+ else
+ return 0;
+ }
+
+ bool hasEntry(unsigned int x, unsigned int y, unsigned int z);
+
+ // Set all entries to 0
+ void clear();
+
+ unsigned char getMaxEntry() const;
+ unsigned int getVoxelFilledCount() const;
+ void binarize();
+
+ void bisectData();
+
+ void setVoxelFilledCount(int c){voxelFilledCount = c;}
+ void setMaxEntry(unsigned char m){maxEntry = m;}
+
+ unsigned int getSize(int dim){return sizes[dim];}
+
+ //! Min valid value is 1 by default. In cases some values are needed to indicate special flags (e.g. stability) the minimum valid number can be set here
+ void setMinValidValue(unsigned char v);
+
+ unsigned char** getRawData(){return data;}
+
+ WorkspaceData* clone();
+protected:
+
+ void ensureData(unsigned int x, unsigned int y, unsigned int z);
+
+ inline void getPos( unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5 ,
+ unsigned int &storePosTr, unsigned int &storePosRot) const
+ {
+ storePosTr = x0 * sizeTr0 + x1 * sizeTr1 + x2;
+ storePosRot = x3 * sizeRot0 + x4 * sizeRot1 + x5;
+ }
+
+ inline void getPos( unsigned int x[6], unsigned int &storePosTr, unsigned int &storePosRot ) const
+ {
+ storePosTr = x[0] * sizeTr0 + x[1] * sizeTr1 + x[2];
+ storePosRot = x[3] * sizeRot0 + x[4] * sizeRot1 + x[5];
+ }
+
+ unsigned int sizes[6];
+ unsigned int sizeTr0,sizeTr1;
+ unsigned int sizeRot0,sizeRot1;
+
+ unsigned char** data;
+
+ unsigned char minValidValue;
+
+ unsigned char maxEntry;
+ unsigned int voxelFilledCount;
+ bool adjustOnOverflow;
+};
+
+
+
+} // namespace VirtualRobot
+
+#endif // _WorkspaceData_h_
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index 7d9cf3ddb907b93d21323cb9e3aead6235748c64..50aa4e78adb3f0b7836c3e338b76c28fa3928220 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -766,12 +766,7 @@ void WorkspaceRepresentation::addPose(const Eigen::Matrix4f &globalPose)
achievedMaxValues[i] = x[i];
}
- // get voxels
- unsigned int v[6];
- if (getVoxelFromPose(x,v))
- {
- data->increaseDatum(v);
- }
+ data->increaseDatum(x, this);
buildUpLoops++;
}
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
index df6cb033b2cefa7550dd6be4a4931098215041c3..2e19e61c98bb86614d3e898f8aa77bbce90e99fb 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -334,6 +334,7 @@ public:
void matrix2Vector(const Eigen::Matrix4f &m, float x[6]) const;
void vector2Matrix(const float x[6], Eigen::Matrix4f &m) const;
void vector2Matrix( const Eigen::Vector3f &pos, const Eigen::Vector3f &rot, Eigen::Matrix4f &m ) const;
+ virtual bool getVoxelFromPose(float x[6], unsigned int v[6]) const;
/*!
Usually not needed. Don't call this method after data has been loaded or created!
@@ -367,8 +368,6 @@ protected:
//! Compress the data
unsigned char *compressData(const unsigned char *source, int size, int &compressedSize);
- virtual bool getVoxelFromPose(float x[6], unsigned int v[6]) const;
-
virtual Eigen::Matrix4f getToLocalTransformation() const;
virtual Eigen::Matrix4f getToGlobalTransformation() const;