PlateCorners::PlateCorners(Mat inputImage, PlateLines* plateLines, CharacterRegion* charRegion, Config* config)
{
this->config = config;
if (this->config->debugPlateCorners)
cout << "PlateCorners constructor" << endl;
this->inputImage = inputImage;
this->plateLines = plateLines;
this->charRegion = charRegion;
this->bestHorizontalScore = 9999999999999;
this->bestVerticalScore = 9999999999999;
Point topPoint = charRegion->getTopLine().midpoint();
Point bottomPoint = charRegion->getBottomLine().closestPointOnSegmentTo(topPoint);
this->charHeight = distanceBetweenPoints(topPoint, bottomPoint);
//this->charHeight = distanceBetweenPoints(charRegion->getCharArea()[0], charRegion->getCharArea()[3]);
//this->charHeight = this->charHeight - 2; // Adjust since this height is a box around our char.
// Adjust the char height for the difference in size...
//this->charHeight = ((float) inputImage.size().height / (float) TEMPLATE_PLATE_HEIGHT) * this->charHeight;
this->charAngle = angleBetweenPoints(charRegion->getCharArea()[0], charRegion->getCharArea()[1]);
}
void LineSegment::init(int x1, int y1, int x2, int y2)
{
this->p1 = Point(x1, y1);
this->p2 = Point(x2, y2);
if (p2.x - p1.x == 0)
this->slope = 0.00000000001;
else
this->slope = (float) (p2.y - p1.y) / (float) (p2.x - p1.x);
this->length = distanceBetweenPoints(p1, p2);
this->angle = angleBetweenPoints(p1, p2);
}
geometry_msgs::Twist
GpsMover::createTwistMessage(geometry_msgs::Point current_location,
double current_heading,
geometry_msgs::Point waypoint) {
// The command to return
geometry_msgs::Twist command;
// Set components we don't care about to 0
command.linear.y = 0;
command.linear.z = 0;
command.angular.x = 0;
command.angular.y = 0;
// Figure out the minimum we have to turn to point directly at the waypoint
double angle_to_waypoint = angleBetweenPoints(current_location, waypoint);
double min_turning_angle =
minAngularChange(current_heading, angle_to_waypoint);
// Figure out how far we are from the waypoint
double dx = waypoint.x - current_location.x;
double dy = waypoint.y - current_location.y;
double distance = sqrt(pow(dx, 2) + pow(dy, 2));
// Figure out how fast we should turn
command.angular.z = magicFunction(
distance, min_turning_angle, linear_distance_factor, linear_heading_factor);
// Figure out if we should be turning left or right
command.angular.z *= (min_turning_angle > 0) ? 1 : -1;
// Figure out how fast we should move forward
command.linear.x = magicFunction(
min_turning_angle, distance, linear_heading_factor, linear_distance_factor);
// Cap our angular and linear speeds
capValue(command.linear.x, max_linear_speed);
capValue(command.angular.z, max_angular_speed);
return command;
}
float angleBetweenSegments(poExtrudedLineSeg seg1, poExtrudedLineSeg seg2) {
poPoint p1, p2, p3;
switch(seg1.place) {
case PO_STROKE_PLACE_CENTER:
p1 = (seg1.p2 + seg1.p1) / 2.f;
p2 = (seg1.p4 + seg1.p3) / 2.f;
p3 = (seg2.p4 + seg2.p3) / 2.f;
break;
case PO_STROKE_PLACE_INSIDE:
p1 = seg1.p1;
p2 = seg1.p3;
p3 = seg2.p3;
break;
case PO_STROKE_PLACE_OUTSIDE:
p1 = seg1.p2;
p2 = seg1.p4;
p3 = seg2.p4;
break;
}
return angleBetweenPoints(p1, p2, p3);
}
// 0.000011479429428388439 gps points per meter
void waypointManager(void){
waypoint * newWP = (waypoint *) malloc(sizeof(waypoint));
dataGPS curPosGPS;
//int gpsRet;
float turnHeading;
double tolerance = 3; // tolerance in meters
printf("Beginning\r\n");
addWaypointxy(-87.322314, 39.483694);
newWP = getCurrentWaypoint();
printf("\r\nTrying to initialize GPS");
//gpsRet = init_GPS();
curPosGPS = getGPS(gpsRet);
if(!curPosGPS.valid){
for(int i = 0; i < 10 && !curPosGPS.valid; i++){
sleep(5);
printf("\r\nWaiting for valid GPS data....\r\n");
curPosGPS = getGPS(gpsRet);
}
}
waypoint * curPosWP = (waypoint *) malloc(sizeof(waypoint));
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
turnHeading = angleBetweenPoints(*newWP, *curPosWP);
turn(turnHeading);
// at this point we should be pointing in the right direction
double line_mb[2];
findLine(*curPosWP, * newWP, line_mb);
printf("\r\nCurrent position: x %lf, y %lf\r\n", curPosWP->x, curPosWP->y);
printf("Waypoint to move to: x %lf, y %lf\r\n", newWP->x, newWP->y);
printf("Turn heading: %f\r\n", turnHeading);
printf("Line: m %lf, b %lf\r\n", line_mb[0], line_mb[1]);
int waypointsLeft = 1;
while(keepGoing && waypointsLeft){
while(keepGoing && distanceBetweenPoints(*curPosWP, *newWP) > tolerance*0.000011479429428388439){
// go straight
printf("Going straight\r\ndistance to target: %lf\r\ntolerance: %lf\r\n", distanceBetweenPoints(*curPosWP, *newWP), tolerance*0.000011479429428388439);
gpio_set_value(LEFT_FWD_GPIO, 1);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 1);
gpio_set_value(RIGHT_BCK_GPIO, 0);
curPosGPS = getGPS(gpsRet);
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
if(!bbCheck(line_mb[0], line_mb[1], * curPosWP, tolerance)){
returnToPreviousWaypoint();
printf("We're outside of the bounds. Returning to the previous waypoint\r\n");
break; // out of the bounding box
}
sleep(2);
}
printf("We've hit the waypoint! Stopping the motors...\r\n");
gpio_set_value(LEFT_FWD_GPIO, 0);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 0);
gpio_set_value(RIGHT_BCK_GPIO, 0);
waypointsLeft = (advanceToNextWaypoint() != -1); // don't keep going if we've reached the end of the waypoints
newWP = getCurrentWaypoint(); // if we've already advanced past the end this will give us the last WP, but
// we will break out of this loop anyway
printf("KeepGoing: %d...should be 0 if we've hit the end of the waypoints", keepGoing);
}
free(curPosWP);
free(newWP);
}
bool Label::updateScreenTransform(const glm::mat4& _mvp, const glm::vec2& _screenSize, bool _testVisibility) {
glm::vec2 screenPosition;
float rot = 0;
glm::vec2 ap1, ap2;
switch (m_type) {
case Type::debug:
case Type::point:
{
glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));
if (_testVisibility && (v1.w <= 0)) {
return false;
}
screenPosition = clipToScreenSpace(v1, _screenSize);
ap1 = ap2 = screenPosition;
break;
}
case Type::line:
{
// project label position from mercator world space to clip coordinates
glm::vec4 v1 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition1, 0.0, 1.0));
glm::vec4 v2 = worldToClipSpace(_mvp, glm::vec4(m_transform.modelPosition2, 0.0, 1.0));
// check whether the label is behind the camera using the perspective division factor
if (_testVisibility && (v1.w <= 0 || v2.w <= 0)) {
return false;
}
// project to screen space
glm::vec2 p1 = clipToScreenSpace(v1, _screenSize);
glm::vec2 p2 = clipToScreenSpace(v2, _screenSize);
rot = angleBetweenPoints(p1, p2) + M_PI_2;
if (rot > M_PI_2 || rot < -M_PI_2) { // un-readable labels
rot += M_PI;
} else {
std::swap(p1, p2);
}
float length = glm::length(p2 - p1);
float exceedHeuristic = 30; // default heuristic : 30%
if (_testVisibility && (m_dim.x > length)) {
float exceed = (1 - (length / m_dim.x)) * 100;
if (exceed > exceedHeuristic) {
return false;
}
}
ap1 = p1;
ap2 = p2;
break;
}
}
align(screenPosition, ap1, ap2);
// update screen position
glm::vec2 offset = m_options.offset;
if (m_transform.state.rotation != 0.f) {
offset = glm::rotate(offset, m_transform.state.rotation);
}
glm::vec2 newScreenPos = screenPosition + offset;
if (newScreenPos != m_transform.state.screenPos) {
m_transform.state.screenPos = newScreenPos;
m_dirty = true;
}
// update screen rotation
if (m_transform.state.rotation != rot) {
m_transform.state.rotation = rot;
m_dirty = true;
}
return true;
}
void makeStrokeForJoint(std::vector<poPoint> &stroke, poExtrudedLineSeg &seg1, poExtrudedLineSeg &seg2, poStrokeJoinProperty join, float stroke_width) {
poPoint top, bottom;
poPoint p1, p2, p3, p4, tmp;
bool top_outside = combineExtrudedLineSegments(seg1, seg2, &top, &bottom);
poPoint joint = (seg1.p4 + seg1.p3) / 2.f;
switch(join) {
case PO_STROKE_JOIN_MITRE:
if(top_outside) {
stroke.push_back(top);
stroke.push_back(bottom);
}
else {
stroke.push_back(bottom);
stroke.push_back(top);
}
break;
case PO_STROKE_JOIN_BEVEL:
if(top_outside) {
stroke.push_back(seg1.p3);
stroke.push_back(bottom);
stroke.push_back(seg2.p1);
stroke.push_back(bottom);
}
else {
stroke.push_back(top);
stroke.push_back(seg1.p4);
stroke.push_back(top);
stroke.push_back(seg2.p2);
}
break;
case PO_STROKE_JOIN_ROUND:
{
float halfW = stroke_width / 2.f;
if(top_outside) {
p1 = bottom;
p2 = seg1.p3;
p3 = joint;
p4 = seg2.p1;
stroke.push_back(p2);
stroke.push_back(p1);
float a1 = angleBetweenPoints(p2, p3, p4);
poPoint diff = p2 - p3;
float a2 = atan2f(diff.y, diff.x);
float step = a1 / 9.f;
float a = a2;
for(int j=0; j<10; j++) {
tmp.set(cosf(a)*halfW, sinf(a)*halfW, 0.f);
stroke.push_back(tmp+p3);
stroke.push_back(p3);
a += step;
}
stroke.push_back(p4);
stroke.push_back(p1);
}
else {
p1 = top;
p2 = seg1.p4;
p3 = joint;
p4 = seg2.p2;
stroke.push_back(p1);
stroke.push_back(p2);
float a1 = angleBetweenPoints(p2, p3, p4);
poPoint diff = p2 - p3;
float a2 = atan2f(diff.y, diff.x);
float step = a1 / 9.f;
float a = a2;
for(int j=0; j<10; j++) {
tmp.set(cosf(a)*halfW, sinf(a)*halfW, 0.f);
stroke.push_back(p3);
stroke.push_back(tmp+p3);
a += step;
}
stroke.push_back(p1);
stroke.push_back(p4);
}
break;
}
}
}
