only keep two options

christianrauch · Dec 13, 2023 · 9c4b73c · 9c4b73c
1 parent 7bcd355
commit 9c4b73c
Showing 1 changed file with 41 additions and 41 deletions.
diff --git a/src/homography_to_pose.cpp b/src/homography_to_pose.cpp
@@ -40,43 +40,43 @@ geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv:
     return t;
 }
 
-estim_pose_f from_homography = [](const apriltag_detection_t* const detection, const Mat3& P, double size) -> geometry_msgs::msg::Transform {
-    // compute extrinsic camera parameter
-    // https://dsp.stackexchange.com/a/2737/31703
-    // H = K * T  =>  T = K^(-1) * H
-    const Mat3 T = P.inverse() * Eigen::Map<const Mat3>(detection->H->data);
-    Mat3 R;
-    R.col(0) = T.col(0).normalized();
-    R.col(1) = T.col(1).normalized();
-    R.col(2) = R.col(0).cross(R.col(1));
-
-    // rotate by half rotation about x-axis to have z-axis
-    // point upwards orthogonal to the tag plane
-    R.col(1) *= -1;
-    R.col(2) *= -1;
-
-    // the corner coordinates of the tag in the canonical frame are (+/-1, +/-1)
-    // hence the scale is half of the edge size
-    const Eigen::Vector3d tt = T.rightCols<1>() / ((T.col(0).norm() + T.col(0).norm()) / 2.0) * (size / 2.0);
-
-    return tf_from_eigen(tt, R);
-};
-
-estim_pose_f apriltag_orthogonal_iteration = [](apriltag_detection_t* const detection, const Mat3& P, double tagsize) -> geometry_msgs::msg::Transform {
-    // https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#pose-estimation
-    apriltag_detection_info_t info;
-    info.det = detection;
-    info.tagsize = tagsize;
-    info.fx = P(0, 0);
-    info.fy = P(1, 1);
-    info.cx = P(0, 1);
-    info.cy = P(1, 0);
-
-    apriltag_pose_t pose;
-    estimate_tag_pose(&info, &pose);
-
-    return tf_from_eigen(Eigen::Map<const Eigen::Vector3d>(pose.t->data), Eigen::Map<const Mat3>(pose.R->data));
-};
+// estim_pose_f from_homography = [](const apriltag_detection_t* const detection, const Mat3& P, double size) -> geometry_msgs::msg::Transform {
+//     // compute extrinsic camera parameter
+//     // https://dsp.stackexchange.com/a/2737/31703
+//     // H = K * T  =>  T = K^(-1) * H
+//     const Mat3 T = P.inverse() * Eigen::Map<const Mat3>(detection->H->data);
+//     Mat3 R;
+//     R.col(0) = T.col(0).normalized();
+//     R.col(1) = T.col(1).normalized();
+//     R.col(2) = R.col(0).cross(R.col(1));
+
+//     // rotate by half rotation about x-axis to have z-axis
+//     // point upwards orthogonal to the tag plane
+//     R.col(1) *= -1;
+//     R.col(2) *= -1;
+
+//     // the corner coordinates of the tag in the canonical frame are (+/-1, +/-1)
+//     // hence the scale is half of the edge size
+//     const Eigen::Vector3d tt = T.rightCols<1>() / ((T.col(0).norm() + T.col(0).norm()) / 2.0) * (size / 2.0);
+
+//     return tf_from_eigen(tt, R);
+// };
+
+// estim_pose_f apriltag_orthogonal_iteration = [](apriltag_detection_t* const detection, const Mat3& P, double tagsize) -> geometry_msgs::msg::Transform {
+//     // https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#pose-estimation
+//     apriltag_detection_info_t info;
+//     info.det = detection;
+//     info.tagsize = tagsize;
+//     info.fx = P(0, 0);
+//     info.fy = P(1, 1);
+//     info.cx = P(0, 1);
+//     info.cy = P(1, 0);
+
+//     apriltag_pose_t pose;
+//     estimate_tag_pose(&info, &pose);
+
+//     return tf_from_eigen(Eigen::Map<const Eigen::Vector3d>(pose.t->data), Eigen::Map<const Mat3>(pose.R->data));
+// };
 
 estim_pose_f apriltag_homography = [](apriltag_detection_t* const detection, const Mat3& P, double tagsize) -> geometry_msgs::msg::Transform {
     apriltag_detection_info_t info;
@@ -123,8 +123,8 @@ estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const Mat3& P
 };
 
 const std::unordered_map<std::string, estim_pose_f> estim_pose_fun{
-    {"from_homography", from_homography},
-    {"apriltag_orthogonal_iteration", apriltag_orthogonal_iteration},
-    {"apriltag_homography", apriltag_homography},
-    {"solve_pnp", solve_pnp},
+    // {"from_homography", from_homography},
+    // {"apriltag_orthogonal_iteration", apriltag_orthogonal_iteration},
+    {"homography", apriltag_homography},
+    {"pnp", solve_pnp},
 };