Adding visualization tools for target_mapping and logging playback
Helps to see where the targets are being seen, and the respective localizations
Added ability to draw robot frame on visualizer
Change-Id: I8af7a6d84874fe626d8dc9452f77702741e72fbb
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index f3ebecb..133281b 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -223,6 +223,7 @@
hdrs = [
"visualize_robot.h",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:init",
"//third_party:opencv",
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index dd332fc..d0dcdc8 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -232,6 +232,15 @@
return std::nullopt;
}
+std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
+ TargetId target_id) {
+ if (target_poses_.count(target_id) > 0) {
+ return TargetMapper::TargetPose{target_id, target_poses_[target_id]};
+ }
+
+ return std::nullopt;
+}
+
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
@@ -290,6 +299,9 @@
// better to properly constrain the gauge freedom. This can be done by
// setting one of the poses as constant so the optimizer cannot change it.
ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
+ // TODO<Jim>: This fixes first target, but breaks optimizer if we don't have
+ // an observation on the first target. We may want to allow other targets as
+ // fixed
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
@@ -302,6 +314,7 @@
ceres::Solver::Options options;
options.max_num_iterations = FLAGS_max_num_iterations;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+ options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 3724832..35c0977 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -46,6 +46,9 @@
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
+ // Version that gets based on internal target_poses
+ std::optional<TargetPose> GetTargetPoseById(TargetId target_id);
+
ceres::examples::MapOfPoses target_poses() { return target_poses_; }
private:
diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index 0bbe507..eb2da9a 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -9,6 +9,24 @@
namespace frc971 {
namespace vision {
+void VisualizeRobot::SetDefaultViewpoint(int image_width, double focal_length) {
+ // 10 meters above the origin, rotated so the camera faces straight down
+ Eigen::Translation3d camera_trans(0, 0, 10.0);
+ Eigen::AngleAxisd camera_rot(M_PI, Eigen::Vector3d::UnitX());
+ Eigen::Affine3d camera_viewpoint = camera_trans * camera_rot;
+ SetViewpoint(camera_viewpoint);
+
+ cv::Mat camera_mat;
+ double half_width = static_cast<double>(image_width) / 2.0;
+ double intr[] = {focal_length, 0.0, half_width, 0.0, focal_length,
+ half_width, 0.0, 0.0, 1.0};
+ camera_mat = cv::Mat(3, 3, CV_64FC1, intr);
+ SetCameraParameters(camera_mat);
+
+ cv::Mat dist_coeffs = cv::Mat(1, 5, CV_64F, 0.0);
+ SetDistortionCoefficients(dist_coeffs);
+}
+
cv::Point VisualizeRobot::ProjectPoint(Eigen::Vector3d T_world_point) {
// Map 3D point in world coordinates to camera frame
Eigen::Vector3d T_camera_point = H_world_viewpoint_.inverse() * T_world_point;
@@ -28,7 +46,7 @@
cv::Point start2d = ProjectPoint(start3d);
cv::Point end2d = ProjectPoint(end3d);
- cv::line(image_, start2d, end2d, cv::Scalar(0, 0, 255));
+ cv::line(image_, start2d, end2d, cv::Scalar(0, 200, 0));
}
void VisualizeRobot::DrawFrameAxes(Eigen::Affine3d H_world_target,
@@ -53,21 +71,37 @@
// Grab x axis direction
cv::Vec3d label_offset = r_mat.col(0);
- // Find 3D coordinate of point at the end of the x-axis, and project it
+ // Find 3D coordinate of point at the end of the x-axis, and put label there
+ // Bump it just a few (5) pixels to the right, to make it read easier
cv::Mat label_coord_res =
camera_mat_ * cv::Mat(tvec + label_offset * axis_scale);
cv::Vec3d label_coord = label_coord_res.col(0);
- label_coord[0] = label_coord[0] / label_coord[2];
+ label_coord[0] = label_coord[0] / label_coord[2] + 5;
label_coord[1] = label_coord[1] / label_coord[2];
cv::putText(image_, label, cv::Point(label_coord[0], label_coord[1]),
cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
}
}
-void VisualizeRobot::DrawBoardOutline(Eigen::Affine3d H_world_board,
+void VisualizeRobot::DrawRobotOutline(Eigen::Affine3d H_world_robot,
std::string label) {
- LOG(INFO) << "Not yet implemented; drawing axes only";
- DrawFrameAxes(H_world_board, label);
+ DrawFrameAxes(H_world_robot, label);
+ const double kBotHalfWidth = 0.75 / 2.0;
+ const double kBotHalfLength = 1.0 / 2.0;
+ // Compute coordinates for front/rear and right/left corners
+ Eigen::Vector3d fr_corner =
+ H_world_robot * Eigen::Vector3d(kBotHalfLength, kBotHalfWidth, 0);
+ Eigen::Vector3d fl_corner =
+ H_world_robot * Eigen::Vector3d(kBotHalfLength, -kBotHalfWidth, 0);
+ Eigen::Vector3d rl_corner =
+ H_world_robot * Eigen::Vector3d(-kBotHalfLength, -kBotHalfWidth, 0);
+ Eigen::Vector3d rr_corner =
+ H_world_robot * Eigen::Vector3d(-kBotHalfLength, kBotHalfWidth, 0);
+
+ DrawLine(fr_corner, fl_corner);
+ DrawLine(fl_corner, rl_corner);
+ DrawLine(rl_corner, rr_corner);
+ DrawLine(rr_corner, fr_corner);
}
} // namespace vision
diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index 391a030..f5e75af 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -31,14 +31,20 @@
// Set camera parameters (for projection into a virtual view)
void SetCameraParameters(cv::Mat camera_intrinsics) {
- camera_mat_ = camera_intrinsics;
+ camera_mat_ = camera_intrinsics.clone();
}
// Set distortion coefficients (defaults to 0)
void SetDistortionCoefficients(cv::Mat dist_coeffs) {
- dist_coeffs_ = dist_coeffs;
+ dist_coeffs_ = dist_coeffs.clone();
}
+ // Sets up a default camera view 10 m above the origin, pointing down
+ // Uses image_width and focal_length to define a default camera projection
+ // matrix
+ void SetDefaultViewpoint(int image_width = 1000,
+ double focal_length = 1000.0);
+
// Helper function to project a point in 3D to the virtual image coordinates
cv::Point ProjectPoint(Eigen::Vector3d point3d);
@@ -50,8 +56,9 @@
void DrawFrameAxes(Eigen::Affine3d H_world_target, std::string label = "",
double axis_scale = 0.25);
- // TODO<Jim>: Need to implement this, and maybe DrawRobotOutline
- void DrawBoardOutline(Eigen::Affine3d H_world_board, std::string label = "");
+ // TODO<Jim>: Also implement DrawBoardOutline? Maybe one function w/
+ // parameters?
+ void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "");
Eigen::Affine3d H_world_viewpoint_; // Where to view the world from
cv::Mat image_; // Image to draw on
diff --git a/frc971/vision/visualize_robot_sample.cc b/frc971/vision/visualize_robot_sample.cc
index dc38352..59acba0 100644
--- a/frc971/vision/visualize_robot_sample.cc
+++ b/frc971/vision/visualize_robot_sample.cc
@@ -26,25 +26,8 @@
cv::Mat image_mat =
cv::Mat::zeros(cv::Size(image_width, image_width), CV_8UC3);
vis_robot.SetImage(image_mat);
-
- // 10 meters above the origin, rotated so the camera faces straight down
- Eigen::Translation3d camera_trans(0, 0, 10.0);
- Eigen::AngleAxisd camera_rot(M_PI, Eigen::Vector3d::UnitX());
- Eigen::Affine3d camera_viewpoint = camera_trans * camera_rot;
- vis_robot.SetViewpoint(camera_viewpoint);
-
- cv::Mat camera_mat;
double focal_length = 1000.0;
- double intr[] = {focal_length, 0.0, image_width / 2.0,
- 0.0, focal_length, image_width / 2.0,
- 0.0, 0.0, 1.0};
- camera_mat = cv::Mat(3, 3, CV_64FC1, intr);
- vis_robot.SetCameraParameters(camera_mat);
-
- Eigen::Affine3d offset_rotate_origin(Eigen::Affine3d::Identity());
-
- cv::Mat dist_coeffs = cv::Mat(1, 5, CV_64F, 0.0);
- vis_robot.SetDistortionCoefficients(dist_coeffs);
+ vis_robot.SetDefaultViewpoint(image_width, focal_length);
// Go around the clock and plot the coordinate frame at different rotations
for (int i = 0; i < 12; i++) {
@@ -52,8 +35,9 @@
Eigen::Vector3d trans;
trans << 1.0 * cos(angle), 1.0 * sin(angle), 0.0;
- offset_rotate_origin = Eigen::Translation3d(trans) *
- Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
+ Eigen::Affine3d offset_rotate_origin =
+ Eigen::Translation3d(trans) *
+ Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX());
vis_robot.DrawFrameAxes(offset_rotate_origin, std::to_string(i));
}