Move target mapping to 2023
Now that we have the camera calibration code in 2023, it makes more
sense to just move it here. Also added json with the target poses from
the field diagrams (already was in frc971/vision for a test).
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I305d235df0b62f41fbe0db94e134fe1309e3c9c4
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index ceec638..3d49755 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -299,29 +299,6 @@
)
cc_binary(
- name = "target_mapping",
- srcs = [
- "target_mapping.cc",
- ],
- data = [
- "//y2022:aos_config",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2022:__subpackages__"],
- deps = [
- ":camera_reader_lib",
- "//aos:init",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_reader",
- "//frc971/control_loops:pose",
- "//frc971/vision:charuco_lib",
- "//frc971/vision:target_mapper",
- "//third_party:opencv",
- "//y2022/control_loops/superstructure:superstructure_status_fbs",
- ],
-)
-
-cc_binary(
name = "calibrate_extrinsics",
srcs = [
"calibrate_extrinsics.cc",
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index e51ed0b..56f4eba 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -70,3 +70,26 @@
"//third_party:opencv",
],
)
+
+cc_binary(
+ name = "target_mapping",
+ srcs = [
+ "target_mapping.cc",
+ ],
+ data = [
+ "//y2023:aos_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":calibration_data",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/control_loops:pose",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2023/vision/target_map.json b/y2023/vision/target_map.json
new file mode 100644
index 0000000..5f256fa
--- /dev/null
+++ b/y2023/vision/target_map.json
@@ -0,0 +1,76 @@
+{
+ "target_poses": [
+ {
+ "id": 1,
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 3.141592653589793
+ },
+ {
+ "id": 2,
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 3.141592653589793
+ },
+ {
+ "id": 3,
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 3.141592653589793
+ },
+ {
+ "id": 4,
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 3.141592653589793
+ },
+ {
+ "id": 5,
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0
+ },
+ {
+ "id": 6,
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0
+ },
+ {
+ "id": 7,
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0
+ },
+ {
+ "id": 8,
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463,
+ "roll": 0.0,
+ "pitch": 0.0,
+ "yaw": 0.0
+ }
+ ]
+}
diff --git a/y2022/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
similarity index 82%
rename from y2022/vision/target_mapping.cc
rename to y2023/vision/target_mapping.cc
index 529e330..d4c8c50 100644
--- a/y2022/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -2,6 +2,7 @@
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/pose.h"
+#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_mapper.h"
#include "opencv2/aruco.hpp"
@@ -11,7 +12,7 @@
#include "opencv2/highgui.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
-#include "y2022/vision/camera_reader.h"
+#include "y2023/vision/calibration_data.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
@@ -20,11 +21,14 @@
DECLARE_string(image_channel);
-namespace y2022 {
+namespace y2023 {
namespace vision {
+using frc971::vision::CharucoExtractor;
using frc971::vision::DataAdapter;
+using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
using frc971::vision::TargetMapper;
+namespace calibration = frc971::vision::calibration;
// Change reference frame from camera to robot
Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camrob_target,
@@ -74,12 +78,39 @@
}
}
+const calibration::CameraCalibration *FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name) {
+ for (const calibration::CameraCalibration *candidate :
+ *calibration_data->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != FLAGS_team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << FLAGS_team_number;
+}
+
Eigen::Affine3d CameraExtrinsics(
const calibration::CameraCalibration *camera_calibration) {
- cv::Mat extrinsics = CameraReader::CameraExtrinsics(camera_calibration);
- Eigen::Matrix4d extrinsics_eigen;
- cv::cv2eigen(extrinsics, extrinsics_eigen);
- return Eigen::Affine3d(extrinsics_eigen);
+ const frc971::vision::calibration::TransformationMatrix *transform =
+ camera_calibration->has_turret_extrinsics()
+ ? camera_calibration->turret_extrinsics()
+ : camera_calibration->fixed_extrinsics();
+
+ cv::Mat result(
+ 4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(transform->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), transform->data()->size());
+
+ Eigen::Matrix4d result_eigen;
+ cv::cv2eigen(result, result_eigen);
+ return Eigen::Affine3d(result_eigen);
}
// Get images from pi and pass apriltag positions to HandleAprilTag()
@@ -92,10 +123,8 @@
std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration =
- CameraReader::FindCameraCalibration(&calibration_data.message(),
- "pi" + std::to_string(pi_number),
- FLAGS_team_number);
+ const calibration::CameraCalibration *calibration = FindCameraCalibration(
+ &calibration_data.message(), "pi" + std::to_string(pi_number));
const auto extrinsics = CameraExtrinsics(calibration);
// TODO(milind): change to /camera once we log at full frequency
@@ -104,7 +133,7 @@
pi_event_loop,
"pi-" + std::to_string(FLAGS_team_number) + "-" +
std::to_string(pi_number),
- TargetType::kAprilTag, kImageChannel,
+ frc971::vision::TargetType::kAprilTag, kImageChannel,
[=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
std::vector<cv::Vec4i> april_ids,
std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
@@ -195,9 +224,9 @@
}
} // namespace vision
-} // namespace y2022
+} // namespace y2023
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- y2022::vision::MappingMain(argc, argv);
+ y2023::vision::MappingMain(argc, argv);
}