/*Precondition: I wanted the program to not be completely unsightly
*Postcondition: Literally calls every function and prints the program to the c file
*/
void printprogram(FILE *ofp, const char * programname, char * first, char * last, int numfunct){
fprintf(ofp, "/*\n"); printstar(1, ofp); fprintf(ofp, "*\t%s", programname);
printstar(5, ofp); fprintf(ofp, "*\t%s %s (%s)", first, last, __DATE__);
printstar(1, ofp); fprintf(ofp, "*/\n"); linespace(ofp);
fprintf(ofp, "/* Pre-processor Directives */\n");
fprintf(ofp, "/*included libraries*/\n"); fprintf(ofp, "\t%cinclude %cstdlib.h%c\n", 35, 60, 62);
fprintf(ofp, "\t%cinclude %cstdio.h%c\n", 35, 60, 62); newline(1,ofp); fprintf(ofp, "/*included constants*/\n");
fprintf(ofp, "/*included functions*/\n"); declarefunc(ofp, numfunct); fprintf(ofp, "/*included external variables*/\n");
fprintf(ofp, "/*included structures*/\n"); linespace(ofp);
fprintf(ofp, "/*main function: describe the main function here*/\n");
fprintf(ofp, "int main%c%c %c\n", 40,41,123); fprintf(ofp, "\t/*initialize variables, files, and pointers below*/\n\n\n\n");
fprintf(ofp, "\treturn 0%c\n", 59); fprintf(ofp, "%c", 125); newline(1,ofp); definefunc(ofp, numfunct);
}
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);
std::vector<float> voteLoc(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])));
voteLoc[X] = p.loc.x + p.reading * cosf(vote[THETA]);
voteLoc[Y] = p.loc.y + p.reading * sinf(vote[THETA]);
assert(vote[RHO] <= maxVal[RHO]);
assert(vote[THETA] <= maxVal[THETA]);
PPRINT(vote[RHO]);
PPRINT(vote[THETA]);
PPRINT(voteLoc[X]);
PPRINT(voteLoc[Y]);
linespace.addVote(vote, voteLoc);
}
}
auto lineF (linespace.getMaxima(lineThresh));
std::ofstream lout ("lines.txt");
for (auto &p: lineF) {
for (auto &q: p.first) {
lout << q << " ";
}
lout << std::endl;
}
std::ofstream lsout("line_segments.txt");
auto lineSegments = getLineSegments(lineF);
for (auto &ls : lineSegments) {
lsout << ls.first.first << ' ' //X1
<< ls.first.second << ' ' //Y1
<< ls.second.first << ' ' //X2
<< ls.second.second << ' ' //Y2
<< std::endl;
}
}
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;
}
