void DatIO::save(string filename)
{
PointBufferPtr pointBuffer = m_model->m_pointCloud;
float buffer[4];
if(pointBuffer)
{
ofstream out(filename.c_str(), std::ios::binary);
if(out.good())
{
size_t numPoints;
size_t numIntensities;
floatArr pointArray = pointBuffer->getPointArray(numPoints);
floatArr intensityArray = pointBuffer->getPointIntensityArray(numIntensities);
float buffer[4];
cout << timestamp << "Writing " << numPoints << " to " << filename << endl;
for(size_t i = 0; i < numPoints; i++)
{
memset(buffer, 0, 4 * sizeof(float));
size_t pos = i * 3;
buffer[0] = pointArray[pos];
buffer[1] = pointArray[pos + 1];
buffer[2] = pointArray[pos + 2];
if(intensityArray)
{
buffer[3] = intensityArray[i];
}
out.write((char*)buffer, 4 * sizeof(float));
}
out.close();
}
else
{
cout << timestamp << "DatIO: Unable to open file " << filename << " for writing." << endl;
}
}
}
int main( int argc, char ** argv )
{
// Parse command line arguments
convert::Options options(argc, argv);
// Exit if options had to generate a usage message
// (this means required parameters are missing)
if ( options.printUsage() )
{
return 0;
}
::std::cout << options << ::std::endl;
// Read input file
ModelPtr model = ModelFactory::readModel(options.getInputFile());
if(model)
{
if(model->m_pointCloud)
{
// Get parameters
bool convert = options.convertIntensity();
bool filterIntensity = options.getMinIntensity() > -1e10 || options.getMaxIntensity() < 1e10;
// Check if we have to modify some data
if(convert || filterIntensity)
{
PointBufferPtr points = model->m_pointCloud;
size_t numPoints = points->getNumPoints();
if(points)
{
size_t n;
if(convert && !points->getPointColorArray(n))
{
cout << timestamp << "Allocating new point color array." << endl;
ucharArr colors(new unsigned char[numPoints * 3]);
points->setPointColorArray(colors, numPoints);
}
float max_i = -1e10;
float min_i = +1e10;
float maxCutOff = options.getMaxIntensity();
float minCutOff = options.getMinIntensity();
floatArr intensities = points->getPointIntensityArray(n);
if(intensities)
{
// Cutoff intensity values
for(size_t i = 0; i < numPoints; i++)
{
//cout << intensities[i] << endl;
if(intensities[i] < minCutOff) intensities[i] = minCutOff;
if(intensities[i] > maxCutOff) intensities[i] = maxCutOff;
if(intensities[i] < min_i) min_i = intensities[i];
if(intensities[i] > max_i) max_i = intensities[i];
}
// Map intensities to 0 .. 255
cout << max_i << " " << min_i << endl;
float r_diff = max_i - min_i;
if(r_diff > 0)
{
float b_size = r_diff / 255.0;
for(int a = 0; a < numPoints; a++)
{
//cout << intensities[a] << endl;
float value = intensities[a];
value -= min_i;
value /= b_size;
intensities[a] = value;
//cout << intensities[a] << endl << endl;
}
}
// Convert
if(convert)
{
ucharArr colors = points->getPointColorArray(n);
GradientType gradType = GREY;
string gradientName = options.getColorMap();
if(gradientName == "HOT") gradType = HOT;
if(gradientName == "HSV") gradType = HSV;
if(gradientName == "SHSV") gradType = SHSV;
if(gradientName == "JET") gradType = JET;
ColorMap colorMap(255);
for(size_t i = 0; i < numPoints; i++)
{
float color[3];
colorMap.getColor(color, (size_t)intensities[i], gradType );
colors[3 * i ] = (unsigned char)(color[0] * 255);
colors[3 * i + 1] = (unsigned char)(color[1] * 255);
colors[3 * i + 2] = (unsigned char)(color[2] * 255);
//cout << intensities[i] << endl;
//cout << (int) colors[3 * i ] << " " << (int)colors[3 * i + 1] << " " << (int)colors[3 * i + 2] << endl;
}
points->setPointColorArray(colors, numPoints);
}
model->m_pointCloud = points;
}
else
{
cout << timestamp << "Model contains no point intensities to convert." << endl;
}
}
}
}
ModelFactory::saveModel(model, options.getOutputFile());
}
else
{
cout << timestamp << "Error reading file " << options.getInputFile() << endl;
}
}
