aruco_map.cpp

Go to the documentation of this file.

/*
 * Detecting and pose estimation of ArUco markers maps
 * Copyright (C) 2018 Copter Express Technologies
 *
 * Author: Oleg Kalachev <okalachev@gmail.com>
 *
 * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 */

/*
 * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
 * under the BSD license.
 */

#include <math.h>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <aruco_pose/MarkerArray.h>
#include <aruco_pose/Marker.h>

#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>

#include "draw.h"
#include "utils.h"

using std::vector;
using cv::Mat;
using sensor_msgs::Image;
using sensor_msgs::CameraInfo;
using aruco_pose::MarkerArray;

typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray> SyncPolicy;

class ArucoMap : public nodelet::Nodelet {
private:
        ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
        image_transport::Publisher debug_pub_;
        message_filters::Subscriber<Image> image_sub_;
        message_filters::Subscriber<CameraInfo> info_sub_;
        message_filters::Subscriber<MarkerArray> markers_sub_;
        boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
        cv::Ptr<cv::aruco::Board> board_;
        Mat camera_matrix_, dist_coeffs_;
        geometry_msgs::TransformStamped transform_;
        geometry_msgs::PoseWithCovarianceStamped pose_;
        vector<geometry_msgs::TransformStamped> markers_transforms_;
        aruco_pose::MarkerArray markers_;
        tf2_ros::TransformBroadcaster br_;
        tf2_ros::StaticTransformBroadcaster static_br_;
        tf2_ros::Buffer tf_buffer_;
        tf2_ros::TransformListener tf_listener_{tf_buffer_};
        visualization_msgs::MarkerArray vis_array_;
        std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
        int image_width_, image_height_, image_margin_;
        bool auto_flip_, image_axis_;

public:
        virtual void onInit()
        {
                ros::NodeHandle &nh_ = getNodeHandle();
                ros::NodeHandle &nh_priv_ = getPrivateNodeHandle();

                image_transport::ImageTransport it_priv(nh_priv_);

                // TODO: why image_transport doesn't work here?
                img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
                markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);

                board_ = cv::makePtr<cv::aruco::Board>();
                board_->dictionary = cv::aruco::getPredefinedDictionary(
                                         static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
                camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);

                std::string type, map;
                type = nh_priv_.param<std::string>("type", "map");
                transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
                known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
                auto_flip_ = nh_priv_.param("auto_flip", false);
                image_width_ = nh_priv_.param("image_width" , 2000);
                image_height_ = nh_priv_.param("image_height", 2000);
                image_margin_ = nh_priv_.param("image_margin", 200);
                image_axis_ = nh_priv_.param("image_axis", true);
                markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
                markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");

                // createStripLine();

                if (type == "map") {
                        param(nh_priv_, "map", map);
                        loadMap(map);
                } else if (type == "gridboard") {
                        createGridBoard(nh_priv_);
                } else {
                        NODELET_FATAL("unknown type: %s", type.c_str());
                        ros::shutdown();
                }

                pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
                vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
                debug_pub_ = it_priv.advertise("debug", 1);

                image_sub_.subscribe(nh_, "image_raw", 1);
                info_sub_.subscribe(nh_, "camera_info", 1);
                markers_sub_.subscribe(nh_, "markers", 1);

                sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
                sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));

                publishMarkersFrames();
                publishMarkers();
                publishMapImage();
                vis_markers_pub_.publish(vis_array_);

                NODELET_INFO("ready");
        }

        void callback(const sensor_msgs::ImageConstPtr& image,
                      const sensor_msgs::CameraInfoConstPtr& cinfo,
                      const aruco_pose::MarkerArrayConstPtr& markers)
        {
                int valid = 0;
                int count = markers->markers.size();
                std::vector<int> ids;
                std::vector<std::vector<cv::Point2f>> corners;
                cv::Vec3d rvec, tvec;

                parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
                if (markers->markers.empty()) goto publish_debug;

                ids.reserve(count);
                corners.reserve(count);

                for(auto const &marker : markers->markers) {
                        ids.push_back(marker.id);
                        std::vector<cv::Point2f> marker_corners = {
                                cv::Point2f(marker.c1.x, marker.c1.y),
                                cv::Point2f(marker.c2.x, marker.c2.y),
                                cv::Point2f(marker.c3.x, marker.c3.y),
                                cv::Point2f(marker.c4.x, marker.c4.y)
                        };
                        corners.push_back(marker_corners);
                }

                if (known_tilt_.empty()) {
                        // simple estimation
                        valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
                                                             rvec, tvec, false);
                        if (!valid) goto publish_debug;

                        transform_.header.stamp = markers->header.stamp;
                        transform_.header.frame_id = markers->header.frame_id;
                        pose_.header = transform_.header;
                        fillPose(pose_.pose.pose, rvec, tvec);
                        fillTransform(transform_.transform, rvec, tvec);

                } else {
                        Mat obj_points, img_points;
                        // estimation with "snapping"
                        cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
                        if (obj_points.empty()) goto publish_debug;

                        double center_x = 0, center_y = 0, center_z = 0;
                        alignObjPointsToCenter(obj_points, center_x, center_y, center_z);

                        valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
                        if (!valid) goto publish_debug;

                        fillTransform(transform_.transform, rvec, tvec);
                        try {
                                geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
                                                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
                                snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
                        } catch (const tf2::TransformException& e) {
                                NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
                        }

                        geometry_msgs::TransformStamped shift;
                        shift.transform.translation.x = -center_x;
                        shift.transform.translation.y = -center_y;
                        shift.transform.translation.z = -center_z;
                        shift.transform.rotation.w = 1;
                        tf2::doTransform(shift, transform_, transform_);

                        // for debug topic
                        tvec[0] = transform_.transform.translation.x;
                        tvec[1] = transform_.transform.translation.y;
                        tvec[2] = transform_.transform.translation.z;

                        transform_.header.stamp = markers->header.stamp;
                        transform_.header.frame_id = markers->header.frame_id;
                        pose_.header = transform_.header;
                        transformToPose(transform_.transform, pose_.pose.pose);
                }

                if (!transform_.child_frame_id.empty()) {
                        br_.sendTransform(transform_);
                }
                pose_pub_.publish(pose_);

publish_debug:
                // publish debug image (even if no map detected)
                if (debug_pub_.getNumSubscribers() > 0) {
                        Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
                        cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
                        if (valid) {
                                _drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
                        }
                        cv_bridge::CvImage out_msg;
                        out_msg.header.frame_id = image->header.frame_id;
                        out_msg.header.stamp = image->header.stamp;
                        out_msg.encoding = sensor_msgs::image_encodings::BGR8;
                        out_msg.image = mat;
                        debug_pub_.publish(out_msg.toImageMsg());
                }
        }

        void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
        {
                // Align object points to the center of mass
                double sum_x = 0;
                double sum_y = 0;
                double sum_z = 0;

                for (int i = 0; i < obj_points.rows; i++) {
                        sum_x += obj_points.at<float>(i, 0);
                        sum_y += obj_points.at<float>(i, 1);
                        sum_z += obj_points.at<float>(i, 2);
                }

                center_x = sum_x / obj_points.rows;
                center_y = sum_y / obj_points.rows;
                center_z = sum_z / obj_points.rows;

                for (int i = 0; i < obj_points.rows; i++) {
                        obj_points.at<float>(i, 0) -= center_x;
                        obj_points.at<float>(i, 1) -= center_y;
                        obj_points.at<float>(i, 2) -= center_z;
                }
        }

        void loadMap(std::string filename)
        {
                std::ifstream f(filename);
                std::string line;

                if (!f.good()) {
                        NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
                        ros::shutdown();
                }

                while (std::getline(f, line)) {
                        int id;
                        double length, x, y, z, yaw, pitch, roll;

                        std::istringstream s(line);

                        // Read first character to see whether it's a comment
                        char first = 0;
                        if (!(s >> first)) {
                                // No non-whitespace characters, must be a blank line
                                continue;
                        }

                        if (first == '#') {
                                NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
                                continue;
                        } else if (isdigit(first)) {
                                // Put the digit back into the stream
                                // Note that this is a non-modifying putback, so this should work with istreams
                                // (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
                                s.putback(first);
                        } else {
                                // Probably garbage data; inform user and throw an exception, possibly killing nodelet
                                NODELET_FATAL("Malformed input: %s", line.c_str());
                                ros::shutdown();
                                throw std::runtime_error("Malformed input");
                        }

                        if (!(s >> id >> length >> x >> y)) {
                                NODELET_ERROR("Not enough data in line: %s; "
                                          "Each marker must have at least id, length, x, y fields", line.c_str());
                                continue;
                        }
                        // Be less strict about z, yaw, pitch roll
                        if (!(s >> z)) {
                                NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
                                z = 0;
                        }
                        if (!(s >> yaw)) {
                                NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
                                yaw = 0;
                        }
                        if (!(s >> pitch)) {
                                NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
                                pitch = 0;
                        }
                        if (!(s >> roll)) {
                                NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
                                roll = 0;
                        }
                        addMarker(id, length, x, y, z, yaw, pitch, roll);
                }

                NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
        }

        void createGridBoard(ros::NodeHandle& nh)
        {
                NODELET_INFO("generate gridboard");
                NODELET_WARN("gridboard maps are deprecated");

                int markers_x, markers_y, first_marker;
                double markers_side, markers_sep_x, markers_sep_y;
                std::vector<int> marker_ids;
                markers_x = nh.param("markers_x", 10);
                markers_y = nh.param("markers_y", 10);
                first_marker = nh.param("first_marker", 0);

                param(nh, "markers_side", markers_side);
                param(nh, "markers_sep_x", markers_sep_x);
                param(nh, "markers_sep_y", markers_sep_y);

                if (nh.getParam("marker_ids", marker_ids)) {
                        if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
                                NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
                                ros::shutdown();
                        }
                } else {
                        // Fill marker_ids automatically
                        marker_ids.resize(markers_x * markers_y);
                        for (int i = 0; i < markers_x * markers_y; i++)
                        {
                                marker_ids.at(i) = first_marker++;
                        }
                }

                double max_y = markers_y * markers_side + (markers_y - 1) * markers_sep_y;
                for(int y = 0; y < markers_y; y++) {
                        for(int x = 0; x < markers_x; x++) {
                                double x_pos = x * (markers_side + markers_sep_x);
                                double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
                                NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
                                addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
                        }
                }
        }

        // void createStripLine()
        // {
        //      visualization_msgs::Marker marker;
        //      marker.header.frame_id = transform_.child_frame_id;
        //      marker.action = visualization_msgs::Marker::ADD;
        //      marker.ns = "aruco_map_link";
        //      marker.type = visualization_msgs::Marker::LINE_STRIP;
        //      marker.scale.x = 0.02;
        //      marker.color.g = 1;
        //      marker.color.a = 0.8;
        //      marker.frame_locked = true;
        //      marker.pose.orientation.w = 1;
        //      vis_array_.markers.push_back(marker);
        // }

        void addMarker(int id, double length, double x, double y, double z,
                                   double yaw, double pitch, double roll)
        {
                // Check whether the id is in range for current dictionary
                int num_markers = board_->dictionary->bytesList.rows;
                if (num_markers <= id) {
                        NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
                                      "Please see https://github.com/CopterExpress/clover/blob/master/aruco_pose/README.md#parameters for details",
                                          id, num_markers);
                        return;
                }
                // Check if marker is already in the board
                if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
                        NODELET_ERROR("Marker id %d is already in the map", id);
                        return;
                }
                // Create transform
                tf::Quaternion q;
                q.setRPY(roll, pitch, yaw);
                tf::Transform transform(q, tf::Vector3(x, y, z));

                /* marker's corners:
                        0    1
                        3    2
                */
                double halflen = length / 2;
                tf::Point p0(-halflen, halflen, 0);
                tf::Point p1(halflen, halflen, 0);
                tf::Point p2(halflen, -halflen, 0);
                tf::Point p3(-halflen, -halflen, 0);
                p0 = transform * p0;
                p1 = transform * p1;
                p2 = transform * p2;
                p3 = transform * p3;

                vector<cv::Point3f> obj_points = {
                        cv::Point3f(p0.x(), p0.y(), p0.z()),
                        cv::Point3f(p1.x(), p1.y(), p1.z()),
                        cv::Point3f(p2.x(), p2.y(), p2.z()),
                        cv::Point3f(p3.x(), p3.y(), p3.z())
                };

                board_->ids.push_back(id);
                board_->objPoints.push_back(obj_points);

                // Add marker's static transform
                if (!markers_frame_.empty()) {
                        geometry_msgs::TransformStamped marker_transform;
                        marker_transform.header.frame_id = markers_parent_frame_;
                        marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
                        tf::transformTFToMsg(transform, marker_transform.transform);
                        markers_transforms_.push_back(marker_transform);
                }

                // Add marker to array
                aruco_pose::Marker marker;
                marker.id = id;
                marker.length = length;
                marker.pose.position.x = x;
                marker.pose.position.y = y;
                marker.pose.position.z = z;
                tf::quaternionTFToMsg(q, marker.pose.orientation);
                markers_.markers.push_back(marker);

                // Add visualization marker
                visualization_msgs::Marker vis_marker;
                vis_marker.header.frame_id = transform_.child_frame_id;
                vis_marker.action = visualization_msgs::Marker::ADD;
                vis_marker.id = vis_array_.markers.size();
                vis_marker.ns = "aruco_map_marker";
                vis_marker.type = visualization_msgs::Marker::CUBE;
                vis_marker.scale.x = length;
                vis_marker.scale.y = length;
                vis_marker.scale.z = 0.001;
                vis_marker.color.r = 1;
                vis_marker.color.g = 0.5;
                vis_marker.color.b = 0.5;
                vis_marker.color.a = 0.8;
                vis_marker.pose.position.x = x;
                vis_marker.pose.position.y = y;
                vis_marker.pose.position.z = z;
                tf::quaternionTFToMsg(q, marker.pose.orientation);
                vis_marker.frame_locked = true;
                vis_array_.markers.push_back(vis_marker);

                // Add linking line
                // geometry_msgs::Point p;
                // p.x = x;
                // p.y = y;
                // p.z = z;
                // vis_array_.markers.at(0).points.push_back(p);
        }

        void publishMarkersFrames()
        {
                if (!markers_transforms_.empty()) {
                        static_br_.sendTransform(markers_transforms_);
                }
        }

        void publishMarkers()
        {
                markers_pub_.publish(markers_);
        }

        void publishMapImage()
        {
                cv::Size size(image_width_, image_height_);
                cv::Mat image;
                cv_bridge::CvImage msg;

                if (!board_->ids.empty()) {
                        _drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
                        msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
                } else {
                        // empty map
                        image.create(size, CV_8UC1);
                        image.setTo(cv::Scalar::all(255));
                        msg.encoding = sensor_msgs::image_encodings::MONO8;
                }

                msg.image = image;
                img_pub_.publish(msg.toImageMsg());
        }
};

PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)