ModelPtr MultiPointCloud::model( )
{
// Count all points that need to be exported
pc_attr_it it;
size_t c = 0;
for(it = m_clouds.begin(); it != m_clouds.end(); it++)
{
PointCloud* pc = it->second->cloud;
if(pc->isActive())
{
vector<uColorVertex>::iterator p_it;
for(p_it = pc->m_points.begin(); p_it != pc->m_points.end(); p_it++)
{
c++;
}
}
}
// Create a new model and save points
PointBufferPtr pcBuffer( new PointBuffer);
floatArr pointBuffer(new float[3 * c]);
ucharArr colorBuffer(new unsigned char[3 * c]);
c = 0;
for(it = m_clouds.begin(); it != m_clouds.end(); it++)
{
PointCloud* pc = it->second->cloud;
if(pc->isActive())
{
vector<uColorVertex>::iterator p_it;
for(p_it = pc->m_points.begin(); p_it != pc->m_points.end(); p_it++)
{
size_t bufferPos = 3 * c;
uColorVertex v = *p_it;
pointBuffer[bufferPos ] = v.x;
pointBuffer[bufferPos + 1] = v.y;
pointBuffer[bufferPos + 2] = v.z;
colorBuffer[bufferPos ] = v.r;
colorBuffer[bufferPos + 1] = v.g;
colorBuffer[bufferPos + 2] = v.b;
c++;
}
}
}
pcBuffer->setPointArray(pointBuffer, c);
pcBuffer->setPointColorArray(colorBuffer, c);
ModelPtr modelPtr(new Model);
modelPtr->m_pointCloud = pcBuffer;
return modelPtr;
}
ModelPtr DatIO::read(string filename, int n, int reduction)
{
ModelPtr model( new Model);
PointBufferPtr pointBuffer(new PointBuffer);
// Allocate point buffer and read data from file
int c = 0;
ifstream in(filename.c_str(), std::ios::binary);
int numPoints = 0;
in.read((char*)&numPoints, sizeof(int));
// Determine modulo value suitable for desired reduction
int mod_filter = 1;
if(reduction != 0 && numPoints > reduction)
{
mod_filter = (int)(numPoints / reduction);
cout << timestamp << "Reduction mode. Reading every " << mod_filter << "th point." << endl;
numPoints = reduction;
}
else
{
reduction = numPoints; // needed for progress estimation
}
float* pointArray = new float[3 * numPoints];
unsigned char* colorArray = new unsigned char[3 * numPoints];
float* buffer = new float[n-1];
int reflect;
string msg = timestamp.getElapsedTime() + "Loading points";
ProgressBar progress(numPoints, msg);
int d = 0;
while(in.good())
{
memset(buffer, 0, (n - 1) * sizeof(float));
in.read((char*)buffer, (n - 1) * sizeof(float));
in.read((char*)&reflect, sizeof(int));
if(c % mod_filter == 0 && d < numPoints)
{
// Copy point data to point array
int pos = 3 * d;
pointArray[pos] = buffer[0];
pointArray[pos + 1] = buffer[1];
pointArray[pos + 2] = buffer[2];
if(n > 3)
{
colorArray[pos] = (unsigned char)reflect;
colorArray[pos + 1] = (unsigned char)reflect;
colorArray[pos + 2] = (unsigned char)reflect;
}
else
{
colorArray[pos] = (unsigned char)0;
colorArray[pos + 1] = (unsigned char)0;
colorArray[pos + 2] = (unsigned char)0;
}
d++;
++progress;
}
c++;
}
delete[] buffer;
in.close();
// Truncate arrays to actual size
pointArray = (float*)realloc(pointArray, 3 * d * sizeof(float));
colorArray = (unsigned char*)realloc(colorArray, 3 * d * sizeof(unsigned char));
cout << timestamp << "Creating point buffer with " << d << "points." << endl;
// Setup model pointer
floatArr parr(pointArray);
ucharArr carr(colorArray);
pointBuffer->setPointArray(parr, d);
pointBuffer->setPointColorArray(carr, d);
model->m_pointCloud = pointBuffer;
return model;
}
