CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup code

Browse Source
This commit is contained in:
Oleg Kalachev
2017-11-30 00:41:31 +03:00
parent 48c9e64f19
commit 9a12e295ac
1 changed files with 5 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

217
aruco_pose/src/aruco_pose.cpp
View File

@@ -3,7 +3,6 @@
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
//#include <tf2_ros/transform_broadcaster.h>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
@@ -17,13 +16,9 @@
#include <stdio.h>
#include <tf/transform_broadcaster.h>
// #include <aruco_pose/MarkerArray.h>
// #include <aruco_pose/Marker.h>
namespace aruco_pose {
class ArucoPose : public nodelet::Nodelet {
// tf2_ros::TransformBroadcaster br;
tf::TransformBroadcaster br;
cv::Ptr<cv::aruco::Dictionary> dictionary;
cv::Ptr<cv::aruco::DetectorParameters> parameters;
@@ -37,15 +32,10 @@ class ArucoPose : public nodelet::Nodelet {
virtual void onInit();
void createBoard();
void publishVisualizationMarkers();
cv::Point3f getObjPointsCenter(cv::Mat objPoints);
void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
public:
ArucoPose() {};
virtual ~ArucoPose() {};
};
void ArucoPose::onInit() {
@@ -67,8 +57,6 @@ void ArucoPose::onInit() {
pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
publishVisualizationMarkers();
ROS_INFO("aruco_pose nodelet inited");
}
@@ -152,7 +140,7 @@ void ArucoPose::createBoard()
else if (type == "custom")
{
// Not implemented yet
ROS_FATAL("Custom boards are not implemented yet.")
ROS_FATAL("Custom boards are not implemented yet.");
}
else
{
@@ -160,34 +148,6 @@ void ArucoPose::createBoard()
}
}
void ArucoPose::publishVisualizationMarkers()
{
// Create latched publisher
static auto viz_markers_pub = nh_.advertise<visualization_msgs::MarkerArray>("viz", 1, true);
visualization_msgs::MarkerArray viz;
visualization_msgs::Marker marker;
marker.header.frame_id = "world";
marker.type = visualization_msgs::Marker::CUBE;
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.001;
marker.color.r = 1;
marker.color.g = 1;
marker.color.b = 1;
marker.color.a = 0.9;
marker.frame_locked = true;
viz.markers.push_back(marker);
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.scale.z = 0.3;
marker.color.r = 0;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 0.8;
marker.text = "240";
viz.markers.push_back(marker);
viz_markers_pub.publish(viz);
}
cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
float min_x = std::numeric_limits<float>::max();
float max_x = std::numeric_limits<float>::min();
@@ -217,75 +177,14 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
cv::Mat distCoeffs(8, 1, CV_64F);
parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
// std::cout << "dist " << distCoeffs << " mat " << cameraMatrix;
// std::cout << markerIds.size() << std::endl;
// cv::Vec3d rvec, tvec;
// int valid = cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec);
// std::vector< cv::Vec3d > rvecs, tvecs;
//cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.3362, cameraMatrix, distCoeffs, rvecs, tvecs);
// cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.15, cameraMatrix, distCoeffs, rvecs, tvecs);
// Publish markers
// aruco_pose::MarkerArray markerArray;
// markerArray.header.frame_id = msg->header.frame_id;
// markerArray.header.stamp = msg->header.stamp;
// markerArray.markers.resize(markerIds.size());
// for (int i = 0; i < markerIds.size(); i++) {
// markerArray.markers[i].id = markerIds[i];
// markerArray.markers[i].pose.x = tvect[0];
// markerArray.markers[i].pose.y = tvect[1];
// markerArray.markers[i].pose.z = tvect[2];
// markerArray.markers[i].header.stamp = msg->header.stamp;
// markerArray.markers[i].header.frame_id = msg->header.frame_id;
// }
// marker_pub.publish(markerArray);
/*
for (int i = 0; i < markerIds.size(); i++) {
//if (markerIds[i] == 242) {
if (markerIds[i] == 9) {
tf::Transform transform = aruco2tf(rvecs[i], tvecs[i]);
tf::StampedTransform stampedTransform(transform, msg->header.stamp, msg->header.frame_id, frame_id);
br.sendTransform(stampedTransform);
// geometry_msgs::TransformStamped transformStamped;
// transformStamped.header.stamp = msg->header.stamp;
// transformStamped.header.frame_id = cinfo->header.frame_id;
// transformStamped.child_frame_id = frame_id;
// transformStamped.transform = aruco2tf(rvecs[i], tvecs[i]);
// transformStamped.transform.translation = transformStamped.transform.translation.normalize();
// br.sendTransform(transformStamped);
break;
}
}
*/
// std::cout << "markers: ";
// for (auto const& c : markerIds) std::cout << c << ' ';
// return;
if (markerIds.size() > 0) {
// for (auto const& c : markerCorners) std::cout << c << ' ';
// for (auto const& c : markerIds) std::cout << c << ' ';
// cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
// std::vector< cv::Vec3d > rvecs, tvecs;
// cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.3362, cameraMatrix, distCoeffs, rvecs, tvecs);
// draw axis for each marker
// for(int i=0; i<markerIds.size(); i++)
// cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
cv::Mat rvec, tvec, objPoints;
int valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
rvec, tvec, false, objPoints);
if (valid) {
// Send map transform
tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
br.sendTransform(transform);
@@ -296,7 +195,7 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
ps.pose.orientation.w = 1;
pose_pub.publish(ps);
// Publish reference point
// Send reference point
cv::Point3f ref = getObjPointsCenter(objPoints);
tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
tf::Quaternion q(0, 0, 0);
@@ -308,37 +207,15 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs:
ref_transform.setRotation(q);
br.sendTransform(ref_transform);
// geometry_msgs::TransformStamped transformMsg;
// transform.header.stamp = msg->header.stamp;
// transform.header.frame_id = cinfo->header.frame_id;
// transform.child_frame_id = frame_id;
// transform.transform = aruco2tf(rvec, tvec);
// tf::transformStampedTFToMsg(transform, transformMsg);
// br.sendTransform(transformMsg);
// std::cout << rvec << ";" << tvec << std::endl;
// geometry_msgs::TransformStamped transformStamped;
// transformStamped.header.stamp = msg->header.stamp;
// transformStamped.header.frame_id = cinfo->header.frame_id;
// transformStamped.child_frame_id = frame_id;
// transformStamped.transform.translation.x = tvec[0];
// transformStamped.transform.translation.y = tvec[1];
// transformStamped.transform.translation.z = tvec[1];
// transformStamped.transform.rotation.w = 1;
// br.sendTransform(transformStamped);
if(img_pub.getNumSubscribers() > 0)
{
//show input with augmented information
// for(int i=0; i<markerIds.size(); i++) {
// cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
// }
// Publish debug image
cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3);
cv_bridge::CvImage out_msg;
out_msg.header.frame_id = msg->header.frame_id;
out_msg.header.stamp = msg->header.stamp;
out_msg.encoding = sensor_msgs::image_encodings::BGR8; // sensor_msgs::image_encodings::RGB8;
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = image;
img_pub.publish(out_msg.toImageMsg());
}
@@ -362,14 +239,6 @@ tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
cv::Mat rot;
cv::Rodrigues(rvec, rot);
// rot = rot.t(); // inverse rotation
//tvec = -rot * tvec; // translation of inverse
// camPose is a 4x4 matrix with the pose of the camera in the object frame
// cv::Mat camPose = cv::Mat::eye(4, 4, R.type());
// R.copyTo(camPose.rowRange(0, 3).colRange(0, 3)); // copies R into camPose
// tvec.copyTo(camPose.rowRange(0, 3).colRange(3, 4)); // copies tvec into camPose
tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
@@ -377,82 +246,6 @@ tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
return tf::Transform(tf_rot, tf_orig);
}
/*
tf::Transform ArucoPose::aruco2tf(cv::Vec3d rvec, cv::Vec3d tvec) {
cv::Mat rot(3, 3, CV_64FC1);
// cv::Mat Rvec64;
// rvec.convertTo(rvec, CV_64FC1);
cv::Rodrigues(rvec, rot);
cv::Mat tran64;
// tvec.convertTo(tran64, CV_64FC1);
cv::Mat rotate_to_ros(3, 3, CV_64FC1);
rotate_to_ros.at<float>(0,0) = 1.0;
rotate_to_ros.at<float>(0,1) = 0.0;
rotate_to_ros.at<float>(0,2) = 0.0;
rotate_to_ros.at<float>(1,0) = 0.0;
rotate_to_ros.at<float>(1,1) = -1.0;
rotate_to_ros.at<float>(1,2) = 0.0;
rotate_to_ros.at<float>(2,0) = 0.0;
rotate_to_ros.at<float>(2,1) = 0.0;
rotate_to_ros.at<float>(2,2) = -1.0;
rot = rot*rotate_to_ros.t();
tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
tf::Vector3 tf_orig(tvec[0], tvec[1], tvec[2]);
return tf::Transform(tf_rot, tf_orig);
}
*/
// tf::Transform ArucoPose::aruco2tf(cv::Vec3d rvec, cv::Vec3d tvec) {
// /* Code it based on https://github.com/Sahloul/ar_sys/blob/master/src/utils.cpp#L44 */
// /* TODO: rewrite */
// cv::Mat rot(3, 3, CV_64FC1);
// cv::Rodrigues(rvec, rot);
// cv::Mat rotate_to_sys(3, 3, CV_64FC1);
// /**
// /* Fixed the rotation to meet the ROS system
// /* Doing a basic rotation around X with theta=PI
// /* By Sahloul
// /* See http://en.wikipedia.org/wiki/Rotation_matrix for details
// */
// // 1 0 0
// // 0 -1 0
// // 0 0 -1
// rotate_to_sys.at<float>(0,0) = 1.0;
// rotate_to_sys.at<float>(0,1) = 0.0;
// rotate_to_sys.at<float>(0,2) = 0.0;
// rotate_to_sys.at<float>(1,0) = 0.0;
// rotate_to_sys.at<float>(1,1) = -1.0;
// rotate_to_sys.at<float>(1,2) = 0.0;
// rotate_to_sys.at<float>(2,0) = 0.0;
// rotate_to_sys.at<float>(2,1) = 0.0;
// rotate_to_sys.at<float>(2,2) = -1.0;
// rot = rot * rotate_to_sys.t();
// tf::Matrix3x3 tf_rot(rot.at<float>(0,0), rot.at<float>(0,1), rot.at<float>(0,2),
// rot.at<float>(1,0), rot.at<float>(1,1), rot.at<float>(1,2),
// rot.at<float>(2,0), rot.at<float>(2,1), rot.at<float>(2,2));
// tf::Vector3 tf_orig(tvec[0], tvec[1], tvec[2]);
// tf::Transform tft(tf_rot, tf_orig);
// return tft;
// }
PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
}