Instantly share code, notes, and snippets.

Embed
What would you like to do?
A simple, generic point cloud class example implemented with C++
/*
* This file is licensed under the MIT License (https://opensource.org/licenses/MIT)
* Developed by Tomi Paananen.
* https://github.com/tompaana/
* http://tomipaananen.azurewebsites.net/?p=2543
*/
#include "pointcloud.h"
#include <limits>
#include <stdexcept>
#include <stdio.h>
template class PointCloud<short>;
template class PointCloud<unsigned short>;
template class PointCloud<int>;
template class PointCloud<unsigned int>;
/**
* Constructor.
*
* @param pointHasValue If true, the points in the cloud will have a value associated with them.
* @param chunkSizeAsPointCount Defines the size of chunks to be allocated when the point count
* increases.
*/
template <typename T>
PointCloud<T>::PointCloud(bool pointHasValue, unsigned int chunkSizeAsPointCount)
: mData(NULL),
mCurrentPointCount(0),
mPointsHaveValues(pointHasValue),
mChunkSizeAsPointCount(chunkSizeAsPointCount),
mPointLengthAsElementCount(DimensionCount + (pointHasValue ? 1 : 0))
{
mCurrentAllocatedDataSizeAsElementCount = mPointLengthAsElementCount * mChunkSizeAsPointCount;
mData = (T*)malloc(mCurrentAllocatedDataSizeAsElementCount * sizeof(T));
memset(mData, Undefined, mCurrentAllocatedDataSizeAsElementCount * sizeof(T));
T min = std::numeric_limits<T>::min();
T max = std::numeric_limits<T>::max();
for (short i = 0; i < DimensionCount; ++i)
{
mMin[i] = max;
mMax[i] = min;
}
}
/**
* Destructor.
*/
template <typename T>
PointCloud<T>::~PointCloud()
{
delete[] mData;
}
/**
* @returns The data of this point cloud as an array.
*/
template <typename T>
const T* PointCloud<T>::data() const
{
return mData;
}
/**
* @returns The current point count.
*/
template <typename T>
unsigned long PointCloud<T>::pointCount() const
{
return mCurrentPointCount;
}
/**
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @returns True, if a point with the given coordinates exists in the point cloud. False otherwise.
*/
template <typename T>
bool PointCloud<T>::hasPoint(T x, T y, T z) const
{
return (accessorToPoint(x, y, z) != NULL);
}
/**
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @returns The value of the point in the given coordinates. Will return 'Undefined' if no point is
* found and will throw an exception if the points in this point cloud do not have a value
* associated with them.
*/
template <typename T>
T PointCloud<T>::pointValue(T x, T y, T z) const
{
if (!mPointsHaveValues)
{
throw std::exception("The points in this point cloud have no values");
}
T* point = accessorToPoint(x, y, z);
if (point != NULL)
{
return point[mPointLengthAsElementCount - 1];
}
return Undefined;
}
/**
* Adds a point to the point cloud with the given coordinates (and value if supported).
* Note that there cannot be two points with the exact same coordinates.
*
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @param value The value of the point (given that this point cloud has values associated with the points).
* @returns True, if the point was added. False otherwise.
*/
template <typename T>
bool PointCloud<T>::addPoint(T x, T y, T z, T value)
{
if (hasPoint(x, y, z))
{
return false;
}
unsigned long index = mCurrentPointCount * mPointLengthAsElementCount;
bool canAdd = (index + 1 < mCurrentAllocatedDataSizeAsElementCount);
if (!canAdd && !increaseDataByChunkSize())
{
return false;
}
mData[index] = x;
mData[index + 1] = y;
mData[index + 2] = z;
if (mPointsHaveValues)
{
mData[index + 3] = value;
}
T point[DimensionCount] = { x, y, z };
for (short i = 0; i < DimensionCount; ++i)
{
if (mMin[i] > point[i])
{
mMin[i] = point[i];
}
if (mMax[i] < point[i])
{
mMax[i] = point[i];
}
}
mCurrentPointCount++;
return true;
}
/**
* Sets the value of the point in the given coordinates.
* If this point cloud has no value associated with the points, an exception will be thrown.
*
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @param value The value of the point.
* @param addPointIfDoesNotExist If true, the point will be added if one didn't exist before.
*/
template <typename T>
bool PointCloud<T>::setPointValue(T x, T y, T z, T value, bool addPointIfDoesNotExist)
{
bool wasPointValueSet = false;
if (!mPointsHaveValues)
{
throw std::exception("The points in this point cloud have no values");
}
T* pointStart = accessorToPoint(x, y, z);
if (pointStart != NULL)
{
pointStart[mPointLengthAsElementCount - 1] = value;
wasPointValueSet = true;
}
else if (addPointIfDoesNotExist)
{
wasPointValueSet = addPoint(x, y, z, value);
}
return wasPointValueSet;
}
/**
* Adds the given value to the current value of the point in the given coordinates.
* If this point cloud has no value associated with the points, an exception will be thrown.
*
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @param valueToAdd The value to add to the current value of the point.
*/
template <typename T>
bool PointCloud<T>::addToPointValue(T x, T y, T z, T valueToAdd)
{
if (!mPointsHaveValues)
{
throw std::exception("The points in this point cloud have no values");
}
T* pointStart = accessorToPoint(x, y, z);
if (pointStart != NULL)
{
pointStart[mPointLengthAsElementCount - 1] += valueToAdd;
return true;
}
return false;
}
/**
* @returns True, if the point cloud has reached its maximum allocation. False otherwise.
*/
template <typename T>
bool PointCloud<T>::maxSizeReached() const
{
return ((mCurrentAllocatedDataSizeAsElementCount + mChunkSizeAsPointCount * mPointLengthAsElementCount) * sizeof(T)
> MaxDataSizeInBytes);
}
/**
* @returns The length of a point as element count (e.g. if a point consists of three coordinates
* and has one value associated with it, this method will return 4).
*/
template <typename T>
unsigned short PointCloud<T>::pointLengthAsElementCount() const
{
return mPointLengthAsElementCount;
}
/**
* @returns The size allocated for this point cloud as element count.
* For example, if the length of a point is 4 (see pointLengthAsElementCount()) and there are
* chunks allocated to house 1000 points, the return value of this method will be 4000.
* Note that the allocated size can (and in many cases is) larger than the size of the defined points.
*/
template <typename T>
unsigned long PointCloud<T>::allocatedDataSizeAsElementCount() const
{
return mCurrentAllocatedDataSizeAsElementCount;
}
/**
* @returns The minimum boundaries (of all dimensions) of the points in the point cloud.
*/
template <typename T>
const T* PointCloud<T>::min() const
{
return mMin;
}
/**
* @returns The maximum boundaries (of all dimensions) of the points in the point cloud.
*/
template <typename T>
const T* PointCloud<T>::max() const
{
return mMax;
}
/**
* Allocates a new chunk for the point cloud unless the maximum size is reached.
*
* @returns True, if a new chunk was allocated. False otherwise.
*/
template <typename T>
bool PointCloud<T>::increaseDataByChunkSize()
{
if (maxSizeReached())
{
return false;
}
unsigned int chunkSizeAsElementCount = mPointLengthAsElementCount * mChunkSizeAsPointCount;
unsigned long newallocatedDataSizeAsElementCount =
mCurrentAllocatedDataSizeAsElementCount + chunkSizeAsElementCount;
T* newData = (T*)malloc(newallocatedDataSizeAsElementCount * sizeof(T));
memcpy(newData, mData, mCurrentAllocatedDataSizeAsElementCount * sizeof(T));
T* newChunkStart = newData + mCurrentAllocatedDataSizeAsElementCount;
memset(newChunkStart, Undefined, chunkSizeAsElementCount * sizeof(T));
delete[] mData;
mData = newData;
mCurrentAllocatedDataSizeAsElementCount = newallocatedDataSizeAsElementCount;
return true;
}
/**
* @param x The X coordinate of the point.
* @param y The Y coordinate of the point.
* @param z The Z coordinate of the point.
* @returns The accessor (a pointer to the beginning) of the point in the given coordinates.
* NULL, if no point found.
*/
template <typename T>
inline T* PointCloud<T>::accessorToPoint(T x, T y, T z) const
{
const long maxIndex = mCurrentPointCount * mPointLengthAsElementCount;
for (long i = 0; i < maxIndex; i += mPointLengthAsElementCount)
{
if (mData[i] == x && mData[i + 1] == y && mData[i + 2] == z)
{
return &mData[i];
}
}
return NULL;
}
/*
* This file is licensed under the MIT License (https://opensource.org/licenses/MIT)
* Developed by Tomi Paananen.
* https://github.com/tompaana/
* http://tomipaananen.azurewebsites.net/?p=2543
*/
#ifndef POINTCLOUD_H
#define POINTCLOUD_H
const unsigned short DimensionCount = 3;
const unsigned long MaxDataSizeInBytes = 104857600;
const unsigned int DefaultChunkSizeAsPointCount = 1000;
const int Undefined = 0;
template <typename T>
class PointCloud
{
public:
PointCloud(bool pointsHaveValues, unsigned int chunkSizeAsPointCount = DefaultChunkSizeAsPointCount);
virtual ~PointCloud();
public:
const T* data() const;
unsigned long pointCount() const;
bool hasPoint(T x, T y, T z) const;
T pointValue(T x, T y, T z) const;
bool addPoint(T x, T y, T z, T value = Undefined);
bool setPointValue(T x, T y, T z, T value, bool addPointIfDoesNotExist = true);
bool addToPointValue(T x, T y, T z, T valueToAdd);
bool maxSizeReached() const;
unsigned short pointLengthAsElementCount() const;
unsigned long allocatedDataSizeAsElementCount() const;
const T* min() const;
const T* max() const;
private:
bool increaseDataByChunkSize();
T* accessorToPoint(T x, T y, T z) const;
public:
private:
T* mData;
unsigned long mCurrentPointCount;
unsigned long mCurrentAllocatedDataSizeAsElementCount; // == mCurrentPointCount * mPointLengthAsElementCount
unsigned int mChunkSizeAsPointCount;
unsigned short mPointLengthAsElementCount; // E.g. three dimensions (x, y, z) plus a value -> 4
T mMin[DimensionCount];
T mMax[DimensionCount];
bool mPointsHaveValues;
};
#endif // POINTCLOUD_H
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment