Move camera.h to the frc971/ folder
The code for dealing with camera transformations is reasonably
year-generic, so move it into frc971/.
Change-Id: I7751ccd01c3e45093c8e548d65e0087b615f0ae0
diff --git a/y2019/BUILD b/y2019/BUILD
index 328c93f..e286fc7 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -36,7 +36,7 @@
"//frc971:constants",
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
- "//y2019/control_loops/drivetrain:camera",
+ "//frc971/control_loops/drivetrain:camera",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
"//y2019/control_loops/superstructure/elevator:elevator_plants",
"//y2019/control_loops/superstructure/intake:intake_plants",
diff --git a/y2019/constants.h b/y2019/constants.h
index 5b00847..d1d0c7c 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -6,13 +6,13 @@
#include <stdint.h>
#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
-#include "y2019/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/pose.h"
namespace y2019 {
@@ -32,7 +32,7 @@
class Field {
public:
typedef ::frc971::control_loops::TypedPose<double> Pose;
- typedef ::y2019::control_loops::TypedTarget<double> Target;
+ typedef ::frc971::control_loops::TypedTarget<double> Target;
typedef ::frc971::control_loops::TypedLineSegment<double> Obstacle;
static constexpr size_t kNumTargets = 32;
@@ -201,8 +201,9 @@
static constexpr size_t kNumCameras = 5;
::std::array<CameraCalibration, kNumCameras> cameras;
- control_loops::TypedCamera<Field::kNumTargets, Field::kNumObstacles,
- double>::NoiseParameters camera_noise_parameters;
+ frc971::control_loops::TypedCamera<Field::kNumTargets, Field::kNumObstacles,
+ double>::NoiseParameters
+ camera_noise_parameters;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index bc76d7a..aabd3e5 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -100,30 +100,11 @@
)
cc_library(
- name = "camera",
- srcs = ["camera.h"],
- visibility = ["//y2019:__pkg__"],
- deps = [
- "//aos/containers:sized_array",
- "//frc971/control_loops:pose",
- ],
-)
-
-cc_test(
- name = "camera_test",
- srcs = ["camera_test.cc"],
- deps = [
- ":camera",
- "//aos/testing:googletest",
- ],
-)
-
-cc_library(
name = "localizer",
hdrs = ["localizer.h"],
deps = [
- ":camera",
"//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:camera",
"//frc971/control_loops/drivetrain:hybrid_ekf",
],
)
@@ -133,9 +114,9 @@
srcs = ["target_selector.cc"],
hdrs = ["target_selector.h"],
deps = [
- ":camera",
":target_selector_fbs",
"//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:camera",
"//frc971/control_loops/drivetrain:localizer",
"//y2019:constants",
"//y2019/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
deleted file mode 100644
index e21bf87..0000000
--- a/y2019/control_loops/drivetrain/camera.h
+++ /dev/null
@@ -1,374 +0,0 @@
-#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
-#define Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
-
-#include <vector>
-
-#include "aos/containers/sized_array.h"
-#include "frc971/control_loops/pose.h"
-
-namespace y2019 {
-namespace control_loops {
-
-// Represents a target on the field. Currently just consists of a pose and a
-// indicator for whether it is occluded (occlusion is only used by the simulator
-// for testing).
-// Orientation convention:
-// -The absolute position of the pose is the center of the vision target on the
-// field.
-// -The yaw of the pose shall be such that the positive X-axis in the Target's
-// frame wil be pointed straight through the target--i.e., if you are looking
-// at the target head-on, then you will be facing in the same direction as the
-// positive X-axis.
-// E.g., if the Target has a global position of (1, 1, 0) and yaw of pi / 2,
-// then it is at position (1, 1, 0) on the field and is oriented so that if
-// someone were to stand at (1, 0, 0) and turn themselves to have a yaw of
-// pi / 2, they would see the target 1 meter straight ahead of them.
-//
-// Generally, the position of a target should not change dynamically; if we do
-// start having targets that move, we may want to start optimizing certain
-// things (e.g., caching the position of the Target--currently, if the Pose of a
-// target is in an absolute frame, then calling abs_pos will be inexpensive; if
-// that changes, then we start having to recalculate transformations on every
-// frame).
-template <typename Scalar = double>
-class TypedTarget {
- public:
- typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
- // The nature of the target as a goal--to mark what modes it is a valid
- // potential goal pose and to mark targets on the opposite side of the field
- // as not being viable targets.
- enum class GoalType {
- // None marks targets that are on the opposite side of the field and not
- // viable goal poses.
- kNone,
- // Spots where we can touch hatch panels.
- kHatches,
- // Spots where we can mess with balls.
- kBalls,
- // Spots for both (cargo ship, human loading).
- kBoth,
- };
- // Which target this is within a given field quadrant:
- enum class TargetType {
- kHPSlot,
- kFaceCargoBay,
- kNearSideCargoBay,
- kMidSideCargoBay,
- kFarSideCargoBay,
- kNearRocket,
- kFarRocket,
- kRocketPortal,
- };
- TypedTarget(const Pose &pose, double radius = 0,
- TargetType target_type = TargetType::kHPSlot,
- GoalType goal_type = GoalType::kBoth)
- : pose_(pose),
- radius_(radius),
- target_type_(target_type),
- goal_type_(goal_type) {}
- TypedTarget() {}
- Pose pose() const { return pose_; }
- Pose *mutable_pose() { return &pose_; }
-
- bool occluded() const { return occluded_; }
- void set_occluded(bool occluded) { occluded_ = occluded; }
- double radius() const { return radius_; }
- GoalType goal_type() const { return goal_type_; }
- void set_goal_type(GoalType goal_type) { goal_type_ = goal_type; }
- TargetType target_type() const { return target_type_; }
- void set_target_type(TargetType target_type) { target_type_ = target_type; }
-
- // Get a list of points for plotting. These points should be plotted on
- // an x/y plane in the global frame with lines connecting the points.
- // Essentially, this provides a Polygon that is a reasonable representation
- // of a Target.
- // This should not be called from real-time code, as it will probably
- // dynamically allocate memory.
- ::std::vector<Pose> PlotPoints() const {
- // For the actual representation, we will use a triangle such that the
- // base of the triangle corresponds to the surface the target is on.
- // The third point is shown behind the target, so that the user can
- // visually identify which side of the target is the front.
- Pose base1(&pose_, {0, 0.125, 0}, 0);
- Pose base2(&pose_, {0, -0.125, 0}, 0);
- Pose behind(&pose_, {0.05, 0, 0}, 0);
- // Include behind at the start and end to indicate that we want to draw
- // a closed polygon.
- return {behind, base1, base2, behind};
- }
-
- private:
- Pose pose_;
- bool occluded_ = false;
- // The effective radius of the target--for placing discs, this should be the
- // radius of the disc; for fetching discs and working with balls this should
- // be near zero.
- // TODO(james): We may actually want a non-zero (possibly negative?) number
- // here for balls.
- double radius_ = 0.0;
- TargetType target_type_ = TargetType::kHPSlot;
- GoalType goal_type_ = GoalType::kBoth;
-}; // class TypedTarget
-
-typedef TypedTarget<double> Target;
-
-// Represents a camera that can see targets and provide information about their
-// relative positions.
-// Note on coordinate systems:
-// -The camera's Pose shall be such that the camera frame's positive X-axis is
-// pointed straight out of the lens (as always, positive Z will be up; we
-// assume that all cameras mounted level, or compensated for such that this
-// code won't care).
-// -The origin of the camera shall be "at" the camera. For this code, I don't
-// think we care too much about the details of the camera model, so we can just
-// assume that it is an idealized pinhole camera with the pinhole being the
-// location of the camera.
-//
-// Template parameters:
-// -num_targets: The number of targets on the field, should be the same for
-// all the actual cameras on the robot (although it may change in tests).
-// -Scalar: The floating point type to use (double vs. float).
-// -num_obstacles: The number of obstacles on the field to account for; like
-// the number of targets, it should be consistent across actual cameras,
-// although for simulation we may add extra obstacles for testing.
-template <int num_targets, int num_obstacles, typename Scalar = double>
-class TypedCamera {
- public:
- typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
- typedef ::frc971::control_loops::TypedLineSegment<Scalar> LineSegment;
-
- // TargetView contains the information associated with a sensor reading
- // from the camera--the readings themselves and noise values, *from the
- // Camera's persective* for each reading.
- // Note that the noise terms are just accounting for the inaccuracy you
- // expect to get due to visual noise, pixel-level resolution, etc. These
- // do not account for the fact that, e.g., there is noise in the Pose of the
- // robot which can translate into noise in the target reading.
- // The noise terms are standard deviations, and so have units identical
- // to that of the actual readings.
- struct TargetView {
- struct Reading {
- // The heading as reported from the camera; zero = straight ahead,
- // positive = target in the left half of the image.
- Scalar heading; // radians
- // The distance from the camera to the target.
- Scalar distance; // meters
- // Height of the target from the camera.
- Scalar height; // meters
- // The angle of the target relative to line between the camera and
- // the center of the target.
- Scalar skew; // radians
- };
- Reading reading;
- Reading noise;
-
- // The target that this view corresponds to.
- const TypedTarget<Scalar> *target = nullptr;
- // The Pose the camera was at when viewing the target:
- Pose camera_pose;
- };
-
- // Important parameters for dealing with camera noise calculations.
- // Ultimately, this should end up coming from the constants file.
- struct NoiseParameters {
- // The maximum distance from which we can see a target head-on (when the
- // target is not head-on, we adjust for that).
- Scalar max_viewable_distance; // meters
-
- // All noises are standard deviations of the noise, assuming an ~normal
- // distribution.
-
- // Noise in the heading measurement, which should be constant regardless of
- // other factors.
- Scalar heading_noise; // radians
- // Noise in the distance measurement when the target is 1m away and head-on
- // to us. This is adjusted by assuming the noise is proportional to the
- // apparent width of the target (because the target also has height, this
- // may not be strictly correct).
- // TODO(james): Is this a good model? It should be reasonable, but there
- // may be more complexity somewhere.
- Scalar nominal_distance_noise; // meters
- // The noise in the skew measurement when the target is 1m away and head-on
- // to us. Calculated in the same manner with the same caveats as the
- // distance noise.
- Scalar nominal_skew_noise; // radians
- // Noise in the height measurement, same rules as for skew and distance.
- // TODO(james): Figure out how much noise we will actually get in the
- // height, since there will be extremely low resolution on it.
- Scalar nominal_height_noise; // meters
- };
-
- // Provide a default constructor to make life easier.
- TypedCamera() {}
-
- // Creates a camera:
- // pose: The Pose of the camera, relative to the robot at least transitively.
- // fov: The field-of-view of the camera, in radians. Note that this is the
- // *total* field-of-view in the horizontal plane (left-right), so the angle
- // from the left edge of the image to the right edge.
- // targets: The list of targets on the field that could be seen by the camera.
- // obstacles: The list of known obstacles on the field.
- TypedCamera(const Pose &pose, Scalar fov,
- const NoiseParameters &noise_parameters,
- const ::std::array<TypedTarget<Scalar>, num_targets> &targets,
- const ::std::array<LineSegment, num_obstacles> &obstacles)
- : pose_(pose),
- fov_(fov),
- noise_parameters_(noise_parameters),
- targets_(targets),
- obstacles_(obstacles) {}
-
- // Returns a list of TargetViews for all the currently visible targets.
- // These will contain ground-truth TargetViews, so the readings will be
- // perfect; a pseudo-random number generator should be used to add noise
- // separately for simulation.
- ::aos::SizedArray<TargetView, num_targets> target_views() const {
- ::aos::SizedArray<TargetView, num_targets> views;
- Pose camera_abs_pose = pose_.Rebase(nullptr);
- // Because there are num_targets in targets_ and because AddTargetIfVisible
- // adds at most 1 view to views, we should never exceed the size of
- // SizedArray.
- for (const auto &target : targets_) {
- AddTargetIfVisible(target, camera_abs_pose, &views);
- }
- return views;
- }
-
- // Returns a list of list of points for plotting. Each list of points should
- // be plotted as a line; currently, each list is just a pair drawing a line
- // from the camera aperture to the target location.
- // This should not be called from real-time code, as it will probably
- // dynamically allocate memory.
- ::std::vector<::std::vector<Pose>> PlotPoints() const {
- ::std::vector<::std::vector<Pose>> list_of_lists;
- for (const auto &view : target_views()) {
- list_of_lists.push_back({pose_, view.target->pose().Rebase(&pose_)});
- }
- return list_of_lists;
- }
-
- const Pose &pose() const { return pose_; }
- Scalar fov() const { return fov_; }
-
- // Estimates the noise values of a target based on the raw readings.
- // Also estimates whether we would expect the target to be visible, and
- // populates is_visible if is_visible is not nullptr.
- void PopulateNoise(TargetView *view, bool *is_visible = nullptr) const {
- // Calculate the width of the target as it appears in the image.
- // This number is unitless and if greater than 1, implies that the target is
- // visible to the camera and if less than 1 implies it is too small to be
- // registered on the camera.
- const Scalar cosskew = ::std::cos(view->reading.skew);
- Scalar apparent_width = ::std::max<Scalar>(
- 0.0, cosskew * noise_parameters_.max_viewable_distance /
- view->reading.distance);
- // If we got wildly invalid distance or skew measurements, then set a very
- // small apparent width.
- if (view->reading.distance < 0 || cosskew < 0) {
- apparent_width = 0.01;
- }
- // As both a sanity check and for the sake of numerical stability, manually
- // set apparent_width to something "very small" if it is near zero.
- if (apparent_width < 0.01) {
- apparent_width = 0.01;
- }
-
- if (is_visible != nullptr) {
- *is_visible = apparent_width >= 1.0;
- }
-
- view->noise.heading = noise_parameters_.heading_noise;
-
- const Scalar normalized_width =
- apparent_width / noise_parameters_.max_viewable_distance;
- view->noise.distance =
- noise_parameters_.nominal_distance_noise / normalized_width;
- view->noise.skew =
- noise_parameters_.nominal_skew_noise / normalized_width;
- view->noise.height =
- noise_parameters_.nominal_height_noise / normalized_width;
- }
-
- private:
-
- // If the specified target is visible from the current camera Pose, adds it to
- // the views array.
- void AddTargetIfVisible(
- const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
- ::aos::SizedArray<TargetView, num_targets> *views) const;
-
- // The Pose of this camera.
- Pose pose_;
-
- // Field of view of the camera, in radians.
- Scalar fov_;
-
- // Various constants to calclate sensor noise; see definition of
- // NoiseParameters for more detail.
- NoiseParameters noise_parameters_;
-
- // A list of all the targets on the field.
- // TODO(james): Is it worth creating some sort of cache for the targets and
- // obstacles? e.g., passing around pointer to the targets/obstacles.
- ::std::array<TypedTarget<Scalar>, num_targets> targets_;
- // Known obstacles on the field which can interfere with our view of the
- // targets. An "obstacle" is a line segment which we cannot see through, as
- // such a logical obstacle (e.g., the cargo ship) may consist of many
- // obstacles in this list to account for all of its sides.
- ::std::array<LineSegment, num_obstacles> obstacles_;
-}; // class TypedCamera
-
-template <int num_targets, int num_obstacles, typename Scalar>
-void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
- const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
- ::aos::SizedArray<TargetView, num_targets> *views) const {
- if (target.occluded()) {
- return;
- }
-
- // Precompute the current absolute pose of the camera, because we will reuse
- // it a bunch.
- const Pose relative_pose = target.pose().Rebase(&camera_abs_pose);
- const Scalar heading = relative_pose.heading();
- const Scalar distance = relative_pose.xy_norm();
- const Scalar skew =
- ::aos::math::NormalizeAngle(relative_pose.rel_theta() - heading);
-
- // Check if the camera is in the angular FOV.
- if (::std::abs(heading) > fov_ / 2.0) {
- return;
- }
-
- TargetView view;
- view.reading.heading = heading;
- view.reading.distance = distance;
- view.reading.skew = skew;
- view.reading.height = relative_pose.rel_pos().z();
- view.target = ⌖
- view.camera_pose = camera_abs_pose;
-
- bool is_visible = false;
-
- PopulateNoise(&view, &is_visible);
-
- if (!is_visible) {
- return;
- }
-
- // Final visibility check is for whether there are any obstacles blocking or
- // line of sight.
- for (const auto &obstacle : obstacles_) {
- if (obstacle.Intersects({camera_abs_pose, target.pose()})) {
- return;
- }
- }
-
- // At this point, we've passed all the checks to ensure that the target is
- // visible and we can add it to the list of targets.
- views->push_back(view);
-}
-
-} // namespace control_loops
-} // namespace y2019
-
-#endif // Y2019_CONTROL_LOOPS_DRIVETRAIN_CAMERA_H_
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
deleted file mode 100644
index f9b6e8d..0000000
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ /dev/null
@@ -1,213 +0,0 @@
-#include "y2019/control_loops/drivetrain/camera.h"
-
-#include "gtest/gtest.h"
-
-namespace y2019 {
-namespace control_loops {
-namespace testing {
-
-// Check that a Target's basic operations work.
-TEST(TargetTest, BasicTargetTest) {
- Target target({{1, 2, 3}, M_PI / 2.0}, 1.234,
- Target::TargetType::kFaceCargoBay, Target::GoalType::kHatches);
-
- EXPECT_EQ(1.0, target.pose().abs_pos().x());
- EXPECT_EQ(2.0, target.pose().abs_pos().y());
- EXPECT_EQ(3.0, target.pose().abs_pos().z());
- EXPECT_EQ(M_PI / 2.0, target.pose().abs_theta());
- EXPECT_EQ(1.234, target.radius());
- EXPECT_EQ(Target::GoalType::kHatches, target.goal_type());
- EXPECT_EQ(Target::TargetType::kFaceCargoBay, target.target_type());
-
- EXPECT_FALSE(target.occluded());
- target.set_occluded(true);
- EXPECT_TRUE(target.occluded());
-
- ::std::vector<Target::Pose> plot_pts = target.PlotPoints();
- ASSERT_EQ(4, plot_pts.size());
- for (const Target::Pose &pt : plot_pts) {
- EXPECT_EQ(3.0, pt.abs_pos().z());
- EXPECT_EQ(M_PI / 2.0, pt.abs_theta());
- // We don't particularly care about the plot point details, just check that
- // they are all roughly in the right vicinity:
- EXPECT_LT((pt.abs_pos() - target.pose().abs_pos()).norm(), 0.25);
- }
- EXPECT_EQ(plot_pts[0].abs_pos(), plot_pts[3].abs_pos());
-}
-
-typedef TypedCamera</*num_targets=*/3, /*num_obstacles=*/1, double> TestCamera;
-
-class CameraTest : public ::testing::Test {
- public:
- // Set up three targets in a row, at (-1, 0), (0, 0), and (1, 0).
- // Make the right-most target (1, 0) be facing away from the camera, and give
- // the middle target some skew.
- // Place the camera at (0, -5) so the targets are a few meters away.
- // Place one obstacle in a place where it blocks the left-most target (-1, 0).
- CameraTest()
- : targets_{{Target(Target::Pose({-1.0, 0.0, 0.0}, M_PI_2)),
- Target(Target::Pose({0.0, 0.0, kMiddleHeight},
- M_PI_2 + kMiddleSkew)),
- Target(Target::Pose({1.0, 0.0, 0.0}, -M_PI_2))}},
- obstacles_{{TestCamera::LineSegment({{-2.0, -0.5, 0.0}, 0.0},
- {{-0.5, -0.5, 0.0}, 0.0})}},
- base_pose_({0.0, -5.0, 0.0}, M_PI_2),
- camera_({&base_pose_, {0.0, 0.0, 0.0}, 0.0}, M_PI_2, noise_parameters_,
- targets_, obstacles_) {}
-
- protected:
- static constexpr double kMiddleSkew = 0.1;
- static constexpr double kMiddleHeight = 0.5;
- ::std::array<Target, 3> targets_;
- ::std::array<TestCamera::LineSegment, 1> obstacles_;
-
- TestCamera::NoiseParameters noise_parameters_ = {
- .max_viewable_distance = 10.0,
- .heading_noise = 0.03,
- .nominal_distance_noise = 0.06,
- .nominal_skew_noise = 0.1,
- .nominal_height_noise = 0.01};
-
- // Provide base_pose_ as the base for the Pose used in the camera, to make it
- // so that we can easily move the camera around for testing.
- TestCamera::Pose base_pose_;
- TestCamera camera_;
-};
-
-constexpr double CameraTest::kMiddleSkew;
-constexpr double CameraTest::kMiddleHeight;
-
-constexpr double kEps = 1e-15;
-
-// Check that, in the default setup we have, the correct targets are visible in
-// the expected locations.
-TEST_F(CameraTest, BasicCameraTest) {
- const auto views = camera_.target_views();
- // We should only be able to see one target (the middle one).
- ASSERT_EQ(1u, views.size());
- // And, check the actual result for correctness.
- EXPECT_NEAR(0.0, views[0].reading.heading, kEps);
- EXPECT_NEAR(5.0, views[0].reading.distance, kEps);
- EXPECT_NEAR(kMiddleSkew, views[0].reading.skew, kEps);
- EXPECT_NEAR(kMiddleHeight, views[0].reading.height, kEps);
- // Check that the noise outputs are sane; leave other tests to check the exact
- // values of the noise outputs.
- // All noise values should be strictly positive.
- EXPECT_GT(views[0].noise.heading, 0.0);
- EXPECT_GT(views[0].noise.distance, 0.0);
- EXPECT_GT(views[0].noise.skew, 0.0);
- EXPECT_GT(views[0].noise.height, 0.0);
-
- // Check that the PlotPoints for debugging are as expected (should be a single
- // line from the camera to the one visible target).
- const auto plot_pts = camera_.PlotPoints();
- ASSERT_EQ(1u, plot_pts.size());
- ASSERT_EQ(2u, plot_pts[0].size());
- EXPECT_EQ(camera_.pose().abs_pos(), plot_pts[0][0].abs_pos());
- EXPECT_EQ(views[0].target->pose().abs_pos(), plot_pts[0][1].abs_pos());
-}
-
-// Check that occluding the middle target makes it invisible.
-TEST_F(CameraTest, OcclusionTest) {
- auto views = camera_.target_views();
- // We should only be able to see one target (the middle one).
- ASSERT_EQ(1u, views.size());
- targets_[1].set_occluded(true);
- TestCamera occluded_camera(camera_.pose(), camera_.fov(), noise_parameters_,
- targets_, obstacles_);
- views = occluded_camera.target_views();
- // We should no longer see any targets.
- ASSERT_EQ(0u, views.size());
-}
-
-// Checks that targets outside of the field-of-view don't show up.
-TEST_F(CameraTest, FovTest) {
- // Initially, we should still see just the middle target.
- EXPECT_EQ(1u, camera_.target_views().size());
- // Point camera so that the middle target is just barely in its field of view.
- base_pose_.set_theta(3.0 * M_PI / 4.0 - 0.01);
- EXPECT_EQ(1u, camera_.target_views().size());
- // Point camera so that the middle target is just outside of its FoV.
- base_pose_.set_theta(3.0 * M_PI / 4.0 + 0.01);
- EXPECT_EQ(0u, camera_.target_views().size());
- // Check the same things, but on the other edge of the FoV:
- base_pose_.set_theta(M_PI / 4.0 + 0.01);
- EXPECT_EQ(1u, camera_.target_views().size());
- base_pose_.set_theta(M_PI / 4.0 - 0.01);
- EXPECT_EQ(0u, camera_.target_views().size());
-}
-
-// Checks that targets don't show up when very far away.
-TEST_F(CameraTest, FarAwayTest) {
- EXPECT_EQ(1u, camera_.target_views().size());
- // If we move the camera really far away we can't see it any more:
- base_pose_.mutable_pos()->y() = -1000.0;
- EXPECT_EQ(0u, camera_.target_views().size());
-}
-
-// Checks that targets which are highly skewed only show up if we are
-// arbitrarily close.
-TEST_F(CameraTest, HighlySkewedTest) {
- // Skew the target a bunch.
- targets_[1] = Target({{0.0, 0.0, 0.0}, 0.01});
- TestCamera occluded_camera(camera_.pose(), camera_.fov(), noise_parameters_,
- targets_, obstacles_);
- EXPECT_EQ(0u, occluded_camera.target_views().size());
- // But if we get really close we should still see it...
- base_pose_.mutable_pos()->y() = -0.1;
- EXPECT_EQ(1u, camera_.target_views().size());
-}
-
-using Reading = TestCamera::TargetView::Reading;
-
-// Checks that reading noises have the expected characteristics (mostly, going
-// up linearly with distance):
-TEST_F(CameraTest, DistanceNoiseTest) {
- const Reading normal_noise = camera_.target_views()[0].noise;
- // Get twice as close:
- base_pose_.mutable_pos()->y() /= 2.0;
- const Reading closer_noise = camera_.target_views()[0].noise;
- EXPECT_EQ(normal_noise.distance / 2.0, closer_noise.distance);
- EXPECT_EQ(normal_noise.skew / 2.0, closer_noise.skew);
- EXPECT_EQ(normal_noise.height / 2.0, closer_noise.height);
- // Heading reading should be equally good.
- EXPECT_EQ(normal_noise.heading, closer_noise.heading);
-}
-
-class CameraViewParamTest : public CameraTest,
- public ::testing::WithParamInterface<Reading> {};
-
-// Checks that invalid or absurd measurements result in large but finite noises
-// and non-visible targets.
-TEST_P(CameraViewParamTest, InvalidReading) {
- TestCamera::TargetView view;
- view.reading = GetParam();
- bool visible = true;
- camera_.PopulateNoise(&view, &visible);
- // Target should not be visible
- EXPECT_FALSE(visible);
- // We should end up with finite but very large noises when things are invalid.
- EXPECT_TRUE(::std::isfinite(view.noise.heading));
- EXPECT_TRUE(::std::isfinite(view.noise.distance));
- EXPECT_TRUE(::std::isfinite(view.noise.skew));
- EXPECT_TRUE(::std::isfinite(view.noise.height));
- // Don't check heading noise because it is always constant.
- EXPECT_LT(10, view.noise.distance);
- EXPECT_LT(10, view.noise.skew);
- EXPECT_LT(5, view.noise.height);
-}
-
-INSTANTIATE_TEST_CASE_P(
- InvalidMeasurements, CameraViewParamTest,
- ::testing::Values(
- // heading, distance, height, skew
- Reading({100.0, -10.0, -10.0, -3.0}), Reading({0.0, 1.0, 0.0, -3.0}),
- Reading({0.0, 1.0, 0.0, 3.0}), Reading({0.0, 1.0, 0.0, 9.0}),
- Reading({0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0, 0.0}),
- Reading({0.0, ::std::numeric_limits<double>::infinity(), 0.0, 0.0}),
- Reading({0.0, 1.0, 0.0, ::std::numeric_limits<double>::infinity()}),
- Reading({0.0, 1.0, 0.0, ::std::numeric_limits<double>::quiet_NaN()})));
-
-} // namespace testing
-} // namespace control_loops
-} // namespace y2019
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index d716535..1575efb 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -5,7 +5,7 @@
#include <memory>
#include "frc971/control_loops/pose.h"
-#include "y2019/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
namespace y2019 {
@@ -16,9 +16,11 @@
class TypedLocalizer
: public ::frc971::control_loops::drivetrain::HybridEkf<Scalar> {
public:
- typedef TypedCamera<num_targets, num_obstacles, Scalar> Camera;
+ typedef frc971::control_loops::TypedCamera<num_targets, num_obstacles, Scalar>
+ Camera;
typedef typename Camera::TargetView TargetView;
typedef typename Camera::Pose Pose;
+ typedef typename frc971::control_loops::Target Target;
typedef ::frc971::control_loops::drivetrain::HybridEkf<Scalar> HybridEkf;
typedef typename HybridEkf::State State;
typedef typename HybridEkf::StateSquare StateSquare;
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 319065e..aa7f22f 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -30,6 +30,7 @@
kNumTargetsPerFrame, double>
TestLocalizer;
typedef typename TestLocalizer::Camera TestCamera;
+typedef typename TestLocalizer::Target Target;
typedef typename TestCamera::Pose Pose;
typedef typename TestCamera::LineSegment Obstacle;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 4ab65ef..fdc183f 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -4,7 +4,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/drivetrain/camera.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
@@ -24,10 +24,12 @@
: public ::frc971::control_loops::drivetrain::TargetSelectorInterface {
public:
typedef ::frc971::control_loops::TypedPose<double> Pose;
+ typedef frc971::control_loops::Target Target;
// For the virtual camera that we use to identify targets, ignore all
// obstacles and just assume that we have perfect field of view.
- typedef TypedCamera<y2019::constants::Field::kNumTargets,
- /*num_obstacles=*/0, double> FakeCamera;
+ typedef frc971::control_loops::TypedCamera<
+ y2019::constants::Field::kNumTargets,
+ /*num_obstacles=*/0, double> FakeCamera;
TargetSelector(::aos::EventLoop *event_loop);