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