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

First version of aruco_pose nodelet

Browse Source
This commit is contained in:
Oleg Kalachev
2017-11-23 05:59:25 +03:00
parent 6b4777063b
commit e975213a13
6 changed files with 827 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

210
aruco_pose/CMakeLists.txt Normal file
View File

@@ -0,0 +1,210 @@
cmake_minimum_required(VERSION 2.8.3)
project(aruco_pose)
add_definitions(-std=c++11 -Wall -g)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
nodelet
pluginlib
roscpp
image_transport
cv_bridge
tf
#tf2
#tf2_ros
#aruco_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
#add_message_files(
# FILES
# Marker.msg
# MarkerArray.msg
#)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
#generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
#)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
LIBRARIES aruco_pose
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}
src/aruco_pose.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/aruco_pose_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
link_directories(/opt/ros/kinetic/lib)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
"/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_aruco_pose.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

5
aruco_pose/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,5 @@
<library path="lib/libaruco_pose">
<class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
<description/>
</class>
</library>

62
aruco_pose/package.xml Normal file
View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<package>
<name>aruco_pose</name>
<version>0.0.0</version>
<description>ArUco maps precise pose estimation nodelet</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/aruco_pose</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
<build_depend>tf</build_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
<!-- Other tools can request additional information be placed here -->
</export>
</package>

409
aruco_pose/src/aruco_pose.cpp Normal file
View File

@@ -0,0 +1,409 @@
#include <algorithm>
#include <nodelet/nodelet.h>
#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>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#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;
cv::Ptr<cv::aruco::GridBoard> board;
std::string frame_id_;
image_transport::CameraSubscriber img_sub;
image_transport::Publisher img_pub;
ros::Publisher marker_pub;
ros::Publisher pose_pub;
ros::NodeHandle nh_, nh_priv_;
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() {
ROS_INFO("Initializing aruco_pose");
nh_ = getNodeHandle();
nh_priv_ = getPrivateNodeHandle();
nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
parameters = cv::aruco::DetectorParameters::create();
createBoard();
image_transport::ImageTransport it(nh_);
img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
image_transport::ImageTransport it_priv(nh_priv_);
img_pub = it_priv.advertise("debug", 1);
pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
publishVisualizationMarkers();
ROS_INFO("aruco_pose nodelet inited");
}
void ArucoPose::createBoard()
{
std::string type;
nh_priv_.param<std::string>("type", type, "gridboard");
if (type == "gridboard")
{
ROS_INFO("Initialize gridboard");
int markers_x, markers_y, first_marker;
float markers_side, markers_sep;
nh_priv_.param<int>("markers_x", markers_x, 10);
nh_priv_.param<int>("markers_y", markers_y, 10);
nh_priv_.param<int>("first_marker", first_marker, 0);
if (!nh_priv_.getParam("markers_side", markers_side))
ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
if (!nh_priv_.getParam("markers_sep", markers_sep))
ROS_ERROR("gridboard: required parameter ~markers_sep is not set.");
board = cv::aruco::GridBoard::create(markers_x, markers_y, markers_side, markers_sep, dictionary, first_marker);
// Publish map image for debugging
cv::Mat map_image;
board->draw( cv::Size(2000, 2000), map_image, 0, 1);
cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
cv_bridge::CvImage map_image_msg;
map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
map_image_msg.image = map_image;
map_image_pub.publish(map_image_msg.toImageMsg());
}
else
{
ROS_ERROR("Incorrect map type '%s'", type.c_str());
}
}
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();
float min_y = min_x, max_y = max_x;
for (int i = 0; i < objPoints.rows; i++) {
max_x = std::max(max_x, objPoints.at<float>(i, 0));
max_y = std::max(max_y, objPoints.at<float>(i, 1));
min_x = std::min(max_x, objPoints.at<float>(i, 0));
min_y = std::min(max_y, objPoints.at<float>(i, 1));
}
cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
return res;
}
#include "fix.cpp"
void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
std::vector<int> markerIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<std::vector<cv::Point2f>> rejectedCandidates;
cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
cv::Mat cameraMatrix(3, 3, CV_64F);
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) {
tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
br.sendTransform(transform);
// Publish map pose
static geometry_msgs::PoseStamped ps;
ps.header.frame_id = frame_id_;
ps.header.stamp = msg->header.stamp;
ps.pose.orientation.w = 1;
pose_pub.publish(ps);
// Publish 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);
static tf::StampedTransform ref_transform;
ref_transform.stamp_ = msg->header.stamp;
ref_transform.frame_id_ = frame_id_;
ref_transform.child_frame_id_ = "aruco_map_reference";
ref_transform.setOrigin(ref_vector3);
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);
// }
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.image = image;
img_pub.publish(out_msg.toImageMsg());
}
}
}
}
void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
}
}
for (int k = 0; k < cinfo->D.size(); k++) {
distCoeffs.at<double>(k) = cinfo->D[k];
}
}
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));
tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
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)
}

61
aruco_pose/src/fix.cpp Normal file
View File

@@ -0,0 +1,61 @@
using namespace cv;
// Temporal fix!
// TODO: remove
// fix strange bug in our OpenCV version
void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
CV_Assert(board->ids.size() == board->objPoints.size());
CV_Assert(detectedIds.total() == detectedCorners.total());
size_t nDetectedMarkers = detectedIds.total();
std::vector< Point3f > objPnts;
objPnts.reserve(nDetectedMarkers);
std::vector< Point2f > imgPnts;
imgPnts.reserve(nDetectedMarkers);
// look for detected markers that belong to the board and get their information
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
int currentId = detectedIds.getMat().ptr< int >(0)[i];
for(unsigned int j = 0; j < board->ids.size(); j++) {
if(currentId == board->ids[j]) {
for(int p = 0; p < 4; p++) {
objPnts.push_back(board->objPoints[j][p]);
imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
}
}
}
}
// create output
Mat(objPnts).copyTo(objPoints);
Mat(imgPnts).copyTo(imgPoints);
}
int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
CV_Assert(_corners.total() == _ids.total());
// get object and image points for the solvePnP function
Mat /*objPoints, */imgPoints;
_getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
CV_Assert(imgPoints.total() == objPoints.total());
if(objPoints.total() == 0) // 0 of the detected markers in board
return 0;
// std::cout << "objPoints: " << objPoints << std::endl;
// std::cout << "imgPoints: " << imgPoints << std::endl;
solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
// divide by four since all the four corners are concatenated in the array for each marker
return (int)objPoints.total() / 4;
}

80
clever/src/aruco_vpe.py Normal file
View File

@@ -0,0 +1,80 @@
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseStamped, PointStamped, Quaternion
import tf2_ros
from tf2_geometry_msgs import do_transform_pose
import tf.transformations
from util import orientation_from_euler, euler_from_orientation
rospy.init_node('aruco_vpe')
LOOKUP_TIMEOUT = rospy.Duration(.1)
CAMERA_FRAME_ID = rospy.get_param('~camera_frame_id', 'bottom_camera_optical')
# TF2 stuff
tf_broadcaster = tf2_ros.TransformBroadcaster()
static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
vision_position_pub = rospy.Publisher('mavros/vision_pose/pose', PoseStamped, queue_size=1)
_vision_position_pub = rospy.Publisher('fake_vision_pose', PoseStamped, queue_size=1)
last_published = None
q = Quaternion()
q.w = 1
ps = PoseStamped()
ps.pose.orientation = q
def send_transform(transform, child_frame_id):
transform.child_frame_id = child_frame_id
tf_broadcaster.sendTransform(transform)
def publish_vpe(pose):
global last_published
stamp = pose.header.stamp
def lookup_transform(target_frame, source_frame):
return tf_buffer.lookup_transform(target_frame, source_frame, stamp, LOOKUP_TIMEOUT)
# Refine aruco_map
reference_in_local_origin = lookup_transform('local_origin', 'aruco_map_reference')
roll, pitch, yaw = euler_from_orientation(reference_in_local_origin.transform.rotation)
reference_in_local_origin.transform.rotation = orientation_from_euler(0, 0, yaw)
send_transform(reference_in_local_origin, 'aruco_map_reference_horiz')
aruco_map_in_reference = lookup_transform('aruco_map_reference', 'aruco_map_raw')
aruco_map_in_reference.header.frame_id = 'aruco_map_reference_horiz'
send_transform(aruco_map_in_reference, 'aruco_map')
# Reset VPE
if last_published is None or stamp - last_published > rospy.Duration(2):
rospy.loginfo('Reset VPE')
aruco_map_in_local_origin = lookup_transform('local_origin', 'aruco_map')
aruco_map_in_local_origin.child_frame_id = 'vpe_origin'
static_tf_broadcaster.sendTransform(aruco_map_in_local_origin)
# Calculate VPE
ps.header.frame_id = 'fcu_horiz'
ps.header.stamp = stamp
vpe_raw = tf_buffer.transform(ps, 'aruco_map', LOOKUP_TIMEOUT)
vpe_raw.header.frame_id = 'vpe_origin'
vpe = tf_buffer.transform(vpe_raw, 'local_origin', LOOKUP_TIMEOUT)
_vision_position_pub.publish(vpe_raw)
vision_position_pub.publish(vpe)
last_published = stamp
rospy.Subscriber('aruco_pose/pose', PoseStamped, publish_vpe, queue_size=1)
rospy.loginfo('aruco_vpe inited')
rospy.spin()