NormalCalculator.icpp

/***********************************************************************
NormalCalculator - Functor classes to calculate a normal vector for each
point in a LiDAR data set.
Copyright (c) 2008-2011 Oliver Kreylos

This file is part of the LiDAR processing and analysis package.

The LiDAR processing and analysis package 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; either
version 2 of the License, or (at your option) any later version.

The LiDAR processing and analysis package 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.

You should have received a copy of the GNU General Public License along
with the LiDAR processing and analysis package; if not, write to the
Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
02111-1307 USA
***********************************************************************/

#define NORMALCALCULATOR_IMPLEMENTATION

#include "NormalCalculator.h"

#include <iostream>
#include <iomanip>
#include <Misc/ThrowStdErr.h>
#include <Misc/File.h>
#include <Math/Math.h>

namespace {

/**************
Helper classes:
**************/

class FindPoint // Class to find a point inside an octree node
	{
	/* Elements: */
	private:
	Point queryPoint; // The position of the point to find
	const LidarPoint* foundPoint; // The found LiDAR point
	
	/* Constructors and destructors: */
	public:
	FindPoint(const Point& sQueryPoint)
		:queryPoint(sQueryPoint),
		 foundPoint(0)
		{
		}
	
	/* Methods: */
	void operator()(const LidarPoint& lp)
		{
		if(Geometry::sqrDist(lp,queryPoint)==Scalar(0))
			foundPoint=&lp;
		}
	const Point& getQueryPoint(void) const
		{
		return queryPoint;
		}
	Scalar getQueryRadius2(void) const
		{
		return Scalar(0);
		}
	const LidarPoint* getFoundPoint(void) const
		{
		return foundPoint;
		}
	};

class NormalAverager // Class to average normal vectors of collapsed points during subsampling
	{
	/* Elements: */
	private:
	Point queryPoint; // The LiDAR point whose neighbors to find
	Scalar radius2; // Squared radius of search sphere around query point
	const Vector& pointNormal; // Normal vector onto which to project near normals before averaging
	const LidarPoint* pointBase; // Base pointer of processed node's point array
	const Vector* normalBase; // Base pointer of processed node's normal vector array
	Vector normal; // The averaged normal vector
	
	/* Constructors and destructors: */
	public:
	NormalAverager(const Point& sQueryPoint,Scalar sRadius2,const Vector& sPointNormal)
		:queryPoint(sQueryPoint),
		 radius2(sRadius2),
		 pointNormal(sPointNormal),
		 pointBase(0),normalBase(0),
		 normal(Vector::zero)
		{
		}
	
	/* Methods: */
	void setArrays(const LidarPoint* sPointBase,const Vector* sNormalBase) // Sets the point and normal vector arrays for the next process
		{
		pointBase=sPointBase;
		normalBase=sNormalBase;
		}
	void operator()(const LidarPoint& lp)
		{
		/* Check if the point is inside the search radius: */
		if(Geometry::sqrDist(lp,queryPoint)<=radius2)
			{
			/* Get the point's normal vector by mapping its index into the normal array: */
			const Vector& nearNormal=normalBase[&lp-pointBase];
			
			/* Accumulate the result normal: */
			if(nearNormal*pointNormal>=Scalar(0))
				normal+=nearNormal;
			else
				normal-=nearNormal;
			}
		}
	const Point& getQueryPoint(void) const
		{
		return queryPoint;
		}
	Scalar getQueryRadius2(void) const
		{
		return radius2;
		}
	const Vector& getNormal(void) const // Returns the averaged normal vector
		{
		return normal;
		}
	};

}

/*************************************
Methods of class NodeNormalCalculator:
*************************************/

template <class NormalCalculatorParam>
inline
void*
NodeNormalCalculator<NormalCalculatorParam>::calcThreadMethod(
	unsigned int threadIndex)
	{
	Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
	
	/* Create this thread's normal calculator: */
	NormalCalculator nc(normalCalculator);
	
	while(true)
		{
		/* Wait on the calculation barrier until there is a job: */
		calcBarrier.synchronize();
		if(shutdownThreads)
			break;
		
		/* Process the current node: */
		unsigned int firstI=(threadIndex*currentNode->getNumPoints())/numThreads;
		unsigned int lastI=((threadIndex+1)*currentNode->getNumPoints())/numThreads;
		for(unsigned int i=firstI;i<lastI;++i)
			{
			/* Prepare the normal calculator: */
			nc.prepare((*currentNode)[i]);
			
			/* Process the point's neighborhood: */
			lpo.processPointsDirected(nc);
			
			/* Get the point's normal vector: */
			if(nc.getNumPoints()>=3)
				{
				/* Get the fitted plane's normal vector: */
				normalBuffer[i]=nc.calcPlane().getNormal();
				}
			else
				{
				/* Solitary point; assign dummy normal vector: */
				normalBuffer[i]=Vector::zero;
				
				if(saveOutliers)
					{
					Threads::Mutex::Lock outlierLock(outlierMutex);
					outliers[numOutliers]=(*currentNode)[i];
					++numOutliers;
					}
				}
			}
		
		/* Synchronize on the calculation barrier to signal job completion: */
		calcBarrier.synchronize();
		}
	
	return 0;
	}

template <class NormalCalculatorParam>
inline
void*
NodeNormalCalculator<NormalCalculatorParam>::subsampleThreadMethod(
	unsigned int threadIndex)
	{
	Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
	
	while(true)
		{
		/* Wait on the subsampling barrier until there is a job: */
		subsampleBarrier.synchronize();
		if(shutdownThreads)
			break;
		
		/* Find the points that were collapsed onto each node point and average their normal vectors: */
		Scalar averageRadius2=Math::sqr(currentNode->getDetailSize()*Scalar(1.5));
		unsigned int firstI=(threadIndex*currentNode->getNumPoints())/numThreads;
		unsigned int lastI=((threadIndex+1)*currentNode->getNumPoints())/numThreads;
		for(unsigned int i=firstI;i<lastI;++i)
			{
			/*****************************************************************
			Find the exact normal vector of this point's "ancestor" to
			properly average normals:
			*****************************************************************/
			
			/* Find the child node containing this point's ancestor: */
			int pointChildIndex=currentNode->getDomain().findChild((*currentNode)[i]);
			LidarProcessOctree::Node* pointChild=currentChildren[pointChildIndex];
			
			/* Find the point's ancestor: */
			FindPoint fp((*currentNode)[i]);
			lpo.processNodePointsDirected(pointChild,fp);
			if(fp.getFoundPoint()==0)
				{
				/* This is an internal corruption in the octree file. Print a helpful and non-offensive error message: */
				Misc::throwStdErr("Fatal error: Octree file corrupted around position (%f, %f, %f)",(*currentNode)[i][0],(*currentNode)[i][1],(*currentNode)[i][2]);
				}
			
			/* Retrieve the ancestor's normal vector: */
			const Vector& pointNormal=childNormalBuffers[pointChildIndex][fp.getFoundPoint()-pointChild->getPoints()];
			
			/* Create a functor to average normal vectors from the point's neighborhood: */
			NormalAverager normalAverager((*currentNode)[i],averageRadius2,pointNormal);
			for(int childIndex=0;childIndex<8;++childIndex)
				{
				/* Check if the child node's domain overlaps the search sphere: */
				if(currentChildren[childIndex]->getDomain().sqrDist((*currentNode)[i])<=averageRadius2)
					{
					/* Search for neighbors in this child node: */
					normalAverager.setArrays(currentChildren[childIndex]->getPoints(),childNormalBuffers[childIndex]);
					lpo.processNodePointsDirected(currentChildren[childIndex],normalAverager);
					}
				}
			
			/* Store the averaged normal vector: */
			normalBuffer[i]=normalAverager.getNormal();
			if(normalBuffer[i]!=Vector::zero)
				normalBuffer[i].normalize();
			}
		
		/* Synchronize on the subsampling barrier to signal job completion: */
		subsampleBarrier.synchronize();
		}
	
	return 0;
	}

template <class NormalCalculatorParam>
inline
NodeNormalCalculator<NormalCalculatorParam>::NodeNormalCalculator(
	LidarProcessOctree& sLpo,
	const typename NodeNormalCalculator<NormalCalculatorParam>::NormalCalculator& sNormalCalculator,
	const char* normalFileName,
	unsigned int sNumThreads)
	:lpo(sLpo),
	 normalCalculator(sNormalCalculator),
	 normalBuffer(new Vector[lpo.getMaxNumPointsPerNode()]),
	 normalDataSize(sizeof(Scalar)*3),
	 normalFile(normalFileName,LidarFile::ReadWrite),
	 saveOutliers(false),numOutliers(0),outliers(0),outlierFile(0),
	 numThreads(sNumThreads),shutdownThreads(false),
	 calcThreads(new Threads::Thread[numThreads]),calcBarrier(numThreads+1),
	 subsampleThreads(new Threads::Thread[numThreads]),subsampleBarrier(numThreads+1),
	 currentNode(0),
	 numProcessedNodes(0)
	{
	/* Create the child normal buffers: */
	for(int i=0;i<8;++i)
		childNormalBuffers[i]=new Vector[lpo.getMaxNumPointsPerNode()];
	
	/* Write the normal file's header: */
	normalFile.setEndianness(Misc::LittleEndian);
	LidarDataFileHeader dfh((unsigned int)(normalDataSize));
	dfh.write(normalFile);
	
	/* Start the worker threads: */
	for(unsigned int i=0;i<numThreads;++i)
		{
		calcThreads[i].start(this,&NodeNormalCalculator::calcThreadMethod,i);
		subsampleThreads[i].start(this,&NodeNormalCalculator::subsampleThreadMethod,i);
		}
	}

template <class NormalCalculatorParam>
inline
NodeNormalCalculator<NormalCalculatorParam>::~NodeNormalCalculator(
	void)
	{
	/* Shut down all threads: */
	shutdownThreads=true;
	calcBarrier.synchronize();
	subsampleBarrier.synchronize();
	for(unsigned int i=0;i<numThreads;++i)
		{
		calcThreads[i].join();
		subsampleThreads[i].join();
		}
	delete[] calcThreads;
	delete[] subsampleThreads;
	
	/* Delete all buffers: */
	delete[] normalBuffer;
	for(int i=0;i<8;++i)
		delete[] childNormalBuffers[i];
	
	delete[] outliers;
	delete outlierFile;
	}

template <class NormalCalculatorParam>
inline
void
NodeNormalCalculator<NormalCalculatorParam>::saveOutlierPoints(
	const char* outlierFileName)
	{
	saveOutliers=true;
	numOutliers=0;
	outliers=new Point[lpo.getMaxNumPointsPerNode()];
	outlierFile=new Misc::File(outlierFileName,"wt");
	}

template <class NormalCalculatorParam>
inline
void
NodeNormalCalculator<NormalCalculatorParam>::operator()(
	LidarProcessOctree::Node& node,
	unsigned int nodeLevel)
	{
	currentNode=&node;
	
	/* Check if this node is a leaf or an interior node: */
	if(node.isLeaf())
		{
		/* Wake up the calculation threads: */
		calcBarrier.synchronize();
		
		/* Wait for their completion: */
		calcBarrier.synchronize();
		
		if(saveOutliers)
			{
			/* Write all outlier points to the file: */
			for(size_t i=0;i<numOutliers;++i)
				fprintf(outlierFile->getFilePtr(),"%.6f %.6f %.6f\n",outliers[i][0],outliers[i][1],outliers[i][2]);
			
			numOutliers=0;
			}
		}
	else
		{
		/* Get pointers to the node's children and load their normal vector arrays: */
		normalFile.flush();
		for(int childIndex=0;childIndex<8;++childIndex)
			{
			currentChildren[childIndex]=lpo.getChild(currentNode,childIndex);
			if(currentChildren[childIndex]->getNumPoints()>0)
				{
				normalFile.setReadPosAbs(LidarDataFileHeader::getFileSize()+normalDataSize*currentChildren[childIndex]->getDataOffset());
				normalFile.read(childNormalBuffers[childIndex],currentChildren[childIndex]->getNumPoints());
				}
			}
		
		/* Wake up the subsampling threads: */
		subsampleBarrier.synchronize();
		
		/* Wait for their completion: */
		subsampleBarrier.synchronize();
		}
	
	/* Write the node's normal vectors to the normal file: */
	normalFile.setWritePosAbs(LidarDataFileHeader::getFileSize()+normalDataSize*currentNode->getDataOffset());
	normalFile.write(normalBuffer,currentNode->getNumPoints());
	normalFile.flush();
	
	/* Update the progress counter: */
	++numProcessedNodes;
	int percent=int((numProcessedNodes*100+lpo.getNumNodes()/2)/lpo.getNumNodes());
	std::cout<<"\b\b\b\b"<<std::setw(3)<<percent<<"%"<<std::flush;
	}