Make skew in Ekf match skew from cameras
Also, add a few extra comments to the camera code, and add a couple
extra logging statements to the Ekf to help with future debugging.
Change-Id: I5fcfc852a3de42a2c320a3ff724d6d72e01f94ad
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 04e4157..2ab4b4f 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -110,7 +110,8 @@
Scalar distance; // meters
// Height of the target from the camera.
Scalar height; // meters
- // The angle of the target relative to the frame of the camera.
+ // The angle of the target relative to line between the camera and
+ // the center of the target.
Scalar skew; // radians
};
Reading reading;
@@ -281,7 +282,8 @@
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());
+ 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) {
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index d491c14..8c699c5 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -212,7 +212,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate(1e-8);
+ VerifyEstimatorAccurate(1e-7);
}
// Tests that not having cameras with an initial disturbance results in
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 3e20a1e..095c8a9 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -254,6 +254,7 @@
}
if (camera_views.size() == 0) {
+ LOG(DEBUG, "Unable to identify potential target matches.\n");
// If we can't get a match, provide H = zero, which will make this
// correction step a nop.
*h = [](const State &, const Input &) { return Output::Zero(); };
@@ -279,6 +280,10 @@
const TargetView target_view = target_views[ii];
const Scalar match_score = scores(ii, view_idx);
if (match_score > kRejectionScore) {
+ LOG(DEBUG,
+ "Rejecting target at (%f, %f, %f, %f) due to high score.\n",
+ target_view.reading.heading, target_view.reading.distance,
+ target_view.reading.skew, target_view.reading.height);
h_functions->push_back(
[](const State &, const Input &) { return Output::Zero(); });
dhdx_functions->push_back([](const State &) {
@@ -296,8 +301,8 @@
best_view.target->pose().Rebase(&camera.pose());
const Scalar heading = relative_pose.heading();
const Scalar distance = relative_pose.xy_norm();
- const Scalar skew =
- ::aos::math::NormalizeAngle(relative_pose.rel_theta());
+ const Scalar skew = ::aos::math::NormalizeAngle(
+ relative_pose.rel_theta() - heading);
return Output(heading, distance, skew);
});
@@ -333,8 +338,10 @@
Scalar ddistdx = -diffx / distance;
Scalar ddistdy = -diffy / distance;
- // Skew = target.theta - camera.theta:
- Scalar dskewdtheta = -1.0;
+ // Skew = target.theta - camera.theta - heading
+ // = target.theta - arctan2(target_pos - camera_pos)
+ Scalar dskewdx = -dheadingdx;
+ Scalar dskewdy = -dheadingdy;
Eigen::Matrix<Scalar, kNOutputs, kNStates> H;
H.setZero();
H(0, 0) = dheadingdx;
@@ -342,7 +349,8 @@
H(0, 2) = dheadingdtheta;
H(1, 0) = ddistdx;
H(1, 1) = ddistdy;
- H(2, 2) = dskewdtheta;
+ H(2, 0) = dskewdx;
+ H(2, 1) = dskewdy;
return H;
}
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index c8b2d84..1dd6a1a 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -53,17 +53,21 @@
Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
- const double y = extrinsics.y;
- const double z = extrinsics.z;
- const double r1 = extrinsics.r1;
- const double r2 = extrinsics.r2;
+ const double y = extrinsics.y; // height
+ const double z = extrinsics.z; // distance
+ const double r1 = extrinsics.r1; // skew
+ const double r2 = extrinsics.r2; // heading
const double rup = intrinsics.mount_angle;
const double rbarrel = intrinsics.barrel_mount;
const double fl = intrinsics.focal_length;
+ // Start by translating point in target-space to be at correct height.
::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
{
+ // Rotate to compensate for skew angle, to get into a frame still at the
+ // same (x, y) position as the target but rotated to be facing straight
+ // towards the camera.
const double theta = r1;
const double s = sin(theta);
const double c = cos(theta);
@@ -72,9 +76,13 @@
pts.transpose();
}
+ // Translate the coordinate frame to have (x, y) centered at the camera, but
+ // still oriented to be facing along the line from the camera to the target.
pts(2) += z;
{
+ // Rotate out the heading so that the frame is oriented to line up with the
+ // camera's viewpoint in the yaw-axis.
const double theta = r2;
const double s = sin(theta);
const double c = cos(theta);
@@ -85,6 +93,8 @@
// TODO: Apply 15 degree downward rotation.
{
+ // Compensate for rotation in the pitch of the camera up/down to get into
+ // the coordinate frame lined up with the plane of the camera sensor.
const double theta = rup;
const double s = sin(theta);
const double c = cos(theta);
@@ -94,6 +104,10 @@
pts.transpose();
}
+ // Compensate for rotation of the barrel of the camera, i.e. about the axis
+ // that points straight out from the camera lense, using an AngleAxis instead
+ // of manually constructing the rotation matrices because once you get into
+ // this frame you no longer need to be masochistic.
// TODO: Maybe barrel should be extrinsics to allow rocking?
// Also, in this case, barrel should go above the rotation above?
pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
@@ -102,6 +116,8 @@
// TODO: Final image projection.
const ::Eigen::Matrix<double, 1, 3> res = pts;
+ // Finally, scale to account for focal length and translate to get into
+ // pixel-space.
const float scale = fl / res.z();
return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index dea9ba6..d662316 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -46,9 +46,15 @@
struct ExtrinsicParams {
static constexpr size_t kNumParams = 4;
+ // Height of the target
double y = 18.0 * 0.0254;
+ // Distance to the target
double z = 23.0 * 0.0254;
+ // Skew of the target relative to the line-of-sight from the camera to the
+ // target.
double r1 = 1.0 / 180 * M_PI;
+ // Heading from the camera to the target, relative to the center of the view
+ // from the camera.
double r2 = -1.0 / 180 * M_PI;
void set(double *data) {