Add solver for april tag mapping
Takes in robot locations (at control loop frequency), and less frequent
localizations off of april tags. Builds a graph of constraints between
subsequent target detections and solves for the optimal target locations
on the field.
Change-Id: I1256f5914b7656d9e14735f9d994572688c3aaa8
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index b2b815a..7872926 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -89,6 +89,37 @@
)
cc_library(
+ name = "target_mapper",
+ srcs = ["target_mapper.cc"],
+ hdrs = ["target_mapper.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":geometry_lib",
+ "//aos/events:simulated_event_loop",
+ "//frc971/control_loops:control_loop",
+ "//frc971/vision/ceres:pose_graph_2d_lib",
+ "//third_party:opencv",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "target_mapper_test",
+ srcs = [
+ "target_mapper_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":target_mapper",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ ],
+)
+
+cc_library(
name = "geometry_lib",
hdrs = [
"geometry.h",
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
new file mode 100644
index 0000000..5622b55
--- /dev/null
+++ b/frc971/vision/ceres/BUILD
@@ -0,0 +1,24 @@
+# Copy of ceres example code from their repo
+cc_library(
+ name = "pose_graph_2d_lib",
+ hdrs = [
+ "angle_local_parameterization.h",
+ "normalize_angle.h",
+ "pose_graph_2d_error_term.h",
+ "read_g2o.h",
+ "types.h",
+ ],
+ copts = [
+ # Needed to silence GFlags complaints.
+ "-Wno-sign-compare",
+ "-Wno-unused-parameter",
+ "-Wno-format-nonliteral",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_gflags_gflags//:gflags",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/frc971/vision/ceres/angle_local_parameterization.h b/frc971/vision/ceres/angle_local_parameterization.h
new file mode 100644
index 0000000..a81637c
--- /dev/null
+++ b/frc971/vision/ceres/angle_local_parameterization.h
@@ -0,0 +1,64 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+
+#include "ceres/local_parameterization.h"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// Defines a local parameterization for updating the angle to be constrained in
+// [-pi to pi).
+class AngleLocalParameterization {
+ public:
+ template <typename T>
+ bool operator()(const T* theta_radians,
+ const T* delta_theta_radians,
+ T* theta_radians_plus_delta) const {
+ *theta_radians_plus_delta =
+ NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+ return true;
+ }
+
+ static ceres::LocalParameterization* Create() {
+ return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+ 1,
+ 1>);
+ }
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/frc971/vision/ceres/normalize_angle.h b/frc971/vision/ceres/normalize_angle.h
new file mode 100644
index 0000000..c215671
--- /dev/null
+++ b/frc971/vision/ceres/normalize_angle.h
@@ -0,0 +1,53 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+
+#include <cmath>
+
+#include "ceres/ceres.h"
+
+namespace ceres {
+namespace examples {
+
+// Normalizes the angle in radians between [-pi and pi).
+template <typename T>
+inline T NormalizeAngle(const T& angle_radians) {
+ // Use ceres::floor because it is specialized for double and Jet types.
+ T two_pi(2.0 * M_PI);
+ return angle_radians -
+ two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/frc971/vision/ceres/pose_graph_2d_error_term.h b/frc971/vision/ceres/pose_graph_2d_error_term.h
new file mode 100644
index 0000000..2df31f6
--- /dev/null
+++ b/frc971/vision/ceres/pose_graph_2d_error_term.h
@@ -0,0 +1,119 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Cost function for a 2D pose graph formulation.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+namespace examples {
+
+template <typename T>
+Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
+ const T cos_yaw = ceres::cos(yaw_radians);
+ const T sin_yaw = ceres::sin(yaw_radians);
+
+ Eigen::Matrix<T, 2, 2> rotation;
+ rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
+ return rotation;
+}
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement.
+//
+// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
+// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
+//
+// where r_a is the rotation matrix that rotates a vector represented in frame A
+// into the global frame, and Normalize(*) ensures the angles are in the range
+// [-pi, pi).
+class PoseGraph2dErrorTerm {
+ public:
+ PoseGraph2dErrorTerm(double x_ab,
+ double y_ab,
+ double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information)
+ : p_ab_(x_ab, y_ab),
+ yaw_ab_radians_(yaw_ab_radians),
+ sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const x_a,
+ const T* const y_a,
+ const T* const yaw_a,
+ const T* const x_b,
+ const T* const y_b,
+ const T* const yaw_b,
+ T* residuals_ptr) const {
+ const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+ const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
+
+ residuals_map.template head<2>() =
+ RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
+ residuals_map(2) = ceres::examples::NormalizeAngle(
+ (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+ // Scale the residuals by the square root information matrix to account for
+ // the measurement uncertainty.
+ residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(double x_ab,
+ double y_ab,
+ double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information) {
+ return (new ceres::
+ AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
+ new PoseGraph2dErrorTerm(
+ x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The position of B relative to A in the A frame.
+ const Eigen::Vector2d p_ab_;
+ // The orientation of frame B relative to frame A.
+ const double yaw_ab_radians_;
+ // The inverse square root of the measurement covariance matrix.
+ const Eigen::Matrix3d sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/read_g2o.h b/frc971/vision/ceres/read_g2o.h
new file mode 100644
index 0000000..fea32e9
--- /dev/null
+++ b/frc971/vision/ceres/read_g2o.h
@@ -0,0 +1,143 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Reads a file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+ std::map<int, Pose, std::less<int>, Allocator>* poses) {
+ int id;
+ Pose pose;
+ *infile >> id >> pose;
+
+ // Ensure we don't have duplicate poses.
+ if (poses->find(id) != poses->end()) {
+ LOG(ERROR) << "Duplicate vertex with ID: " << id;
+ return false;
+ }
+ (*poses)[id] = pose;
+
+ return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+ std::vector<Constraint, Allocator>* constraints) {
+ Constraint constraint;
+ *infile >> constraint;
+
+ constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, a vertex is defined as follows:
+//
+// VERTEX_SE2 ID x_meters y_meters yaw_radians
+//
+// A constraint is defined as follows:
+//
+// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose,
+ typename Constraint,
+ typename MapAllocator,
+ typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+ std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+ std::vector<Constraint, VectorAllocator>* constraints) {
+ CHECK(poses != NULL);
+ CHECK(constraints != NULL);
+
+ poses->clear();
+ constraints->clear();
+
+ std::ifstream infile(filename.c_str());
+ if (!infile) {
+ return false;
+ }
+
+ std::string data_type;
+ while (infile.good()) {
+ // Read whether the type is a node or a constraint.
+ infile >> data_type;
+ if (data_type == Pose::name()) {
+ if (!ReadVertex(&infile, poses)) {
+ return false;
+ }
+ } else if (data_type == Constraint::name()) {
+ ReadConstraint(&infile, constraints);
+ } else {
+ LOG(ERROR) << "Unknown data type: " << data_type;
+ return false;
+ }
+
+ // Clear any trailing whitespace from the line.
+ infile >> std::ws;
+ }
+
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_READ_G2O_H_
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
new file mode 100644
index 0000000..3c13824
--- /dev/null
+++ b/frc971/vision/ceres/types.h
@@ -0,0 +1,101 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
+// the graph has a unique integer ID with a position and orientation. There are
+// delta transformation constraints between two vertices.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+
+#include <fstream>
+
+#include "Eigen/Core"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() { return "VERTEX_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
+ input >> pose.x >> pose.y >> pose.yaw_radians;
+ // Normalize the angle between -pi to pi.
+ pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+ return input;
+}
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint2d {
+ int id_begin;
+ int id_end;
+
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The inverse of the covariance matrix for the measurement. The order of the
+ // entries are x, y, and yaw.
+ Eigen::Matrix3d information;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() { return "EDGE_SE2"; }
+};
+
+inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+ input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+ constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
+ constraint.information(0, 1) >> constraint.information(0, 2) >>
+ constraint.information(1, 1) >> constraint.information(1, 2) >>
+ constraint.information(2, 2);
+
+ // Set the lower triangular part of the information matrix.
+ constraint.information(1, 0) = constraint.information(0, 1);
+ constraint.information(2, 0) = constraint.information(0, 2);
+ constraint.information(2, 1) = constraint.information(1, 2);
+
+ // Normalize the angle between -pi to pi.
+ constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+ return input;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
new file mode 100644
index 0000000..b806c03
--- /dev/null
+++ b/frc971/vision/target_mapper.cc
@@ -0,0 +1,324 @@
+#include "frc971/vision/target_mapper.h"
+
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/vision/ceres/angle_local_parameterization.h"
+#include "frc971/vision/ceres/normalize_angle.h"
+#include "frc971/vision/ceres/pose_graph_2d_error_term.h"
+#include "frc971/vision/geometry.h"
+
+DEFINE_uint64(max_num_iterations, 100,
+ "Maximum number of iterations for the ceres solver");
+
+namespace frc971::vision {
+
+Eigen::Affine3d PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d pose2d) {
+ Eigen::Affine3d H_world_pose =
+ Eigen::Translation3d(pose2d.x, pose2d.y, 0.0) *
+ Eigen::AngleAxisd(pose2d.yaw_radians, Eigen::Vector3d::UnitZ());
+ return H_world_pose;
+}
+
+ceres::examples::Pose2d PoseUtils::Affine3dToPose2d(Eigen::Affine3d H) {
+ Eigen::Vector3d T = H.translation();
+ double heading = std::atan2(H.rotation()(1, 0), H.rotation()(0, 0));
+ return ceres::examples::Pose2d{T[0], T[1],
+ ceres::examples::NormalizeAngle(heading)};
+}
+
+ceres::examples::Pose2d PoseUtils::ComputeRelativePose(
+ ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2) {
+ Eigen::Affine3d H_world_1 = Pose2dToAffine3d(pose_1);
+ Eigen::Affine3d H_world_2 = Pose2dToAffine3d(pose_2);
+
+ // Get the location of 2 in the 1 frame
+ Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
+ return Affine3dToPose2d(H_1_2);
+}
+
+ceres::examples::Pose2d PoseUtils::ComputeOffsetPose(
+ ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative) {
+ auto H_world_1 = Pose2dToAffine3d(pose_1);
+ auto H_1_2 = Pose2dToAffine3d(pose_2_relative);
+ auto H_world_2 = H_world_1 * H_1_2;
+
+ return Affine3dToPose2d(H_world_2);
+}
+
+namespace {
+double ExponentiatedSinTerm(double theta) {
+ return (theta == 0.0 ? 1.0 : std::sin(theta) / theta);
+}
+
+double ExponentiatedCosTerm(double theta) {
+ return (theta == 0.0 ? 0.0 : (1 - std::cos(theta)) / theta);
+}
+} // namespace
+
+ceres::examples::Pose2d DataAdapter::InterpolatePose(
+ const TimestampedPose &pose_start, const TimestampedPose &pose_end,
+ aos::distributed_clock::time_point time) {
+ auto delta_pose =
+ PoseUtils::ComputeRelativePose(pose_start.pose, pose_end.pose);
+ // Time from start of period, on the scale 0-1 where 1 is the end.
+ double interpolation_scalar =
+ static_cast<double>((time - pose_start.time).count()) /
+ static_cast<double>((pose_end.time - pose_start.time).count());
+
+ double theta = delta_pose.yaw_radians;
+ // Take the log of the transformation matrix:
+ // https://mathoverflow.net/questions/118533/how-to-compute-se2-group-exponential-and-logarithm
+ StdFormLine dx_line = {.a = ExponentiatedSinTerm(theta),
+ .b = -ExponentiatedCosTerm(theta),
+ .c = delta_pose.x};
+ StdFormLine dy_line = {.a = ExponentiatedCosTerm(theta),
+ .b = ExponentiatedSinTerm(theta),
+ .c = delta_pose.y};
+
+ std::optional<cv::Point2d> solution = dx_line.Intersection(dy_line);
+ CHECK(solution.has_value());
+
+ // Re-exponentiate with the new values scaled by the interpolation scalar to
+ // get an interpolated tranformation matrix
+ double a = solution->x * interpolation_scalar;
+ double b = solution->y * interpolation_scalar;
+ double alpha = theta * interpolation_scalar;
+
+ ceres::examples::Pose2d interpolated_pose = {
+ .x = a * ExponentiatedSinTerm(theta) - b * ExponentiatedCosTerm(theta),
+ .y = a * ExponentiatedCosTerm(theta) + b * ExponentiatedSinTerm(theta),
+ .yaw_radians = alpha};
+
+ return PoseUtils::ComputeOffsetPose(pose_start.pose, interpolated_pose);
+} // namespace frc971::vision
+
+std::pair<std::vector<ceres::examples::Constraint2d>,
+ std::vector<ceres::examples::Pose2d>>
+DataAdapter::MatchTargetDetections(
+ const std::vector<TimestampedPose> ×tamped_robot_poses,
+ const std::vector<TimestampedDetection> ×tamped_target_detections) {
+ // Interpolate robot poses
+ std::map<aos::distributed_clock::time_point, ceres::examples::Pose2d>
+ interpolated_poses;
+
+ CHECK_GT(timestamped_robot_poses.size(), 1ul)
+ << "Need more than 1 robot pose";
+ auto robot_pose_it = timestamped_robot_poses.begin();
+ for (const auto ×tamped_detection : timestamped_target_detections) {
+ aos::distributed_clock::time_point target_time = timestamped_detection.time;
+ // Find the robot point right before this localization
+ while (robot_pose_it->time > target_time ||
+ (robot_pose_it + 1)->time <= target_time) {
+ robot_pose_it++;
+ CHECK(robot_pose_it < timestamped_robot_poses.end() - 1)
+ << "Need a robot pose before and after every target detection";
+ }
+
+ auto start = robot_pose_it;
+ auto end = robot_pose_it + 1;
+ interpolated_poses.emplace(target_time,
+ InterpolatePose(*start, *end, target_time));
+ }
+
+ // Match consecutive detections
+ std::vector<ceres::examples::Constraint2d> target_constraints;
+ std::vector<ceres::examples::Pose2d> robot_delta_poses;
+
+ auto last_detection = timestamped_target_detections[0];
+ auto last_robot_pose =
+ interpolated_poses[timestamped_target_detections[0].time];
+
+ for (auto it = timestamped_target_detections.begin() + 1;
+ it < timestamped_target_detections.end(); it++) {
+ // Skip two consecutive detections of the same target, because the solver
+ // doesn't allow this
+ if (it->id == last_detection.id) {
+ continue;
+ }
+
+ auto robot_pose = interpolated_poses[it->time];
+ auto robot_delta_pose =
+ PoseUtils::ComputeRelativePose(last_robot_pose, robot_pose);
+ auto confidence = ComputeConfidence(last_detection.time, it->time);
+
+ target_constraints.emplace_back(ComputeTargetConstraint(
+ last_detection, PoseUtils::Pose2dToAffine3d(robot_delta_pose), *it,
+ confidence));
+ robot_delta_poses.emplace_back(robot_delta_pose);
+
+ last_detection = *it;
+ last_robot_pose = robot_pose;
+ }
+
+ return {target_constraints, robot_delta_poses};
+}
+
+Eigen::Matrix3d DataAdapter::ComputeConfidence(
+ aos::distributed_clock::time_point start,
+ aos::distributed_clock::time_point end) {
+ constexpr size_t kX = 0;
+ constexpr size_t kY = 1;
+ constexpr size_t kTheta = 2;
+
+ // Uncertainty matrix between start and end
+ Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
+
+ {
+ // Noise for odometry-based robot position measurements
+ Eigen::Matrix3d Q_odometry = Eigen::Matrix3d::Zero();
+ Q_odometry(kX, kX) = std::pow(0.045, 2);
+ Q_odometry(kY, kY) = std::pow(0.045, 2);
+ Q_odometry(kTheta, kTheta) = std::pow(0.01, 2);
+
+ // Add uncertainty for robot position measurements from start to end
+ int iterations = (end - start) / frc971::controls::kLoopFrequency;
+ P += static_cast<double>(iterations) * Q_odometry;
+ }
+
+ {
+ // Noise for vision-based target localizations
+ Eigen::Matrix3d Q_vision = Eigen::Matrix3d::Zero();
+ Q_vision(kX, kX) = std::pow(0.09, 2);
+ Q_vision(kY, kY) = std::pow(0.09, 2);
+ Q_vision(kTheta, kTheta) = std::pow(0.02, 2);
+
+ // Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
+ P += 2.0 * Q_vision;
+ }
+
+ return P.inverse();
+}
+
+ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
+ const TimestampedDetection &target_detection_start,
+ const Eigen::Affine3d &H_robotstart_robotend,
+ const TimestampedDetection &target_detection_end,
+ const Eigen::Matrix3d &confidence) {
+ // Compute the relative pose (constraint) between the two targets
+ Eigen::Affine3d H_targetstart_targetend =
+ target_detection_start.H_robot_target.inverse() * H_robotstart_robotend *
+ target_detection_end.H_robot_target;
+ ceres::examples::Pose2d target_constraint =
+ PoseUtils::Affine3dToPose2d(H_targetstart_targetend);
+
+ return ceres::examples::Constraint2d{
+ target_detection_start.id, target_detection_end.id,
+ target_constraint.x, target_constraint.y,
+ target_constraint.yaw_radians, confidence};
+}
+
+TargetMapper::TargetMapper(
+ std::map<TargetId, ceres::examples::Pose2d> target_poses,
+ std::vector<ceres::examples::Constraint2d> target_constraints)
+ : target_poses_(target_poses), target_constraints_(target_constraints) {}
+
+std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
+ std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
+ for (auto target_pose : target_poses) {
+ if (target_pose.id == target_id) {
+ return target_pose;
+ }
+ }
+
+ return std::nullopt;
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+void TargetMapper::OutputPoses(
+ const std::map<int, ceres::examples::Pose2d> &poses) {
+ for (const auto &pair : poses) {
+ std::cout << pair.first << ": " << pair.second.x << " " << pair.second.y
+ << ' ' << pair.second.yaw_radians << std::endl;
+ }
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+void TargetMapper::BuildOptimizationProblem(
+ std::map<int, ceres::examples::Pose2d> *poses,
+ const std::vector<ceres::examples::Constraint2d> &constraints,
+ ceres::Problem *problem) {
+ CHECK_NOTNULL(poses);
+ CHECK_NOTNULL(problem);
+ if (constraints.empty()) {
+ LOG(WARNING) << "No constraints, no problem to optimize.";
+ return;
+ }
+
+ ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
+ ceres::LocalParameterization *angle_local_parameterization =
+ ceres::examples::AngleLocalParameterization::Create();
+
+ for (std::vector<ceres::examples::Constraint2d>::const_iterator
+ constraints_iter = constraints.begin();
+ constraints_iter != constraints.end(); ++constraints_iter) {
+ const ceres::examples::Constraint2d &constraint = *constraints_iter;
+
+ std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter =
+ poses->find(constraint.id_begin);
+ CHECK(pose_begin_iter != poses->end())
+ << "Pose with ID: " << constraint.id_begin << " not found.";
+ std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter =
+ poses->find(constraint.id_end);
+ CHECK(pose_end_iter != poses->end())
+ << "Pose with ID: " << constraint.id_end << " not found.";
+
+ const Eigen::Matrix3d sqrt_information =
+ constraint.information.llt().matrixL();
+ // Ceres will take ownership of the pointer.
+ ceres::CostFunction *cost_function =
+ ceres::examples::PoseGraph2dErrorTerm::Create(
+ constraint.x, constraint.y, constraint.yaw_radians,
+ sqrt_information);
+ problem->AddResidualBlock(
+ cost_function, loss_function, &pose_begin_iter->second.x,
+ &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
+ &pose_end_iter->second.x, &pose_end_iter->second.y,
+ &pose_end_iter->second.yaw_radians);
+
+ problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
+ angle_local_parameterization);
+ problem->SetParameterization(&pose_end_iter->second.yaw_radians,
+ angle_local_parameterization);
+ }
+
+ // The pose graph optimization problem has three DOFs that are not fully
+ // constrained. This is typically referred to as gauge freedom. You can apply
+ // a rigid body transformation to all the nodes and the optimization problem
+ // will still have the exact same cost. The Levenberg-Marquardt algorithm has
+ // internal damping which mitigates this issue, but it is 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.
+ std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter =
+ poses->begin();
+ CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+ problem->SetParameterBlockConstant(&pose_start_iter->second.x);
+ problem->SetParameterBlockConstant(&pose_start_iter->second.y);
+ problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
+}
+
+// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
+ CHECK_NOTNULL(problem);
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = FLAGS_max_num_iterations;
+ options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, problem, &summary);
+
+ std::cout << summary.FullReport() << '\n';
+
+ return summary.IsSolutionUsable();
+}
+
+void TargetMapper::Solve() {
+ ceres::Problem problem;
+ BuildOptimizationProblem(&target_poses_, target_constraints_, &problem);
+
+ CHECK(SolveOptimizationProblem(&problem))
+ << "The solve was not successful, exiting.";
+
+ OutputPoses(target_poses_);
+}
+
+} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
new file mode 100644
index 0000000..55d3c7e
--- /dev/null
+++ b/frc971/vision/target_mapper.h
@@ -0,0 +1,134 @@
+#ifndef FRC971_VISION_TARGET_MAPPER_H_
+#define FRC971_VISION_TARGET_MAPPER_H_
+
+#include <unordered_map>
+
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/vision/ceres/types.h"
+
+namespace frc971::vision {
+
+// Estimates positions of vision targets (ex. April Tags) using
+// target detections relative to a robot (which were computed using robot
+// positions at the time of those detections). Solves SLAM problem to estimate
+// target locations using deltas between consecutive target detections.
+class TargetMapper {
+ public:
+ using TargetId = int;
+
+ struct TargetPose {
+ TargetId id;
+ ceres::examples::Pose2d pose;
+ };
+
+ // target_poses are initial guesses for the actual locations of the targets on
+ // the field.
+ // target_constraints are the deltas between consecutive target detections,
+ // and are usually prepared by the DataAdapter class below.
+ TargetMapper(std::map<TargetId, ceres::examples::Pose2d> target_poses,
+ std::vector<ceres::examples::Constraint2d> target_constraints);
+
+ void Solve();
+
+ static std::optional<TargetPose> GetTargetPoseById(
+ std::vector<TargetPose> target_poses, TargetId target_id);
+
+ std::map<TargetId, ceres::examples::Pose2d> target_poses() {
+ return target_poses_;
+ }
+
+ private:
+ // Output the poses to std::cout with format: ID: x y yaw_radians.
+ static void OutputPoses(const std::map<int, ceres::examples::Pose2d> &poses);
+
+ // Constructs the nonlinear least squares optimization problem from the
+ // pose graph constraints.
+ void BuildOptimizationProblem(
+ std::map<TargetId, ceres::examples::Pose2d> *target_poses,
+ const std::vector<ceres::examples::Constraint2d> &constraints,
+ ceres::Problem *problem);
+
+ // Returns true if the solve was successful.
+ bool SolveOptimizationProblem(ceres::Problem *problem);
+
+ std::map<TargetId, ceres::examples::Pose2d> target_poses_;
+ std::vector<ceres::examples::Constraint2d> target_constraints_;
+};
+
+// Utility functions for dealing with ceres::examples::Pose2d structs
+class PoseUtils {
+ public:
+ // Embeds a 2d pose into a 3d affine transformation to be used in 3d
+ // computation
+ static Eigen::Affine3d Pose2dToAffine3d(ceres::examples::Pose2d pose2d);
+ // Assumes only x and y translation, and only z rotation (yaw)
+ static ceres::examples::Pose2d Affine3dToPose2d(Eigen::Affine3d H);
+
+ // Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
+ // pose_2)
+ static ceres::examples::Pose2d ComputeRelativePose(
+ ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2);
+
+ // Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
+ // equivalent to (pose_1 * pose_2_relative)
+ static ceres::examples::Pose2d ComputeOffsetPose(
+ ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative);
+};
+
+// Transforms robot position and target detection data into target constraints
+// to be used for mapping. Interpolates continous-time data, converting it to
+// discrete detection time steps.
+class DataAdapter {
+ public:
+ // Pairs pose with a time point
+ struct TimestampedPose {
+ aos::distributed_clock::time_point time;
+ ceres::examples::Pose2d pose;
+ };
+
+ // Pairs target detection with a time point
+ struct TimestampedDetection {
+ aos::distributed_clock::time_point time;
+ // Pose of target relative to robot
+ Eigen::Affine3d H_robot_target;
+ TargetMapper::TargetId id;
+ };
+
+ // Pairs consecutive target detections into constraints, and interpolates
+ // robot poses based on time points to compute motion between detections. This
+ // prepares data to be used by TargetMapper. Also returns vector of delta
+ // robot poses corresponding to each constraint, to be used for testing.
+ //
+ // Assumes both inputs are in chronological order.
+ static std::pair<std::vector<ceres::examples::Constraint2d>,
+ std::vector<ceres::examples::Pose2d>>
+ MatchTargetDetections(
+ const std::vector<TimestampedPose> ×tamped_robot_poses,
+ const std::vector<TimestampedDetection> ×tamped_target_detections);
+
+ // Computes inverse of covariance matrix, assuming there was a target
+ // detection between robot movement over the given time period. Ceres calls
+ // this matrix the "information"
+ static Eigen::Matrix3d ComputeConfidence(
+ aos::distributed_clock::time_point start,
+ aos::distributed_clock::time_point end);
+
+ private:
+ static ceres::examples::Pose2d InterpolatePose(
+ const TimestampedPose &pose_start, const TimestampedPose &pose_end,
+ aos::distributed_clock::time_point time);
+
+ // Computes the constraint between the start and end pose of the targets: the
+ // relative pose between the start and end target locations in the frame of
+ // the start target. Takes into account the robot motion in the time between
+ // the two detections.
+ static ceres::examples::Constraint2d ComputeTargetConstraint(
+ const TimestampedDetection &target_detection_start,
+ const Eigen::Affine3d &H_robotstart_robotend,
+ const TimestampedDetection &target_detection_end,
+ const Eigen::Matrix3d &confidence);
+};
+
+} // namespace frc971::vision
+
+#endif // FRC971_VISION_TARGET_MAPPER_H_
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
new file mode 100644
index 0000000..37b037b
--- /dev/null
+++ b/frc971/vision/target_mapper_test.cc
@@ -0,0 +1,413 @@
+#include "frc971/vision/target_mapper.h"
+
+#include <random>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/random_seed.h"
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+namespace frc971::vision {
+
+namespace {
+constexpr double kToleranceMeters = 0.05;
+constexpr double kToleranceRadians = 0.05;
+} // namespace
+
+#define EXPECT_POSE_NEAR(pose1, pose2) \
+ EXPECT_NEAR(pose1.x, pose2.x, kToleranceMeters); \
+ EXPECT_NEAR(pose1.y, pose2.y, kToleranceMeters); \
+ EXPECT_NEAR(pose1.yaw_radians, pose2.yaw_radians, kToleranceRadians);
+
+#define EXPECT_POSE_EQ(pose1, pose2) \
+ EXPECT_DOUBLE_EQ(pose1.x, pose2.x); \
+ EXPECT_DOUBLE_EQ(pose1.y, pose2.y); \
+ EXPECT_DOUBLE_EQ(pose1.yaw_radians, pose2.yaw_radians);
+
+#define EXPECT_BETWEEN_EXCLUSIVE(value, a, b) \
+ { \
+ auto low = std::min(a, b); \
+ auto high = std::max(a, b); \
+ EXPECT_GT(value, low); \
+ EXPECT_LT(value, high); \
+ }
+
+namespace {
+// Expects angles to be normalized
+double DeltaAngle(double a, double b) {
+ double delta = std::abs(a - b);
+ return std::min(delta, (2.0 * M_PI) - delta);
+}
+} // namespace
+
+// Expects angles to be normalized
+#define EXPECT_ANGLE_BETWEEN_EXCLUSIVE(theta, a, b) \
+ EXPECT_LT(DeltaAngle(a, theta), DeltaAngle(a, b)); \
+ EXPECT_LT(DeltaAngle(b, theta), DeltaAngle(a, b));
+
+#define EXPECT_POSE_IN_RANGE(interpolated_pose, pose_start, pose_end) \
+ EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.x, pose_start.x, pose_end.x); \
+ EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.y, pose_start.y, pose_end.y); \
+ EXPECT_ANGLE_BETWEEN_EXCLUSIVE(interpolated_pose.yaw_radians, \
+ pose_start.yaw_radians, \
+ pose_end.yaw_radians);
+
+// Both confidence matrixes should have the same dimensions and be square
+#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
+ { \
+ ASSERT_EQ(confidence1.rows(), confidence2.rows()); \
+ ASSERT_EQ(confidence1.rows(), confidence1.cols()); \
+ ASSERT_EQ(confidence2.rows(), confidence2.cols()); \
+ for (size_t i = 0; i < confidence1.rows(); i++) { \
+ EXPECT_GT(confidence1(i, i), confidence2(i, i)); \
+ } \
+ }
+
+namespace {
+ceres::examples::Pose2d MakePose(double x, double y, double yaw_radians) {
+ return ceres::examples::Pose2d{x, y, yaw_radians};
+}
+
+bool TargetIsInView(TargetMapper::TargetPose target_detection) {
+ // And check if it is within the fov of the robot /
+ // camera, assuming camera is pointing in the
+ // positive x-direction of the robot
+ double angle_to_target =
+ atan2(target_detection.pose.y, target_detection.pose.x);
+
+ // Simulated camera field of view, in radians
+ constexpr double kCameraFov = M_PI_2;
+ if (fabs(angle_to_target) <= kCameraFov / 2.0) {
+ VLOG(2) << "Found target in view, based on T = " << target_detection.pose.x
+ << ", " << target_detection.pose.y << " with angle "
+ << angle_to_target;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+aos::distributed_clock::time_point TimeInMs(size_t ms) {
+ return aos::distributed_clock::time_point(std::chrono::milliseconds(ms));
+}
+
+} // namespace
+
+TEST(DataAdapterTest, Interpolation) {
+ std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+ {TimeInMs(0), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
+ {TimeInMs(5), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
+ {TimeInMs(10), ceres::examples::Pose2d{3.0, 1.0, M_PI_2}},
+ {TimeInMs(15), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
+ {TimeInMs(20), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
+ {TimeInMs(25), ceres::examples::Pose2d{10.0, -32.0, M_PI_2}},
+ {TimeInMs(30), ceres::examples::Pose2d{-15.0, 12.0, 0.0}},
+ {TimeInMs(35), ceres::examples::Pose2d{-15.0, 12.0, 0.0}}};
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+ {{TimeInMs(5),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 0},
+ {TimeInMs(9),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 1},
+ {TimeInMs(9),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 2},
+ {TimeInMs(15),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 0},
+ {TimeInMs(16),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 2},
+ {TimeInMs(27),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
+ 1}};
+ auto [target_constraints, robot_delta_poses] =
+ DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections);
+
+ // Check that target constraints got inserted in the
+ // correct spots
+ EXPECT_EQ(target_constraints.size(),
+ timestamped_target_detections.size() - 1);
+ for (auto it = target_constraints.begin(); it < target_constraints.end();
+ it++) {
+ auto timestamped_it = timestamped_target_detections.begin() +
+ (it - target_constraints.begin());
+ EXPECT_EQ(it->id_begin, timestamped_it->id);
+ EXPECT_EQ(it->id_end, (timestamped_it + 1)->id);
+ }
+
+ // Check that poses were interpolated correctly.
+ // Keep track of the computed robot pose by adding the delta poses
+ auto computed_robot_pose = timestamped_robot_poses[1].pose;
+
+ computed_robot_pose =
+ PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[0]);
+ EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
+ timestamped_robot_poses[2].pose);
+
+ computed_robot_pose =
+ PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[1]);
+ EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
+ timestamped_robot_poses[2].pose);
+ EXPECT_POSE_EQ(robot_delta_poses[1], MakePose(0.0, 0.0, 0.0));
+
+ computed_robot_pose =
+ PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[2]);
+ EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[3].pose);
+
+ computed_robot_pose =
+ PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[3]);
+ EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[4].pose);
+
+ computed_robot_pose =
+ PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[4]);
+ EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[5].pose,
+ timestamped_robot_poses[6].pose);
+
+ // Check the confidence matrices. Don't check the actual values
+ // in case the constants change, just check the confidence of contraints
+ // relative to each other, as constraints over longer time periods should have
+ // lower confidence.
+ const auto confidence_0ms =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(0));
+ const auto confidence_1ms =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(1));
+ const auto confidence_4ms =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(4));
+ const auto confidence_6ms =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(6));
+ const auto confidence_11ms =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(11));
+
+ // Check relative magnitude of different confidences.
+ // Confidences for 0-5ms, 5-10ms, and 10-15ms periods are equal
+ // because they fit within one control loop iteration.
+ EXPECT_EQ(confidence_0ms, confidence_1ms);
+ EXPECT_EQ(confidence_1ms, confidence_4ms);
+ EXPECT_CONFIDENCE_GT(confidence_4ms, confidence_6ms);
+ EXPECT_CONFIDENCE_GT(confidence_6ms, confidence_11ms);
+
+ // Check that confidences (information) of actual constraints are correct
+ EXPECT_EQ(target_constraints[0].information, confidence_4ms);
+ EXPECT_EQ(target_constraints[1].information, confidence_0ms);
+ EXPECT_EQ(target_constraints[2].information, confidence_6ms);
+ EXPECT_EQ(target_constraints[3].information, confidence_1ms);
+ EXPECT_EQ(target_constraints[4].information, confidence_11ms);
+}
+
+TEST(TargetMapperTest, TwoTargetsOneConstraint) {
+ std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+ target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+ target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+
+ std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+ {TimeInMs(5), ceres::examples::Pose2d{2.0, 0.0, 0.0}},
+ {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+ {TimeInMs(15), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+ {{TimeInMs(5),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, 0.0, M_PI}),
+ 0},
+ {TimeInMs(10),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.0, 0.0, 0.0}),
+ 1}};
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections)
+ .first;
+
+ frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+ mapper.Solve();
+
+ ASSERT_EQ(mapper.target_poses().size(), 2);
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+}
+
+TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
+ std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+ target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+ target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, -M_PI_2};
+
+ std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+ {TimeInMs(5), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+ {TimeInMs(10), ceres::examples::Pose2d{3.0, 0.0, 0.0}},
+ {TimeInMs(15), ceres::examples::Pose2d{4.0, 0.0, 0.0}},
+ {TimeInMs(20), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+ {{TimeInMs(5),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{6.0, 0.0, M_PI}),
+ 0},
+ {TimeInMs(10),
+ PoseUtils::Pose2dToAffine3d(
+ ceres::examples::Pose2d{-8.0, 0.0, -M_PI_2}),
+ 1},
+ {TimeInMs(15),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{1.0, 0.0, M_PI}),
+ 0}};
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections)
+ .first;
+
+ frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+ mapper.Solve();
+
+ ASSERT_EQ(mapper.target_poses().size(), 2);
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, -M_PI_2));
+}
+
+TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
+ std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+ target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
+ target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+
+ std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
+ {TimeInMs(5), ceres::examples::Pose2d{1.99, 0.0, 0.0}},
+ {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
+ {TimeInMs(15), ceres::examples::Pose2d{-1.01, -0.01, 0.004}}};
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+ {{TimeInMs(5),
+ PoseUtils::Pose2dToAffine3d(
+ ceres::examples::Pose2d{3.01, 0.001, M_PI - 0.001}),
+ 0},
+ {TimeInMs(10),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.01, 0.0, 0.0}),
+ 1}};
+
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections)
+ .first;
+
+ frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+ mapper.Solve();
+
+ ASSERT_EQ(mapper.target_poses().size(), 2);
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+}
+
+TEST(TargetMapperTest, MultiTargetCircleMotion) {
+ // Build set of target locations wrt global origin
+ // For simplicity, do this on a grid of the field
+ double field_half_length = 7.5; // half length of the field
+ double field_half_width = 5.0; // half width of the field
+ std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
+ std::vector<TargetMapper::TargetPose> actual_target_poses;
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ TargetMapper::TargetId target_id = i * 3 + j;
+ TargetMapper::TargetPose target_pose{
+ target_id, ceres::examples::Pose2d{field_half_length * (1 - i),
+ field_half_width * (1 - j), 0.0}};
+ actual_target_poses.emplace_back(target_pose);
+ target_poses[target_id] = target_pose.pose;
+ VLOG(2) << "VERTEX_SE2 " << target_id << " " << target_pose.pose.x << " "
+ << target_pose.pose.y << " " << target_pose.pose.yaw_radians;
+ }
+ }
+
+ // Now, create a bunch of robot poses and target
+ // observations
+ size_t dt = 1;
+
+ std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses;
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+ constexpr size_t kTotalSteps = 100;
+ for (size_t step_count = 0; step_count < kTotalSteps; step_count++) {
+ size_t t = dt * step_count;
+ // Circle clockwise around the center of the field
+ double robot_theta = t;
+ double robot_x = (field_half_length / 2.0) * cos(robot_theta);
+ double robot_y = (-field_half_width / 2.0) * sin(robot_theta);
+
+ ceres::examples::Pose2d robot_pose{robot_x, robot_y, robot_theta};
+ for (TargetMapper::TargetPose target_pose : actual_target_poses) {
+ TargetMapper::TargetPose target_detection = {
+ .id = target_pose.id,
+ .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
+ if (TargetIsInView(target_detection)) {
+ // Define random generator with Gaussian
+ // distribution
+ const double mean = 0.0;
+ const double stddev = 1.0;
+ // Can play with this to see how it impacts
+ // randomness
+ constexpr double kNoiseScale = 0.01;
+ std::default_random_engine generator(aos::testing::RandomSeed());
+ std::normal_distribution<double> dist(mean, stddev);
+
+ target_detection.pose.x += dist(generator) * kNoiseScale;
+ target_detection.pose.y += dist(generator) * kNoiseScale;
+ robot_pose.x += dist(generator) * kNoiseScale;
+ robot_pose.y += dist(generator) * kNoiseScale;
+
+ auto time_point =
+ aos::distributed_clock::time_point(std::chrono::milliseconds(t));
+ timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
+ .time = time_point, .pose = robot_pose});
+ timestamped_target_detections.emplace_back(
+ DataAdapter::TimestampedDetection{
+ .time = time_point,
+ .H_robot_target =
+ PoseUtils::Pose2dToAffine3d(target_detection.pose),
+ .id = target_detection.id});
+ }
+ }
+ }
+
+ {
+ // Add in a robot pose after all target poses
+ auto final_robot_pose =
+ timestamped_robot_poses[timestamped_robot_poses.size() - 1];
+ timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
+ .time = final_robot_pose.time + std::chrono::milliseconds(dt),
+ .pose = final_robot_pose.pose});
+ }
+
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_robot_poses,
+ timestamped_target_detections)
+ .first;
+ frc971::vision::TargetMapper mapper(target_poses, target_constraints);
+ mapper.Solve();
+
+ for (auto [target_pose_id, mapper_target_pose] : mapper.target_poses()) {
+ TargetMapper::TargetPose actual_target_pose =
+ TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
+ .value();
+ EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
+ }
+
+ //
+ // See what happens when we don't start with the
+ // correct values
+ //
+ for (auto [target_id, target_pose] : target_poses) {
+ // Skip first pose, since that needs to be correct
+ // and is fixed in the solver
+ if (target_id != 0) {
+ ceres::examples::Pose2d bad_pose{0.0, 0.0, M_PI / 2.0};
+ target_poses[target_id] = bad_pose;
+ }
+ }
+
+ frc971::vision::TargetMapper mapper_bad_poses(target_poses,
+ target_constraints);
+ mapper_bad_poses.Solve();
+
+ for (auto [target_pose_id, mapper_target_pose] :
+ mapper_bad_poses.target_poses()) {
+ TargetMapper::TargetPose actual_target_pose =
+ TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
+ .value();
+ EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
+ }
+}
+
+} // namespace frc971::vision