wheeldetect/3part/Cyclops/include/OneDollar/SimpleCornerDetector.h

#ifndef SimpleCornerDetector_h__
#define SimpleCornerDetector_h__

#include <vector>
#include <list>
#include <deque>
#include "PagicUtils.h"

using std::deque;
using std::list;
using std::vector;

namespace pagic
{
	class CornerInfo
	{
	public:
		CornerInfo();
		~CornerInfo();
		int m_id;
		double m_angle;
		Point2d m_pos;
		int m_cenIdx;
		int m_leftIdx, m_rightIdx;		
	};
	void releaseCornerInfoVec(vector<CornerInfo*>& vec);
	void releaseCornerInfoDeq(deque<CornerInfo*>& deq);
	void initCornerInfo(CornerInfo& val);

	class SimpleCornerDetector
	{
	public:
		SimpleCornerDetector();
		virtual CornerInfo* genCornerInfo(const vector<Point2d>& posVec, int cenIdx);
		virtual CornerInfo* isCorner(const vector<Point2d>& posVec, int cenIdx);
		virtual void detectCorners(const vector<Point2d>& posVec, vector<CornerInfo*>& cornerVec);

		double getSkipWin() const { return m_skipWin; }
		void setSkipWin(double val) { m_skipWin = val; }

		int getWin() const { return m_win; }
		void setWin(int val) { m_win = val; }

		double getMaxAngle() const { return m_maxAngle; }
		void setMaxAngle(double val) { m_maxAngle = val; }

        bool filterDuplicatedCorners() const { return m_filterDuplicatedCorners; }
        void setFilterDuplicatedCorners(bool val) { m_filterDuplicatedCorners = val; }

		void genCornerInfoVec(const vector<Point2d>& posVec, vector<CornerInfo*>& cifVec,
			int startIdx = 1);

		template<class It>
		CornerInfo* _genCornerInfo(It pis, It pie, int cenIdx, int win)
		{
			int Sized = pie - pis;
			int s = cenIdx - win;
			s = std::max((int)0, s);
			int e = cenIdx + win;
			e = std::min(Sized, e);

			It si = pis + s;
			It ei = pis + e;

			CornerInfo* pRet = new CornerInfo;
			CornerInfo &ret = *pRet;
			initCornerInfo(ret);

			if(cenIdx == Sized - 1 || cenIdx == 0) {
				return pRet;
			}

			int skipWin = m_skipWin;

			It ci = pis + cenIdx;

			if(m_isSymmetricAngleComp) {
				It l = si;
				It r = ei - 1;
				while(l != ci - skipWin && r != ci + skipWin) {
					double a = angle(*ci, *l, *r);
					if(abs(ret.m_angle) < abs(a)) {
						ret.m_angle = a;
						ret.m_cenIdx = cenIdx;
						ret.m_leftIdx = -1;
						ret.m_rightIdx = -1;						
					}
					l++;
					r--;
				}				
			} else {
				for(It l = si; l != ci - skipWin; l++) {
					for(It r = ci + skipWin + 1; r != ei; r++) {
						double a = angle(*ci, *l, *r);
						if(abs(ret.m_angle) < abs(a)) {
							ret.m_angle = a;
							ret.m_cenIdx = cenIdx;
							ret.m_leftIdx = -1;
							ret.m_rightIdx = -1;
						}
					}
				}
			}

			return pRet;
		}

		template<class PosCtn, class CifCtn>
		void genCornerInfoVec(const PosCtn& posVec, CifCtn& cifVec,
			int startIdx = 1)
		{
			cifVec.resize(posVec.size() - 2, 0);
			startIdx = std::max(1, startIdx);
			for(size_t i = startIdx; i < posVec.size() - 1; i++) {
				CornerInfo* pCif = _genCornerInfo(posVec.begin(), posVec.end(), i, m_win);

				pCif->m_pos = posVec[i];

				//pCif->m_angle += 180;
				if(pCif->m_angle < 0) {
					pCif->m_angle += 360.0;
				}
				if(cifVec[i - 1]) {
					delete cifVec[i - 1];
				}
				cifVec[i - 1] = pCif;
#ifdef _DEBUG
				printf("add cif id %d, pos %lf %lf, cenIdx %d, angle %lf\n",
					pCif->m_id, pCif->m_pos.x, pCif->m_pos.y, pCif->m_cenIdx, pCif->m_angle);
#endif
			}
		}

    protected:
		int m_win;	//search window Sized
		
		double m_maxAngle;	//degree
		
		double m_skipWin;

		bool m_filterDuplicatedCorners;
		bool m_isSymmetricAngleComp;
		
	protected:
		//return angle degree
		double angle(const Point2d& cen, 
			const Point2d& l,
			const Point2d& r);
		void filterDuplicatedCorner(vector<CornerInfo*>& cornerVec, int win = 1);
		void filterDuplicatedCorner(deque<CornerInfo*>& cornerVec, int win = 1);
		int copyCCPWIn(const vector<Point2d>& posVec, int i, int win, vector<Point2d>& winVec);

		template<class T>
		int copyWin(const vector<T>& oriVec, int i, int win, vector<T>& winVec)
		{
			int s = i - m_win;
			s = std::max((int)0, s);
			int e = i + m_win;
			e = std::min((int)oriVec.size(), e);
			winVec = vector<T>(oriVec.begin() + s, oriVec.begin() + e);
			return s;
		}

	};


}

#endif // SimpleCornerDetector_h__