Fixing the data sent as camera_to_target and adding field_to_camera
I am pretty sure camera_to_target is the inverse of what gets returned by solvePnPRansac.
Adding field_to_camera to make it more convenient for Alex to show robot pose estimate
Change-Id: I98d1ef64f8b142325e6f94b213885ae6868333ca
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 23986bf..67613a1 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -70,6 +70,7 @@
const cv::Mat &descriptors,
const std::vector<std::vector<cv::DMatch>> &matches,
const std::vector<cv::Mat> &camera_target_list,
+ const std::vector<cv::Mat> &field_camera_list,
aos::Sender<sift::ImageMatchResult> *result_sender,
bool send_details);
@@ -170,6 +171,7 @@
const cv::Mat &descriptors,
const std::vector<std::vector<cv::DMatch>> &matches,
const std::vector<cv::Mat> &camera_target_list,
+ const std::vector<cv::Mat> &field_camera_list,
aos::Sender<sift::ImageMatchResult> *result_sender, bool send_details) {
auto builder = result_sender->MakeBuilder();
const auto camera_calibration_offset =
@@ -187,6 +189,7 @@
std::vector<flatbuffers::Offset<sift::CameraPose>> camera_poses;
+ CHECK_EQ(camera_target_list.size(), field_camera_list.size());
for (size_t i = 0; i < camera_target_list.size(); ++i) {
cv::Mat camera_target = camera_target_list[i];
CHECK(camera_target.isContinuous());
@@ -194,12 +197,21 @@
reinterpret_cast<float *>(camera_target.data), camera_target.total());
const flatbuffers::Offset<sift::TransformationMatrix> transform_offset =
sift::CreateTransformationMatrix(*builder.fbb(), data_offset);
+
+ cv::Mat field_camera = field_camera_list[i];
+ CHECK(field_camera.isContinuous());
+ const auto fc_data_offset = builder.fbb()->CreateVector<float>(
+ reinterpret_cast<float *>(field_camera.data), field_camera.total());
+ const flatbuffers::Offset<sift::TransformationMatrix> fc_transform_offset =
+ sift::CreateTransformationMatrix(*builder.fbb(), fc_data_offset);
+
const flatbuffers::Offset<sift::TransformationMatrix>
field_to_target_offset =
aos::CopyFlatBuffer(FieldToTarget(i), builder.fbb());
sift::CameraPose::Builder pose_builder(*builder.fbb());
pose_builder.add_camera_to_target(transform_offset);
+ pose_builder.add_field_to_camera(fc_transform_offset);
pose_builder.add_field_to_target(field_to_target_offset);
camera_poses.emplace_back(pose_builder.Finish());
}
@@ -278,6 +290,7 @@
// The minimum number of matches in a training image for us to use it.
static constexpr int kMinimumMatchCount = 10;
std::vector<cv::Mat> camera_target_list;
+ std::vector<cv::Mat> field_camera_list;
for (size_t i = 0; i < per_image_matches.size(); ++i) {
const PerImageMatches &per_image = per_image_matches[i];
@@ -285,15 +298,49 @@
continue;
}
+ // Transformations (rotations and translations) for various
+ // coordinate frames. R_XXXX_vec is the Rodrigues (angle-axis)
+ // representation the 3x3 rotation R_XXXX
+ // Tranform from camera to target frame
cv::Mat R_camera_target_vec, R_camera_target, T_camera_target;
+ // Tranform from camera to field origin (global) reference frame
+ cv::Mat R_camera_field_vec, R_camera_field, T_camera_field;
+ // Inverse of camera to field-- defines location of camera in
+ // global (field) reference frame
+ cv::Mat R_field_camera_vec, R_field_camera, T_field_camera;
+
// Compute the pose of the camera (global origin relative to camera)
cv::solvePnPRansac(per_image.training_points_3d, per_image.query_points,
CameraIntrinsics(), CameraDistCoeffs(),
- R_camera_target_vec, T_camera_target);
- T_camera_target = T_camera_target.t();
- // Convert Camera from angle-axis (3x1) to homogenous (3x3) representation
- cv::Rodrigues(R_camera_target_vec, R_camera_target);
+ R_camera_field_vec, T_camera_field);
+ // Get matrix version of R_camera_field
+ cv::Rodrigues(R_camera_field_vec, R_camera_field);
+
+ // Compute H_field_camera = H_camera_field^-1
+ R_field_camera = R_camera_field.t();
+ T_field_camera = -R_field_camera.dot(T_camera_field);
+ T_field_camera = T_field_camera.t();
+
+ // Extract the field_target transformation
+ const cv::Mat H_field_target(4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ FieldToTarget(i)->data())));
+ const cv::Mat R_field_target =
+ H_field_target(cv::Range(0, 3), cv::Range(0, 3));
+ const cv::Mat T_field_target =
+ H_field_target(cv::Range(3, 4), cv::Range(0, 3));
+
+ // Use it to get the relative pose from camera to target
+ R_camera_target = R_camera_field.dot(R_field_target);
+ T_camera_target = R_camera_field.dot(T_field_target) + T_camera_field;
+
+ // Convert Camera from angle-axis (3x1) to homogenous (3x3) representation
+ // and turn T into 3x1 vector
+ cv::Rodrigues(R_camera_target_vec, R_camera_target);
+ T_camera_target = T_camera_target.t();
+
+ // Set H_camera_target
{
CHECK_EQ(cv::Size(3, 3), R_camera_target.size());
CHECK_EQ(cv::Size(3, 1), T_camera_target.size());
@@ -302,15 +349,29 @@
T_camera_target.copyTo(camera_target(cv::Range(3, 4), cv::Range(0, 3)));
camera_target.at<float>(3, 3) = 1;
CHECK(camera_target.isContinuous());
- camera_target_list.push_back(camera_target);
+ camera_target_list.push_back(camera_target.clone());
+ }
+
+ // Set H_field_camera
+ {
+ CHECK_EQ(cv::Size(3, 3), R_field_camera.size());
+ CHECK_EQ(cv::Size(3, 1), T_field_camera.size());
+ cv::Mat field_camera = cv::Mat::zeros(4, 4, CV_32F);
+ R_field_camera.copyTo(field_camera(cv::Range(0, 3), cv::Range(0, 3)));
+ T_field_camera.copyTo(field_camera(cv::Range(3, 4), cv::Range(0, 3)));
+ field_camera.at<float>(3, 3) = 1;
+ CHECK(field_camera.isContinuous());
+ field_camera_list.push_back(field_camera.clone());
}
}
// Now, send our two messages-- one large, with details for remote
// debugging(features), and one smaller
SendImageMatchResult(image, keypoints, descriptors, matches,
- camera_target_list, &detailed_result_sender_, true);
+ camera_target_list, field_camera_list,
+ &detailed_result_sender_, true);
SendImageMatchResult(image, keypoints, descriptors, matches,
- camera_target_list, &result_sender_, false);
+ camera_target_list, field_camera_list, &result_sender_,
+ false);
}
void CameraReader::ReadImage() {
diff --git a/y2020/vision/sift/sift.fbs b/y2020/vision/sift/sift.fbs
index 8806957..43ea152 100644
--- a/y2020/vision/sift/sift.fbs
+++ b/y2020/vision/sift/sift.fbs
@@ -105,16 +105,21 @@
// Contains the information the EKF wants from an image matched against a single
// training image.
//
-// This is represented as a transformation to a target in field coordinates.
+// This is represented as a transformation from the camera to the target
+// (camera_to_target) and a transformatoin from the field to the target
+// (field_to_target).
+//
+// We also send the map from the field to the camera, which can be computed
+// with the first two, to make it easier to display.
table CameraPose {
- // Transformation matrix from the target to the camera's origin.
+ // Transformation matrix from the camera to the target.
// (0, 0, 0) is the aperture of the camera (we pretend it's an ideal pinhole
// camera). Positive Z is out of the camera. Positive X and Y are right
// handed, but which way they face depends on the camera extrinsics.
camera_to_target:TransformationMatrix;
// Field coordinates of the target, represented as a transformation matrix
- // from the target to the field.
+ // from the field to the target.
// (0, 0, 0) is the center of the field, on the level of most of the field
// (not the region under the truss). Positive X is towards the red alliance's
// PLAYER STATION. Positive Z is up. The coordinate system is right-handed.
@@ -125,6 +130,9 @@
// The value here will be selected from a small, static set of targets we
// train images on.
field_to_target:TransformationMatrix;
+
+ // The pose of the camera in the field coordinate frame
+ field_to_camera:TransformationMatrix;
}
table ImageMatchResult {