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