Solve mapping problem in 3d
Solving for z, pitch, and roll will help us get better estimates for
the 2d pose that we actually care about.
Also, since we are only using the box of pis for mapping, deleted
functionality for mapping with robot position so I didn't have to
support that in 3d.
Change-Id: I87e99a148c4953e2e67b0b0e9b07aa9abe1cd158
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index f1d4b83..094021d 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -155,7 +155,7 @@
":target_map_fbs",
"//aos/events:simulated_event_loop",
"//frc971/control_loops:control_loop",
- "//frc971/vision/ceres:pose_graph_2d_lib",
+ "//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
@@ -167,12 +167,15 @@
srcs = [
"target_mapper_test.cc",
],
+ data = [":target_map.json"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
":target_mapper",
"//aos/events:simulated_event_loop",
"//aos/testing:googletest",
+ "//aos/testing:path",
"//aos/testing:random_seed",
+ "//aos/util:math",
],
)
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
index 5622b55..f2cb96b 100644
--- a/frc971/vision/ceres/BUILD
+++ b/frc971/vision/ceres/BUILD
@@ -1,10 +1,8 @@
# Copy of ceres example code from their repo
cc_library(
- name = "pose_graph_2d_lib",
+ name = "pose_graph_3d_lib",
hdrs = [
- "angle_local_parameterization.h",
- "normalize_angle.h",
- "pose_graph_2d_error_term.h",
+ "pose_graph_3d_error_term.h",
"read_g2o.h",
"types.h",
],
diff --git a/frc971/vision/ceres/angle_local_parameterization.h b/frc971/vision/ceres/angle_local_parameterization.h
deleted file mode 100644
index a81637c..0000000
--- a/frc971/vision/ceres/angle_local_parameterization.h
+++ /dev/null
@@ -1,64 +0,0 @@
-// 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
deleted file mode 100644
index c215671..0000000
--- a/frc971/vision/ceres/normalize_angle.h
+++ /dev/null
@@ -1,53 +0,0 @@
-// 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
deleted file mode 100644
index 2df31f6..0000000
--- a/frc971/vision/ceres/pose_graph_2d_error_term.h
+++ /dev/null
@@ -1,119 +0,0 @@
-// 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/pose_graph_3d_error_term.h b/frc971/vision/ceres/pose_graph_3d_error_term.h
new file mode 100644
index 0000000..1f3e8de
--- /dev/null
+++ b/frc971/vision/ceres/pose_graph_3d_error_term.h
@@ -0,0 +1,132 @@
+// 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 EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+#include "types.h"
+
+namespace ceres {
+namespace examples {
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement. We have two poses x_a
+// and x_b. Through sensor measurements we can measure the transformation of
+// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
+// between the current estimate of the poses and the measurement.
+//
+// In this formulation, we have chosen to represent the rigid transformation as
+// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
+// [x, y, z, w].
+
+// The estimated measurement is:
+// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
+// [ q_ab ] [ q_a^{-1] * q_b ]
+//
+// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
+// quaternion. Now we can compute an error metric between the estimated and
+// measurement transformation. For the orientation error, we will use the
+// standard multiplicative error resulting in:
+//
+// error = [ p_ab - \hat{p}_ab ]
+// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
+//
+// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
+// the measurement has an uncertainty associated with how accurate it is, we
+// will weight the errors by the square root of the measurement information
+// matrix:
+//
+// residuals = I^{1/2) * error
+// where I is the information matrix which is the inverse of the covariance.
+class PoseGraph3dErrorTerm {
+ public:
+ PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information)
+ : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const p_a_ptr,
+ const T* const q_a_ptr,
+ const T* const p_b_ptr,
+ const T* const q_b_ptr,
+ T* residuals_ptr) const {
+ Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
+ Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
+
+ Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
+ Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
+
+ // Compute the relative transformation between the two frames.
+ Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+ Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+ // Represent the displacement between the two frames in the A frame.
+ Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+ // Compute the error between the two orientation estimates.
+ Eigen::Quaternion<T> delta_q =
+ t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
+
+ // Compute the residuals.
+ // [ position ] [ delta_p ]
+ // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
+ Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
+ residuals.template block<3, 1>(0, 0) =
+ p_ab_estimated - t_ab_measured_.p.template cast<T>();
+ residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+ // Scale the residuals by the measurement uncertainty.
+ residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(
+ const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+ return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
+ new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The measurement for the position of B relative to A in the A frame.
+ const Pose3d t_ab_measured_;
+ // The square root of the measurement information matrix.
+ const Eigen::Matrix<double, 6, 6> sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
index 3c13824..d3f19ed 100644
--- a/frc971/vision/ceres/types.h
+++ b/frc971/vision/ceres/types.h
@@ -27,75 +27,86 @@
// 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_
+#ifndef EXAMPLES_CERES_TYPES_H_
+#define EXAMPLES_CERES_TYPES_H_
-#include <fstream>
+#include <istream>
+#include <map>
+#include <string>
+#include <vector>
#include "Eigen/Core"
-#include "normalize_angle.h"
+#include "Eigen/Geometry"
namespace ceres {
namespace examples {
-// The state for each vertex in the pose graph.
-struct Pose2d {
- double x;
- double y;
- double yaw_radians;
+struct Pose3d {
+ Eigen::Vector3d p;
+ Eigen::Quaterniond q;
// The name of the data type in the g2o file format.
- static std::string name() { return "VERTEX_SE2"; }
+ static std::string name() { return "VERTEX_SE3:QUAT"; }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-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);
+inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
+ input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >> pose.q.y() >>
+ pose.q.z() >> pose.q.w();
+ // Normalize the quaternion to account for precision loss due to
+ // serialization.
+ pose.q.normalize();
return input;
}
+typedef std::map<int,
+ Pose3d,
+ std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d>>>
+ MapOfPoses;
+
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
-struct Constraint2d {
+struct Constraint3d {
int id_begin;
int id_end;
- double x;
- double y;
- double yaw_radians;
+ // The transformation that represents the pose of the end frame E w.r.t. the
+ // begin frame B. In other words, it transforms a vector in the E frame to
+ // the B frame.
+ Pose3d t_be;
// The inverse of the covariance matrix for the measurement. The order of the
- // entries are x, y, and yaw.
- Eigen::Matrix3d information;
+ // entries are x, y, z, delta orientation.
+ Eigen::Matrix<double, 6, 6> information;
// The name of the data type in the g2o file format.
- static std::string name() { return "EDGE_SE2"; }
+ static std::string name() { return "EDGE_SE3:QUAT"; }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-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);
+inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
+ Pose3d& t_be = constraint.t_be;
+ input >> constraint.id_begin >> constraint.id_end >> t_be;
- // 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);
+ for (int i = 0; i < 6 && input.good(); ++i) {
+ for (int j = i; j < 6 && input.good(); ++j) {
+ input >> constraint.information(i, j);
+ if (i != j) {
+ constraint.information(j, i) = constraint.information(i, j);
+ }
+ }
+ }
return input;
}
+typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d>>
+ VectorOfConstraints;
+
} // namespace examples
} // namespace ceres
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#endif // EXAMPLES_CERES_TYPES_H_
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index c4d98d9..80b1fd4 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -27,7 +27,7 @@
"The mininum number of aruco targets in charuco board required to match.");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
-DEFINE_uint32(age, 5, "Age to start dropping frames at.");
+DEFINE_uint32(age, 100, "Age to start dropping frames at.");
DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
DECLARE_bool(enable_ftrace);
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 50a9d7d..38bab6d 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -6,12 +6,14 @@
id:uint64 (id: 0);
// Pose of target relative to field origin.
- // NOTE: As of now, we only solve for the 2d pose (x, y, yaw)
- // and all other values will be 0.
+ // To get the pose of the target in the field frame, do:
+ // Translation3d(x, y, z) *
+ // (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
x:double (id: 1);
y:double (id: 2);
z:double (id: 3);
+ // Orientation of the target.
roll:double (id: 4);
pitch:double (id: 5);
yaw:double (id: 6);
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 73156f1..5f256fa 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/vision/target_map.json
@@ -1,22 +1,76 @@
{
- "target_poses": [
- {
- "id": 584,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- },
- {
- "id": 585,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- },
- {
- "id": 586,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- }
- ]
-}
\ No newline at end of file
+ "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/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 9222039..5ce62a0 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -1,9 +1,8 @@
#include "frc971/vision/target_mapper.h"
+#include "absl/strings/str_format.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/ceres/pose_graph_3d_error_term.h"
#include "frc971/vision/geometry.h"
DEFINE_uint64(max_num_iterations, 100,
@@ -11,161 +10,63 @@
namespace frc971::vision {
-Eigen::Affine3d PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d pose2d) {
+Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
+ const ceres::examples::Pose3d &pose3d) {
Eigen::Affine3d H_world_pose =
- Eigen::Translation3d(pose2d.x, pose2d.y, 0.0) *
- Eigen::AngleAxisd(pose2d.yaw_radians, Eigen::Vector3d::UnitZ());
+ Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
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::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
+ return ceres::examples::Pose3d{.p = H.translation(),
+ .q = Eigen::Quaterniond(H.rotation())};
}
-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);
+ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2) {
+ Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
+ Eigen::Affine3d H_world_2 = Pose3dToAffine3d(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);
+ return Affine3dToPose3d(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);
+ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2_relative) {
+ auto H_world_1 = Pose3dToAffine3d(pose_1);
+ auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
auto H_world_2 = H_world_1 * H_1_2;
- return Affine3dToPose2d(H_world_2);
+ return Affine3dToPose3d(H_world_2);
}
-namespace {
-double ExponentiatedSinTerm(double theta) {
- return (theta == 0.0 ? 1.0 : std::sin(theta) / theta);
+Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
+ const Eigen::Vector3d &rpy) {
+ Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
+ Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
+
+ return yaw_angle * pitch_angle * roll_angle;
}
-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;
-
- // Skip this target detection if we have no robot poses before it
- if (robot_pose_it->time > target_time) {
- continue;
- }
-
- // 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));
- }
-
- // In the case that all target detections were before the first robot
- // detection, we would have no interpolated poses at this point
- CHECK_GT(interpolated_poses.size(), 0ul)
- << "Need a robot pose before and after every target detection";
-
- // 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,
- last_detection.distance_from_camera,
- it->distance_from_camera);
-
- 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::Vector3d PoseUtils::QuaternionToEulerAngles(
+ const Eigen::Quaterniond &q) {
+ return RotationMatrixToEulerAngles(q.toRotationMatrix());
}
-std::vector<ceres::examples::Constraint2d> DataAdapter::MatchTargetDetections(
+Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
+ const Eigen::Matrix3d &R) {
+ double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
+ double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
+ double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
+
+ return Eigen::Vector3d(roll, pitch, yaw);
+}
+
+ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
const std::vector<DataAdapter::TimestampedDetection>
×tamped_target_detections,
aos::distributed_clock::duration max_dt) {
@@ -173,7 +74,7 @@
<< "Must have at least 2 detections";
// Match consecutive detections
- std::vector<ceres::examples::Constraint2d> target_constraints;
+ ceres::examples::VectorOfConstraints target_constraints;
for (auto it = timestamped_target_detections.begin() + 1;
it < timestamped_target_detections.end(); it++) {
auto last_detection = *(it - 1);
@@ -200,23 +101,30 @@
return target_constraints;
}
-Eigen::Matrix3d DataAdapter::ComputeConfidence(
+TargetMapper::ConfidenceMatrix DataAdapter::ComputeConfidence(
aos::distributed_clock::time_point start,
aos::distributed_clock::time_point end, double distance_from_camera_start,
double distance_from_camera_end) {
constexpr size_t kX = 0;
constexpr size_t kY = 1;
- constexpr size_t kTheta = 2;
+ constexpr size_t kZ = 2;
+ constexpr size_t kOrientation1 = 3;
+ constexpr size_t kOrientation2 = 4;
+ constexpr size_t kOrientation3 = 5;
// Uncertainty matrix between start and end
- Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix P = TargetMapper::ConfidenceMatrix::Zero();
{
// Noise for odometry-based robot position measurements
- Eigen::Matrix3d Q_odometry = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix Q_odometry =
+ TargetMapper::ConfidenceMatrix::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);
+ Q_odometry(kZ, kZ) = std::pow(0.045, 2);
+ Q_odometry(kOrientation1, kOrientation1) = std::pow(0.01, 2);
+ Q_odometry(kOrientation2, kOrientation2) = std::pow(0.01, 2);
+ Q_odometry(kOrientation3, kOrientation3) = std::pow(0.01, 2);
// Add uncertainty for robot position measurements from start to end
int iterations = (end - start) / frc971::controls::kLoopFrequency;
@@ -227,10 +135,14 @@
// Noise for vision-based target localizations. Multiplying this matrix by
// the distance from camera to target squared results in the uncertainty in
// that measurement
- Eigen::Matrix3d Q_vision = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix Q_vision =
+ TargetMapper::ConfidenceMatrix::Zero();
Q_vision(kX, kX) = std::pow(0.045, 2);
Q_vision(kY, kY) = std::pow(0.045, 2);
- Q_vision(kTheta, kTheta) = std::pow(0.02, 2);
+ Q_vision(kZ, kZ) = std::pow(0.045, 2);
+ Q_vision(kOrientation1, kOrientation1) = std::pow(0.02, 2);
+ Q_vision(kOrientation2, kOrientation2) = std::pow(0.02, 2);
+ Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2);
// Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
P += Q_vision * std::pow(distance_from_camera_start, 2);
@@ -240,48 +152,43 @@
return P.inverse();
}
-ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
+ceres::examples::Constraint3d DataAdapter::ComputeTargetConstraint(
const TimestampedDetection &target_detection_start,
- const Eigen::Affine3d &H_robotstart_robotend,
const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence) {
+ const TargetMapper::ConfidenceMatrix &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_start.H_robot_target.inverse() *
target_detection_end.H_robot_target;
- ceres::examples::Pose2d target_constraint =
- PoseUtils::Affine3dToPose2d(H_targetstart_targetend);
+ ceres::examples::Pose3d target_constraint =
+ PoseUtils::Affine3dToPose3d(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};
-}
-
-ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
- const TimestampedDetection &target_detection_start,
- const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence) {
- return ComputeTargetConstraint(target_detection_start,
- Eigen::Affine3d(Eigen::Matrix4d::Identity()),
- target_detection_end, confidence);
+ return ceres::examples::Constraint3d{
+ target_detection_start.id,
+ target_detection_end.id,
+ {target_constraint.p, target_constraint.q},
+ confidence};
}
TargetMapper::TargetMapper(
std::string_view target_poses_path,
- std::vector<ceres::examples::Constraint2d> target_constraints)
+ const ceres::examples::VectorOfConstraints &target_constraints)
: target_constraints_(target_constraints) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[target_pose_fbs->id()] = ceres::examples::Pose2d{
- target_pose_fbs->x(), target_pose_fbs->y(), target_pose_fbs->yaw()};
+ target_poses_[target_pose_fbs->id()] = ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
+ target_pose_fbs->z()),
+ PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(target_pose_fbs->roll(), target_pose_fbs->pitch(),
+ target_pose_fbs->yaw()))};
}
}
TargetMapper::TargetMapper(
- std::map<TargetId, ceres::examples::Pose2d> target_poses,
- std::vector<ceres::examples::Constraint2d> target_constraints)
+ const ceres::examples::MapOfPoses &target_poses,
+ const ceres::examples::VectorOfConstraints &target_constraints)
: target_poses_(target_poses), target_constraints_(target_constraints) {}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
@@ -295,71 +202,70 @@
return std::nullopt;
}
-// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
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);
+ const ceres::examples::VectorOfConstraints &constraints,
+ ceres::examples::MapOfPoses *poses, ceres::Problem *problem) {
+ CHECK(poses != nullptr);
+ CHECK(problem != nullptr);
if (constraints.empty()) {
- LOG(WARNING) << "No constraints, no problem to optimize.";
+ LOG(INFO) << "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();
+ ceres::LossFunction *loss_function = NULL;
+ ceres::LocalParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization;
- for (std::vector<ceres::examples::Constraint2d>::const_iterator
- constraints_iter = constraints.begin();
+ for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
+ constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
- const ceres::examples::Constraint2d &constraint = *constraints_iter;
+ const ceres::examples::Constraint3d &constraint = *constraints_iter;
- std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter =
+ ceres::examples::MapOfPoses::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 =
+ ceres::examples::MapOfPoses::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 =
+ const Eigen::Matrix<double, 6, 6> 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);
+ ceres::examples::PoseGraph3dErrorTerm::Create(constraint.t_be,
+ sqrt_information);
- problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
- angle_local_parameterization);
- problem->SetParameterization(&pose_end_iter->second.yaw_radians,
- angle_local_parameterization);
+ problem->AddResidualBlock(cost_function, loss_function,
+ pose_begin_iter->second.p.data(),
+ pose_begin_iter->second.q.coeffs().data(),
+ pose_end_iter->second.p.data(),
+ pose_end_iter->second.q.coeffs().data());
+
+ problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
}
- // The pose graph optimization problem has three DOFs that are not fully
+ // The pose graph optimization problem has six 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();
+ ceres::examples::MapOfPoses::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);
+ problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
+ problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
-// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
CHECK_NOTNULL(problem);
@@ -378,13 +284,11 @@
void TargetMapper::Solve(std::string_view field_name,
std::optional<std::string_view> output_dir) {
ceres::Problem problem;
- BuildOptimizationProblem(&target_poses_, target_constraints_, &problem);
+ BuildOptimizationProblem(target_constraints_, &target_poses_, &problem);
CHECK(SolveOptimizationProblem(&problem))
<< "The solve was not successful, exiting.";
- // TODO(milind): add origin to first target offset to all poses
-
auto map_json = MapToJson(field_name);
VLOG(1) << "Solved target poses: " << map_json;
@@ -404,9 +308,15 @@
for (const auto &[id, pose] : target_poses_) {
TargetPoseFbs::Builder target_pose_builder(fbb);
target_pose_builder.add_id(id);
- target_pose_builder.add_x(pose.x);
- target_pose_builder.add_y(pose.y);
- target_pose_builder.add_yaw(pose.yaw_radians);
+
+ target_pose_builder.add_x(pose.p(0));
+ target_pose_builder.add_y(pose.p(1));
+ target_pose_builder.add_z(pose.p(2));
+
+ auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
+ target_pose_builder.add_roll(rpy.x());
+ target_pose_builder.add_pitch(rpy.y());
+ target_pose_builder.add_yaw(rpy.z());
target_poses_fbs.emplace_back(target_pose_builder.Finish());
}
@@ -420,4 +330,21 @@
{.multi_line = true});
}
+std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
+ auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
+ os << absl::StrFormat(
+ "{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: "
+ "%.3f, yaw: %.3f}",
+ pose.p(0), pose.p(1), pose.p(2), rpy(0), rpy(1), rpy(2));
+ return os;
+}
+
+std::ostream &operator<<(std::ostream &os,
+ ceres::examples::Constraint3d constraint) {
+ os << absl::StrFormat("{id_begin: %d, id_end: %d, pose: ",
+ constraint.id_begin, constraint.id_end)
+ << constraint.t_be << "}";
+ return os;
+}
+
} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index edf2260..7e2c323 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -4,6 +4,7 @@
#include <unordered_map>
#include "aos/events/simulated_event_loop.h"
+#include "ceres/ceres.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
@@ -16,12 +17,11 @@
class TargetMapper {
public:
using TargetId = int;
+ using ConfidenceMatrix = Eigen::Matrix<double, 6, 6>;
struct TargetPose {
TargetId id;
- // TOOD(milind): switch everything to 3d once we're more confident in 2d
- // solving
- ceres::examples::Pose2d pose;
+ ceres::examples::Pose3d pose;
};
// target_poses_path is the path to a TargetMap json with initial guesses for
@@ -29,11 +29,11 @@
// target_constraints are the deltas between consecutive target detections,
// and are usually prepared by the DataAdapter class below.
TargetMapper(std::string_view target_poses_path,
- std::vector<ceres::examples::Constraint2d> target_constraints);
+ const ceres::examples::VectorOfConstraints &target_constraints);
// Alternate constructor for tests.
// Takes in the actual intial guesses instead of a file containing them
- TargetMapper(std::map<TargetId, ceres::examples::Pose2d> target_poses,
- std::vector<ceres::examples::Constraint2d> target_constraints);
+ TargetMapper(const ceres::examples::MapOfPoses &target_poses,
+ const ceres::examples::VectorOfConstraints &target_constraints);
// Solves for the target map. If output_dir is set, the map will be saved to
// output_dir/field_name.json
@@ -46,56 +46,55 @@
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
- std::map<TargetId, ceres::examples::Pose2d> target_poses() {
- return target_poses_;
- }
+ ceres::examples::MapOfPoses target_poses() { return target_poses_; }
private:
// 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);
+ const ceres::examples::VectorOfConstraints &constraints,
+ ceres::examples::MapOfPoses *poses, 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_;
+ ceres::examples::MapOfPoses target_poses_;
+ ceres::examples::VectorOfConstraints target_constraints_;
};
-// Utility functions for dealing with ceres::examples::Pose2d structs
+// Utility functions for dealing with ceres::examples::Pose3d 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);
+ // Embeds a 3d pose into an affine transformation
+ static Eigen::Affine3d Pose3dToAffine3d(
+ const ceres::examples::Pose3d &pose3d);
+ // Inverse of above function
+ static ceres::examples::Pose3d Affine3dToPose3d(const 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);
+ static ceres::examples::Pose3d ComputeRelativePose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &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);
+ static ceres::examples::Pose3d ComputeOffsetPose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2_relative);
+
+ // Converts a rotation with roll, pitch, and yaw into a quaternion
+ static Eigen::Quaterniond EulerAnglesToQuaternion(const Eigen::Vector3d &rpy);
+ // Inverse of above function
+ static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
+ // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
+ static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
};
// 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.
+// to be used for mapping.
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;
@@ -107,58 +106,38 @@
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);
-
// Pairs consecutive target detections that are not too far apart in time into
// constraints. Meant to be used on a system without a position measurement.
// Assumes timestamped_target_detections is in chronological order.
// max_dt is the maximum time between two target detections to match them up.
// If too much time passes, the recoding device (box of pis) could have moved
// too much
- static std::vector<ceres::examples::Constraint2d> MatchTargetDetections(
+ static ceres::examples::VectorOfConstraints MatchTargetDetections(
const std::vector<TimestampedDetection> ×tamped_target_detections,
aos::distributed_clock::duration max_dt = std::chrono::milliseconds(1));
// 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(
+ static TargetMapper::ConfidenceMatrix ComputeConfidence(
aos::distributed_clock::time_point start,
aos::distributed_clock::time_point end, double distance_from_camera_start,
double distance_from_camera_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);
- // Same as above function, but assumes no robot motion between the two
- // detections
- static ceres::examples::Constraint2d ComputeTargetConstraint(
+ // the start target.
+ static ceres::examples::Constraint3d ComputeTargetConstraint(
const TimestampedDetection &target_detection_start,
const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence);
+ const TargetMapper::ConfidenceMatrix &confidence);
};
+std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose);
+std::ostream &operator<<(std::ostream &os,
+ ceres::examples::Constraint3d constraint);
+
} // 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
index ab06c5b..9440813 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -3,7 +3,9 @@
#include <random>
#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
#include "aos/testing/random_seed.h"
+#include "aos/util/math.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -12,46 +14,65 @@
namespace {
constexpr double kToleranceMeters = 0.05;
constexpr double kToleranceRadians = 0.05;
+// Conversions between euler angles and quaternion result in slightly-off
+// doubles
+constexpr double kOrientationEqTolerance = 1e-10;
+constexpr std::chrono::milliseconds kMaxDt = std::chrono::milliseconds(3);
constexpr std::string_view kFieldName = "test";
} // 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); \
+// Angles normalized by aos::math::NormalizeAngle()
+#define EXPECT_NORMALIZED_ANGLES_NEAR(theta1, theta2, tolerance) \
+ { \
+ double delta = std::abs(aos::math::DiffAngle(theta1, theta2)); \
+ /* Have to check delta - 2pi for the case that one angle is very */ \
+ /* close to -pi, and the other is very close to +pi */ \
+ EXPECT_TRUE(delta < tolerance || std::abs(aos::math::DiffAngle( \
+ delta, 2.0 * M_PI)) < tolerance); \
}
-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
+#define EXPECT_POSE_NEAR(pose1, pose2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ EXPECT_NEAR(pose1.p(i), pose2.p(i), kToleranceMeters); \
+ } \
+ auto rpy_1 = PoseUtils::QuaternionToEulerAngles(pose1.q); \
+ auto rpy_2 = PoseUtils::QuaternionToEulerAngles(pose2.q); \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), kToleranceRadians) \
+ } \
+ }
-// 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_EQ(pose1, pose2) \
+ EXPECT_EQ(pose1.p, pose2.p); \
+ EXPECT_EQ(pose1.q, pose2.q);
-#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);
+#define EXPECT_QUATERNION_NEAR(q1, q2) \
+ EXPECT_NEAR(q1.x(), q2.x(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.y(), q2.y(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.z(), q2.z(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.w(), q2.w(), kOrientationEqTolerance) << q1 << " != " << q2;
+
+// Expects same roll, pitch, and yaw values (not equivalent rotation)
+#define EXPECT_RPY_EQ(rpy_1, rpy_2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), \
+ kOrientationEqTolerance) \
+ } \
+ }
+
+#define EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw) \
+ { \
+ auto rpy = Eigen::Vector3d(roll, pitch, yaw); \
+ auto converted_rpy = PoseUtils::QuaternionToEulerAngles( \
+ PoseUtils::EulerAnglesToQuaternion(rpy)); \
+ EXPECT_RPY_EQ(converted_rpy, rpy); \
+ }
// Both confidence matrixes should have the same dimensions and be square
#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
@@ -65,18 +86,21 @@
}
namespace {
-ceres::examples::Pose2d MakePose(double x, double y, double yaw_radians) {
- return ceres::examples::Pose2d{x, y, yaw_radians};
+ceres::examples::Pose3d Make2dPose(double x, double y, double yaw_radians) {
+ return ceres::examples::Pose3d{Eigen::Vector3d(x, y, 0.0),
+ PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(0.0, 0.0, yaw_radians))};
}
// Assumes camera and robot origin are the same
DataAdapter::TimestampedDetection MakeTimestampedDetection(
aos::distributed_clock::time_point time, Eigen::Affine3d H_robot_target,
TargetMapper::TargetId id) {
- auto target_pose = PoseUtils::Affine3dToPose2d(H_robot_target);
+ auto target_pose = PoseUtils::Affine3dToPose3d(H_robot_target);
return DataAdapter::TimestampedDetection{
time, H_robot_target,
- std::sqrt(std::pow(target_pose.x, 2) + std::pow(target_pose.y, 2)), id};
+ std::sqrt(std::pow(target_pose.p(0), 2) + std::pow(target_pose.p(1), 2)),
+ id};
}
bool TargetIsInView(TargetMapper::TargetPose target_detection) {
@@ -84,14 +108,14 @@
// 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);
+ atan2(target_detection.pose.p(1), target_detection.pose.p(0));
// 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;
+ if (std::abs(angle_to_target) <= kCameraFov / 2.0) {
+ VLOG(2) << "Found target in view, based on T = "
+ << target_detection.pose.p(0) << ", " << target_detection.pose.p(1)
+ << " with angle " << angle_to_target;
return true;
} else {
return false;
@@ -104,6 +128,44 @@
} // namespace
+TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
+ // Make sure that the conversions are consistent back and forth.
+ // These angles shouldn't get changed to a different, equivalent roll pitch
+ // yaw.
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
+
+ // Now, do a sweep of roll, pitch, and yaws in the normalized
+ // range.
+ // - roll: (-pi/2, pi/2)
+ // - pitch: (-pi/2, pi/2)
+ // - yaw: [-pi, pi)
+ constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxYaw = M_PI;
+ constexpr double kDeltaTheta = M_PI / 16;
+
+ for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
+ roll += kDeltaTheta) {
+ for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
+ pitch += kDeltaTheta) {
+ for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
+ SCOPED_TRACE(
+ absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
+ }
+ }
+ }
+}
+
TEST(DataAdapterTest, ComputeConfidence) {
// Check the confidence matrices. Don't check the actual values
// in case the constants change, just check that the confidence of contraints
@@ -113,10 +175,12 @@
constexpr double kDistanceStart = 0.5;
constexpr double kDistanceEnd = 2.0;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (size_t dt = 0; dt < 15; dt++) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(dt), kDistanceStart, kDistanceEnd);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(dt),
+ kDistanceStart, kDistanceEnd);
if (dt != 0) {
// Confidence only decreases every 5ms (control loop period)
@@ -134,11 +198,13 @@
// Vary distance at start
constexpr int kDt = 3;
constexpr double kDistanceEnd = 1.5;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (double distance_start = 0.0; distance_start < 3.0;
distance_start += 0.5) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(kDt), distance_start, kDistanceEnd);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
+ distance_start, kDistanceEnd);
if (distance_start != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
@@ -150,10 +216,12 @@
// Vary distance at end
constexpr int kDt = 2;
constexpr double kDistanceStart = 2.5;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (double distance_end = 0.0; distance_end < 3.0; distance_end += 0.5) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(kDt), kDistanceStart, distance_end);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
+ kDistanceStart, distance_end);
if (distance_end != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
@@ -163,103 +231,23 @@
}
TEST(DataAdapterTest, MatchTargetDetections) {
- 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}),
- 1.0, 0},
- {TimeInMs(9),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 1},
- {TimeInMs(9),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 2},
- {TimeInMs(15),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 0},
- {TimeInMs(16),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 2},
- {TimeInMs(27),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.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);
-}
-
-TEST(DataAdapterTest, MatchTargetDetectionsWithoutRobotPosition) {
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
{MakeTimestampedDetection(
- TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -5.0, 0.0}),
+ TimeInMs(5), PoseUtils::Pose3dToAffine3d(Make2dPose(5.0, -5.0, 0.0)),
2),
- MakeTimestampedDetection(TimeInMs(6),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{5.0, -4.0, M_PI}),
- 0),
- MakeTimestampedDetection(TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{3.0, -3.0, M_PI}),
- 1),
- MakeTimestampedDetection(TimeInMs(13),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{4.0, -7.0, M_PI_2}),
- 2),
- MakeTimestampedDetection(TimeInMs(14),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{4.0, -4.0, M_PI_2}),
- 2)};
+ MakeTimestampedDetection(
+ TimeInMs(6),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(5.0, -4.0, M_PI)), 0),
+ MakeTimestampedDetection(
+ TimeInMs(10),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(3.0, -3.0, M_PI)), 1),
+ MakeTimestampedDetection(
+ TimeInMs(13),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(4.0, -7.0, M_PI_2)), 2),
+ MakeTimestampedDetection(
+ TimeInMs(14),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(4.0, -4.0, M_PI_2)), 2)};
- constexpr auto kMaxDt = std::chrono::milliseconds(3);
auto target_constraints =
DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
@@ -271,201 +259,225 @@
timestamped_target_detections.size() - 3);
// Between 5ms and 6ms detections
- EXPECT_DOUBLE_EQ(target_constraints[0].x, 0.0);
- EXPECT_DOUBLE_EQ(target_constraints[0].y, 1.0);
- EXPECT_DOUBLE_EQ(target_constraints[0].yaw_radians, -M_PI);
+ EXPECT_DOUBLE_EQ(target_constraints[0].t_be.p(0), 0.0);
+ EXPECT_DOUBLE_EQ(target_constraints[0].t_be.p(1), 1.0);
+ EXPECT_QUATERNION_NEAR(
+ target_constraints[0].t_be.q,
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(0.0, 0.0, M_PI)));
EXPECT_EQ(target_constraints[0].id_begin, 2);
EXPECT_EQ(target_constraints[0].id_end, 0);
// Between 10ms and 13ms detections
- EXPECT_DOUBLE_EQ(target_constraints[1].x, -1.0);
- EXPECT_DOUBLE_EQ(target_constraints[1].y, 4.0);
- EXPECT_DOUBLE_EQ(target_constraints[1].yaw_radians, -M_PI_2);
+ EXPECT_DOUBLE_EQ(target_constraints[1].t_be.p(0), -1.0);
+ EXPECT_DOUBLE_EQ(target_constraints[1].t_be.p(1), 4.0);
+ EXPECT_QUATERNION_NEAR(
+ target_constraints[1].t_be.q,
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(0.0, 0.0, -M_PI_2)));
EXPECT_EQ(target_constraints[1].id_begin, 1);
EXPECT_EQ(target_constraints[1].id_end, 2);
}
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};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-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 =
{MakeTimestampedDetection(
- TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, 0.0, M_PI}),
+ TimeInMs(5), PoseUtils::Pose3dToAffine3d(Make2dPose(3.0, 0.0, M_PI)),
0),
MakeTimestampedDetection(
- TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.0, 0.0, 0.0}),
+ TimeInMs(6), PoseUtils::Pose3dToAffine3d(Make2dPose(-7.0, 0.0, 0.0)),
1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
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));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-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};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-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 =
{MakeTimestampedDetection(
TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{6.0, 0.0, M_PI}),
- 0),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(6.0, 0.0, M_PI_2)), 0),
MakeTimestampedDetection(
- TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{-8.0, 0.0, -M_PI_2}),
- 1),
+ TimeInMs(7),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(6.0, 10.0, -M_PI)), 1),
MakeTimestampedDetection(
- TimeInMs(15),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{1.0, 0.0, M_PI}),
- 0)};
+ TimeInMs(12),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(1.0, 0.0, M_PI)), 0),
+ MakeTimestampedDetection(
+ TimeInMs(13),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(-9.0, 0.0, -M_PI_2)), 1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
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));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-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};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-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 =
{MakeTimestampedDetection(
TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{3.01, 0.001, M_PI - 0.001}),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(3.01, 0.001, M_PI - 0.001)),
0),
- MakeTimestampedDetection(TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{-4.01, 0.0, 0.0}),
- 1)};
+ MakeTimestampedDetection(
+ TimeInMs(7),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(-7.01, 0.0, 0.0)), 1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
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));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-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;
+class ChargedUpTargetMapperTest : public ::testing::Test {
+ public:
+ ChargedUpTargetMapperTest() : generator_(aos::testing::RandomSeed()) {}
+
+ // Generates noisy target detection if target in camera FOV
+ std::optional<DataAdapter::TimestampedDetection> MaybeGenerateNoisyDetection(
+ const ceres::examples::Pose3d &robot_pose,
+ const TargetMapper::TargetPose &target_pose, size_t t,
+ bool force_in_view = false) {
+ TargetMapper::TargetPose target_detection = {
+ .id = target_pose.id,
+ .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
+ if (force_in_view || TargetIsInView(target_detection)) {
+ // Define random generator with Gaussian
+ // distribution
+ constexpr double kMean = 0.0;
+ constexpr double kStdDev = 1.0;
+ // Can play with this to see how it impacts
+ // randomness
+ constexpr double kNoiseScalePosition = 0.01;
+ constexpr double kNoiseScaleOrientation = 0.0005;
+ std::normal_distribution<double> dist(kMean, kStdDev);
+
+ target_detection.pose.p(0) += dist(generator_) * kNoiseScalePosition;
+ target_detection.pose.p(1) += dist(generator_) * kNoiseScalePosition;
+ target_detection.pose.p(2) += dist(generator_) * kNoiseScalePosition;
+
+ target_detection.pose.q.w() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.x() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.y() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.z() += dist(generator_) * kNoiseScaleOrientation;
+
+ auto time_point = TimeInMs(t);
+ return MakeTimestampedDetection(
+ time_point, PoseUtils::Pose3dToAffine3d(target_detection.pose),
+ target_detection.id);
}
+
+ return std::nullopt;
}
+ private:
+ std::default_random_engine generator_;
+};
+
+// Drive in a circle around the 2023 field, and add a bit of randomness to 3d
+// pose detections
+TEST_F(ChargedUpTargetMapperTest, FieldCircleMotion) {
+ // Read target map
+ auto target_map_fbs = aos::JsonFileToFlatbuffer<TargetMap>(
+ aos::testing::ArtifactPath("frc971/vision/target_map.json"));
+
+ std::vector<TargetMapper::TargetPose> actual_target_poses;
+ ceres::examples::MapOfPoses target_poses;
+ for (auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
+ auto target_pose = TargetMapper::TargetPose{
+ static_cast<int>(target_pose_fbs->id()),
+ ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
+ target_pose_fbs->z()),
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
+ target_pose_fbs->roll(), target_pose_fbs->pitch(),
+ target_pose_fbs->yaw()))}};
+ actual_target_poses.emplace_back(target_pose);
+ target_poses[target_pose.id] = target_pose.pose;
+ }
+
+ double kFieldHalfLength = 16.54 / 2.0; // half length of the field
+ double kFieldHalfWidth = 8.02 / 2.0; // half width of the field
+
// Now, create a bunch of robot poses and target
// observations
- size_t dt = 1;
+ constexpr size_t kDt = 5;
+ constexpr double kRobotZ = 1.0;
- 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;
+ size_t t = kDt * 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};
+ constexpr double kRadiusScalar = 0.5;
+ double robot_x = (kRadiusScalar * kFieldHalfLength) * cos(robot_theta);
+ double robot_y = (kRadiusScalar * -kFieldHalfWidth) * sin(robot_theta);
+
+ auto robot_pose =
+ ceres::examples::Pose3d{.p = Eigen::Vector3d(robot_x, robot_y, kRobotZ),
+ .q = PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(robot_theta, 0.0, 0.0))};
+
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(MakeTimestampedDetection(
- time_point, PoseUtils::Pose2dToAffine3d(target_detection.pose),
- target_detection.id));
+ auto optional_detection =
+ MaybeGenerateNoisyDetection(robot_pose, target_pose, t);
+ if (optional_detection.has_value()) {
+ timestamped_target_detections.emplace_back(*optional_detection);
}
}
}
+ // The above circular motion only captures targets 1-4, so add another
+ // detection with the camera looking at targets 5-8, and 4 at the same time to
+ // have a connection to the rest of the targets
{
- // 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 last_robot_pose =
+ ceres::examples::Pose3d{.p = Eigen::Vector3d(0.0, 0.0, kRobotZ),
+ .q = PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(0.0, 0.0, M_PI))};
+ for (size_t id = 4; id <= 8; id++) {
+ auto target_pose =
+ TargetMapper::GetTargetPoseById(actual_target_poses, id).value();
+ auto optional_detection = MaybeGenerateNoisyDetection(
+ last_robot_pose, target_pose, kTotalSteps * kDt, true);
+
+ ASSERT_TRUE(optional_detection.has_value())
+ << "Didn't detect target " << target_pose.id;
+ timestamped_target_detections.emplace_back(*optional_detection);
+ }
}
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
@@ -483,8 +495,10 @@
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};
+ if (target_id != 1) {
+ ceres::examples::Pose3d bad_pose{
+ Eigen::Vector3d::Zero(),
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d::Zero())};
target_poses[target_id] = bad_pose;
}
}