Marker.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT,
 * SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE
 * OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLABS" HS BEEN
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
 * "AS IS" BASIS, AND "MLAB" HAS NO OBLIGATION TO PROVIDE MAINTENANCE,
 * SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MLAB_LOCALIZATION_MARKER_H_
#define _MLAB_LOCALIZATION_MARKER_H_

#include <opencv2/calib3d/calib3d.hpp>

#include <factory/Factory.h>

#include <transform/Pose.h>

namespace mira { namespace localization {


class Marker : public Object
{
    MIRA_OBJECT(Marker)
public:
    Marker() {}

    // the translation and rotational vector computed by cv::solvePnP.
    Marker(const cv::Mat& Tvec, const cv::Mat& Rvec)
    {
        if (Tvec.rows != 3 || Tvec.cols != 1)
            MIRA_THROW(XInvalidParameter, "Can't construct marker pose: "
                "Unexpected size of tvec (rows=" << Tvec.rows << ", cols=" << Tvec.cols <<")");
        if (Rvec.rows != 3 || Rvec.cols != 1)
            MIRA_THROW(XInvalidParameter, "Can't construct marker pose: "
                "Unexpected size of rvec (rows=" << Rvec.rows << ", cols=" << Rvec.cols <<")");

        // convert rvec to (3x3) rotation matrix
        cv::Mat rotMat(3, 3,  CV_32F);
        cv::Rodrigues(Rvec, rotMat);

        // ... convert to Eigen matrix
        Eigen::Map<Eigen::Matrix3f> eigenRotMat((float*)rotMat.data);

        // ... and compute the euler angles
        Eigen::Vector3f ea = eulerAngles(eigenRotMat, 2, 1, 0);

        pose = Pose3(Tvec.at<float>(0, 0), Tvec.at<float>(0, 1), Tvec.at<float>(0, 2),
                     ea[0], ea[1], -ea[2]);
    }

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        serialization::VersionType version = r.version(2, serialization::AcceptDesiredVersion(), this);

        r.member("Pose", pose, "Marker pose", REFLECT_CTRLFLAG_MEMBER_AS_ROPROPERTY);
        r.member("ID",   id,  "Identifier", REFLECT_CTRLFLAG_MEMBER_AS_ROPROPERTY);

        if (version >= 2)
            r.member("Timestamp", markerTimestamp, "Timestamp", REFLECT_CTRLFLAG_MEMBER_AS_ROPROPERTY);
    }

public:
    Pose3 pose;                             
    std::string id;                         
    Time markerTimestamp = Time::invalid(); 
};


typedef boost::shared_ptr<Marker> MarkerPtr;

typedef std::multimap<std::string, MarkerPtr> MarkerMap;


}} // namespace

#endif