lineapproximator.h

这是一个GPS相关的程序

/* Plot Graphic Library

	Copyright (C) 2002 Pelikhan, Inc. All rights reserved
	Go to http://eauminerale.syldavie.csam.ucl.ac.be/peli/pgl/pgl.html for the latest PGL binaries
	This software is NOT freeware.

	This software is provided "as is", with no warranty.
	Read the license agreement provided with the files

	This software, and all accompanying files, data and materials, are distributed "AS IS" and with no warranties of any kind, 
	whether express or implied.  his disclaimer of warranty constitutes an essential part of the agreement.  
	In no event shall Pelikhan, or its principals, shareholders, officers, employees, affiliates, contractors, subsidiaries, 
	or parent organizations, be liable for any incidental, consequential, or punitive damages whatsoever relating to the use of PGL, 
	or your relationship with Pelikhan.

	Author: Jonathan de Halleux, [e-mail]
*/
#if !defined(AFX_LINEAPPROXIMATOR_H__F5E6E8DC_1185_4AC0_A061_7B3309700E9D__INCLUDED_)
#define AFX_LINEAPPROXIMATOR_H__F5E6E8DC_1185_4AC0_A061_7B3309700E9D__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include <crtdbg.h>
#ifndef ASSERT
	#ifdef _DEBUG
		#define ASSERT(a)	_ASSERT(a);
	#else
		#define ASSERT(a)
	#endif
#endif
#ifdef min
	#undef min
#endif
#ifdef max
	#undef max
#endif
	
#include <iostream>
#include <vector>
#include <list>
#include <limits>
#include <algorithm>

/*! 
  \defgroup LAGroup Line approximation algorithms
*/
namespace hull
{

/*! \brief A point (x,y)

\param T float or double

  A pair of (x,y) values.
\ingroup LAGroup
*/
template<typename T>
class TPoint
{
public:
	//! Default constructor
	TPoint( T _x = 0, T _y=0):x(_x), y(_y){};
	//! Assignement constructor
	TPoint& operator = ( const TPoint<T>& t)	{	if (&t != this){ x=t.x; y=t.y;} return *this;};

	//! x value
	T x;
	//! y value
	T y;
};

/*! \brief A point (x,y) with references

\param T float or double

  \ingroup LAGroup
*/
template<typename T>
class TPointRef
{
public:
	//! Default contructor
	TPointRef():x(xDummy),y(yDummy){};
	/*! \brief Constructor with 2 values

	\param _x value to get reference to.
	\param _y value to get reference to.
	*/
	TPointRef(T& _x, T& _y):x(_x),y(_y){};
	//! Assignement constructor
	TPointRef<T>& operator = (const TPointRef<T>& t)	{	if (this != &t){ x=t.x;y=t.y;} return *this;};
	//! Assignement constructor with point
	TPointRef<T>& operator = (TPoint<T> t)		{	x=t.x;y=t.y; return *this;};
	//! x, reference to a value
	T& x;
	//! y, reference to a value
	T& y;
private:
	static T xDummy;
	static T yDummy;
};

template<typename T> T TPointRef<T>::xDummy=0;
template<typename T> T TPointRef<T>::yDummy=0;

/*! \brief Virtual base class for Line approximator classes

\par Template arguments

\param T float or double
\param TPointContainer a container of points (structure with x,y) with random access iterators
\param TKeyContainer a container of TPointContainer::const_iterator (structure with x,y) with single direction iterators


\par Notations:

  - points : data to interpolate
  - keys : selected points from available set that interpolate within the desired tolerance

\par Containers:

  - Points are stored in a #PointContainer. #PointContainer is a container such that
	#- has random access iterator,
	#- value_type is a structure/class with x,y members
  - Keys are stored in a #KeyContainer. #KeyContainer is a container such that:
	#- has single directional iterator,
	#- value_type is PointContainer::const_iterator 

\par Data normalization:

  To avoid numerical instability when computing cross product and so, data is normalized ( see #NormalizePoints ).
  To enable/disable normalization, use #SetNormalization.

  \ingroup LAGroup
*/
template<typename T, typename TPointContainer, typename TKeyContainer>
class TLine
{
public:

	//! \name Structures and typedefs
	//@{
	//! Point container
	typedef TPointContainer PointContainer;
	//! Key container
	typedef TKeyContainer KeyContainer;
	//! 2D homogenous point
	struct SHomog
	{
	public:
		//! Default constructor
		SHomog(T _x=0, T _y=0, T _w=1)	{ x=_x; y=_y; w=_w;};	

		T x;
		T y;
		T w;
	};

	/*! \brief returns square of euclidian distance
	
	*/
	static T SqrDist( const TPointContainer::const_iterator& p, const TPointContainer::const_iterator& q)
	{
		T dx=p->x-q->x;
		T dy=p->y-q->y;
		return dx*dx+dy*dy;
	}

	/*! \brief Cross product

	\param p an iterator with x,y members
	\param q an iterator with x,y members
	\result r cross product of p,q

  	The euclidian cross product of p,q:
	\f[ \vec r = \vec p \times \vec q = \left( \begin{array}{c} p_x q_y - p_y q_x \\ -q_y + p_y \\ q_x - p_x \end{array}\right)\f]
	*/
	static void CrossProduct( const TPointContainer::const_iterator& p, const TPointContainer::const_iterator& q, SHomog& r)
	{
			r.w = p->x * q->y - p->y * q->x;
			r.x = - q->y + p->y;
			r.y = q->x - p->x;
	};
		
	/*! \brief Dot product

	\param p an iterator with x,y members
	\param q an 2D homogenous point
	\result dot product of p,q

	The euclidian dot product of p,q:
	\f[ <\vec p, \vec q> = q_w + p_x q_x + p_y q_y \f]
	*/
	static T DotProduct( const TPointContainer::const_iterator& p, const SHomog& q)
	{
		return q.w + p->x*q.x + p->y*q.y;
	};
	
	/*! \brief Dot product

	\param p an iterator with x,y members
	\param q an iterator with x,y members
	\result dot product of p,q 

	The euclidian dot product of p,q:
	\f[ < \vec p,\vec q> = p_x q_x + p_y q_y \f]
	*/
	static T DotProduct( const TPointContainer::const_iterator& p, const TPointContainer::const_iterator& q)
	{
		return p->x*q->x + p->y*q->y;
	};

	/*! \brief Linear combination of points

	\param a p multiplier
	\param p a point
	\param b q multiplier
	\param q a point
	\param r linear combination of p,q

	The linear combination is:
	\f[ \vec r = a \vec p + b \vec q \f]
	*/
	static void LinComb( T a, const TPointContainer::const_iterator& p, T b, const TPointContainer::const_iterator& q, const TPointContainer::const_iterator& r)
	{
		r->x = a * p->x + b * q->x;
		r->y = a * p->y + b * q->y;
	};

	//! Internal limit structure
	struct SLimits
	{
		T dMinX;
		T dMaxX;
		T dMinY;
		T dMaxY;
		T GetCenterX()	{	return static_cast<T>((dMaxX+dMinX)/2.0);};
		T GetCenterY()	{	return static_cast<T>((dMaxY+dMinY)/2.0);};
		T GetWidth()	{	return static_cast<T>(dMaxX-dMinX);};
		T GetHeight()	{	return static_cast<T>(dMaxY-dMinY);};
	};

	//! T container
	typedef std::vector<T> TVector;
	//@}

	//! Default constructor
	TLine():m_bNormalization(true){};

	//! \name Point handling
	//@{
	//! returns number of points
	size_t GetPointSize() const				{	return m_cPoints.size();};
	//! return vector of points
	PointContainer& GetPoints()				{	return m_cPoints;};
	//! return vector of points, const
	const PointContainer& GetPoints() const	{	return m_cPoints;};
	//@}

	//! \name Key handling
	//@{
	//! returns number of keys
	size_t GetKeySize() const					{	return m_cKeys.size();};
	//! return keys
	KeyContainer& GetKeys()					{	return m_cKeys;};
	//! return keys, const
	const KeyContainer& GetKeys() const		{	return m_cKeys;};
	//@}

	//! \name Helpers
	//@{
	//! Setting points from vectors
	void SetPoints( const std::vector<T>& vX, const std::vector<T>& vY);
	//! Returning keys to vectors
	void GetKeys( std::vector<T>& vX, std::vector<T>& vY) const;
	//@}

	//! \name Bounding box
	//@{
	//! compute the bounding box
	void ComputeBoundingBox();
	//! return the point bounding box
	const SLimits& GetBoundingBox() const		{	return m_limits;};
	//@}

	//! \name Normalization
	//@{
	/*! \brief Point normalization
	
	 Let \f$(x_i,y_i)\f$, the original points and \f$(\hat x_x, \hat y_i)\f$ the normalized points:
	 \f[
	 \hat x_i = \frac{x_i - \bar x]}{\max_i (x_i-x_j)}
	 \f]
	where \f$\bar x\f$, \f$\bar y\f$ denote respectively the mean value of the \f$x_i\f$ and \f$y_i\f$.

	\sa DeNormalizePoints
	*/
	void NormalizePoints();

	/*! \brief Roll back normalization

	\sa NormalizePoints
	*/
	void DeNormalizePoints();
	//! enabled, disable normalization
	void SetNormalization( bool bEnabled = true)	{	m_bNormalization = true;};
	//! returns true if normalizing
	bool IsNormalization() const					{	return m_bNormalization;};
	//@}

	//! \name Double points checking and loop...
	//@{
	/*! \brief Discard double points

	For each pair of points \f$p_i, p_{i+1}\f$, discards \f$p_{i+1}\f$ if 
	\f[ \| p_i - p_{i+1} \| < \varepsilon \f]
	*/
	void DiscardDoublePoints();
	//! Test for loops
	void FindLoop(size_t uStartPoint, size_t& uEndPoint);
	//@}

protected:
	//! \name Attributes
	//@{
	TPointContainer m_cPoints;
	TKeyContainer m_cKeys;
	SLimits m_limits;
	bool m_bNormalization;
	//@}
};

namespace priv
{
	template<class Container, class Pred>
	struct PredX : public std::binary_function< Container::iterator, Container::iterator, bool>
	{
		bool operator()( const Container::value_type& p1, const Container::value_type& p2)	
		{	return m_pred(p1.x, p2.x); };
	protected:
		Pred m_pred;
	};

	template<class Container, class Pred>
	struct PredY : public std::binary_function< Container::iterator, Container::iterator, bool>
	{
		bool operator()( const Container::value_type& p1, const Container::value_type& p2)	
		{	return m_pred(p1.y, p2.y); };
	protected:
		Pred m_pred;
	};
};

template <typename T, typename TPointContainer, typename TKeyContainer>
void TLine<T, TPointContainer,TKeyContainer>::ComputeBoundingBox()
{
	if (m_cPoints.size() < 2)
		return;

	PointContainer::const_iterator it = (*((const TPointContainer*)&m_cPoints)).begin();


	//finding minimum and maximum...
	m_limits.dMinX=std::min_element( m_cPoints.begin(), m_cPoints.end(), priv::PredX<TPointContainer, std::less<T> >() )->x ;
	m_limits.dMaxX=std::max_element( m_cPoints.begin(), m_cPoints.end(), priv::PredX<TPointContainer, std::less<T> >() )->x ;
	m_limits.dMinY=std::min_element( m_cPoints.begin(), m_cPoints.end(), priv::PredY<TPointContainer, std::less<T> >() )->y ;
	m_limits.dMaxY=std::max_element( m_cPoints.begin(), m_cPoints.end(), priv::PredY<TPointContainer, std::less<T> >() )->y ;

	if ( fabs( m_limits.GetWidth() ) < std::numeric_limits<T>::epsilon() )
	{
		m_limits.dMaxX = m_limits.dMinX+1;
	}
	if ( fabs( m_limits.GetHeight() ) < std::numeric_limits<T>::epsilon() )
	{
		m_limits.dMaxY = m_limits.dMinY+1;
	}
}

namespace priv
{
	template<typename T>
	struct Rect
	{
		Rect( T xm, T ym, T dx, T dy)
			:m_xm(xm),m_ym(ym),m_dx(dx),m_dy(dy){};
	protected:
		T m_xm;
		T m_ym;
		T m_dx;
		T m_dy;
	};

	template<typename T>
	struct NormalizePoint : public std::unary_function< TPoint<T>& , int>, public Rect<T>
	{
		NormalizePoint( T xm, T ym, T dx, T dy)
			: Rect<T>(xm,ym,dx,dy){};
		int operator() ( TPoint<T>& point)