int main()
{
for(int i=1;i<10;i++)
{
lineF(i);
}
for(int i=8;i>0;i--)
{
lineF(i);
}
}
void GraphicsScene::mouseMoveEvent ( QGraphicsSceneMouseEvent* mouseEvent )
{
if( _haveToDraw )
{
if( mouseEvent->lastScenePos() != mouseEvent->scenePos() )
{
QLineF lineF( mouseEvent->lastScenePos(), mouseEvent->scenePos() );
g_currentObjects.append( lineF );
g_allTheObjects.append( lineF );
addLine( lineF );
}
}
}
void doHough(std::vector<payload> readings, int lineThresh, int pointThresh)
{
std::vector<float> maxVal(2), res(2);
std::vector<float> minVal(2);
std::vector<float> vote(2);
/**
* LINE SPACE
* RHO: The perpendicular distance of the line from origin
* THETA: Angle the perpendicular from the origin to the line
* makes with +X axis
*
* beta: Angular error of ultrasonic sensor, -15deg < beta < 15deg
*/
maxVal[RHO] = 15000.0F; // in millimetres
maxVal[THETA] = (float) 2 * M_PI; // in radians
res[RHO] = 100;
res[THETA] = M_PI / 180 * 5;
houghSpace linespace (res, maxVal);
for (auto &p: readings) {
for (float beta = -M_PI/12; beta <= M_PI/12; beta += M_PI/180) {
// Refer to the paper for the derivation
vote[THETA] = p.loc.theta + beta;
vote[THETA] = fmod(vote[THETA], 2 * M_PI);
if (vote[THETA] < 0) {
vote[THETA] += 2 * M_PI;
}
vote[RHO] = std::abs(p.reading + (p.loc.x * cosf(vote[THETA]))
+ (p.loc.y * sinf(vote[THETA])));
assert(vote[RHO] <= maxVal[RHO]);
assert(vote[THETA] <= maxVal[THETA]);
PPRINT(vote[RHO]);
PPRINT(vote[THETA]);
linespace.addVote(vote);
}
}
std::vector< std::vector<float> > lineF (linespace.getMaxima(lineThresh));
std::ofstream lout ("lines.txt");
for (auto &p: lineF) {
for (auto &q: p) {
lout << q << " ";
}
lout << std::endl;
}
//std::cout << linespace;
/**
* POINT SPACE
* X: X-coordinate of point
* Y: Y-coordinate of point
*
* beta: Angular error of ultrasonic sensor, -15deg < beta < 15deg
*/
maxVal[X] = 15000.0F; // in millimetres
maxVal[Y] = 15000.0F; // in millimetres
minVal[X] = -15000.0F;
minVal[Y] = -15000.0F;
res[X] = 10;
res[Y] = 10;
houghSpace pointspace (res, maxVal, minVal);
for (auto &p: readings) {
for (float beta = -M_PI/12; beta <= M_PI/12; beta += M_PI/180) {
vote[X] = p.loc.x + (p.reading * cosf(p.loc.theta + beta));
vote[Y] = p.loc.y + (p.reading * sinf(p.loc.theta + beta));
assert(vote[X] <= maxVal[X]);
assert(vote[Y] <= maxVal[Y]);
PPRINT(vote[X]);
PPRINT(vote[Y]);
pointspace.addVote(vote);
}
}
std::vector< std::vector<float> > pointF (pointspace.getMaxima(pointThresh));
std::ofstream pout ("points.txt");
for (auto &p: pointF) {
for (auto &q: p) {
pout << q << " ";
}
pout << std::endl;
}
//std::cout << pointspace;
}
