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

started with imitation

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-24 17:48:16 +01:00
parent 99bfaa924c
commit fd36b56631
6 changed files with 86 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
src/aruco_detector.cpp
View File

@@ -11,7 +11,7 @@
#include <opencv2/video/tracking.hpp>
#include <aruco/aruco.h>
static const std::string OPENCV_WINDOW = "Original image";
// static const std::string OPENCV_WINDOW = "Original image";
static const std::string ARUCO_WINDOW = "Aruco window";
class ImageConverter {
@@ -31,13 +31,13 @@ class ImageConverter {
&ImageConverter::imageCb, this);
// Create output windows
cv::namedWindow(OPENCV_WINDOW);
// cv::namedWindow(OPENCV_WINDOW);
cv::namedWindow(ARUCO_WINDOW);
}
~ImageConverter() {
// Clean Up
cv::destroyWindow(OPENCV_WINDOW);
// cv::destroyWindow(OPENCV_WINDOW);
cv::destroyWindow(ARUCO_WINDOW);
}
@@ -56,18 +56,18 @@ class ImageConverter {
}
// Output Original Image (bcs y not)
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
// cv::imshow(OPENCV_WINDOW, cv_ptr->image);
// Do Aruco
cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
274.139508945831, 0.0, 141.184472810944,
0.0, 275.741846757374, 106.693773654172,
690., 0.0, 320,
0.0, 690., 225,
0.0, 0.0, 1.0);
cv::Mat distortion = (cv::Mat_<double>(4, 1) <<
-0.0870160932911717,
0.128210165050533,
0.003379500659424,
-0.00106205540818586);
-0.05,
0,
0,
-0.0015);
aruco::CameraParameters cam(cam_mat, distortion, cv_ptr->image.size());
aruco::MarkerDetector detector;
std::vector<aruco::Marker> markers;
@@ -78,9 +78,9 @@ class ImageConverter {
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>(0);
float y = markers[i].Tvec.at<float>(1);
float z = markers[i].Tvec.at<float>(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);
@@ -111,7 +111,7 @@ class ImageConverter {
aruco_tf.setOrigin(tf::Vector3(x, y, z));
// aruco_tf.setRotation(q);
this->aruco_pub.sendTransform(tf::StampedTransform(
aruco_tf, ros::Time::now(), "CameraTop_optical_frame",
aruco_tf, ros::Time::now(), "odom",
id.c_str()));
}
cv::imshow(ARUCO_WINDOW, aruco_demo);