diff --git a/src/examples/maps/map_apriltag_logitech_1234.yaml b/src/examples/maps/map_apriltag_logitech_1234.yaml new file mode 100644 index 0000000000000000000000000000000000000000..850c04adf9e76e6588cae936acb5086d4caf51e0 --- /dev/null +++ b/src/examples/maps/map_apriltag_logitech_1234.yaml @@ -0,0 +1,46 @@ +map name: "4 tags printed on a A4 sheet vertical recorded at IRI with logitech webcam." + +nlandmarks: 4 # This must match the number of landmarks in the list that follows. Otherwise it is an error. + +###################### +# World frame is considered to be at 75cm from the sheet with RDF convention (like a camera). +# Landmarks facing the Z axis (view axis of the camera) have the same orientation as world frame. +###################### +landmarks: + + - id : 0 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 0 + tag width: 0.055 + position: [-0.061, -0.185, 0.75] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 1 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 1 + tag width: 0.055 + position: [0.061, -0.185, 0.75] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 2 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 2 + tag width: 0.055 + position: [-0.061, 0, 0.75] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + + - id : 3 # use same as tag id + type: "APRILTAG" # This must match the KEY used in the LandmarkFactory. Otherwise it is an error. + tag id: 3 + tag width: 0.055 + position: [0.061, 0, 0.75] + orientation: [0, 0, 0] # roll pitch yaw in degrees + position fixed: true + orientation fixed: true + diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp index 7366b72586df604d5d96fffeba5e1afcf3f3c5e9..b6081b0843aa601bce2ce4862aefdf93a7f43d95 100644 --- a/src/examples/test_apriltag.cpp +++ b/src/examples/test_apriltag.cpp @@ -59,14 +59,17 @@ int main(int argc, char *argv[]) SensorCameraPtr sen_cam = std::static_pointer_cast<SensorCamera>(sen); ProcessorBasePtr prc = problem->installProcessor("TRACKER LANDMARK APRILTAG", "apriltags", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml"); + std::cout << wolf_root + "/src/examples/maps/map_apriltag_logitech.yaml" << std::endl; + problem->loadMap(wolf_root + "/src/examples/maps/map_apriltag_logitech_1234.yaml"); + // set prior Eigen::Matrix6s covariance = Matrix6s::Identity(); Scalar stdev_m = 0.00001; // standard deviation on original translation Scalar stdev_deg = 0.00001; // standard deviation on original rotation covariance.topLeftCorner(3,3) = stdev_m*stdev_m * covariance.topLeftCorner(3,3); covariance.bottomRightCorner(3,3) = (M_TORAD*stdev_deg)*(M_TORAD*stdev_deg) * covariance.bottomRightCorner(3,3); - FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1); + FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1); // first argument is the name of the program. // following arguments are path to image (from wolf_root)