Skip to content

Commit

Permalink
conversion
Browse files Browse the repository at this point in the history
  • Loading branch information
@christianrauch
christianrauch committed Jun 2, 2024
1 parent 6d910e1 commit 1aa460f
Show file tree
Hide file tree
Showing 4 changed files with 97 additions and 126 deletions.
21 changes: 21 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,18 +61,39 @@ set_property(TARGET tags PROPERTY
)


# # conversion functions
# add_library(conversion OBJECT
# src/conversion.cpp
# )
# target_link_libraries(conversion
# apriltag::apriltag
# Eigen3::Eigen
# opencv_core
# )
# ament_target_dependencies(conversion
# geometry_msgs
# tf2
# )
# set_property(TARGET conversion PROPERTY
# POSITION_INDEPENDENT_CODE ON
# )


# pose estimation methods
add_library(pose_estimation OBJECT
src/pose_estimation.cpp
src/conversion.cpp
)
target_link_libraries(pose_estimation
apriltag::apriltag
Eigen3::Eigen
opencv_core
opencv_calib3d
# conversion
)
ament_target_dependencies(pose_estimation
geometry_msgs
tf2
)
set_property(TARGET pose_estimation PROPERTY
POSITION_INDEPENDENT_CODE ON
Expand Down
75 changes: 49 additions & 26 deletions src/conversion.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,28 @@
#include <tf2/convert.h>
#include <opencv2/core/mat.hpp>

// template<>
// geometry_msgs::msg::Vector3
// tf2::toMsg(const cv::Mat_<double>& vec)
// {
// assert(vec.rows == 3 || vec.cols == 3);

// geometry_msgs::msg::Vector3 t;
// t.x = vec.at<double>(0);
// t.y = vec.at<double>(1);
// t.z = vec.at<double>(2);
// return t;
// }
#include "conversion.hpp"
#include <opencv2/core/quaternion.hpp>


template<>
void tf2::convert(const Eigen::Quaterniond& eq, geometry_msgs::msg::Quaternion& gq)
{
gq.w = eq.w();
gq.x = eq.x();
gq.y = eq.y();
gq.z = eq.z();
}

template<>
void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& t)
{
assert(mat.nrows == 3 || mat.ncols == 3);

t.x = mat.data[0];
t.y = mat.data[1];
t.z = mat.data[2];
}

template<>
void
tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
void tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
{
assert(vec.rows == 3 || vec.cols == 3);

Expand All @@ -25,13 +31,30 @@ tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
t.z = vec.at<double>(2);
}

// template<>
// void
// tf2::toMsg(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
// {
// assert(vec.rows == 3 || vec.cols == 3);
template<>
geometry_msgs::msg::Transform
tf2::toMsg(const apriltag_pose_t& pose)
{
const Eigen::Quaterniond q(Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(pose.R->data));

// t.x = vec.at<double>(0);
// t.y = vec.at<double>(1);
// t.z = vec.at<double>(2);
// }
geometry_msgs::msg::Transform t;
tf2::convert(*pose.t, t.translation);
tf2::convert(q, t.rotation);
return t;
}

template<>
geometry_msgs::msg::Transform
tf2::toMsg(const transrot& pose)
{
// const cv::Quat<double> cvq = cv::Quat<double>::createFromRvec(pose.second);

// convert compact rotation vector to angle-axis to quaternion
const Eigen::Map<const Eigen::Vector3d> rvec((double*) pose.second.data);
const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()});

geometry_msgs::msg::Transform t;
tf2::convert(pose.first, t.translation);
tf2::convert(q, t.rotation);
return t;
}
27 changes: 24 additions & 3 deletions src/conversion.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,27 @@
#include <tf2/convert.h>
#include <Eigen/Geometry>
#include <apriltag/common/matd.h>
#include <apriltag_pose.h>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <opencv2/core/mat.hpp>
#include <tf2/convert.h>


typedef std::pair<cv::Mat_<double>, cv::Mat_<double>> transrot;

template<>
void tf2::convert(const Eigen::Quaterniond& eq, geometry_msgs::msg::Quaternion& gq);

template<>
void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& t);

template<>
void tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t);

template<>
geometry_msgs::msg::Transform
tf2::toMsg(const apriltag_pose_t& pose);

template<>
void
tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t);
geometry_msgs::msg::Transform
tf2::toMsg(const transrot& pose);
100 changes: 3 additions & 97 deletions src/pose_estimation.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,104 +1,10 @@
#include "pose_estimation.hpp"
#include "conversion.hpp"
#include <Eigen/Dense>
#include <apriltag/apriltag_pose.h>
#include <apriltag/common/homography.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/quaternion.hpp>
// #include <tf2_eigen/tf2_eigen.hpp>
#include <tf2/convert.h>
#include "conversion.hpp"


// std::string& operator+ (const int k, std::string& c)
// {
// size_t len = c.length();
// for (size_t i = 0; i < len; i++)
// c[i] += k;
// return c;
// }

// std::string& operator= (const int k)
// {
// size_t len = k;
// for (size_t i = 0; i < len; i++)
// c[i] += k;
// return k;
// }

// template<>
// geometry_msgs::msg::Vector3
// toMsg(const matd_t& t)
// {
// assert(vec.rows == 3 || vec.cols == 3);

// geometry_msgs::msg::Vector3 t;
// t.x = vec.at<double>(0);
// t.y = vec.at<double>(1);
// t.z = vec.at<double>(2);
// return t;
// }



geometry_msgs::msg::Transform
toMsg(const apriltag_pose_t& pose)
{
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 = 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();
t.rotation.z = q.z();

// geometry_msgs::msg::Transform t = tf2::eigenToTransform(const Eigen::Isometry3d & TODO);

return t;
}

geometry_msgs::msg::Transform
toMsg(const cv::Mat_<double>& tvec, const cv::Mat_<double>& rvec)
{
const cv::Quat<double> q = cv::Quat<double>::createFromRvec(rvec);

// convert rotation vector to angle-axis to quaternion
const Eigen::Map<const Eigen::Vector3d> v((double*)rvec.data);
const Eigen::Quaterniond qq({v.norm(), v.normalized()});

geometry_msgs::msg::Transform t;

// t.translation.x = tvec.at<double>(0);
// t.translation.y = tvec.at<double>(1);
// t.translation.z = tvec.at<double>(2);

// t.translation = tf2::toMsg<cv::Mat_<double>, geometry_msgs::msg::Vector3>(tvec);
// tf2::convert<cv::Mat_<double>, geometry_msgs::msg::Vector3>(tvec, t.translation);
tf2::convert(tvec, t.translation);

t.rotation.w = q.w;
t.rotation.x = q.x;
t.rotation.y = q.y;
t.rotation.z = q.z;

return t;
}

// geometry_msgs::msg::Transform tf_from_eigen(const Eigen::Vector3d &t, const Eigen::Quaterniond &q)
// {
// geometry_msgs::msg::Transform tf;

// // tf.translation = tf2::toMsg(t);
// tf2::convert(t, tf.translation);
// // tf2::toMsg(t, tf.translation);
// // tf.rotation = tf2::toMsg(q);
// // tf2::toMsg(q, tf.rotation);

// return tf;
// }

geometry_msgs::msg::Transform
homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
Expand All @@ -108,7 +14,7 @@ homography(apriltag_detection_t* const detection, const std::array<double, 4>& i
apriltag_pose_t pose;
estimate_pose_for_tag_homography(&info, &pose);

return toMsg(pose);
return tf2::toMsg<apriltag_pose_t, geometry_msgs::msg::Transform>(const_cast<const apriltag_pose_t&>(pose));
}

geometry_msgs::msg::Transform
Expand Down Expand Up @@ -137,7 +43,7 @@ pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, do
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);

return toMsg(tvec, rvec);
return tf2::toMsg<transrot, geometry_msgs::msg::Transform>(std::make_pair(tvec, rvec));
}

const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{
Expand Down

0 comments on commit 1aa460f

Please sign in to comment.