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; } } }