double calculate_point_to_line_distance(GeodesicLocation point, double[] line) { double distance = Math.Abs(line[0] * point.Latitude + line[1] * point.Longitude + line[2]); distance /= Math.Sqrt((line[0] * line[0]) + (line[1] * line[1])); return(distance); }
double[] get_line_factors(GeodesicLocation p1, GeodesicLocation p2) { double a = p1.Longitude - p2.Longitude; double b = p2.Latitude - p1.Latitude; double c = p1.Latitude * p2.Longitude - p2.Latitude * p1.Longitude; return(new double[] { a, b, c }); }
public void merge_detecions(Detection new_det) { color[0] += new_det.color[0]; color[1] += new_det.color[1]; color[2] += new_det.color[2]; update_color_id(); seen_times++; area_m = (area_m * (seen_times - 1) + new_det.area_m) / seen_times; double latitude = (gps_location.Latitude * (seen_times - 1) + new_det.gps_location.Latitude) / seen_times; double longitude = (gps_location.Longitude * (seen_times - 1) + new_det.gps_location.Longitude) / seen_times; gps_location = new GeodesicLocation(latitude, longitude); }
public void calculate_extreme_points() { /* * Need to run calculate_max_meters_horizontal() and calculate_max_meters_vertical() first and calculate_current_location!!! */ double distance = Math.Sqrt(((max_meters_horizontal / 2) * (max_meters_horizontal / 2)) + ((max_meters_vertical / 2) * (max_meters_vertical / 2))); double a1 = telemetry.attitude.yaw - beta; double a2 = telemetry.attitude.yaw - 180 + beta; double a3 = telemetry.attitude.yaw + beta; double a4 = telemetry.attitude.yaw + 180 - beta; // print("{:.8f},{:.8f}".format(latitude, longitude)) point_lu = geod.Location(currentLocation, a1, distance); //print("{:.8f},{:.8f}".format(g['lat2'], g['lon2'])) point_ld = geod.Location(currentLocation, a2, distance); //print("{:.8f},{:.8f}".format(g['lat2'], g['lon2'])) point_ru = geod.Location(currentLocation, a3, distance); //print("{:.8f},{:.8f}".format(g['lat2'], g['lon2'])) point_rd = geod.Location(currentLocation, a4, distance); //print("{:.8f},{:.8f}".format(g['lat2'], g['lon2'])) line_u = get_line_factors(point_lu, point_ru); line_d = get_line_factors(point_ld, point_rd); line_l = get_line_factors(point_lu, point_ld); line_r = get_line_factors(point_ru, point_rd); distance_vertical_geo = calculate_point_to_line_distance(point_lu, line_d); distance_horizontal_geo = calculate_point_to_line_distance(point_lu, line_r); }
public Point get_detection_on_image_cords(GeodesicLocation p) { double du = calculate_point_to_line_distance(p, line_u); double dd = calculate_point_to_line_distance(p, line_d); double dl = calculate_point_to_line_distance(p, line_l); double dr = calculate_point_to_line_distance(p, line_r); if (du <= distance_vertical_geo && dd <= distance_vertical_geo && dl <= distance_horizontal_geo && dr <= distance_horizontal_geo) { double x = dl / distance_horizontal_geo * GlobalValues.CAMERA_WIDTH; double y = du / distance_vertical_geo * GlobalValues.CAMERA_HEIGHT; return(new Point((int)x, (int)y)); } else { return(Point.Empty); } }
public void update_current_location() { currentLocation = new GeodesicLocation(telemetry.gps_location.latitude, telemetry.gps_location.longitude); }
public void update_lat_lon(GeodesicLocation location) { gps_location = location; }
private void mode_0() { int ind = 0; long milliseconds = DateTimeOffset.Now.ToUnixTimeMilliseconds(); List <Detection> confirmed_detections = new List <Detection>(); List <Detection> all_detections = new List <Detection>(); int confirmed_detection_id = 0; int time_index = 0; while (true) { String path = "1" + ".jpg"; Mat myimg = CvInvoke.Imread(path, ImreadModes.Color); Image <Bgr, Byte> img_bgr = myimg.ToImage <Bgr, Byte>(); List <Detection> detections = detector.detect(img_bgr); positionCalculator.update_current_location(); positionCalculator.update_meters_per_pixel(); positionCalculator.calculate_max_meters_area(); positionCalculator.calculate_extreme_points(); bool update_detections_file = false; foreach (Detection d in detections) { GeodesicLocation location = positionCalculator.calculate_point_lat_long(d.mid); d.update_lat_lon(location); d.area_m = positionCalculator.calculate_area_in_meters_2(d.area); d.draw_detection(img_bgr); foreach (Detection conf_d in confirmed_detections) { if (d.check_detection(conf_d)) { conf_d.merge_detecions(d); conf_d.last_seen = time_index; d.to_delete = true; update_detections_file = true; break; } } if (d.to_delete) { continue; } foreach (Detection all_d in all_detections) { if (d.check_detection(all_d)) { all_d.merge_detecions(d); all_d.last_seen = time_index; d.to_delete = true; break; } } if (!d.to_delete) { d.last_seen = time_index; } } detections.RemoveAll(d => d.to_delete); all_detections.AddRange(detections); foreach (Detection all_d in all_detections) { if (all_d.seen_times > 8) { all_d.detection_id = confirmed_detection_id; confirmed_detection_id++; String filename = Path.Combine(detectionsFolder.Path + @"\" + all_d.detection_id.ToString() + ".jpg"); all_d.filename = filename; Task t = write_to_file(detectionsFile, all_d.getString()); save_frame_crop(img_bgr, all_d.rectangle, filename); confirmed_detections.Add(all_d); all_d.to_delete = true; } else if (all_d.seen_times > 4) { if (time_index - all_d.last_seen > 800) { all_d.to_delete = true; } } else { if (time_index - all_d.last_seen > 20) { all_d.to_delete = true; } } } all_detections.RemoveAll(d => d.to_delete); String detections_file_text = ""; foreach (Detection c in confirmed_detections) { if (update_detections_file) { detections_file_text += c.getString(); } Point my_mid = positionCalculator.get_detection_on_image_cords(c.gps_location); if (!my_mid.IsEmpty) { c.draw_confirmed_detection(img_bgr, my_mid); } } if (update_detections_file) { Task t_dets = rewrite_file(detectionsFile, detections_file_text); } time_index++; CvInvoke.Imshow("img", img_bgr); CvInvoke.WaitKey(1); ind++; if (GlobalValues.PRINT_FPS && DateTimeOffset.Now.ToUnixTimeMilliseconds() - milliseconds > 1000) { milliseconds = DateTimeOffset.Now.ToUnixTimeMilliseconds(); System.Diagnostics.Debug.WriteLine(ind); ind = 0; } } }