pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

somehow managed to implement rotation cleanly

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-29 17:31:52 +01:00
parent b6e7ab8aa4
commit b95417293b
4 changed files with 77 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/aruco_detector.cpp
View File

@@ -10,6 +10,7 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>
#include <teleoperation/utils.hpp>
static const std::string ARUCO_WINDOW = "Aruco window";
@@ -25,9 +26,6 @@ class ImageConverter {
ImageConverter() :
it_(nh_)
{
ros::Rate loop_rate(0.1);
loop_rate.sleep();
// Subscribe to input video feed
image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
&ImageConverter::imageCb, this);
@@ -75,39 +73,48 @@ class ImageConverter {
// Draw and print aruco
cv::Mat aruco_demo = cv_ptr->image.clone();
for (int i = 0; i < markers.size(); i++) {
markers[i].draw(aruco_demo, cv::Scalar(0, 0, 255), 2);
float x = -markers[i].Tvec.at<float>(2);
float y = markers[i].Tvec.at<float>(0);
float z = -markers[i].Tvec.at<float>(1);
// cv::Mat rot_mat;
// cv::Rodrigues(markers[i].Rvec, rot_mat);
// float roll = atan2(
// rot_mat.at<float>(2,1),
// rot_mat.at<float>(2,2));
// float pitch = -atan2(
// rot_mat.at<float>(2,0),
// sqrt(
// pow(rot_mat.at<float>(2,1),2) +
// pow(rot_mat.at<float>(2,2),2)));
// float yaw = atan2(
// rot_mat.at<float>(1,0),
// rot_mat.at<float>(0,0));
markers[i].draw(aruco_demo, cv::Scalar(0, 0, 255), 2);
cv::Mat rot_cv;
cv::Rodrigues(markers[i].Rvec, rot_cv);
tf::Matrix3x3 rot(
rot_cv.at<float>(0, 0),
rot_cv.at<float>(0, 1),
rot_cv.at<float>(0, 2),
rot_cv.at<float>(1, 0),
rot_cv.at<float>(1, 1),
rot_cv.at<float>(1, 2),
rot_cv.at<float>(2, 0),
rot_cv.at<float>(2, 1),
rot_cv.at<float>(2, 2)
);
tf::Vector3 trans(
markers[i].Tvec.at<float>(0),
markers[i].Tvec.at<float>(1),
markers[i].Tvec.at<float>(2)
);
tf::Matrix3x3 hmat(
0, 0, -1,
1, 0, 0,
0, -1, 0
);
tf::Matrix3x3 another(
0, 0, -1,
1, 0, 0,
0, -1, 0
);
rot = hmat * rot * another; // This all somehow works as expected
trans = hmat * trans;
tf::Transform aruco_tf(rot, trans);
std::string id = "Aruco_";
// int to str basically
std::ostringstream ss;
ss << markers[i].id;
id += ss.str();
id += stuff_to_str(markers[i].id);
id += "_frame";
tf::Transform aruco_tf;
aruco_tf.setIdentity();
// tf::Quaternion q;
// q.setRPY(roll, pitch, yaw);
aruco_tf.setOrigin(tf::Vector3(x, y, z));
// aruco_tf.setRotation(q);
this->aruco_pub.sendTransform(tf::StampedTransform(
aruco_tf, ros::Time::now(), "odom",
id.c_str()));