Merge "Log the training data"
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b11f6f..0178790 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -40,6 +40,11 @@
HandleSuperstructureStatus(status);
});
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+ ekf_.P());
+ });
+
image_fetchers_.emplace_back(
event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
"/pi1/camera"));
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 1e72c39..e81691c 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -124,7 +124,6 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
last_frame_(monotonic_now()) {
set_team_id(frc971::control_loops::testing::kTeamNumber);
- SetStartingPosition({3.0, 2.0, 0.0});
set_battery_voltage(12.0);
if (!FLAGS_output_file.empty()) {
@@ -167,6 +166,8 @@
},
chrono::milliseconds(5));
+ test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });
+
// Run for enough time to allow the gyro/imu zeroing code to run.
RunFor(std::chrono::seconds(10));
}
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index 3d7bedb..a130ed6 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -25,6 +25,7 @@
"//y2020/vision/sift:sift_fbs_python",
"@bazel_tools//tools/python/runfiles",
"@opencv_contrib_nonfree_amd64//:python_opencv",
+ "//external:python-glog",
],
)
@@ -73,6 +74,7 @@
"//y2020/vision/sift:sift_fbs_python",
"@bazel_tools//tools/python/runfiles",
"@opencv_contrib_nonfree_amd64//:python_opencv",
+ "//external:python-glog",
],
)
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
index 22eb6ce..157f9e6 100644
--- a/y2020/vision/tools/python_code/define_training_data.py
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -1,5 +1,6 @@
import argparse
import cv2
+import glog
import json
import math
import numpy as np
@@ -17,7 +18,7 @@
global current_mouse
current_mouse = (x, y)
if event == cv2.EVENT_LBUTTONUP:
- #print("Adding point at %d, %d" % (x,y))
+ glog.debug("Adding point at %d, %d" % (x,y))
point_list.append([x, y])
pass
@@ -218,7 +219,7 @@
pts_3d_proj_2d, jac_2d = cv2.projectPoints(pts_3d_np, R, T, cam_mat,
distortion_coeffs)
if inliers is None:
- print("WARNING: Didn't get any inliers when reprojecting polygons")
+ glog.warn("WARNING: Didn't get any inliers when reprojecting polygons")
return img
for i in range(len(pts_2d)):
pt_2d = pts_2d_np[i][0]
@@ -239,7 +240,7 @@
image = cv2.imread("test_images/train_power_port_red.png")
polygon_list = define_polygon(image)
- print(polygon_list)
+ glog.debug(polygon_list)
def sample_define_points_by_list_usage():
@@ -251,5 +252,5 @@
(689, 679)]
polygon_list = define_points_by_list(image, test_points)
- print(polygon_list)
+ glog.debug(polygon_list)
diff --git a/y2020/vision/tools/python_code/field_display.py b/y2020/vision/tools/python_code/field_display.py
index 93f645c..11d098d 100644
--- a/y2020/vision/tools/python_code/field_display.py
+++ b/y2020/vision/tools/python_code/field_display.py
@@ -17,17 +17,18 @@
# Convert field coordinates (meters) to image coordinates (pixels).
-# Field origin is (x,y) at center of red alliance driver station wall,
-# with x pointing into the field.
+# Field origin is (x,y) at center of the field,
+# with x pointing towards the red alliance driver station
# The default field image is 1598 x 821 pixels, so 1 cm per pixel on field
# I.e., Field is 1598 x 821 pixels = 15.98 x 8.21 meters
-# Our origin in image coordinates is at (1598, 410) pixels, facing in the -x image direction
+# Our origin in image coordinates is at (799, 410) pixels, with +x direction
+# to the right
def field_coord_to_image_coord(robot_position):
# Scale by 100 pixel / cm
- robot_pos_img_coord = np.array([[1598, 410]]).T + np.int32(
- 100.0 * np.array([[-robot_position[0][0], robot_position[1][0]]]).T)
+ robot_pos_img_coord = np.array([[799, 410]]).T + np.int32(
+ 100.0 * np.array([[robot_position[0][0], -robot_position[1][0]]]).T)
return robot_pos_img_coord
diff --git a/y2020/vision/tools/python_code/image_match_test.py b/y2020/vision/tools/python_code/image_match_test.py
index d6b7deb..09d2764 100644
--- a/y2020/vision/tools/python_code/image_match_test.py
+++ b/y2020/vision/tools/python_code/image_match_test.py
@@ -1,36 +1,20 @@
import cv2
import math
-from matplotlib import pyplot as plt
import numpy as np
import time
import field_display
import train_and_match as tam
+import target_definition
+import camera_definition
### DEFINITIONS
+target_definition.USE_BAZEL = False
+target_list = target_definition.compute_target_definition()
+camera_list = camera_definition.camera_list
-# List of images to train on
-training_image_names = [
- 'test_images/train_power_port_blue.png',
- # Using webcam captured image for testing-- best to use only one of the two
- # training images for the power_port_red
- 'test_images/train_power_port_red_webcam.png',
- # 'test_images/train_power_port_red.png',
- 'test_images/train_loading_bay_blue.png',
- 'test_images/train_loading_bay_red.png'
-]
-
-# Target points on the image -- needs to match training images-- one per image
-# These are manually captured by examining the images, and entering the pixel
-# values from the target center for each image.
-# These are currently only used for visualization of the target
-target_pt_list = [
- np.float32([[393, 147]]).reshape(-1, 1, 2), # train_power_port_blue.png
- np.float32([[305, 97]]).reshape(-1, 1, 2), #train_power_port_red_webcam.png
- # np.float32([[570,192]]).reshape(-1,1,2), # train_power_port_red.png
- np.float32([[366, 236]]).reshape(-1, 1, 2), # train_loading_bay_blue.png
- np.float32([[366, 236]]).reshape(-1, 1, 2), # train_loading_bay_red.png
-]
+# For now, just use the first one
+camera_params = camera_list[0]
# Put list of all possible images we want to test, and then we'll select one
# Note, if you query on the same image as training, only that image will match
@@ -45,8 +29,8 @@
'test_images/test_raspi3_sample.jpg', #7
'test_images/test_VR_sample1.png', #8
'test_images/train_loading_bay_blue.png', #9
- 'test_images/train_loading_bay_red.png'
-] #10
+ 'test_images/train_loading_bay_red.png' #10
+]
training_image_index = 0
# TODO: Should add argParser here to select this
@@ -54,15 +38,25 @@
##### Let's get to work!
-# Load the training and query images
-training_images = tam.load_images(training_image_names)
+training_image_names = []
+training_images = []
+train_keypoint_lists = []
+train_descriptor_lists = []
+target_points_tmp = []
+target_pt_list = None
+# Popluate the data structures used by this function
+for target in target_list:
+ # Image names
+ print("Target filename:", target.image_filename)
+ training_image_names.append(target.image_filename)
+ # The training images
+ training_images.append(target.image)
+ # Keypoints and desciptors
+ train_keypoint_lists.append(target.keypoint_list)
+ train_descriptor_lists.append(target.descriptor_list)
+ target_points_tmp.append(target.target_point_2d)
-# Create feature extractor
-feature_extractor = tam.load_feature_extractor()
-
-# Extract keypoints and descriptors for model
-train_keypoint_lists, train_descriptor_lists = tam.detect_and_compute(
- feature_extractor, training_images)
+target_pt_list = np.asarray(target_points_tmp).reshape(-1, 1, 2)
# # Create the matcher. This only needs to be created once
ts = time.monotonic() # Do some basic timing
@@ -72,6 +66,11 @@
for i in range(len(train_keypoint_lists)):
print("Model ", i, " has ", len(train_keypoint_lists[i]), " features: ")
+# Create feature extractor
+# TODO<Jim>: Should probably make sure we're using the same extractor for
+# training and query
+feature_extractor = tam.load_feature_extractor()
+
# Load the query image in. Based on index in our list, or using camera if index is -1
query_images = []
@@ -121,183 +120,160 @@
for i in range(len(train_keypoint_lists)):
print("Model ", i, " has # good matches: ", len(good_matches_list[i]))
- # TEMP: Use first list for what follows
- # TODO: Should look for best match after homography and display that one
- # OR: show results on all good matches
- good_matches = good_matches_list[0]
+ # If we're querying static images, show full results
+ if (query_image_index is not -1):
+ print("Showing results for static query image")
+ homography_list, matches_mask_list = tam.show_results(
+ training_images, train_keypoint_lists, query_images,
+ query_keypoint_lists, target_pt_list, good_matches_list)
# Next, find homography (map between training and query images)
homography_list, matches_mask_list = tam.compute_homographies(
train_keypoint_lists, query_keypoint_lists, good_matches_list)
- if matches_mask_list[0].count(1) < tam.MIN_MATCH_COUNT:
- print(
- "Not continuing pose calc because not enough good matches after homography-- only had ",
- matches_mask_list[0].count(1))
- continue
+ # Image used to store field plot
+ img_ret = None
+ for i, good_matches in enumerate(good_matches_list):
+ # TODO: Should look for best match after homography and display that one
+ # OR: show results on all good matches
+ print("Processing match for model %d" % i)
- # Next, let's go to compute pose
- # I've chosen some coordinate frames to help track things. Using indices:
- # w: world coordinate frame (at center wall of driver station)
- # tarj: camera pose when jth target (e.g., power panel) was captured
- # ci: ith camera
- # b: body frame of robot (robot location)
- #
- # And, T for translation, R for rotation, "est" for estimated
- # So, T_w_b_est is the estimated translation from world to body coords
- #
- # A few notes:
- # -- CV uses rotated frame, where z points out of camera, y points down
- # -- This causes a lot of mapping around of points, but overall OK
+ if matches_mask_list[i].count(1) < tam.MIN_MATCH_COUNT:
+ print(
+ "Not continuing pose calc because not enough good matches after homography-- only had ",
+ matches_mask_list[i].count(1))
+ continue
- ### TODO: Still need to clean this rest up
+ # Next, let's go to compute pose
+ # I've chosen some coordinate frames to help track things. Using indices:
+ # w: world coordinate frame (at center wall of driver station)
+ # tarj: camera pose when jth target (e.g., power panel) was captured
+ # ci: ith camera
+ # b: body frame of robot (robot location)
+ #
+ # And, T for translation, R for rotation, "est" for estimated
+ # So, T_w_b_est is the estimated translation from world to body coords
+ #
+ # A few notes:
+ # -- CV uses rotated frame, where z points out of camera, y points down
+ # -- This causes a lot of mapping around of points, but overall OK
- # TODO: Need to generalize this for each target-- need to have 3D global locations of each target's keypoints
- field_length = 15.98
- target_center_offset_y = 1.67
- target_center_height = 2.49
- if training_image_index is 0:
- # webcam
- fx = 810.
- fy = 810.
- cx = 320.
- cy = 240.
- cy_above_ground = 1. # meters above ground
- depth = 4.57 # meters
- target_center_offset_y = 1.67
- elif training_image_index is 1:
- # Made up for screenshot from manual
- cx = int(1165 / 2)
- cy_above_ground = -0.5856 # meters above ground
- cy = 776 / 2
- fx = 1143 # pixels (or 0.00160m or 1143 pixels?)
- depth = 4.57 # meters
- elif training_image_index is 2:
- cx = 732 / 2
- cy_above_ground = 0.454 # meters above ground
- cy = 436 / 2
- target_width = 5 * 12 * 0.0254 # width in ft * in/ft * m/in
- target_height = target_width * cy / cx # width in ft * in/ft * m/in * cy / cx
- FOV_x = 54 * math.pi / 180. # camera FOV in radians (from 54 degree lens)
- fx = cx / math.tan(FOV_x / 2) # pixels
- depth = (target_height / 2) / math.tan(FOV_x / 2) # meters
+ src_pts_3d = []
+ for m in good_matches:
+ src_pts_3d.append(target_list[i].keypoint_list_3d[m.trainIdx])
+ pt = query_keypoint_lists[0][m.queryIdx].pt
+ print("Color at ", pt, " is ", query_images[0][int(pt[1])][int(
+ pt[0])])
+ query_images[0] = cv2.circle(
+ query_images[0], (int(pt[0]), int(pt[1])), 5, (0, 255, 0), 3)
- src_pts_3d = []
- for m in good_matches:
- # Project the matches all back to 3D, assuming they're all at the same depth
- # Add in the distance across the field and any lateral offset of the target
- src_pts_3d.append([(
- field_length,
- -(train_keypoint_lists[training_image_index][m.trainIdx].pt[0] - cx
- ) * depth / fx - target_center_offset_y,
- -(train_keypoint_lists[training_image_index][m.trainIdx].pt[1] - cy
- ) * depth / fx + cy_above_ground)])
+ cv2.imshow('DEBUG', query_images[0]), cv2.waitKey(0)
+ # Reshape 3d point list to work with computations to follow
+ src_pts_3d_array = np.asarray(np.float32(src_pts_3d)).reshape(-1, 3, 1)
- # Reshape 3d point list to work with computations to follow
- src_pts_3d_array = np.asarray(np.float32(src_pts_3d)).reshape(-1, 3, 1)
+ # Load from camera parameters
+ cam_mat = camera_params.camera_int.camera_matrix
+ distortion_coeffs = camera_params.camera_int.distortion_coeffs
- cam_mat = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
- distortion_coeffs = np.zeros((5, 1))
+ # Create list of matching query point locations
+ dst_pts = np.float32([
+ query_keypoint_lists[0][m.queryIdx].pt for m in good_matches
+ ]).reshape(-1, 1, 2)
- # Create list of matching query point locations
- dst_pts = np.float32([
- query_keypoint_lists[0][m.queryIdx].pt for m in good_matches
- ]).reshape(-1, 1, 2)
+ ts = time.monotonic()
+ # TODO: May want to supply it with estimated guess as starting point
+ # Find offset from camera to original location of camera relative to target
+ retval, R_ci_w_estj, T_ci_w_estj, inliers = cv2.solvePnPRansac(
+ src_pts_3d_array, dst_pts, cam_mat, distortion_coeffs)
- ts = time.monotonic()
- # TODO: May want to supply it with estimated guess as starting point
- # Find offset from camera to original location of camera relative to target
- retval, R_ci_w_estj, T_ci_w_estj, inliers = cv2.solvePnPRansac(
- src_pts_3d_array, dst_pts, cam_mat, distortion_coeffs)
+ tf = time.monotonic()
+ print("Solving Pose took ", tf - ts, " secs")
+ print("Found ", len(inliers), " inliers, out of ", len(dst_pts),
+ " matched points")
+ ### THIS is the estimate of the robot on the field!
+ R_w_ci_estj, T_w_ci_estj = field_display.invert_pose_Rodrigues(
+ R_ci_w_estj, T_ci_w_estj)
+ #print("Pose from PnP is:\n", R_ci_w_estj, "\n", T_ci_w_estj)
+ # The rotation estimate is for camera with z pointing forwards.
+ # Compute the rotation as if x is forward
+ R_w_ci_estj_robot = field_display.camera_rot_to_world_Rodrigues(
+ R_w_ci_estj)
+ #print("Pose in world coords is:\n", R_w_ci_estj, "\n", T_w_ci_estj,
+ # "\nWith robot coord frame rotation as \n", R_w_ci_estj_robot)
- tf = time.monotonic()
- print("Solving Pose took ", tf - ts, " secs")
- print("Found ", len(inliers), " inliers, out of ", len(dst_pts),
- " matched points")
- ### THIS is the estimate of the robot on the field!
- R_w_ci_estj, T_w_ci_estj = field_display.invert_pose_Rodrigues(
- R_ci_w_estj, T_ci_w_estj)
- print("Pose from PnP is:\n", R_ci_w_estj, "\n", T_ci_w_estj)
- # The rotation estimate is for camera with z pointing forwards.
- # Compute the rotation as if x is forward
- R_w_ci_estj_robot = field_display.camera_rot_to_world_Rodrigues(
- R_w_ci_estj)
- print("Pose in world coords is:\n", R_w_ci_estj, "\n", T_w_ci_estj,
- "\nWith robot coord frame rotation as \n", R_w_ci_estj_robot)
+ # Use the rotational component about the z axis to define the heading
+ # TODO<jim>: Change this to use rotation of x-unit vector
+ heading_est = R_w_ci_estj_robot[2][0]
+ # Plot location of the robot, along with heading
+ color = (255, 0, 0) # Blue
+ if i in (0, 1):
+ color = (0, 0, 255) # Red
+ img_ret = field_display.plot_bot_on_field(img_ret, color, T_w_ci_estj,
+ heading_est)
- # Use the rotational component about the z axis to define the heading
- heading_est = R_w_ci_estj_robot[2][0]
- # Plot location of the robot, along with heading
- img_ret = field_display.plot_bot_on_field(None, (0, 255, 255), T_w_ci_estj,
- heading_est)
+ # Compute vector to target
+ T_w_target_pt = np.array(target_list[i].target_position).reshape(3, 1)
+ vector_to_target = T_w_target_pt - T_w_ci_estj
+ # Compute distance to target (full 3D)
+ distance_to_target = np.linalg.norm(vector_to_target)
+ # Compute distance to target (x,y)
+ distance_to_target_ground = np.linalg.norm(vector_to_target[0:2])
+ #print("Distance comparison: all: ", distance_to_target, " vs. ground: ", distance_to_target_ground)
- # Compute vector to target
- T_w_target_pt = np.array(
- [[field_length, -target_center_offset_y, target_center_height]]).T
- vector_to_target = T_w_target_pt - T_w_ci_estj
- # Compute distance to target (full 3D)
- distance_to_target = np.linalg.norm(vector_to_target)
- # Compute distance to target (x,y)
- distance_to_target_ground = np.linalg.norm(vector_to_target[0:2])
- #print("Distance comparison: all: ", distance_to_target, " vs. ground: ", distance_to_target_ground)
+ angle_to_target_abs = math.atan2(vector_to_target[1][0],
+ vector_to_target[0][0])
+ angle_to_target_robot = angle_to_target_abs - heading_est
+ img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0),
+ T_w_ci_estj, T_w_target_pt)
+ # THESE ARE OUR ESTIMATES OF HEADING, DISTANCE, AND SKEW TO TARGET
+ log_file.write('%lf, %lf, %lf\n' %
+ (heading_est, distance_to_target_ground,
+ angle_to_target_robot))
- angle_to_target_abs = math.atan2(vector_to_target[1][0],
- vector_to_target[0][0])
- angle_to_target_robot = angle_to_target_abs - heading_est
- img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0),
- T_w_ci_estj, T_w_target_pt)
- # THESE ARE OUR ESTIMATES OF HEADING, DISTANCE, AND SKEW TO TARGET
- log_file.write('%lf, %lf, %lf\n' % (heading_est, distance_to_target_ground,
- angle_to_target_robot))
+ # A bunch of code to visualize things...
+ #
+ # Draw target on the image
+ h, w, ch = query_images[0].shape
+ query_image_copy = query_images[0].copy()
+ # Map target_pt onto image, for display
+ target_point_2d_trans = cv2.perspectiveTransform(
+ target_pt_list[i].reshape(-1, 1, 2),
+ homography_list[i]).astype(int)
+ # Ballpark the size of the circle so it looks right on image
+ radius = int(
+ 32 * abs(homography_list[i][0][0] + homography_list[i][1][1]) /
+ 2) # Average of scale factors
+ query_image_copy = cv2.circle(query_image_copy,
+ (target_point_2d_trans.flatten()[0],
+ target_point_2d_trans.flatten()[1]),
+ radius, (0, 255, 0), 3)
- # A bunch of code to visualize things...
- #
- # Draw target on the image
- h, w, ch = query_images[0].shape
- query_image_copy = query_images[0].copy()
- # Map target_pt onto image, for display
- transformed_target = cv2.perspectiveTransform(target_pt_list[0],
- homography_list[0])
- # Ballpark the size of the circle so it looks right on image
- radius = int(32 * abs(homography_list[0][0][0] + homography_list[0][1][1])
- / 2) # Average of scale factors
- query_image_copy = cv2.circle(
- query_image_copy,
- (transformed_target.flatten()[0], transformed_target.flatten()[1]),
- radius, (0, 255, 0), 3)
+ # Create empty image the size of the field
+ img_heading = field_display.load_field_image()
+ img_heading[:, :, :] = 0
+ f_h, f_w, f_ch = img_heading.shape
- # If we're querying static images, show full results
- if (query_image_index is not -1):
- homography_list, matches_mask_list = tam.show_results(
- training_images, train_keypoint_lists, query_images,
- query_keypoint_lists, target_pt_list, good_matches_list)
+ # Create heading view, and paste it to bottom of field
+ img_heading = field_display.plot_camera_to_target(
+ img_heading, (0, 255, 0), heading_est, distance_to_target_ground,
+ angle_to_target_robot)
+ vis = np.concatenate((img_ret, img_heading), axis=0)
- # Create empty image the size of the field
- img_heading = field_display.load_field_image()
- img_heading[:, :, :] = 0
- f_h, f_w, f_ch = img_heading.shape
+ # Paste query image to right of other views (scale to keep aspect ratio)
+ img_query_scaled = cv2.resize(query_image_copy,
+ (int(2 * w * f_h / h), 2 * f_h))
+ vis = np.concatenate((vis, img_query_scaled), axis=1)
- # Create heading view, and paste it to bottom of field
- img_heading = field_display.plot_camera_to_target(
- img_heading, (0, 255, 0), heading_est, distance_to_target_ground,
- angle_to_target_robot)
- vis = np.concatenate((img_ret, img_heading), axis=0)
+ # Scale down to make it fit on screen
+ vis = cv2.resize(vis, (int(vis.shape[1] / 4), int(vis.shape[0] / 4)))
+ cv2.imshow('field_display', vis)
- # Paste query image to right of other views (scale to keep aspect ratio)
- img_query_scaled = cv2.resize(query_image_copy,
- (int(2 * w * f_h / h), 2 * f_h))
- vis = np.concatenate((vis, img_query_scaled), axis=1)
-
- # Scale down to make it fit on screen
- vis = cv2.resize(vis, (int(vis.shape[1] / 4), int(vis.shape[0] / 4)))
- cv2.imshow('field_display', vis)
-
- #Waits for a user input to quit the application
- pause_time = 0
- if (query_image_index is -1):
- pause_time = 1
- if cv2.waitKey(pause_time) & 0xFF == ord('q'):
- break
+ #Waits for a user input to quit the application
+ pause_time = 0
+ if (query_image_index is -1):
+ pause_time = 1
+ if cv2.waitKey(pause_time) & 0xFF == ord('q'):
+ break
# Done. Clean things up
if (query_image_index is -1):
@@ -305,4 +281,3 @@
cap.release()
cv2.destroyAllWindows()
-
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index fba847d..65f3342 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -1,6 +1,7 @@
#!/usr/bin/python3
import cv2
+import glog
import numpy as np
import sys
import flatbuffers
@@ -35,23 +36,23 @@
if (len(sys.argv) > 2):
if sys.argv[2] == "test":
- print("Loading test data")
+ glog.info("Loading test data")
import camera_definition_test
import target_definition_test
target_data_list = target_definition_test.compute_target_definition(
)
camera_calib_list = camera_definition_test.camera_list
else:
- print("Unhandled arguments: '%s'" % sys.argv[2])
+ glog.error("Unhandled arguments: '%s'" % sys.argv[2])
quit()
else:
- print("Loading target configuration data")
+ glog.info("Loading target configuration data")
import camera_definition
import target_definition
target_data_list = target_definition.compute_target_definition()
camera_calib_list = camera_definition.camera_list
- print("Writing file to ", output_path)
+ glog.info("Writing file to ", output_path)
fbb = flatbuffers.Builder(0)
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index b467608..1727dff 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -1,5 +1,7 @@
import argparse
import cv2
+# TODO<Jim>: Add gflags for handling command-line flags
+import glog
import math
import numpy as np
@@ -7,6 +9,8 @@
import define_training_data as dtd
import train_and_match as tam
+# TODO<Jim>: Allow command-line setting of logging level
+glog.setLevel("WARN")
global VISUALIZE_KEYPOINTS
global USE_BAZEL
USE_BAZEL = True
@@ -39,6 +43,7 @@
self.descriptor_list = []
self.target_rotation = None
self.target_position = None
+ self.target_point_2d = None
def extract_features(self, feature_extractor=None):
if feature_extractor is None:
@@ -68,8 +73,8 @@
# Filter and project points for each polygon in the list
filtered_keypoints, _, _, _, keep_list = dtd.filter_keypoints_by_polygons(
keypoint_list, None, [self.polygon_list[poly_ind]])
- print("Filtering kept %d of %d features" % (len(keep_list),
- len(keypoint_list)))
+ glog.info("Filtering kept %d of %d features" % (len(keep_list),
+ len(keypoint_list)))
filtered_point_array = np.asarray(
[(keypoint.pt[0], keypoint.pt[1])
for keypoint in filtered_keypoints]).reshape(-1, 2)
@@ -89,24 +94,27 @@
ideal_target_list = []
training_target_list = []
- # Some general info about our field and targets
- # Assume camera centered on target at 1 m above ground and distance of 4.85m
- field_length = 15.98
+ # Using coordinate system as defined in sift.fbs:
+ # field origin (0, 0, 0) at floor height at center of field
+ # Robot orientation with x-axis pointing towards RED ALLIANCE player station
+ # y-axis to left and z-axis up (right-hand coordinate system)
+ # Thus, the red power port target location will be at (-15.98/2,1.67,0),
+ # with orientation (facing out of the field) of M_PI
+
+ # Field constants
+ field_length = 15.983
power_port_total_height = 3.10
- power_port_center_y = 1.67
- power_port_width = 1.22
- power_port_bottom_wing_height = 1.88
+ power_port_edge_y = 1.089
+ power_port_width = 1.225
+ power_port_bottom_wing_height = 1.828
power_port_wing_width = 1.83
- loading_bay_edge_y = 1.11
- loading_bay_width = 1.52
- loading_bay_height = 0.94
+ loading_bay_edge_y = 0.784
+ loading_bay_width = 1.524
+ loading_bay_height = 0.933
+ wing_angle = 20. * math.pi / 180.
# Pick the target center location at halfway between top and bottom of the top panel
- target_center_height = (
- power_port_total_height + power_port_bottom_wing_height) / 2.
-
- # TODO: Still need to figure out what this angle actually is
- wing_angle = 20. * math.pi / 180.
+ power_port_target_height = (power_port_total_height + power_port_bottom_wing_height) / 2.
###
### Red Power Port
@@ -124,32 +132,31 @@
# These are "virtual" 3D points based on the expected geometry
power_port_red_main_panel_polygon_points_3d = [
- (field_length, -power_port_center_y + power_port_width / 2.,
- 0.), (field_length, -power_port_center_y + power_port_width / 2.,
- power_port_bottom_wing_height),
- (field_length,
- -power_port_center_y + power_port_width / 2. + power_port_wing_width,
+ (-field_length/2., power_port_edge_y, 0.),
+ (-field_length/2., power_port_edge_y,
power_port_bottom_wing_height),
- (field_length, -power_port_center_y + power_port_width / 2.,
+ (-field_length/2., power_port_edge_y
+ - power_port_wing_width, power_port_bottom_wing_height),
+ (-field_length/2., power_port_edge_y,
power_port_total_height),
- (field_length, -power_port_center_y - power_port_width / 2.,
+ (-field_length/2., power_port_edge_y + power_port_width,
power_port_total_height),
- (field_length, -power_port_center_y - power_port_width / 2.,
+ (-field_length/2., power_port_edge_y + power_port_width,
power_port_bottom_wing_height),
- (field_length, -power_port_center_y - power_port_width / 2., 0.)
+ (-field_length/2., power_port_edge_y + power_port_width, 0.)
]
power_port_red_wing_panel_polygon_points_2d = [(689, 74), (1022, 302),
(689, 302)]
# These are "virtual" 3D points based on the expected geometry
power_port_red_wing_panel_polygon_points_3d = [
- (field_length, -power_port_center_y - power_port_width / 2.,
+ (-field_length/2., power_port_edge_y + power_port_width,
power_port_total_height),
- (field_length - power_port_wing_width * math.sin(wing_angle),
- -power_port_center_y - power_port_width / 2. -
- power_port_wing_width * math.cos(wing_angle),
+ (-field_length/2. + power_port_wing_width * math.sin(wing_angle),
+ power_port_edge_y + power_port_width
+ + power_port_wing_width * math.cos(wing_angle),
power_port_bottom_wing_height),
- (field_length, -power_port_center_y - power_port_width / 2.,
+ (-field_length/2., power_port_edge_y + power_port_width,
power_port_bottom_wing_height)
]
@@ -159,19 +166,24 @@
ideal_power_port_red.polygon_list_3d.append(
power_port_red_main_panel_polygon_points_3d)
- ideal_power_port_red.polygon_list.append(
- power_port_red_wing_panel_polygon_points_2d)
- ideal_power_port_red.polygon_list_3d.append(
- power_port_red_wing_panel_polygon_points_3d)
-
# Define the pose of the target
# Location is on the ground, at the center of the target
# Orientation is with "x" pointing out of the field, and "z" up
# This way, if robot is facing target directly, the x-axes are aligned
# and the skew to target is zero
- ideal_power_port_red.target_rotation = np.identity(3, np.double)
- ideal_power_port_red.target_position = np.array(
- [field_length, -power_port_center_y, 0.])
+ ideal_power_port_red.target_rotation = -np.identity(3, np.double)
+ ideal_power_port_red.target_rotation[2][2] = 1.
+ ideal_power_port_red.target_position = np.array([-field_length/2.,
+ power_port_edge_y + power_port_width/2.,
+ power_port_target_height])
+
+ # Target point on the image -- needs to match training image
+ # These are manually captured by examining the images,
+ # and entering the pixel values from the target center for each image.
+ # These are currently only used for visualization of the target
+ ideal_power_port_red.target_point_2d = np.float32([[570,192]]).reshape(-1,1,2), # train_power_port_red.png
+
+ # np.float32([[305, 97]]).reshape(-1, 1, 2), #train_power_port_red_webcam.png
# Add the ideal 3D target to our list
ideal_target_list.append(ideal_power_port_red)
@@ -195,11 +207,10 @@
# These are "virtual" 3D points based on the expected geometry
loading_bay_red_polygon_points_3d = [
- (field_length, loading_bay_edge_y + loading_bay_width, 0.),
- (field_length, loading_bay_edge_y + loading_bay_width,
- loading_bay_height), (field_length, loading_bay_edge_y,
- loading_bay_height), (field_length,
- loading_bay_edge_y, 0.)
+ (field_length/2., loading_bay_edge_y + loading_bay_width, 0.),
+ (field_length/2., loading_bay_edge_y + loading_bay_width, loading_bay_height),
+ (field_length/2., loading_bay_edge_y, loading_bay_height),
+ (field_length/2., loading_bay_edge_y, 0.)
]
ideal_loading_bay_red.polygon_list.append(
@@ -208,8 +219,10 @@
loading_bay_red_polygon_points_3d)
# Location of target
ideal_loading_bay_red.target_rotation = np.identity(3, np.double)
- ideal_loading_bay_red.target_position = np.array(
- [field_length, -loading_bay_edge_y - loading_bay_width / 2., 0.])
+ ideal_loading_bay_red.target_position = np.array([field_length/2.,
+ loading_bay_edge_y + loading_bay_width/2.,
+ loading_bay_height/2.])
+ ideal_loading_bay_red.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2), # train_loading_bay_red.png
ideal_target_list.append(ideal_loading_bay_red)
training_target_loading_bay_red = TargetData(
@@ -233,31 +246,31 @@
# These are "virtual" 3D points based on the expected geometry
power_port_blue_main_panel_polygon_points_3d = [
- (0., power_port_center_y - power_port_width / 2.,
- 0.), (0., power_port_center_y - power_port_width / 2.,
- power_port_bottom_wing_height),
- (0.,
- power_port_center_y - power_port_width / 2. - power_port_wing_width,
+ (field_length/2., -power_port_edge_y, 0.),
+ (field_length/2., -power_port_edge_y,
power_port_bottom_wing_height),
- (0., power_port_center_y - power_port_width / 2.,
- power_port_total_height),
- (0., power_port_center_y + power_port_width / 2.,
- power_port_total_height),
- (0., power_port_center_y + power_port_width / 2.,
+ (field_length/2., -power_port_edge_y + power_port_wing_width,
power_port_bottom_wing_height),
- (0., power_port_center_y + power_port_width / 2., 0.)
+ (field_length/2., -power_port_edge_y,
+ power_port_total_height),
+ (field_length/2., -power_port_edge_y - power_port_width,
+ power_port_total_height),
+ (field_length/2., -power_port_edge_y - power_port_width,
+ power_port_bottom_wing_height),
+ (field_length/2., -power_port_edge_y - power_port_width, 0.)
]
power_port_blue_wing_panel_polygon_points_2d = [(692, 50), (1047, 285),
(692, 285)]
# These are "virtual" 3D points based on the expected geometry
power_port_blue_wing_panel_polygon_points_3d = [
- (0., power_port_center_y + power_port_width / 2.,
+ (field_length/2., -power_port_edge_y - power_port_width,
power_port_total_height),
- (power_port_wing_width * math.sin(wing_angle), power_port_center_y -
- power_port_width / 2. + power_port_wing_width * math.cos(wing_angle),
+ (field_length/2. - power_port_wing_width * math.sin(wing_angle),
+ -power_port_edge_y - power_port_width -
+ power_port_wing_width * math.cos(wing_angle),
power_port_bottom_wing_height),
- (0., power_port_center_y + power_port_width / 2.,
+ (field_length/2., -power_port_edge_y - power_port_width,
power_port_bottom_wing_height)
]
@@ -267,16 +280,12 @@
ideal_power_port_blue.polygon_list_3d.append(
power_port_blue_main_panel_polygon_points_3d)
- ideal_power_port_blue.polygon_list.append(
- power_port_blue_wing_panel_polygon_points_2d)
- ideal_power_port_blue.polygon_list_3d.append(
- power_port_blue_wing_panel_polygon_points_3d)
-
# Location of target. Rotation is pointing in -x direction
- ideal_power_port_blue.target_rotation = -np.identity(3, np.double)
- ideal_power_port_blue.target_rotation[2][2] = 1.
- ideal_power_port_blue.target_position = np.array(
- [0., power_port_center_y, 0.])
+ ideal_power_port_blue.target_rotation = np.identity(3, np.double)
+ ideal_power_port_blue.target_position = np.array([field_length/2.,
+ -power_port_edge_y - power_port_width/2.,
+ power_port_target_height])
+ ideal_power_port_blue.target_point_2d = np.float32([[567, 180]]).reshape(-1, 1, 2), # train_power_port_blue.png
ideal_target_list.append(ideal_power_port_blue)
training_target_power_port_blue = TargetData(
@@ -299,18 +308,10 @@
# These are "virtual" 3D points based on the expected geometry
loading_bay_blue_polygon_points_3d = [
- (0., -loading_bay_edge_y - loading_bay_width, 0.),
- (0., -loading_bay_edge_y - loading_bay_width,
- loading_bay_height), (0., -loading_bay_edge_y,
- loading_bay_height), (0., -loading_bay_edge_y,
- 0.)
- ]
- loading_bay_red_polygon_points_3d = [
- (field_length, loading_bay_edge_y + loading_bay_width, 0.),
- (field_length, loading_bay_edge_y + loading_bay_width,
- loading_bay_height), (field_length, loading_bay_edge_y,
- loading_bay_height), (field_length,
- loading_bay_edge_y, 0.)
+ (-field_length/2., -loading_bay_edge_y - loading_bay_width, 0.),
+ (-field_length/2., -loading_bay_edge_y - loading_bay_width, loading_bay_height),
+ (-field_length/2., -loading_bay_edge_y, loading_bay_height),
+ (-field_length/2., -loading_bay_edge_y, 0.)
]
ideal_loading_bay_blue.polygon_list.append(
@@ -321,8 +322,10 @@
# Location of target
ideal_loading_bay_blue.target_rotation = -np.identity(3, np.double)
ideal_loading_bay_blue.target_rotation[2][2] = 1.
- ideal_loading_bay_blue.target_position = np.array(
- [0., loading_bay_edge_y + loading_bay_width / 2., 0.])
+ ideal_loading_bay_blue.target_position = np.array([-field_length/2.,
+ -loading_bay_edge_y - loading_bay_width/2.,
+ loading_bay_height/2.])
+ ideal_loading_bay_blue.target_point_2d = np.float32([[366, 236]]).reshape(-1, 1, 2), # train_loading_bay_blue.png
ideal_target_list.append(ideal_loading_bay_blue)
training_target_loading_bay_blue = TargetData(
@@ -338,9 +341,7 @@
camera_params = camera_definition.web_cam_params
for ideal_target in ideal_target_list:
- if not USE_BAZEL:
- print("\nPreparing target for image %s" %
- ideal_target.image_filename)
+ glog.info("\nPreparing target for image %s" % ideal_target.image_filename)
ideal_target.extract_features(feature_extractor)
ideal_target.filter_keypoints_by_polygons()
ideal_target.compute_reprojection_maps()
@@ -389,9 +390,7 @@
AUTO_PROJECTION = True
if AUTO_PROJECTION:
- print(
- "\n\nAuto projection of training keypoints to 3D using ideal images"
- )
+ glog.info("\n\nAuto projection of training keypoints to 3D using ideal images")
# Match the captured training image against the "ideal" training image
# and use those matches to pin down the 3D locations of the keypoints
@@ -400,9 +399,7 @@
training_target = training_target_list[target_ind]
ideal_target = ideal_target_list[target_ind]
- if not USE_BAZEL:
- print("\nPreparing target for image %s" %
- training_target.image_filename)
+ glog.info("\nPreparing target for image %s" % training_target.image_filename)
# Extract keypoints and descriptors for model
training_target.extract_features(feature_extractor)
@@ -417,23 +414,28 @@
[training_target.keypoint_list], [ideal_target.keypoint_list],
matches_list)
+ # Compute H_inv, since this maps points in ideal target to
+ # points in the training target
+ H_inv = np.linalg.inv(homography_list[0])
+
for polygon in ideal_target.polygon_list:
- ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(
- -1, 1, 2)
- H_inv = np.linalg.inv(homography_list[0])
+ ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(-1, 1, 2)
# We use the ideal target's polygons to define the polygons on
# the training target
transformed_polygon = cv2.perspectiveTransform(
ideal_pts_2d, H_inv)
training_target.polygon_list.append(transformed_polygon)
- print("Started with %d keypoints" % len(
- training_target.keypoint_list))
+ # Also project the target point from the ideal to training image
+ training_target_point_2d = cv2.perspectiveTransform(np.asarray(ideal_target.target_point_2d).reshape(-1,1,2), H_inv)
+ training_target.target_point_2d = training_target_point_2d.reshape(-1,1,2)
+
+ glog.info("Started with %d keypoints" % len(training_target.keypoint_list))
training_target.keypoint_list, training_target.descriptor_list, rejected_keypoint_list, rejected_descriptor_list, _ = dtd.filter_keypoints_by_polygons(
training_target.keypoint_list, training_target.descriptor_list,
training_target.polygon_list)
- print("After filtering by polygons, had %d keypoints" % len(
+ glog.info("After filtering by polygons, had %d keypoints" % len(
training_target.keypoint_list))
if VISUALIZE_KEYPOINTS:
tam.show_keypoints(training_target.image,
@@ -488,27 +490,16 @@
if __name__ == '__main__':
ap = argparse.ArgumentParser()
- ap.add_argument(
- "-v",
- "--visualize",
- help="Whether to visualize the results",
- default=False,
- action='store_true')
- ap.add_argument(
- "-n",
- "--no_bazel",
- help="Don't run using Bazel",
- default=True,
- action='store_false')
+ ap.add_argument("--visualize", help="Whether to visualize the results", default=False, action='store_true')
+ ap.add_argument("-n", "--no_bazel", help="Don't run using Bazel", default=True, action='store_false')
+ args = vars(ap.parse_args())
- args = ap.parse_args()
+ VISUALIZE_KEYPOINTS = args["visualize"]
+ if args["visualize"]:
+ glog.info("Visualizing results")
- if args.visualize:
- print("Visualizing results")
- VISUALIZE_KEYPOINTS = True
-
- if not args.no_bazel:
- print("Running on command line (no Bazel)")
- USE_BAZEL = False
+ USE_BAZEL = args["no_bazel"]
+ if args["no_bazel"]:
+ glog.info("Running on command line (no Bazel)")
compute_target_definition()
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index e5a7f5b..93c607c 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -1,4 +1,5 @@
import cv2
+import glog
import math
import numpy as np
import time
@@ -8,6 +9,7 @@
FEATURE_EXTRACTOR_NAME = 'SIFT'
QUERY_INDEX = 0 # We use a list for both training and query info, but only ever have one query item
+glog.setLevel("WARN")
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
@@ -38,7 +40,7 @@
# Load image (in color; let opencv convert to B&W for features)
img_data = cv2.imread(im)
if img_data is None:
- print("Failed to load image: ", im)
+ glog.error("Failed to load image: ", im)
exit()
else:
image_list.append(img_data)
@@ -143,15 +145,12 @@
elif FEATURE_EXTRACTOR_NAME is 'ORB':
matches = matcher.knnMatch(train_keypoint_lists[0], desc_query, k=2)
- print(matches)
good_matches = []
for m in matches:
if m:
if len(m) == 2:
- print(m[0].distance, m[1].distance)
if m[0].distance < 0.7 * m[1].distance:
good_matches.append(m[0])
- print(m[0].distance)
good_matches_list.append(good_matches)
@@ -176,14 +175,13 @@
for i in range(len(train_keypoint_lists)):
good_matches = good_matches_list[i]
if len(good_matches) < MIN_MATCH_COUNT:
- print("Not enough matches are for model ", i, ": ",
- len(good_matches), " out of needed #: ", MIN_MATCH_COUNT)
+ glog.warn("Not enough matches are for model %d: %d out of needed #: %d" % (i, len(good_matches), MIN_MATCH_COUNT))
homography_list.append([])
matches_mask_list.append([])
continue
- print("Got good number of matches for model %d: %d (needed only %d)" %
- (i, len(good_matches), MIN_MATCH_COUNT))
+ glog.info("Got good number of matches for model %d: %d (needed only %d)" %
+ (i, len(good_matches), MIN_MATCH_COUNT))
# Extract and bundle keypoint locations for computations
src_pts = np.float32([
train_keypoint_lists[i][m.trainIdx].pt for m in good_matches
@@ -206,7 +204,7 @@
# Also shows image with query unwarped (to match training image) and target pt
def show_results(training_images, train_keypoint_lists, query_images,
query_keypoint_lists, target_point_list, good_matches_list):
- print("Showing results for ", len(training_images), " training images")
+ glog.info("Showing results for ", len(training_images), " training images")
homography_list, matches_mask_list = compute_homographies(
train_keypoint_lists, query_keypoint_lists, good_matches_list)
@@ -214,15 +212,15 @@
good_matches = good_matches_list[i]
if len(good_matches) < MIN_MATCH_COUNT:
continue
- print("Showing results for model ", i)
+ glog.debug("Showing results for model ", i)
matches_mask_count = matches_mask_list[i].count(1)
if matches_mask_count != len(good_matches):
- print("Homography rejected some matches! From ",
+ glog.info("Homography rejected some matches! From ",
len(good_matches), ", only ", matches_mask_count,
" were used")
if matches_mask_count < MIN_MATCH_COUNT:
- print(
+ glog.info(
"Skipping match because homography rejected matches down to below ",
MIN_MATCH_COUNT)
continue
@@ -235,7 +233,7 @@
query_box_pts = cv2.perspectiveTransform(train_box_pts, H)
# Figure out where the training target goes on the query img
- transformed_target = cv2.perspectiveTransform(target_point_list[i], H)
+ transformed_target = cv2.perspectiveTransform(target_point_list[i].reshape(-1,1,2), H)
# Ballpark the size of the circle so it looks right on image
radius = int(
32 * abs(H[0][0] + H[1][1]) / 2) # Average of scale factors
@@ -260,14 +258,13 @@
img3 = cv2.drawMatches(query_image, query_keypoint_lists[QUERY_INDEX],
training_images[i], train_keypoint_lists[i],
good_matches_list[i], None, **draw_params)
- print("Drawing matches for model ", i,
- ". Query on left, Training image on right")
+ glog.debug("Drawing matches for model ", i,
+ ". Query on left, Training image on right")
cv2.imshow('Matches', img3), cv2.waitKey()
# Next, unwarp the query image so it looks like the training view
H_inv = np.linalg.inv(H)
query_image_warp = cv2.warpPerspective(query_image, H_inv, (w, h))
- print("Showing unwarped query image for model ", i)
cv2.imshow('Unwarped Image', query_image_warp), cv2.waitKey()
# Go ahead and return these, for use elsewhere