|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
#include "tgeometry.h"
|
|
Campbell Barton |
8c6c57 |
#include "cornerdetector.h"
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//======================================================================
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Classe che definisce dei punti che consentono di trovare gli angoli
|
|
Shinya Kitaoka |
d1f6c4 |
class AlgorithmPointI final : public TPointI {
|
|
Toshihiro Shimizu |
890ddd |
public:
|
|
Shinya Kitaoka |
120a6e |
//! Indice originale del punto
|
|
Shinya Kitaoka |
120a6e |
int m_originalIndex;
|
|
Shinya Kitaoka |
120a6e |
//! Quantita' che dice quanto l'angolo e' acuto
|
|
Shinya Kitaoka |
120a6e |
double m_sharpness;
|
|
Shinya Kitaoka |
120a6e |
//! Dice se il punto e' un angolo
|
|
Shinya Kitaoka |
120a6e |
bool m_isCorner;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
//! Costruttore
|
|
Shinya Kitaoka |
120a6e |
/*!
|
|
Shinya Kitaoka |
120a6e |
Gli viene passato un T3DPointD e restituisce un AlgorithmPointI
|
|
Shinya Kitaoka |
120a6e |
*/
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI(const T3DPointD &value, int index)
|
|
Shinya Kitaoka |
120a6e |
: TPointI((int)value.x, (int)value.y)
|
|
Shinya Kitaoka |
120a6e |
, m_originalIndex(index)
|
|
Shinya Kitaoka |
120a6e |
, m_sharpness(0)
|
|
Shinya Kitaoka |
120a6e |
, m_isCorner(false){};
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
//! Costruttore
|
|
Shinya Kitaoka |
120a6e |
/*!
|
|
Shinya Kitaoka |
120a6e |
Gli viene passato un TPointI e restituisce un AlgorithmPointI
|
|
Shinya Kitaoka |
120a6e |
*/
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI(const TPointI &value, int index)
|
|
Shinya Kitaoka |
120a6e |
: TPointI(value)
|
|
Shinya Kitaoka |
120a6e |
, m_originalIndex(index)
|
|
Shinya Kitaoka |
120a6e |
, m_sharpness(0)
|
|
Shinya Kitaoka |
120a6e |
, m_isCorner(false){};
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
//! Dstruttore
|
|
Shinya Kitaoka |
120a6e |
~AlgorithmPointI(){};
|
|
Toshihiro Shimizu |
890ddd |
};
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Definisce l'operatore meno tra due AlgorithmPointI
|
|
Campbell Barton |
8c6c57 |
static AlgorithmPointI operator-(const AlgorithmPointI &op1,
|
|
Campbell Barton |
8c6c57 |
const AlgorithmPointI &op2) {
|
|
Shinya Kitaoka |
120a6e |
return AlgorithmPointI(op1.operator-(op2), 0);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//======================================================================
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Definisce point_container come un vettore di AlgorithmPointI
|
|
Shinya Kitaoka |
3bfa54 |
typedef std::vector<algorithmpointi> point_container;</algorithmpointi>
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Definisce corner_container come un vettore di interi
|
|
Shinya Kitaoka |
3bfa54 |
typedef std::vector<int> corner_container;</int>
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int gMinSampleNum;
|
|
Toshihiro Shimizu |
890ddd |
int gMinDist;
|
|
Toshihiro Shimizu |
890ddd |
int gMaxDist;
|
|
Toshihiro Shimizu |
890ddd |
int gSquaredMinDist;
|
|
Toshihiro Shimizu |
890ddd |
int gSquaredMaxDist;
|
|
Toshihiro Shimizu |
890ddd |
double gMaxAngle;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Definisce un point_container
|
|
Toshihiro Shimizu |
890ddd |
point_container gPoints;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//======================================================================
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Ritorna true se e' possibile effettuare l'interpolazione
|
|
Toshihiro Shimizu |
890ddd |
/*!
|
|
Shinya Kitaoka |
120a6e |
Calcola gli AlgorithmPointI sulla base di points, verificando di punto in
|
|
Shinya Kitaoka |
120a6e |
punto lo step
|
|
Shinya Kitaoka |
120a6e |
tra ascissa e ordinata; inserisce i punti calcolati in gPoints e in base
|
|
Shinya Kitaoka |
120a6e |
alla quntita' di
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI trovati restituisce true(e' possibile interpolare) o
|
|
Shinya Kitaoka |
120a6e |
false(non e' possibile).
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
\param points Vettore di T3DPoints
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Campbell Barton |
8c6c57 |
static bool interpolate(const std::vector<t3dpointd> &points) {</t3dpointd>
|
|
Shinya Kitaoka |
120a6e |
unsigned int curr, next;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
TPointI currStep, xStep, yStep, guideLine;
|
|
Shinya Kitaoka |
120a6e |
TPointI xUnit(1, 0);
|
|
Shinya Kitaoka |
120a6e |
TPointI yUnit(0, 1);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
bool chooseByDistance = false;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
curr = 0;
|
|
Shinya Kitaoka |
120a6e |
next = 1;
|
|
Shinya Kitaoka |
120a6e |
// int i = points.size();
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
while (next <= points.size() - 1) {
|
|
Shinya Kitaoka |
120a6e |
if (points[next] != points[curr]) {
|
|
Shinya Kitaoka |
120a6e |
gPoints.push_back(AlgorithmPointI(points[curr], curr));
|
|
Shinya Kitaoka |
120a6e |
guideLine = TPointI((int)(points[curr].x - points[next].x),
|
|
Shinya Kitaoka |
120a6e |
(int)(points[curr].y - points[next].y));
|
|
Shinya Kitaoka |
120a6e |
currStep = gPoints.back();
|
|
Shinya Kitaoka |
120a6e |
double xStepTheta, yStepTheta;
|
|
Shinya Kitaoka |
120a6e |
// TPointI a;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
while (norm2(TPointI((int)(points[next].x - currStep.x),
|
|
Shinya Kitaoka |
120a6e |
(int)(points[next].y - currStep.y))) > 1) {
|
|
Shinya Kitaoka |
120a6e |
TPointI nextPoint = TPointI((int)points[next].x, (int)points[next].y);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
// TPointI a = nextPoint - currStep;
|
|
Shinya Kitaoka |
120a6e |
// int i = 0;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (currStep.x > nextPoint.x)
|
|
Shinya Kitaoka |
120a6e |
xStep = currStep - xUnit;
|
|
Shinya Kitaoka |
120a6e |
else if (currStep.x < nextPoint.x)
|
|
Shinya Kitaoka |
120a6e |
xStep = currStep + xUnit;
|
|
Shinya Kitaoka |
120a6e |
else {
|
|
Shinya Kitaoka |
120a6e |
xStep = currStep;
|
|
Shinya Kitaoka |
120a6e |
chooseByDistance = true;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (currStep.y > nextPoint.y)
|
|
Shinya Kitaoka |
120a6e |
yStep = currStep - yUnit;
|
|
Shinya Kitaoka |
120a6e |
else if (currStep.y < nextPoint.y)
|
|
Shinya Kitaoka |
120a6e |
yStep = currStep + yUnit;
|
|
Shinya Kitaoka |
120a6e |
else {
|
|
Shinya Kitaoka |
120a6e |
yStep = currStep;
|
|
Shinya Kitaoka |
120a6e |
chooseByDistance = true;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (chooseByDistance) {
|
|
Shinya Kitaoka |
120a6e |
chooseByDistance = false;
|
|
Shinya Kitaoka |
120a6e |
if (norm2(xStep - nextPoint) < norm2(yStep - nextPoint))
|
|
Shinya Kitaoka |
120a6e |
currStep = xStep;
|
|
Shinya Kitaoka |
120a6e |
else
|
|
Shinya Kitaoka |
120a6e |
currStep = yStep;
|
|
Shinya Kitaoka |
120a6e |
} else {
|
|
Shinya Kitaoka |
120a6e |
double aux = norm2(guideLine);
|
|
Shinya Kitaoka |
120a6e |
xStepTheta = acos((xStep - nextPoint) * guideLine /
|
|
Shinya Kitaoka |
120a6e |
(sqrt(norm2(xStep - nextPoint) * aux)));
|
|
Shinya Kitaoka |
120a6e |
yStepTheta = acos((yStep - nextPoint) * guideLine /
|
|
Shinya Kitaoka |
120a6e |
(sqrt(norm2(yStep - nextPoint) * aux)));
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (xStepTheta > yStepTheta)
|
|
Shinya Kitaoka |
120a6e |
currStep = yStep;
|
|
Shinya Kitaoka |
120a6e |
else
|
|
Shinya Kitaoka |
120a6e |
currStep = xStep;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
gPoints.push_back(AlgorithmPointI(currStep, next));
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
curr++;
|
|
Shinya Kitaoka |
120a6e |
next++;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
gPoints.push_back(AlgorithmPointI(points[curr], curr));
|
|
Shinya Kitaoka |
120a6e |
if ((int)gPoints.size() < (2 * gMaxDist + 1)) return false;
|
|
Shinya Kitaoka |
120a6e |
return true;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//----------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Shinya Kitaoka |
120a6e |
//! Verifica se currIndex e' un possibile angolo e in caso affermativo salva
|
|
Shinya Kitaoka |
120a6e |
//! l'"acutezza"
|
|
Shinya Kitaoka |
120a6e |
inline bool isAdmissibleCorner(int currIndex, int precIndex, int nextIndex) {
|
|
Shinya Kitaoka |
120a6e |
int size = gPoints.size();
|
|
Shinya Kitaoka |
120a6e |
if (currIndex < 0 || currIndex >= size || precIndex < 0 ||
|
|
Shinya Kitaoka |
120a6e |
precIndex >= size || nextIndex < 0 || nextIndex >= size)
|
|
Shinya Kitaoka |
120a6e |
return false;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI a = gPoints[currIndex] - gPoints[nextIndex];
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI b = gPoints[currIndex] - gPoints[precIndex];
|
|
Shinya Kitaoka |
120a6e |
AlgorithmPointI c = gPoints[nextIndex] - gPoints[precIndex];
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
double norm2_a = norm2(a);
|
|
Shinya Kitaoka |
120a6e |
double norm2_b = norm2(b);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (!(norm2_a <= gSquaredMaxDist && norm2_a >= gSquaredMinDist) ||
|
|
Shinya Kitaoka |
120a6e |
!(norm2_b <= gSquaredMaxDist && norm2_b >= gSquaredMinDist))
|
|
Shinya Kitaoka |
120a6e |
return false;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
double norm2_c = norm2(c);
|
|
Shinya Kitaoka |
120a6e |
double cosineOfAlpha =
|
|
Shinya Kitaoka |
120a6e |
(norm2_a + norm2_b - norm2_c) / sqrt(4 * norm2_a * norm2_b);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (cosineOfAlpha < -1) cosineOfAlpha = -1;
|
|
Shinya Kitaoka |
120a6e |
if (cosineOfAlpha > 1) cosineOfAlpha = 1;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
double alpha = (180 / 3.14) * acos(cosineOfAlpha);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (alpha <= gMaxAngle) {
|
|
Shinya Kitaoka |
120a6e |
gPoints[currIndex].m_sharpness += 180 - fabs(alpha);
|
|
Shinya Kitaoka |
120a6e |
return true;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
return false;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//----------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Trova i possibili angoli tra i punti di gPoints
|
|
Campbell Barton |
8c6c57 |
static void findCornerCandidates() {
|
|
Shinya Kitaoka |
120a6e |
unsigned int curr, prec, next;
|
|
Shinya Kitaoka |
120a6e |
curr = gMaxDist;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
while (curr != gPoints.size() - gMaxDist) {
|
|
Shinya Kitaoka |
120a6e |
prec = curr - 1;
|
|
Shinya Kitaoka |
120a6e |
next = curr + 1;
|
|
Shinya Kitaoka |
120a6e |
int admissibleCornersCount = 0;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
int countDown = 5;
|
|
Shinya Kitaoka |
120a6e |
while (countDown) {
|
|
Shinya Kitaoka |
120a6e |
--countDown;
|
|
Shinya Kitaoka |
120a6e |
while (isAdmissibleCorner(curr, prec, next)) {
|
|
Shinya Kitaoka |
120a6e |
admissibleCornersCount++;
|
|
Shinya Kitaoka |
120a6e |
countDown = 0;
|
|
Shinya Kitaoka |
120a6e |
--prec;
|
|
Shinya Kitaoka |
120a6e |
++next;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
--prec;
|
|
Shinya Kitaoka |
120a6e |
++next;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (admissibleCornersCount) {
|
|
Shinya Kitaoka |
120a6e |
gPoints[curr].m_sharpness /= admissibleCornersCount;
|
|
Shinya Kitaoka |
120a6e |
if ((gPoints[curr].m_sharpness > (180 - gMaxAngle)) &&
|
|
Shinya Kitaoka |
120a6e |
(admissibleCornersCount > gMinSampleNum))
|
|
Shinya Kitaoka |
120a6e |
gPoints[curr].m_isCorner = true;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
++curr;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//----------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Trova gli angoli tra i punti di gPoints
|
|
Campbell Barton |
8c6c57 |
static void findCorners(int neighborLimit, std::vector<int> &cornerIndexes) {</int>
|
|
Shinya Kitaoka |
120a6e |
unsigned int curr, prec, next;
|
|
Shinya Kitaoka |
120a6e |
curr = gMaxDist;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
while (curr != gPoints.size() - gMaxDist) {
|
|
Shinya Kitaoka |
120a6e |
prec = curr - 1;
|
|
Shinya Kitaoka |
120a6e |
next = curr + 1;
|
|
Shinya Kitaoka |
120a6e |
while ((norm2(gPoints[curr] - gPoints[prec]) <= neighborLimit) &&
|
|
Shinya Kitaoka |
120a6e |
(norm2(gPoints[curr] - gPoints[next]) <= neighborLimit) &&
|
|
Shinya Kitaoka |
120a6e |
gPoints[curr].m_isCorner) {
|
|
Shinya Kitaoka |
120a6e |
if (gPoints[curr].m_sharpness <= gPoints[prec].m_sharpness ||
|
|
Shinya Kitaoka |
120a6e |
gPoints[curr].m_sharpness <= gPoints[next].m_sharpness) {
|
|
Shinya Kitaoka |
120a6e |
gPoints[curr].m_isCorner = false;
|
|
Shinya Kitaoka |
120a6e |
break;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
--prec;
|
|
Shinya Kitaoka |
120a6e |
++next;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
if (gPoints[curr].m_isCorner)
|
|
Shinya Kitaoka |
120a6e |
cornerIndexes.push_back(gPoints[curr].m_originalIndex);
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
++curr;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//----------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! Individua gli eventuali angoli presenti nella curva da calcolare
|
|
Shinya Kitaoka |
120a6e |
void detectCorners(const std::vector<t3dpointd> &inputPoints, int minSampleNum,</t3dpointd>
|
|
Shinya Kitaoka |
120a6e |
int minDist, int maxDist, double maxAngle,
|
|
Shinya Kitaoka |
120a6e |
std::vector<int> &cornerIndexes) {</int>
|
|
Shinya Kitaoka |
120a6e |
gMinSampleNum = minSampleNum;
|
|
Shinya Kitaoka |
120a6e |
gMinDist = minDist;
|
|
Shinya Kitaoka |
120a6e |
gMaxDist = maxDist;
|
|
Shinya Kitaoka |
120a6e |
gSquaredMinDist = minDist * minDist;
|
|
Shinya Kitaoka |
120a6e |
gSquaredMaxDist = maxDist * maxDist;
|
|
Shinya Kitaoka |
120a6e |
gMaxAngle = maxAngle;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
interpolate(inputPoints);
|
|
Shinya Kitaoka |
120a6e |
if ((int)gPoints.size() > 2 * gMaxDist) {
|
|
Shinya Kitaoka |
120a6e |
findCornerCandidates();
|
|
Shinya Kitaoka |
120a6e |
findCorners((int)sqrt((float)gSquaredMaxDist) + 10, cornerIndexes);
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
gPoints.clear();
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
// check for no index equal to an adjacent
|
|
Shinya Kitaoka |
120a6e |
std::vector<int>::iterator it1, it2;</int>
|
|
Shinya Kitaoka |
120a6e |
it1 = it2 = cornerIndexes.begin();
|
|
Shinya Kitaoka |
120a6e |
++it2;
|
|
Shinya Kitaoka |
120a6e |
|
|
Shinya Kitaoka |
120a6e |
while (it2 != cornerIndexes.end()) {
|
|
Shinya Kitaoka |
120a6e |
if (*it1 == *it2) {
|
|
Shinya Kitaoka |
120a6e |
it2 = cornerIndexes.erase(it2);
|
|
Shinya Kitaoka |
120a6e |
} else {
|
|
Shinya Kitaoka |
120a6e |
++it1;
|
|
Shinya Kitaoka |
120a6e |
++it2;
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Shinya Kitaoka |
120a6e |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|