Пример #1
0
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;
}
Пример #2
0
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;
}