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()