CC_FILE_ERROR DepthMapFileFilter::saveToFile(QString filename, ccGBLSensor* sensor)
{
assert(sensor);
if (!sensor)
{
return CC_FERR_BAD_ARGUMENT;
}
//the depth map associated to this sensor
const ccGBLSensor::DepthBuffer& db = sensor->getDepthBuffer();
if (db.zBuff.empty())
{
ccLog::Warning(QString("[DepthMap] sensor '%1' has no associated depth map (you must compute it first)").arg(sensor->getName()));
return CC_FERR_NO_SAVE; //this is not a severe error (the process can go on)
}
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(sensor->getParent());
if (!cloud)
{
ccLog::Warning(QString("[DepthMap] sensor '%1' is not associated to a point cloud!").arg(sensor->getName()));
//this is not a severe error (the process can go on)
}
//opening file
FILE* fp = fopen(qPrintable(filename),"wt");
if (!fp)
{
ccLog::Error(QString("[DepthMap] Can't open file '%1' for writing!").arg(filename));
return CC_FERR_WRITING;
}
fprintf(fp,"// SENSOR DEPTH MAP\n");
fprintf(fp,"// Associated cloud: %s\n",qPrintable(cloud ? cloud->getName() : "none"));
fprintf(fp,"// Pitch = %f [ %f : %f ]\n",
sensor->getPitchStep(),
sensor->getMinPitch(),
sensor->getMaxPitch());
fprintf(fp,"// Yaw = %f [ %f : %f ]\n",
sensor->getYawStep(),
sensor->getMinYaw(),
sensor->getMaxYaw());
fprintf(fp,"// Range = %f\n",sensor->getSensorRange());
fprintf(fp,"// L = %i\n",db.width);
fprintf(fp,"// H = %i\n",db.height);
fprintf(fp,"/////////////////////////\n");
//an array of projected normals (same size a depth map)
ccGBLSensor::NormalGrid* theNorms = NULL;
//an array of projected colors (same size a depth map)
ccGBLSensor::ColorGrid* theColors = NULL;
//if the sensor is associated to a "ccPointCloud", we may also extract
//normals and color!
if (cloud && cloud->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(cloud);
unsigned nbPoints = cloud->size();
if (nbPoints == 0)
{
ccLog::Warning(QString("[DepthMap] sensor '%1' is associated to an empty cloud?!").arg(sensor->getName()));
//this is not a severe error (the process can go on)
}
else
{
//if possible, we create the array of projected normals
if (pc->hasNormals())
{
NormsTableType* decodedNorms = new NormsTableType;
if (decodedNorms->reserve(nbPoints))
{
for (unsigned i=0; i<nbPoints; ++i)
decodedNorms->addElement(pc->getPointNormal(i).u);
theNorms = sensor->projectNormals(pc,*decodedNorms);
decodedNorms->clear();
}
else
{
ccLog::Warning(QString("[DepthMap] not enough memory to load normals on sensor '%1'!").arg(sensor->getName()));
}
decodedNorms->release();
decodedNorms = 0;
}
//if possible, we create the array of projected colors
if (pc->hasColors())
{
GenericChunkedArray<3,ColorCompType>* rgbColors = new GenericChunkedArray<3,ColorCompType>();
rgbColors->reserve(nbPoints);
for (unsigned i=0; i<nbPoints; ++i)
{
//conversion from ColorCompType[3] to unsigned char[3]
const ColorCompType* col = pc->getPointColor(i);
rgbColors->addElement(col);
}
theColors = sensor->projectColors(pc,*rgbColors);
rgbColors->clear();
rgbColors->release();
rgbColors = 0;
}
}
}
const ScalarType* _zBuff = &(db.zBuff.front());
if (theNorms)
theNorms->placeIteratorAtBegining();
if (theColors)
theColors->placeIteratorAtBegining();
for (unsigned k=0; k<db.height; ++k)
{
for (unsigned j=0; j<db.width; ++j, ++_zBuff)
{
//grid index and depth
fprintf(fp,"%u %u %.12f",j,k,*_zBuff);
//color
if (theColors)
{
const ColorCompType* C = theColors->getCurrentValue();
fprintf(fp," %i %i %i",C[0],C[1],C[2]);
theColors->forwardIterator();
}
//normal
if (theNorms)
{
const PointCoordinateType* N = theNorms->getCurrentValue();
fprintf(fp," %f %f %f",N[0],N[1],N[2]);
theNorms->forwardIterator();
}
fprintf(fp,"\n");
}
}
fclose(fp);
fp = 0;
if (theNorms)
{
theNorms->release();
theNorms = 0;
}
if (theColors)
{
theColors->release();
theColors = 0;
}
return CC_FERR_NO_ERROR;
}
CC_FILE_ERROR DepthMapFileFilter::saveToOpenedFile(FILE* fp, ccGBLSensor* sensor)
{
assert(fp && sensor);
if (!sensor->getParent()->isKindOf(CC_TYPES::POINT_CLOUD))
{
ccLog::Warning(QString("[DepthMap] sensor '%1' is not associated to a point cloud!").arg(sensor->getName()));
return CC_FERR_NO_ERROR; //this is not a severe error (the process can go on)
}
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(sensor->getParent());
//the depth map associated to this sensor
const ccGBLSensor::DepthBuffer& db = sensor->getDepthBuffer();
fprintf(fp,"// CLOUDCOMPARE DEPTH MAP\n");
fprintf(fp,"// Associated cloud: %s\n",qPrintable(cloud->getName()));
fprintf(fp,"// dPhi = %f [ %f : %f ]\n",
sensor->getDeltaPhi(),
sensor->getPhiMin(),
sensor->getPhiMax());
fprintf(fp,"// dTheta = %f [ %f : %f ]\n",
sensor->getDeltaTheta(),
sensor->getThetaMin(),
sensor->getThetaMax());
fprintf(fp,"// pMax = %f\n",sensor->getSensorRange());
fprintf(fp,"// L = %i\n",db.width);
fprintf(fp,"// H = %i\n",db.height);
fprintf(fp,"/////////////////////////\n");
//an array of projected normals (same size a depth map)
PointCoordinateType* theNorms = NULL;
//an array of projected colors (same size a depth map)
colorType* theColors = NULL;
//if the sensor is associated to a "ccPointCloud", we may also extract
//normals and color!
if (cloud->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(cloud);
unsigned nbPoints = cloud->size();
if (nbPoints == 0)
{
ccLog::Warning(QString("[DepthMap] sensor '%1' is associated to an empty cloud!").arg(sensor->getName()));
return CC_FERR_NO_ERROR; //this is not a severe error (the process can go on)
}
else
{
//if possible, we create the array of projected normals
if (pc->hasNormals())
{
NormsTableType* decodedNorms = new NormsTableType;
if (decodedNorms->reserve(nbPoints))
{
for (unsigned i=0; i<nbPoints; ++i)
decodedNorms->addElement(pc->getPointNormal(i).u);
theNorms = sensor->projectNormals(pc,*decodedNorms);
decodedNorms->clear();
}
else
{
ccLog::Warning(QString("[DepthMap] not enough memory to load normals on sensor '%1'!").arg(sensor->getName()));
}
decodedNorms->release();
decodedNorms = 0;
}
//if possible, we create the array of projected colors
if (pc->hasColors())
{
GenericChunkedArray<3,colorType>* rgbColors = new GenericChunkedArray<3,colorType>();
rgbColors->reserve(nbPoints);
for (unsigned i=0; i<nbPoints; ++i)
{
//conversion from colorType[3] to unsigned char[3]
const colorType* col = pc->getPointColor(i);
rgbColors->addElement(col);
}
theColors = sensor->projectColors(pc,*rgbColors);
rgbColors->clear();
rgbColors->release();
rgbColors = 0;
}
}
}
PointCoordinateType* _theNorms = theNorms;
colorType* _theColors = theColors;
ScalarType* _zBuff = db.zBuff;
for (unsigned k=0; k<db.height; ++k)
{
for (unsigned j=0; j<db.width; ++j)
{
//grid index and depth
fprintf(fp,"%i %i %.12f",j,k,*_zBuff++);
//color
if (_theColors)
{
fprintf(fp," %i %i %i",_theColors[0],_theColors[1],_theColors[2]);
_theColors += 3;
}
//normal
if (_theNorms)
{
fprintf(fp," %f %f %f",_theNorms[0],_theNorms[1],_theNorms[2]);
_theNorms += 3;
}
fprintf(fp,"\n");
}
}
if (theNorms)
delete[] theNorms;
if (theColors)
delete[] theColors;
return CC_FERR_NO_ERROR;
}