Delete pose2.t only if a second solution has been computed. (#23)

AprilRobotics · Apr 8, 2019 · b3949d2 · b3949d2
1 parent a5ab81d
commit b3949d2
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/apriltag_pose.c b/apriltag_pose.c
@@ -517,8 +517,10 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose)
     if (err1 <= err2) {
         pose->R = pose1.R;
         pose->t = pose1.t;
+        if (pose2.R) {
+            matd_destroy(pose2.t);
+        }
         matd_destroy(pose2.R);
-        matd_destroy(pose2.t);
         return err1;
     } else {
         pose->R = pose2.R;