q3DMASC/q3DMASCTools.cpp

//##########################################################################
//#                                                                        #
//#                     CLOUDCOMPARE PLUGIN: q3DMASC                       #
//#                                                                        #
//#  This program is free software; you can redistribute it and/or modify  #
//#  it under the terms of the GNU General Public License as published by  #
//#  the Free Software Foundation; version 2 or later of the License.      #
//#                                                                        #
//#  This program is distributed in the hope that it will be useful,       #
//#  but WITHOUT ANY WARRANTY; without even the implied warranty of        #
//#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the          #
//#  GNU General Public License for more details.                          #
//#                                                                        #
//#                 COPYRIGHT: Dimitri Lague / CNRS / UEB                  #
//#                                                                        #
//##########################################################################

#include "q3DMASCTools.h"

//Local
#include "PointFeature.h"
#include "NeighborhoodFeature.h"
#include "DualCloudFeature.h"
#include "ContextBasedFeature.h"

//qCC_io
#include <FileIOFilter.h>
#include <LASFields.h>
//qCC_db
#include <ccScalarField.h>
#include <ccPointCloud.h>

//Qt
#include <QTextStream>
#include <QFile>
#include <QFileInfo>
#include <QDir>

//system
#include <assert.h>

using namespace masc;

bool Tools::SaveFeatureDescriptors(QString filename, const Feature::Set& features)
{
	QFile file(filename);
	if (!file.open(QFile::Text | QFile::WriteOnly))
	{
		ccLog::Warning(QString("Can't open file '%1' for writing").arg(filename));
		return false;
	}

	QTextStream stream(&file);

	//header
	stream << "#3DMASC classifier" << endl;

	return true;
}

bool Tools::LoadFile(	QString filename,
						Feature::Set& rawFeatures,
						std::vector<ccPointCloud*>& loadedClouds,
						CorePoints& corePoints)
{
	QFileInfo fi(filename);
	if (!fi.exists())
	{
		ccLog::Warning(QString("Can't find file '%1'").arg(filename));
		return false;
	}

	QFile file(filename);
	if (!file.open(QFile::Text | QFile::ReadOnly))
	{
		ccLog::Warning(QString("Can't open file '%1'").arg(filename));
		return false;
	}

	try
	{
		assert(rawFeatures.empty());
		std::vector<double> scales;
		QMap<QString, ccPointCloud* > clouds;

		QTextStream stream(&file);
		for (int lineNumber = 0; ; ++lineNumber)
		{
			QString line = stream.readLine();
			if (line.isNull())
			{
				//eof
				break;
			}
			++lineNumber;

			if (line.startsWith("#"))
			{
				//comment
				continue;
			}

			//strip out the potential comment at the end of the line as well
			int commentIndex = line.indexOf('#');
			if (commentIndex >= 0)
				line = line.left(commentIndex);

			QString upperLine = line.toUpper();
			if (upperLine.startsWith("CLOUD:")) //clouds
			{
				QString command = line.mid(6);
				QStringList tokens = command.split('=');
				if (tokens.size() != 2)
				{
					ccLog::Warning("Malformed file: expecting 2 tokens after 'cloud:' on line #" + QString::number(lineNumber));
					return false;
				}
				QString pcName = tokens[0].trimmed();
				QString pcFilename = fi.absoluteDir().absoluteFilePath(tokens[1].trimmed());
				//try to open the cloud
				{
					FileIOFilter::LoadParameters parameters;
					parameters.alwaysDisplayLoadDialog = false;
					CC_FILE_ERROR error = CC_FERR_NO_ERROR;
					ccHObject* object = FileIOFilter::LoadFromFile(pcFilename, parameters, error);
					if (error != CC_FERR_NO_ERROR || !object)
					{
						//error message already issued
						if (object)
							delete object;
						return false;
					}
					ccHObject::Container cloudsInFile;
					object->filterChildren(cloudsInFile, false, CC_TYPES::POINT_CLOUD, true);
					if (cloudsInFile.empty())
					{
						ccLog::Warning("File doesn't contain a single cloud");
						delete object;
						return false;
					}
					else if (cloudsInFile.size() > 1)
					{
						ccLog::Warning("File contains more than one cloud, only the first one will be kept");
					}
					ccPointCloud* pc = static_cast<ccPointCloud*>(cloudsInFile.front());
					for (size_t i = 1; i < cloudsInFile.size(); ++i)
					{
						delete cloudsInFile[i];
					}
					if (pc->getParent())
						pc->getParent()->detachChild(pc);
					pc->setName(pcName); //DGM: warning, may not be acceptable in the GUI version?
					clouds.insert(pcName, pc);
					loadedClouds.push_back(pc);
				}
			}
			else if (upperLine.startsWith("CORE_POINTS:")) //core points
			{
				if (corePoints.origin)
				{
					ccLog::Warning("Malformed file: can't declare core points twice! (line #" + QString::number(lineNumber) + ")");
					return false;
				}
				QString command = line.mid(12);
				QStringList tokens = command.split('_');
				if (tokens.empty())
				{
					ccLog::Warning("Malformed file: expecting tokens after 'core_points:' on line #" + QString::number(lineNumber));
					return false;
				}
				QString pcName = tokens[0].trimmed();
				if (!clouds.contains(pcName))
				{
					ccLog::Warning(QString("Malformed file: unknown cloud '%1' on line #%2 (make sure it is declared before the core points)").arg(pcName).arg(lineNumber));
					return false;
				}
				corePoints.origin = clouds[pcName];

				//should we sub-sample the origin cloud?
				if (tokens.size() > 1)
				{
					if (tokens[1].toUpper() == "SS")
					{
						if (tokens.size() < 3)
						{
							ccLog::Warning("Malformed file: missing token after 'SS' on line #" + QString::number(lineNumber));
							return false;
						}
						QString options = tokens[2];
						if (options.startsWith('R'))
						{
							corePoints.selectionMethod = CorePoints::RANDOM;
						}
						else if (options.startsWith('S'))
						{
							corePoints.selectionMethod = CorePoints::SPATIAL;
						}
						else
						{
							ccLog::Warning("Malformed file: unknown option after 'SS' on line #" + QString::number(lineNumber));
							return false;
						}

						//read the subsampling parameter (ignore the first character)
						bool ok = false;
						corePoints.selectionParam = options.mid(1).toDouble(&ok);
						if (!ok)
						{
							ccLog::Warning("Malformed file: expecting a number after 'SS_X' on line #" + QString::number(lineNumber));
							return false;
						}

					} //end of subsampling options
				}
			}
			else if (upperLine.startsWith("SCALES:")) //scales
			{
				if (!scales.empty())
				{
					ccLog::Warning("Malformed file: scales defined twice (line #" + QString::number(lineNumber) + ")");
					return false;
				}

				QString command = line.mid(7);
				QStringList tokens = command.split(';');
				if (tokens.empty())
				{
					ccLog::Warning("Malformed file: expecting at least one token after 'scales:' on line #" + QString::number(lineNumber));
					return false;
				}

				for (const QString& token : tokens)
				{
					if (token.contains(':'))
					{
						//it's probably a range
						QStringList subTokens = token.trimmed().split(':');
						if (subTokens.size() != 3)
						{
							ccLog::Warning(QString("Malformed file: expecting 3 tokens for a range of scales (%1)").arg(token));
							return false;
						}
						bool ok[3] = { true, true, true };
						double start = subTokens[0].trimmed().toDouble(ok);
						double step = subTokens[1].toDouble(ok + 1);
						double stop = subTokens[2].toDouble(ok + 2);
						if (!ok[0] || !ok[1] || !ok[2])
						{
							ccLog::Warning(QString("Malformed file: invalid values in scales range (%1) on line #%2").arg(token).arg(lineNumber));
							return false;
						}
						if (stop < start || step <= 1.0 - 6)
						{
							ccLog::Warning(QString("Malformed file: invalid range (%1) on line #%2").arg(token).arg(lineNumber));
							return false;
						}

						for (double v = start; v <= stop + 1.0e-6; v += step)
						{
							scales.push_back(v);
						}
					}
					else
					{
						bool ok = true;
						double v = token.trimmed().toDouble(&ok);
						if (!ok)
						{
							ccLog::Warning(QString("Malformed file: invalid scale value (%1) on line #%2").arg(token).arg(lineNumber));
							return false;
						}
						scales.push_back(v);
					}
				}
				scales.shrink_to_fit();
			}
			else if (upperLine.startsWith("FEATURE:")) //feature
			{
				QString command = line.mid(8);
				QStringList tokens = command.split('_');
				if (tokens.empty())
				{
					ccLog::Warning("Malformed file: expecting at least one token after 'feature:' on line #" + QString::number(lineNumber));
					return false;
				}

				Feature::Shared feature;

				//read the type
				QString typeStr = tokens[0].trimmed().toUpper();
				{
					for (int iteration = 0; iteration < 1; ++iteration) //fake loop for easy break
					{
						PointFeature::PointFeatureType pointFeatureType = PointFeature::FromUpperString(typeStr);
						if (pointFeatureType != PointFeature::Invalid)
						{
							//we have a point feature
							PointFeature* pointFeature = new PointFeature(pointFeatureType);

							//specific case: 'SF#'
							if (pointFeatureType == PointFeature::SF)
							{
								QString sfIndexStr = typeStr.mid(2);
								bool ok = true;
								int sfIndex = sfIndexStr.toInt(&ok);
								if (!ok)
								{
									ccLog::Warning(QString("Malformed file: expecting a valid integer value after 'SF' on line #%1").arg(lineNumber));
									delete pointFeature;
									return false;
								}
								pointFeature->sourceSFIndex = sfIndex;
							}

							feature.reset(pointFeature);
							break;
						}

						NeighborhoodFeature::NeighborhoodFeatureType neighborhoodFeatureType = NeighborhoodFeature::FromUpperString(typeStr);
						if (neighborhoodFeatureType != NeighborhoodFeature::Invalid)
						{
							//we have a neighborhood feature
							feature = NeighborhoodFeature::Shared(new NeighborhoodFeature(neighborhoodFeatureType));
							break;
						}

						ContextBasedFeature::ContextBasedFeatureType contextBasedFeatureType = ContextBasedFeature::FromUpperString(typeStr);
						if (contextBasedFeatureType != ContextBasedFeature::Invalid)
						{
							//we have a context-based feature
							feature = ContextBasedFeature::Shared(new ContextBasedFeature(contextBasedFeatureType));
							break;
						}

						DualCloudFeature::DualCloudFeatureType dualCloudFeatureType = DualCloudFeature::FromUpperString(typeStr);
						if (dualCloudFeatureType != DualCloudFeature::Invalid)
						{
							//we have a dual cloud feature
							feature = DualCloudFeature::Shared(new DualCloudFeature(dualCloudFeatureType));
							break;
						}

						if (!feature)
						{
							ccLog::Warning(QString("Malformed file: unrecognized token '%1' after 'feature:' on line #%2").arg(typeStr).arg(lineNumber));
							return false;
						}
					}
				}
				assert(feature);

				//read the scales
				bool useAllScales = false;
				{
					QString scaleStr = tokens[1].toUpper();
					if (!scaleStr.startsWith("SC"))
					{
						ccLog::Warning(QString("Malformed file: unrecognized token '%1' (expecting the scale descriptor 'SC...' on line #%2").arg(typeStr).arg(lineNumber));
						return false;
					}

					if (scaleStr == "SC0")
					{
						//no scale
					}
					else if (scaleStr == "SCX")
					{
						//all scales
						useAllScales = true;
					}
					else
					{
						//read the specific scale index
						bool ok = true;
						feature->scale = scaleStr.mid(2).toDouble(&ok);
						if (!ok)
						{
							ccLog::Warning(QString("Malformed file: expecting a valid number after 'SC:' on line #%1").arg(lineNumber));
							return false;
						}
					}
				}

				//process the next tokens (may not be ordered)
				int cloudCount = 0;
				bool statDefined = false;
				bool mathDefined = false;
				for (int i = 2; i < tokens.size(); ++i)
				{
					QString token = tokens[i].trimmed().toUpper();

					//is the token a 'stat' one?
					if (!statDefined)
					{
						if (token == "MEAN")
						{
							feature->stat = Feature::MEAN;
							statDefined = true;
						}
						else if (token == "MODE")
						{
							feature->stat = Feature::MODE;
							statDefined = true;
						}
						else if (token == "STD")
						{
							feature->stat = Feature::STD;
							statDefined = true;
						}
						else if (token == "RANGE")
						{
							feature->stat = Feature::RANGE;
							statDefined = true;
						}
						else if (token == "SKEW")
						{
							feature->stat = Feature::SKEW;
							statDefined = true;
						}

						if (statDefined)
						{
							continue;
						}
					}

					//is the token a cloud name?
					if (cloudCount < 2)
					{
						bool cloudNameMatches = false;
						for (QMap<QString, ccPointCloud* >::const_iterator it = clouds.begin(); it != clouds.end(); ++it)
						{
							QString key = it.key().toUpper();
							if (key == token)
							{
								if (cloudCount == 0)
								{
									feature->cloud1 = it.value();
									feature->cloud1Label = key;
								}
								else if (cloudCount == 1)
								{
									feature->cloud2 = it.value();
									feature->cloud2Label = key;
								}
								else
								{
									//we can't fall here
									assert(false);
								}
								++cloudCount;
								cloudNameMatches = true;
								break;
							}
						}

						if (cloudNameMatches)
						{
							continue;
						}
					}

					//is the token a 'math' one?
					if (!mathDefined)
					{
						if (token == "MINUS")
						{
							feature->op = Feature::MINUS;
							mathDefined = true;
						}
						else if (token == "PLUS")
						{
							feature->op = Feature::PLUS;
							mathDefined = true;
						}
						else if (token == "DIVIDE")
						{
							feature->op = Feature::DIVIDE;
							mathDefined = true;
						}
						else if (token == "MULTIPLY")
						{
							feature->op = Feature::MULTIPLY;
							mathDefined = true;
						}

						if (mathDefined)
						{
							continue;
						}
					}

					//is the token a 'context' descriptor?
					if (feature->getType() == Feature::Type::ContextBasedFeature && token.startsWith("CTX"))
					{
						//read the context label
						QString ctxLabelStr = token.mid(2);
						bool ok = true;
						int ctxLabel = ctxLabelStr.toInt(&ok);
						if (!ok)
						{
							ccLog::Warning(QString("Malformed file: expecting a valid integer value after 'CTX' on line #%1").arg(lineNumber));
							return false;
						}
						static_cast<ContextBasedFeature*>(feature.data())->ctxClassLabel = ctxLabel;
						continue;
					}

					//if we are here, it means we couldn't find a correspondance for the current token
					ccLog::Warning(QString("Malformed file: unrecognized or unexpected token '%1' on line #%2").arg(token).arg(lineNumber));
					return false;
				}

				//now create the various versions of rules (if any)
				if (useAllScales)
				{
					if (scales.empty())
					{
						ccLog::Warning("Malformed file: 'SCx' token used but not scales were defined" + QString(" (line %1)").arg(lineNumber));
						return false;
					}
					feature->scale = scales.front();

					//we will duplicate the original feature AFTER having checked its consistency!
				}

				//now check the consistency of the rule
				QString errorMessage;
				if (!feature->checkValidity(errorMessage))
				{
					ccLog::Warning("Malformed feature: " + errorMessage + QString(" (line %1)").arg(lineNumber));
					return false;
				}

				//save it
				rawFeatures.push_back(feature);

				if (useAllScales)
				{
					for (size_t i = 1; i < scales.size(); ++i)
					{
						//copy the original rule
						Feature::Shared newFeature = feature->clone();
						newFeature->scale = scales[i];

						//as we only change the scale value, all the duplicated features should be valid
						assert(newFeature->checkValidity(errorMessage));

						rawFeatures.push_back(newFeature);
					}
				}
			}
			else
			{
				ccLog::Warning(QString("Line #%1: unrecognized token/command: ").arg(lineNumber) + (line.length() < 10 ? line : line.left(10) + "..."));
				return false;
			}
		}
	}
	catch (const std::bad_alloc&)
	{
		ccLog::Warning("Not enough memory");
		return false;
	}

	rawFeatures.shrink_to_fit();

	return true;
}

CCLib::ScalarField* Tools::RetrieveSF(const ccPointCloud* cloud, const QString& sfName, bool caseSensitive/*=true*/)
{
	if (!cloud)
	{
		assert(false);
		return nullptr;
	}
	int sfIdx = -1;
	if (caseSensitive)
	{
		sfIdx = cloud->getScalarFieldIndexByName(qPrintable(sfName));
	}
	else
	{
		QString sfNameUpper = sfName.toUpper();
		for (unsigned i = 0; i < cloud->getNumberOfScalarFields(); ++i)
		{
			if (QString(cloud->getScalarField(i)->getName()).toUpper() == sfNameUpper)
			{
				sfIdx = static_cast<int>(i);
				break;
			}
		}
	}
	
	if (sfIdx >= 0)
	{
		return cloud->getScalarField(sfIdx);
	}
	else
	{
		return nullptr;
	}
}

bool Tools::PrepareFeatures(const CorePoints& corePoints, Feature::Set& features, QString& error, CCLib::GenericProgressCallback* progressCb/*=nullptr*/)
{
	if (features.empty() || !corePoints.origin)
	{
		//invalid input parameters
		assert(false);
		return false;
	}
	
	for (const Feature::Shared& feature : features)
	{
		QString errorMessage("invalid pointer");
		if (!feature || !feature->checkValidity(errorMessage))
		{
			error = "Invalid rule/feature: " + error;
			return false;
		}

		//prepare the feature
		if (!feature->prepare(corePoints, error, progressCb))
		{
			//something failed (error should be up to date)
			return false;
		}
	}

	return true;
}

bool Tools::RandomSubset(ccPointCloud* cloud, float ratio, CCLib::ReferenceCloud* inRatioSubset, CCLib::ReferenceCloud* outRatioSubset)
{
	if (!cloud)
	{
		ccLog::Warning("Invalid input cloud");
		return false;
	}
	if (!inRatioSubset || !outRatioSubset)
	{
		ccLog::Warning("Invalid input refence clouds");
		return false;
	}
	if (inRatioSubset->getAssociatedCloud() != cloud || outRatioSubset->getAssociatedCloud() != cloud)
	{
		ccLog::Warning("Invalid input reference clouds (associated cloud is wrong)");
		return false;
	}
	if (ratio < 0.0f || ratio > 1.0f)
	{
		ccLog::Warning(QString("Invalid parameter (ratio: %1)").arg(ratio));
		return false;
	}

	unsigned inSampleCount = static_cast<unsigned>(floor(cloud->size() * ratio));
	assert(inSampleCount <= cloud->size());
	unsigned outSampleCount = cloud->size() - inSampleCount;

	//we draw the smallest population (faster)
	unsigned targetCount = inSampleCount;
	bool defaultState = false;
	if (outSampleCount < inSampleCount)
	{
		targetCount = outSampleCount;
		defaultState = true;
	}

	//reserve memory
	std::vector<bool> pointInsideRatio;
	try
	{
		pointInsideRatio.resize(cloud->size(), defaultState);
	}
	catch (const std::bad_alloc&)
	{
		ccLog::Warning("Not enough memory");
		return false;
	}

	if (!inRatioSubset->reserve(inSampleCount) || !outRatioSubset->reserve(outSampleCount))
	{
		ccLog::Warning("Not enough memory");
		inRatioSubset->clear();
		outRatioSubset->clear();
		return false;
	}

	//randomly choose the 'in' or 'out' indexes
	int randIndex = 0;
	unsigned randomCount = 0;
	while (randomCount < targetCount)
	{
		randIndex = ((randIndex + std::rand()) % cloud->size());
		if (pointInsideRatio[randIndex] == defaultState)
		{
			pointInsideRatio[randIndex] = !defaultState;
			++randomCount;
		}
	}

	//now dispatch the points
	{
		for (unsigned i = 0; i < cloud->size(); ++i)
		{
			if (pointInsideRatio[i])
				inRatioSubset->addPointIndex(i);
			else
				outRatioSubset->addPointIndex(i);
		}
		assert(inRatioSubset->size() == inSampleCount);
		assert(outRatioSubset->size() == outSampleCount);
	}

	return true;
}