commit 6f3640a90151904d19b394feb767e6dee1fce802
author Austin Schuh <austin.linux@gmail.com> Fri Feb 28 22:13:36 2020 -0800
committer Austin Schuh <austin.linux@gmail.com> Fri Feb 28 22:14:26 2020 -0800
tree d7c4f6a2156930812dabb6c81a00e88d4f2a738f
parent 6f826539d56bad50deaefb45986b5e202dd43404

Fix nested flatbuffer building and transpose

This makes y2020 vision run now and predict field location

Change-Id: I4303fff6f2a80b058d2330642f1a4c4f68753c68

y2020/vision/camera_reader.cc

diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index ed4082d..23986bf 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -177,12 +177,14 @@
 
   flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<sift::Feature>>>
       features_offset;
+  flatbuffers::Offset<
+      flatbuffers::Vector<flatbuffers::Offset<sift::ImageMatch>>>
+      image_matches_offset;
   if (send_details) {
     features_offset = PackFeatures(builder.fbb(), keypoints, descriptors);
+    image_matches_offset = PackImageMatches(builder.fbb(), matches);
   }
 
-  const auto image_matches_offset = PackImageMatches(builder.fbb(), matches);
-
   std::vector<flatbuffers::Offset<sift::CameraPose>> camera_poses;
 
   for (size_t i = 0; i < camera_target_list.size(); ++i) {
@@ -190,21 +192,23 @@
     CHECK(camera_target.isContinuous());
     const auto data_offset = builder.fbb()->CreateVector<float>(
         reinterpret_cast<float *>(camera_target.data), camera_target.total());
-    auto transform_offset =
+    const flatbuffers::Offset<sift::TransformationMatrix> transform_offset =
         sift::CreateTransformationMatrix(*builder.fbb(), 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_target(
-        aos::CopyFlatBuffer(FieldToTarget(i), builder.fbb()));
+    pose_builder.add_field_to_target(field_to_target_offset);
     camera_poses.emplace_back(pose_builder.Finish());
   }
   const auto camera_poses_offset = builder.fbb()->CreateVector(camera_poses);
 
   sift::ImageMatchResult::Builder result_builder(*builder.fbb());
-  result_builder.add_image_matches(image_matches_offset);
   result_builder.add_camera_poses(camera_poses_offset);
   if (send_details) {
+    result_builder.add_image_matches(image_matches_offset);
     result_builder.add_features(features_offset);
   }
   result_builder.add_image_monotonic_timestamp_ns(
@@ -286,6 +290,7 @@
     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);