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_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};