Add more accessors to Pose/Camera.
I needed some extra accessors to make writing other code easier. Also,
having default constructors makes life easier.
Change-Id: Iccc9e361ce82c4494563b499985d72759ca323d4
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 508a6a6..13bd6e6 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -1,6 +1,8 @@
#ifndef FRC971_CONTROL_LOOPS_POSE_H_
#define FRC971_CONTROL_LOOPS_POSE_H_
+#include <vector>
+
#include "Eigen/Dense"
#include "aos/util/math.h"
@@ -39,6 +41,9 @@
// The type that contains the translational (x, y, z) component of the Pose.
typedef Eigen::Matrix<Scalar, 3, 1> Pos;
+ // Provide a default constructor that crease a pose at the origin.
+ TypedPose() : TypedPose({0.0, 0.0, 0.0}, 0.0) {}
+
// Construct a Pose in the absolute frame with a particular position and yaw.
TypedPose(const Pos &abs_pos, Scalar theta) : pos_(abs_pos), theta_(theta) {}
// Construct a Pose relative to another Pose (base).
@@ -69,9 +74,12 @@
// Provide access to the position and yaw relative to the base Pose.
Pos rel_pos() const { return pos_; }
Scalar rel_theta() const { return theta_; }
+ const TypedPose<Scalar> *base() const { return base_; }
Pos *mutable_pos() { return &pos_; }
void set_theta(Scalar theta) { theta_ = theta; }
+ // Swap out the base Pose, keeping the current relative position/angle.
+ void set_base(const TypedPose<Scalar> *new_base) { base_ = new_base; }
// For 2-D calculation, provide the heading, which is distinct from the
// yaw/theta value. heading is the heading relative to the base Pose if you
@@ -137,6 +145,7 @@
template <typename Scalar = double>
class TypedLineSegment {
public:
+ TypedLineSegment() {}
TypedLineSegment(const TypedPose<Scalar> &pose1,
const TypedPose<Scalar> &pose2)
: pose1_(pose1), pose2_(pose2) {}
@@ -162,9 +171,18 @@
(::aos::math::PointsAreCCW<Scalar>(p1, p2, q1) !=
::aos::math::PointsAreCCW<Scalar>(p1, p2, q2));
}
+
+ TypedPose<Scalar> pose1() const { return pose1_; }
+ TypedPose<Scalar> pose2() const { return pose2_; }
+ TypedPose<Scalar> *mutable_pose1() { return &pose1_; }
+ TypedPose<Scalar> *mutable_pose2() { return &pose2_; }
+
+ ::std::vector<TypedPose<Scalar>> PlotPoints() const {
+ return {pose1_, pose2_};
+ }
private:
- const TypedPose<Scalar> pose1_;
- const TypedPose<Scalar> pose2_;
+ TypedPose<Scalar> pose1_;
+ TypedPose<Scalar> pose2_;
}; // class TypedLineSegment
typedef TypedLineSegment<double> LineSegment;
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
index cb994a2..4fd3f15 100644
--- a/frc971/control_loops/pose_test.cc
+++ b/frc971/control_loops/pose_test.cc
@@ -23,6 +23,8 @@
EXPECT_EQ(1.0, pose.abs_pos().x());
EXPECT_EQ(1.0, pose.abs_pos().y());
+ EXPECT_EQ(1.0, pose.abs_xy().x());
+ EXPECT_EQ(1.0, pose.abs_xy().y());
EXPECT_EQ(0.5, pose.abs_pos().z());
EXPECT_EQ(0.5, pose.rel_theta());
@@ -32,6 +34,11 @@
EXPECT_EQ(3.14, pose.rel_theta());
pose.mutable_pos()->x() = 9.71;
EXPECT_EQ(9.71, pose.rel_pos().x());
+
+ EXPECT_EQ(nullptr, pose.base());
+ Pose new_base;
+ pose.set_base(&new_base);
+ EXPECT_EQ(&new_base, pose.base());
}
// Check that Poses behave as expected when constructed relative to another
@@ -76,6 +83,22 @@
EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
}
+// Tests that basic accessors for LineSegment behave as expected.
+TEST(LineSegmentTest, BasicAccessorTest) {
+ LineSegment l;
+ EXPECT_EQ(0.0, l.pose1().rel_theta());
+ l.mutable_pose1()->set_theta(1.234);
+ EXPECT_EQ(1.234, l.pose1().rel_theta());
+ EXPECT_EQ(0.0, l.pose2().rel_theta());
+ l.mutable_pose2()->set_theta(5.678);
+ EXPECT_EQ(5.678, l.pose2().rel_theta());
+
+ const ::std::vector<Pose> plot_pts = l.PlotPoints();
+ ASSERT_EQ(2u, plot_pts.size());
+ EXPECT_EQ(l.pose1().rel_theta(), plot_pts[0].rel_theta());
+ EXPECT_EQ(l.pose2().rel_theta(), plot_pts[1].rel_theta());
+}
+
// Tests that basic checks for intersection function as expected.
TEST(LineSegmentTest, TrivialIntersectTest) {
Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 7135e8b..ff53b7b 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -35,6 +35,7 @@
public:
typedef ::frc971::control_loops::TypedPose<Scalar> Pose;
TypedTarget(const Pose &pose) : pose_(pose) {}
+ TypedTarget() {}
Pose pose() const { return pose_; }
bool occluded() const { return occluded_; }
@@ -117,6 +118,8 @@
// The target that this view corresponds to.
const TypedTarget<Scalar> *target;
+ // The Pose the camera was at when viewing the target:
+ Pose camera_pose;
};
// Important parameters for dealing with camera noise calculations.
@@ -172,11 +175,12 @@
// 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, &views);
+ AddTargetIfVisible(target, camera_abs_pose, &views);
}
return views;
}
@@ -189,12 +193,12 @@
::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()});
+ list_of_lists.push_back({pose_, view.target->pose()});
}
return list_of_lists;
}
- const Pose pose() const { return pose_; }
+ const Pose &pose() const { return pose_; }
Scalar fov() const { return fov_; }
private:
@@ -202,11 +206,12 @@
// If the specified target is visible from the current camera Pose, adds it to
// the views array.
void AddTargetIfVisible(
- const TypedTarget<Scalar> &target,
+ const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
::aos::SizedArray<TargetView, num_targets> *views) const;
// The Pose of this camera.
const Pose pose_;
+
// Field of view of the camera, in radians.
const Scalar fov_;
@@ -215,6 +220,8 @@
const 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.
const ::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
@@ -225,7 +232,7 @@
template <int num_targets, int num_obstacles, typename Scalar>
void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
- const TypedTarget<Scalar> &target,
+ const TypedTarget<Scalar> &target, const Pose &camera_abs_pose,
::aos::SizedArray<TargetView, num_targets> *views) const {
if (target.occluded()) {
return;
@@ -233,8 +240,6 @@
// Precompute the current absolute pose of the camera, because we will reuse
// it a bunch.
- const Pose camera_abs_pose = pose_.Rebase(nullptr);
-
const Pose relative_pose = target.pose().Rebase(&camera_abs_pose);
const Scalar heading = relative_pose.heading();
const Scalar distance = relative_pose.xy_norm();
@@ -280,6 +285,7 @@
view.noise = {noise_parameters_.heading_noise, distance_noise, height_noise,
skew_noise};
view.target = ⌖
+ view.camera_pose = camera_abs_pose;
views->push_back(view);
}
diff --git a/y2019/control_loops/drivetrain/camera_test.cc b/y2019/control_loops/drivetrain/camera_test.cc
index 9a73ccf..ce1a2a4 100644
--- a/y2019/control_loops/drivetrain/camera_test.cc
+++ b/y2019/control_loops/drivetrain/camera_test.cc
@@ -93,6 +93,14 @@
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.