Skip to content

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@christianrauch
christianrauch committed Dec 13, 2023
1 parent 9c4b73c commit 8892957
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 30 deletions.
8 changes: 5 additions & 3 deletions src/AprilTagNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
const auto sizes = declare_parameter("tag.sizes", std::vector<double>{}, descr("tag sizes per id", true));

// get method for estimating tag pose from homography
estimate_pose = estim_pose_fun.at(declare_parameter("pose_method", "from_homography", descr("TODO", true)));
estimate_pose = estim_pose_fun.at(declare_parameter("pose_method", "pnp", descr("TODO", true)));

// detector parameters in "detector" namespace
declare_parameter("detector.threads", td->nthreads, descr("number of threads"));
Expand Down Expand Up @@ -166,7 +166,9 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci)
{
// camera intrinsic matrix for rectified images
const Mat3 P = Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(msg_ci->p.data()).leftCols<3>();
// const Mat3 P = Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(msg_ci->p.data()).leftCols<3>();

const std::array<double, 4> intrinsics = {msg_ci->p.data()[0], msg_ci->p.data()[5], msg_ci->p.data()[2], msg_ci->p.data()[6]};

// convert to 8bit monochrome image
const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image;
Expand Down Expand Up @@ -219,7 +221,7 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
// set child frame name by generic tag name or configured tag name
tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size;
tf.transform = estimate_pose(det, P, size);
tf.transform = estimate_pose(det, intrinsics, size);

tfs.push_back(tf);
}
Expand Down
69 changes: 45 additions & 24 deletions src/homography_to_pose.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,21 @@
#include "homography_to_pose.hpp"
#include <apriltag/apriltag_pose.h>
#include <apriltag/common/homography.h>
#include <Eigen/Dense>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
// #include <opencv2/core/eigen.hpp>
#include <opencv2/core/quaternion.hpp>


geometry_msgs::msg::Transform tf_from_eigen(const Eigen::Vector3d& translation, const Mat3& rotation)
geometry_msgs::msg::Transform tf_from_apriltag_pose(const apriltag_pose_t &pose)
{
const Eigen::Quaterniond q(rotation);
const Eigen::Quaterniond q(Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(pose.R->data));

geometry_msgs::msg::Transform t;

t.translation.x = translation.x();
t.translation.y = translation.y();
t.translation.z = translation.z();
t.translation.x = pose.t->data[0];
t.translation.y = pose.t->data[1];
t.translation.z = pose.t->data[2];
t.rotation.w = q.w();
t.rotation.x = q.x();
t.rotation.y = q.y();
Expand Down Expand Up @@ -78,32 +79,48 @@ geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv:
// 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;
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);
estim_pose_f apriltag_homography = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
// apriltag_detection_info_t info;
// info.det = detection;
// info.tagsize = tagsize;
// info.fx = intr[0];
// info.fy = intr[1];
// info.cx = intr[2];
// info.cy = intr[3];

apriltag_detection_info_t info = {
detection,
tagsize,
intr[0],
intr[1],
intr[2],
intr[3]
};

apriltag_pose_t pose;
estimate_pose_for_tag_homography(&info, &pose);

return tf_from_eigen(Eigen::Map<const Eigen::Vector3d>(pose.t->data), Eigen::Map<const Mat3>(pose.R->data));
return tf_from_apriltag_pose(pose);
};

estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const Mat3& P, double tagsize) -> geometry_msgs::msg::Transform {
std::vector<cv::Point3d> objectPoints;
estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize) -> geometry_msgs::msg::Transform {
const double half_tagsize = 0.5 * tagsize;
objectPoints.emplace_back(-half_tagsize, -half_tagsize, 0);
objectPoints.emplace_back(+half_tagsize, -half_tagsize, 0);
objectPoints.emplace_back(+half_tagsize, +half_tagsize, 0);
objectPoints.emplace_back(-half_tagsize, +half_tagsize, 0);
// std::vector<cv::Point3d> objectPoints;
// objectPoints.emplace_back(-half_tagsize, -half_tagsize, 0);
// objectPoints.emplace_back(+half_tagsize, -half_tagsize, 0);
// objectPoints.emplace_back(+half_tagsize, +half_tagsize, 0);
// objectPoints.emplace_back(-half_tagsize, +half_tagsize, 0);

const std::vector<cv::Point3d> objectPoints = {
{-half_tagsize, -half_tagsize, 0},
{+half_tagsize, -half_tagsize, 0},
{+half_tagsize, +half_tagsize, 0},
{-half_tagsize, +half_tagsize, 0}
};

std::vector<cv::Point2d> imagePoints;
double tag_x[4] = {-1, 1, 1, -1};
double tag_y[4] = {1, 1, -1, -1};
constexpr double tag_x[4] = {-1, 1, 1, -1};
constexpr double tag_y[4] = {1, 1, -1, -1};
for(int i = 0; i < 4; i++) {
// Homography projection taking tag local frame coordinates to image pixels
double im_x, im_y;
Expand All @@ -113,7 +130,11 @@ estim_pose_f solve_pnp = [](apriltag_detection_t* const detection, const Mat3& P

cv::Mat rvec, tvec;
cv::Matx33d cameraMatrix;
cv::eigen2cv(P, cameraMatrix);
// cv::eigen2cv(P, cameraMatrix);
cameraMatrix(0, 0) = intr[0]; // fx
cameraMatrix(1, 1) = intr[1]; // fy
cameraMatrix(0, 2) = intr[2]; // cx
cameraMatrix(1, 2) = intr[3]; // cy
// with "SOLVEPNP_IPPE_SQUARE"?
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);
// cv::Matx33d R;
Expand Down
4 changes: 1 addition & 3 deletions src/homography_to_pose.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,10 @@
#pragma once

#include <Eigen/Dense>
#include <apriltag/apriltag.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <unordered_map>


typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Mat3;
typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const Mat3&, double)> estim_pose_f;
typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&)> estim_pose_f;

extern const std::unordered_map<std::string, estim_pose_f> estim_pose_fun;

0 comments on commit 8892957

Please sign in to comment.