Add option to map without robot position
We currently don't have a reliable way of getting the box of pis
position (need to put down estimator on IMU pi, or come up with a
different solution). For testing, we will just assume that two
images captured within a short amount of time have no robot motion
between them.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Iff86e22491411ad2593436b5395f25f401d11367
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index b4b1a87..ab4ee6d 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -163,6 +163,41 @@
return {target_constraints, robot_delta_poses};
}
+std::vector<ceres::examples::Constraint2d> DataAdapter::MatchTargetDetections(
+ const std::vector<DataAdapter::TimestampedDetection>
+ ×tamped_target_detections,
+ aos::distributed_clock::duration max_dt) {
+ CHECK_GE(timestamped_target_detections.size(), 2ul)
+ << "Must have at least 2 detections";
+
+ // Match consecutive detections
+ std::vector<ceres::examples::Constraint2d> target_constraints;
+ for (auto it = timestamped_target_detections.begin() + 1;
+ it < timestamped_target_detections.end(); it++) {
+ auto last_detection = *(it - 1);
+
+ // Skip two consecutive detections of the same target, because the solver
+ // doesn't allow this
+ if (it->id == last_detection.id) {
+ continue;
+ }
+
+ // Don't take into account constraints too far apart in time, because the
+ // recording device could have moved too much
+ if ((it->time - last_detection.time) > max_dt) {
+ continue;
+ }
+
+ // TODO(milind): better way to compute confidence since these detections are
+ // likely very close in time together
+ auto confidence = ComputeConfidence(last_detection.time, it->time);
+ target_constraints.emplace_back(
+ ComputeTargetConstraint(last_detection, *it, confidence));
+ }
+
+ return target_constraints;
+}
+
Eigen::Matrix3d DataAdapter::ComputeConfidence(
aos::distributed_clock::time_point start,
aos::distributed_clock::time_point end) {
@@ -217,6 +252,15 @@
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);
+}
+
TargetMapper::TargetMapper(
std::string_view target_poses_path,
std::vector<ceres::examples::Constraint2d> target_constraints)
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 4dbe52b..8f83278 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -116,6 +116,16 @@
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(
+ const std::vector<TimestampedDetection> ×tamped_target_detections,
+ aos::distributed_clock::duration max_dt = std::chrono::milliseconds(5));
+
// 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"
@@ -137,6 +147,12 @@
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(
+ const TimestampedDetection &target_detection_start,
+ const TimestampedDetection &target_detection_end,
+ const Eigen::Matrix3d &confidence);
};
} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index f56cd8d..6b48c81 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -94,7 +94,7 @@
} // namespace
-TEST(DataAdapterTest, Interpolation) {
+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}},
@@ -198,6 +198,50 @@
EXPECT_EQ(target_constraints[4].information, confidence_11ms);
}
+TEST(DataAdapterTest, MatchTargetDetectionsWithoutRobotPosition) {
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
+ {{TimeInMs(5),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -5.0, 0.0}),
+ 2},
+ {TimeInMs(6),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, M_PI}),
+ 0},
+ {TimeInMs(10),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, -3.0, M_PI}),
+ 1},
+ {TimeInMs(13),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{4.0, -7.0, M_PI_2}),
+ 2},
+ {TimeInMs(14),
+ PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{4.0, -4.0, M_PI_2}),
+ 2}};
+
+ constexpr auto kMaxDt = std::chrono::milliseconds(3);
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
+
+ // The constraint between the detection at 6ms and the one at 10 ms is skipped
+ // because dt > kMaxDt.
+ // Also, the constraint between the last two detections is skipped because
+ // they are the same target
+ EXPECT_EQ(target_constraints.size(),
+ 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_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_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};