From e975213a13bf8cb03355ad62d1aaeb2668ee0cb4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 23 Nov 2017 05:59:25 +0300 Subject: [PATCH] First version of aruco_pose nodelet --- aruco_pose/CMakeLists.txt | 210 +++++++++++++++++ aruco_pose/nodelet_plugins.xml | 5 + aruco_pose/package.xml | 62 +++++ aruco_pose/src/aruco_pose.cpp | 409 +++++++++++++++++++++++++++++++++ aruco_pose/src/fix.cpp | 61 +++++ clever/src/aruco_vpe.py | 80 +++++++ 6 files changed, 827 insertions(+) create mode 100644 aruco_pose/CMakeLists.txt create mode 100644 aruco_pose/nodelet_plugins.xml create mode 100644 aruco_pose/package.xml create mode 100644 aruco_pose/src/aruco_pose.cpp create mode 100644 aruco_pose/src/fix.cpp create mode 100644 clever/src/aruco_vpe.py diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt new file mode 100644 index 00000000..90403cf5 --- /dev/null +++ b/aruco_pose/CMakeLists.txt @@ -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) diff --git a/aruco_pose/nodelet_plugins.xml b/aruco_pose/nodelet_plugins.xml new file mode 100644 index 00000000..79d5ddad --- /dev/null +++ b/aruco_pose/nodelet_plugins.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml new file mode 100644 index 00000000..b4da81c3 --- /dev/null +++ b/aruco_pose/package.xml @@ -0,0 +1,62 @@ + + + aruco_pose + 0.0.0 + ArUco maps precise pose estimation nodelet + + + + + Oleg Kalachev + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + nodelet + roscpp + image_transport + cv_bridge + tf + + nodelet + roscpp + image_transport + cv_bridge + tf + + + + + + + + diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp new file mode 100644 index 00000000..b68c1e08 --- /dev/null +++ b/aruco_pose/src/aruco_pose.cpp @@ -0,0 +1,409 @@ +#include +#include +#include +#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #include +// #include + +namespace aruco_pose { + +class ArucoPose : public nodelet::Nodelet { +// tf2_ros::TransformBroadcaster br; + tf::TransformBroadcaster br; + cv::Ptr dictionary; + cv::Ptr parameters; + cv::Ptr 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("pose", 1); + + publishVisualizationMarkers(); + + ROS_INFO("aruco_pose nodelet inited"); +} + +void ArucoPose::createBoard() +{ + std::string type; + nh_priv_.param("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("markers_x", markers_x, 10); + nh_priv_.param("markers_y", markers_y, 10); + nh_priv_.param("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("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("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::max(); + float max_x = std::numeric_limits::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(i, 0)); + max_y = std::max(max_y, objPoints.at(i, 1)); + min_x = std::min(max_x, objPoints.at(i, 0)); + min_y = std::min(max_y, objPoints.at(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 markerIds; + std::vector> markerCorners; + std::vector> 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; iheader.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; iheader.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(i, j) = cinfo->K[3 * i + j]; + } + } + for (int k = 0; k < cinfo->D.size(); k++) { + distCoeffs.at(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(0,0), rot.at(0,1), rot.at(0,2), + rot.at(1,0), rot.at(1,1), rot.at(1,2), + rot.at(2,0), rot.at(2,1), rot.at(2,2)); + tf::Vector3 tf_orig(tvec.at(0,0), tvec.at(1,0), tvec.at(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(0,0) = 1.0; + rotate_to_ros.at(0,1) = 0.0; + rotate_to_ros.at(0,2) = 0.0; + rotate_to_ros.at(1,0) = 0.0; + rotate_to_ros.at(1,1) = -1.0; + rotate_to_ros.at(1,2) = 0.0; + rotate_to_ros.at(2,0) = 0.0; + rotate_to_ros.at(2,1) = 0.0; + rotate_to_ros.at(2,2) = -1.0; + + rot = rot*rotate_to_ros.t(); + + tf::Matrix3x3 tf_rot(rot.at(0,0), rot.at(0,1), rot.at(0,2), + rot.at(1,0), rot.at(1,1), rot.at(1,2), + rot.at(2,0), rot.at(2,1), rot.at(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(0,0) = 1.0; +// rotate_to_sys.at(0,1) = 0.0; +// rotate_to_sys.at(0,2) = 0.0; +// rotate_to_sys.at(1,0) = 0.0; +// rotate_to_sys.at(1,1) = -1.0; +// rotate_to_sys.at(1,2) = 0.0; +// rotate_to_sys.at(2,0) = 0.0; +// rotate_to_sys.at(2,1) = 0.0; +// rotate_to_sys.at(2,2) = -1.0; + +// rot = rot * rotate_to_sys.t(); + + +// tf::Matrix3x3 tf_rot(rot.at(0,0), rot.at(0,1), rot.at(0,2), +// rot.at(1,0), rot.at(1,1), rot.at(1,2), +// rot.at(2,0), rot.at(2,1), rot.at(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) + +} + diff --git a/aruco_pose/src/fix.cpp b/aruco_pose/src/fix.cpp new file mode 100644 index 00000000..294ff569 --- /dev/null +++ b/aruco_pose/src/fix.cpp @@ -0,0 +1,61 @@ +using namespace cv; + +// Temporal fix! +// TODO: remove +// fix strange bug in our OpenCV version + +void _getBoardObjectAndImagePoints(const Ptr &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 &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; +} diff --git a/clever/src/aruco_vpe.py b/clever/src/aruco_vpe.py new file mode 100644 index 00000000..198bb846 --- /dev/null +++ b/clever/src/aruco_vpe.py @@ -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()