PnP（ Perspective-n-Point ）是求解3D到2D点对运动的方法。PnP可以再很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。 PnP问题有多种解法，通用的算法有P3P、EPnP、DLT、UPnP、MRE等，其中P3P、EPnP、DLT、UPnP为线性变换求解，MRE即最小二乘法，是用非线性优化的方式构建最小二乘问题进行迭代求解。

P3P

参考A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation of Absolute Camera Position and Orientation

目的是在已知内参的情况下，通过世界系下的特征点P1 P2 P3,以及相对应的相机观测点f1 f2 f3, 求解相机的位置C和姿态R。

Step 1

/**
******************************************************************************
* @file
* @author  maky <chengwei920412@outlook.com>
* @version
* @date    2018-01-25 11:00:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/
#include <iostream>
#include <random>
#include <vector>
#include <memory>
#include <algorithm>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const Eigen::Vector4d &object, Eigen::Vector3d &point)
{
    point = (intrinsic * ((transform * object).block<3, 1>(0, 0))) / object.z();
    return true;
}
bool reproject(const Eigen::Matrix4d &transform, const Eigen::Matrix3d &intrinsic, const std::vector<Eigen::Vector4d> &objects, std::vector<Eigen::Vector3d> &points)
{
    points.clear();
    for (auto &object : objects) {
        Eigen::Vector3d point;
        if (!reproject(transform, intrinsic, object, point)) {
            continue;
        }
        points.push_back(point);
    }
    return true;
}

bool solve_p3p(const std::vector<Eigen::Vector4d> &objects, const std::vector<Eigen::Vector3d> &points, Eigen::Matrix4d &transform)
{
    if (objects.size() != points.size()) {
        return false;
    }
    if (objects.size() < 4) {
        return false;
    }
    // Extraction of world points
    Eigen::Vector3d P1 = objects[0].head(3);
    Eigen::Vector3d P2 = objects[1].head(3);
    Eigen::Vector3d P3 = objects[2].head(3);

    // Verification that world points are not colinear
    Eigen::Vector3d P21 = P2 - P1;
    Eigen::Vector3d P31 = P3 - P1;
    if (P21.cross(P31).squaredNorm() == 0.0) {
        return false;
    }

    // Extraction of feature vectors
    Eigen::Vector3d f1 = points[0].normalized();
    Eigen::Vector3d f2 = points[1].normalized();
    Eigen::Vector3d f3 = points[2].normalized();

    // Creation of intermediate camera frame
    Eigen::Vector3d e1 = f1;
    Eigen::Vector3d e3 = f1.cross(f2);
    e3.normalize();
    Eigen::Vector3d e2 = e3.cross(e1);

    Eigen::Matrix3d T;
    T << e1.transpose(), e2.transpose(), e3.transpose();

    f3 = T * f3;

    // Reinforce that f3[2] > 0 for having theta in [0;pi]
    if (f3(2) > 0.0) {
        f1 = points[1];
        f2 = points[0];
        f3 = points[2];

        e1 = f1;
        e3 = f1.cross(f2);
        e3.normalize();
        e2 = e3.cross(e1);

        T << e1.transpose(), e2.transpose(), e3.transpose();

        f3 = T * f3;

        P1 = objects[1].head(3);
        P2 = objects[0].head(3);
        P3 = objects[2].head(3);
    }

    // Creation of intermediate world frame

    Eigen::Vector3d n1 = P2 - P1;
    n1.normalize();
    Eigen::Vector3d n3 = n1.cross(P3 - P1);
    n3.normalize();
    Eigen::Vector3d n2 = n3.cross(n1);

    Eigen::Matrix3d N;
    N << n1.transpose(), n2.transpose(), n3.transpose();

    // Extraction of known parameters

    P3 = N * (P3 - P1);

    double d_12 = (P2 - P1).norm();
    double f_1 = f3(0) / f3(2);
    double f_2 = f3(1) / f3(2);
    double p_1 = P3(0);
    double p_2 = P3(1);

    double cos_beta = f1.dot(f2);
    double b = 1.0 / (1.0 - (cos_beta * cos_beta)) - 1.0;

    if (cos_beta < 0.0)
        b = -sqrt(b);
    else
        b = sqrt(b);

    // Definition of temporary variables for avoiding multiple computation

    double f_1_pw2 = f_1 * f_1;
    double f_2_pw2 = f_2 * f_2;
    double p_1_pw2 = p_1 * p_1;
    double p_1_pw3 = p_1_pw2 * p_1;
    double p_1_pw4 = p_1_pw3 * p_1;
    double p_2_pw2 = p_2 * p_2;
    double p_2_pw3 = p_2_pw2 * p_2;
    double p_2_pw4 = p_2_pw3 * p_2;
    double d_12_pw2 = d_12 * d_12;
    double b_pw2 = b * b;

    // Computation of factors of 4th degree polynomial

    Eigen::Matrix<double, 5, 1> factors;

    factors(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;

    factors(1) = 2.0 * p_2_pw3 * d_12 * b + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b
        - 2.0 * f_2 * p_2_pw3 * f_1 * d_12;

    factors(2) = -f_2_pw2 * p_2_pw2 * p_1_pw2
        - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2
        + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + 2.0 * p_1 * p_2_pw2 * d_12
        + 2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b - p_2_pw2 * p_1_pw2 * f_1_pw2
        + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - p_2_pw2 * d_12_pw2 * b_pw2
        - 2.0 * p_1_pw2 * p_2_pw2;

    factors(3) = 2.0 * p_1_pw2 * p_2 * d_12 * b + 2.0 * f_2 * p_2_pw3 * f_1 * d_12
        - 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b - 2.0 * p_1 * p_2 * d_12_pw2 * b;

    factors(4) = -2.0 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b
        + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2.0 * p_1_pw3 * d_12 - p_1_pw2 * d_12_pw2
        + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4
        - 2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 + p_2_pw2 * f_1_pw2 * p_1_pw2
        + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;

    // Computation of roots

    Eigen::Vector4d real_roots;
    {
        double A = factors[0];
        double B = factors[1];
        double C = factors[2];
        double D = factors[3];
        double E = factors[4];

        double A_pw2 = A * A;
        double B_pw2 = B * B;
        double A_pw3 = A_pw2 * A;
        double B_pw3 = B_pw2 * B;
        double A_pw4 = A_pw3 * A;
        double B_pw4 = B_pw3 * B;

        double alpha = -3.0 * B_pw2 / (8.0 * A_pw2) + C / A;
        double beta = B_pw3 / (8.0 * A_pw3) - B * C / (2.0 * A_pw2) + D / A;
        double gamma = -3.0 * B_pw4 / (256.0 * A_pw4) + B_pw2 * C / (16.0 * A_pw3)
            - B * D / (4.0 * A_pw2) + E / A;

        double alpha_pw2 = alpha * alpha;
        double alpha_pw3 = alpha_pw2 * alpha;

        std::complex<double> P(-alpha_pw2 / 12.0 - gamma, 0.0);
        std::complex<double> Q(
            -alpha_pw3 / 108.0 + alpha * gamma / 3.0 - beta * beta / 8.0, 0.0);
        std::complex<double> R = -Q / 2.0
            + sqrt(pow(Q, 2.0) / 4.0 + P * P * P / 27.0);

        std::complex<double> U = pow(R, (1.0 / 3.0));
        std::complex<double> y;

        if (U.real() == 0.0)
            y = -5.0 * alpha / 6.0 - pow(Q, (1.0 / 3.0));
        else
            y = -5.0 * alpha / 6.0 - P / (3.0 * U) + U;

        std::complex<double> w = sqrt(alpha + 2.0 * y);

        std::complex<double> temp;

        temp = -B / (4.0 * A)
            + 0.5 * (w + sqrt(-(3.0 * alpha + 2.0 * y + 2.0 * beta / w)));
        real_roots[0] = temp.real();
        temp = -B / (4.0 * A)
            + 0.5 * (w - sqrt(-(3.0 * alpha + 2.0 * y + 2.0 * beta / w)));
        real_roots[1] = temp.real();
        temp = -B / (4.0 * A)
            + 0.5 * (-w + sqrt(-(3.0 * alpha + 2.0 * y - 2.0 * beta / w)));
        real_roots[2] = temp.real();
        temp = -B / (4.0 * A)
            + 0.5 * (-w - sqrt(-(3.0 * alpha + 2.0 * y - 2.0 * beta / w)));
        real_roots[3] = temp.real();
    }

    // Backsubstitution of each solution
    std::vector<Eigen::Matrix3d> rotations;
    std::vector<Eigen::Vector3d> translations;
    for (int i = 0; i < 4; ++i) {
        // TORSTEN: Checks if this solution has already been used.
        bool used = false;
        for (int j = i - 1; j >= 0 && !used; --j) {
            used = (real_roots(i) == real_roots(j));
        }
        if (used)
            continue;

        double cot_alpha = (-f_1 * p_1 / f_2 - real_roots(i) * p_2 + d_12 * b)
            / (-f_1 * real_roots(i) * p_2 / f_2 + p_1 - d_12);

        double cos_theta = real_roots(i);
        double sin_theta = sqrt(1.0 - (cos_theta * cos_theta));
        double sin_alpha = sqrt(1.0 / ((cot_alpha * cot_alpha) + 1.0));
        double cos_alpha = sqrt(1.0 - (sin_alpha * sin_alpha));

        if (cot_alpha < 0.0)
            cos_alpha = -cos_alpha;

        Eigen::Vector3d C;
        C << d_12 * cos_alpha * (sin_alpha * b + cos_alpha), cos_theta * d_12
            * sin_alpha * (sin_alpha * b + cos_alpha), sin_theta * d_12 * sin_alpha
            * (sin_alpha * b + cos_alpha);

        C = P1 + N.transpose() * C;

        Eigen::Matrix3d R;
        R << -cos_alpha, -sin_alpha * cos_theta, -sin_alpha * sin_theta,
            sin_alpha, -cos_alpha  * cos_theta, -cos_alpha * sin_theta,
            0, -sin_theta, cos_theta;

        R = N.transpose() * R.transpose() * T;

        translations.push_back(C);
        rotations.push_back(R.transpose());
    }

    return true;
}

bool solve_pnp(const std::vector<Eigen::Vector4d> &objects, const std::vector<Eigen::Vector3d> &points, const Eigen::Matrix3d &intrinsic, Eigen::Matrix4d &transform)
{
#if 0
    std::vector<cv::Point3f> object_points;
    for (auto & object : objects) {
        object_points.push_back(cv::Point3f(object.x(), object.y(), object.z()));
    }
    std::vector<cv::Point2f> feature_points;
    for (auto & feature : points) {
        feature_points.push_back(cv::Point2f(feature.x(), feature.y()));
    }
    cv::Mat intrinsic_matrix;
    cv::eigen2cv(intrinsic, intrinsic_matrix);
    cv::Mat r, t, rot;
    cv::Mat trans = cv::Mat::eye(4, 4, CV_32F);
    cv::solvePnP(object_points, feature_points, intrinsic_matrix, cv::Mat(), r, t, false, 0);
    cv::Rodrigues(r, rot);
    rot.copyTo(trans.rowRange(0, 3).colRange(0, 3));
    t.copyTo(trans.rowRange(0, 3).colRange(3, 4));
    cv::cv2eigen(trans, transform);
#else
    double fx = intrinsic(0, 0);
    double fy = intrinsic(1, 1);
    double cx = intrinsic(0, 2);
    double cy = intrinsic(1, 2);
    std::vector<Eigen::Vector3d> normalized_feature;
    for (auto &point : points) {
        Eigen::Vector3d feature(0., 0., 1.);
        feature.x() = point.x() / fx - cx;
        feature.y() = point.y() / fy - cy;
        normalized_feature.push_back(feature);
    }
    if (!solve_p3p(objects, normalized_feature, transform)) {
        return false;
    }
#endif
    return true;
}

int main(int argc, char * argv[])
{
    std::vector<Eigen::Vector4d> object_points;
    {
        Eigen::Vector3d translation = Eigen::Vector3d(0., 0., 4.);
        Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
        transform.translation() = translation;

        std::cout << "object_points: " << std::endl;
        object_points.push_back(Eigen::Vector4d(1., 1., 0, 1.));
        object_points.push_back(Eigen::Vector4d(1., -1., 0, 1.));
        object_points.push_back(Eigen::Vector4d(-1., -1., 0, 1.));
        object_points.push_back(Eigen::Vector4d(-1., 1., 0, 1.));
        for (auto pos = 0; pos < object_points.size(); pos++) {
            auto &point = object_points.at(pos);
            point = transform * point;
            std::cout << point.transpose() << std::endl;
        }
    }
    Eigen::Matrix3d intrinsic;
    intrinsic << 500., 0., 752 / 2., 0., 500., 480 / 2., 0., 0., 1.;
    std::cout << "intrinsic: " << std::endl << intrinsic << std::endl;
    // 
    std::vector<Eigen::Vector3d> feature_points;
    {
        Eigen::Matrix4d true_pose = Eigen::Matrix4d::Identity();
        true_pose.block<3, 1>(0, 3) = Eigen::Vector3d(1., 0., 0.);
        std::cout << "true_pose: " << std::endl << true_pose << std::endl;
        reproject(true_pose, intrinsic, object_points, feature_points);
        std::cout << "point: " << std::endl;
        for (auto &point : feature_points) {
            std::cout << point.transpose() << " --> ";
            point.x() += (float)(std::rand() % 20000 - 10000) / 10000.;
            point.y() += (float)(std::rand() % 20000 - 10000) / 10000.;
            std::cout << point.transpose() << std::endl;
        }
    }
    Eigen::Matrix4d transform;
    if (!solve_pnp(object_points, feature_points, intrinsic, transform)) {
        std::cout << "solve pnp failed." << std::endl;
        return -1;
    }
    std::cout << "transform: " << std::endl << transform << std::endl;

    return 0;
}

AR Demo

/**
******************************************************************************
* @file
* @author  maky <chengwei920412@outlook.com>
* @version
* @date    2018-01-25 11:00:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/
#include <iostream>
#include <iomanip>
#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#if defined(WIN32) || defined(_WIN32) || defined(WIN64) || defined(_WIN64)
#include <Windows.h>
#endif
#include <osg/GL>
#include <osg/Node>
#include <osg/NodeVisitor>
#include <osg/Group>
#include <osg/Switch>
#include <osg/Geode>
#include <osg/Shape>
#include <osg/ShapeDrawable>
#include <osg/Geometry>
#include <osg/NodeCallback>
#include <osg/MatrixTransform>
#include <osgViewer/Viewer>
#include <osgDB/ReadFile>
#include <osg/PositionAttitudeTransform>
#include <osg/Camera>
#include <osg/PolygonMode>
#include <osg/Texture2D>
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osgText/Text>

#include <osgGA/TrackballManipulator>
#include <osgGA/FlightManipulator>
#include <osgGA/DriveManipulator>
#include <osgGA/KeySwitchMatrixManipulator>
#include <osgGA/StateSetManipulator>
#include <osgGA/AnimationPathManipulator>
#include <osgGA/TerrainManipulator>
#include <osgAnimation/BasicAnimationManager>
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
// include the plugins we need  
USE_OSGPLUGIN(ive)
USE_OSGPLUGIN(osg)
USE_OSGPLUGIN(osg2)
USE_OSGPLUGIN(rgb)
USE_OSGPLUGIN(OpenFlight)
USE_OSGPLUGIN(3ds)
//USE_OSGPLUGIN(fbx)  
USE_OSGPLUGIN(png)  
USE_OSGPLUGIN(jpeg)  

#ifdef USE_FREETYPE  
USE_OSGPLUGIN(freetype)
#endif  

USE_DOTOSGWRAPPER_LIBRARY(osg)
USE_DOTOSGWRAPPER_LIBRARY(osgFX)
USE_DOTOSGWRAPPER_LIBRARY(osgParticle)
USE_DOTOSGWRAPPER_LIBRARY(osgShadow)
USE_DOTOSGWRAPPER_LIBRARY(osgSim)
USE_DOTOSGWRAPPER_LIBRARY(osgTerrain)
USE_DOTOSGWRAPPER_LIBRARY(osgText)
USE_DOTOSGWRAPPER_LIBRARY(osgViewer)
USE_DOTOSGWRAPPER_LIBRARY(osgVolume)
USE_DOTOSGWRAPPER_LIBRARY(osgWidget)

USE_SERIALIZER_WRAPPER_LIBRARY(osg)
USE_SERIALIZER_WRAPPER_LIBRARY(osgAnimation)
USE_SERIALIZER_WRAPPER_LIBRARY(osgFX)
USE_SERIALIZER_WRAPPER_LIBRARY(osgManipulator)
USE_SERIALIZER_WRAPPER_LIBRARY(osgParticle)
USE_SERIALIZER_WRAPPER_LIBRARY(osgShadow)
USE_SERIALIZER_WRAPPER_LIBRARY(osgSim)
USE_SERIALIZER_WRAPPER_LIBRARY(osgTerrain)
USE_SERIALIZER_WRAPPER_LIBRARY(osgText)
USE_SERIALIZER_WRAPPER_LIBRARY(osgVolume)

// include the platform specific GraphicsWindow implementation.  
USE_GRAPHICSWINDOW()

template<typename T> class INode;

template<typename T>
class INodeCallback : public ::osg::NodeCallback
{
public:
    explicit INodeCallback() {}
    virtual ~INodeCallback() {}
public:
    virtual void operator()(::osg::Node* node, ::osg::NodeVisitor* nv)
    {
        ::osg::ref_ptr<T> host = dynamic_cast<T*>(node);
        host->run(node, nv);
        ::osg::NodeCallback::traverse(node, nv);
    }
};

template<typename T>
class INode :public ::osg::Group
{
    template<typename T> friend class INodeCallback;
public:
    explicit INode()
        : root_(new ::osg::Switch())
        , transform_(new ::osg::MatrixTransform())
        , update_callback_(new INodeCallback<T>)
    {
        transform_->addChild(root_);
        this->addChild(transform_);
        this->setUpdateCallback(update_callback_);
        root_->setNewChildDefaultValue(true);
    }
    virtual ~INode() {}
public:
    // set node transform matrix
    void setMatrix(const ::osg::Matrix& mat) { transform_->setMatrix(mat); }
    const ::osg::Matrix& getMatrix() const { return transform_->getMatrix(); }

    virtual void rotate(::osg::Vec3 &angle) {}
    virtual void rotation(::osg::Vec3 &angle) {}
    virtual void translate(::osg::Vec3 &location) {}
    virtual void translation(::osg::Vec3 &location) {}
    virtual void setVisibility(bool visibility = true)
    {
        if (visibility) {
            root_->setAllChildrenOn();
        }
        else {
            root_->setAllChildrenOff();
        }
    }
protected:
    ::osg::ref_ptr<::osg::Switch> root(void) { return root_; }
    virtual void updateHandler(::osg::Node* node, ::osg::NodeVisitor* nv) {}
private:
    virtual void run(::osg::Node* node, ::osg::NodeVisitor* nv)
    {
        updateHandler(node, nv);
    }

private:
    ::osg::ref_ptr<::osg::Switch> root_;
    ::osg::ref_ptr<::osg::MatrixTransform> transform_;
    ::osg::ref_ptr<INodeCallback<T>> update_callback_;
};

class ConeNode :public INode<ConeNode>
{
public:
    ConeNode(::osg::Vec4 color = ::osg::Vec4(1.f, 0.f, 0.f, 1.f),
        float height = 0.2f, float radius = 0.05f, int side = 30);
    virtual ~ConeNode();
private:
    ::osg::ref_ptr<::osg::Geode> geode_;
    ::osg::ref_ptr<::osg::Geometry> geometry_;
};

class CylinderNode :public INode<CylinderNode>
{
public:
    CylinderNode(::osg::Vec4 color = ::osg::Vec4(1.f, 0.f, 0.f, 1.f),
        float height = 0.2f, float radius = 0.05f, int side = 30);
    virtual ~CylinderNode();
private:
    ::osg::ref_ptr<::osg::Geode> geode_;
    ::osg::ref_ptr<::osg::Geometry> geometry_;
};

class ArrowNode :public INode<ArrowNode>
{
public:
    explicit ArrowNode(::osg::Vec4 color = ::osg::Vec4(0.f, 0.f, 0.f, 1.f),
        float cylinder_height = 0.9f, float cylinder_radius = 0.01f,
        float cone_height = 0.1f, float cone_radius = 0.03f);
    virtual ~ArrowNode();

private:
    ::osg::ref_ptr<ConeNode> cone_;
    ::osg::ref_ptr<CylinderNode> cylinder_;
    ::osg::ref_ptr<::osg::MatrixTransform> cone_translator_;
    ::osg::ref_ptr<::osg::MatrixTransform> cylinder_translator_;
};

class AxisNode :public INode<AxisNode>
{
public:
    explicit AxisNode(float scale = 1.f);
    virtual ~AxisNode();

private:
    ::osg::ref_ptr<ArrowNode> x_;
    ::osg::ref_ptr<ArrowNode> y_;
    ::osg::ref_ptr<ArrowNode> z_;
    ::osg::ref_ptr<osgText::Text> x_t_;
    ::osg::ref_ptr<osgText::Text> y_t_;
    ::osg::ref_ptr<osgText::Text> z_t_;
    ::osg::ref_ptr<::osg::MatrixTransform> x_rotation_;
    ::osg::ref_ptr<::osg::MatrixTransform> y_rotation_;
    ::osg::ref_ptr<::osg::MatrixTransform> z_rotation_;
};

ConeNode::ConeNode(::osg::Vec4 color, float height, float radius, int side)
{
    if (side < 3) {
        side = 3;
    }
    geometry_ = new ::osg::Geometry();
    // 1. vertices
    ::osg::ref_ptr<::osg::Vec3Array> vertices = new ::osg::Vec3Array;
    vertices->push_back(::osg::Vec3(0.f, 0.f, height));// peak

    Eigen::Matrix<float, 4, 1> circle_vector(radius, 0.f, 0.f, 0.f);
    Eigen::Transform<float, 3, 1> transform = Eigen::Transform<float, 3, 1>::Identity();
    float angle_step = M_PI / side * 2.f;
    for (int pos = 0; pos < side; pos++) {// underside
        Eigen::AngleAxis<float> rotation = Eigen::AngleAxis<float>(angle_step, Eigen::Matrix<float, 3, 1>::UnitZ());
        transform *= rotation;
        Eigen::Matrix<float, 4, 1> target = transform * circle_vector;
        vertices->push_back(::osg::Vec3(target.x(), target.y(), 0));
    }
    geometry_->setVertexArray(vertices);
    // 2. color
    ::osg::ref_ptr<::osg::Vec4Array> colors = new ::osg::Vec4Array;
    //colors->push_back(color);// peak
    //for (int pos = 0; pos < side; pos++) {// underside
    //    colors->push_back(color);
    //}
    //this->setColorArray(colors);
    //this->setColorBinding(::osg::Geometry::BIND_PER_VERTEX);
    colors->push_back(color);
    geometry_->setColorArray(colors);
    geometry_->setColorBinding(::osg::Geometry::BIND_OVERALL);
    // 3. 
    for (int pos = 0; pos < side; pos++) {
        ::osg::ref_ptr<::osg::DrawElementsUInt> face = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::TRIANGLES, 0);
        int side_left = pos;
        int side_right = pos + 1;
        if (side_right >= side) {
            side_right = 0;
        }
        face->push_back(0);
        face->push_back(side_left + 1/* peak */);
        face->push_back(side_right + 1/* peak */);
        geometry_->addPrimitiveSet(face);
    }

    ::osg::ref_ptr<::osg::DrawElementsUInt> base = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::POLYGON, 0);
    for (int pos = 0; pos < side; pos++) {
        base->push_back(pos + 1/* peak */);
    }
    geometry_->addPrimitiveSet(base);
    geode_ = new ::osg::Geode();
    geode_->addDrawable(geometry_);
    geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF);
    geode_->getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
    geode_->getOrCreateStateSet()->setRenderingHint(::osg::StateSet::TRANSPARENT_BIN);
    root()->addChild(geode_);
}
ConeNode::~ConeNode()
{}

CylinderNode::CylinderNode(::osg::Vec4 color, float height, float radius, int side)
{
    if (side < 3) {
        side = 3;
    }

    geometry_ = new ::osg::Geometry();
    // 1. vertices
    ::osg::ref_ptr<::osg::Vec3Array> vertices = new ::osg::Vec3Array;
    Eigen::Matrix<float, 4, 1> circle_vector(radius, 0.f, 0.f, 0.f);
    Eigen::Transform<float, 3, 1> transform = Eigen::Transform<float, 3, 1>::Identity();
    float angle_step = M_PI / side * 2.f;
    for (int pos = 0; pos < side; pos++) {// underside
        Eigen::AngleAxis<float> rotation = Eigen::AngleAxis<float>(angle_step, Eigen::Matrix<float, 3, 1>::UnitZ());
        transform *= rotation;
        Eigen::Matrix<float, 4, 1> target = transform * circle_vector;
        vertices->push_back(::osg::Vec3(target.x(), target.y(), 0));
        vertices->push_back(::osg::Vec3(target.x(), target.y(), height));
    }
    geometry_->setVertexArray(vertices);
    // 2. color
    ::osg::ref_ptr<::osg::Vec4Array> colors = new ::osg::Vec4Array;
    colors->push_back(color);
    geometry_->setColorArray(colors);
    geometry_->setColorBinding(::osg::Geometry::BIND_OVERALL);
    // 3. 
    for (int pos = 0; pos < side; pos++) {
        ::osg::ref_ptr<::osg::DrawElementsUInt> face = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
        int x = pos * 2;
        int y = x + 1;
        int xx = y + 1;
        int yy = xx + 1;
        if (xx >= side * 2) {
            xx = 0;
            yy = 1;
        }
        face->push_back(x);
        face->push_back(y);
        face->push_back(yy);
        face->push_back(xx);
        geometry_->addPrimitiveSet(face);
    }

    ::osg::ref_ptr<::osg::DrawElementsUInt> top = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::POLYGON, 0);
    for (int pos = 0; pos < side; pos++) {
        top->push_back(pos * 2);
    }
    geometry_->addPrimitiveSet(top);

    ::osg::ref_ptr<::osg::DrawElementsUInt> bottom = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::POLYGON, 0);
    for (int pos = 0; pos < side; pos++) {
        bottom->push_back(pos * 2 + 1);
    }
    geometry_->addPrimitiveSet(bottom);

    geode_ = new ::osg::Geode();
    geode_->addDrawable(geometry_);
    geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF);
    geode_->getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
    geode_->getOrCreateStateSet()->setRenderingHint(::osg::StateSet::TRANSPARENT_BIN);
    root()->addChild(geode_);
}
CylinderNode::~CylinderNode()
{}

ArrowNode::ArrowNode(::osg::Vec4 color, float cylinder_height, float cylinder_radius, float cone_height, float cone_radius)
{
    cone_ = new ConeNode(color, cone_height, cone_radius);
    cylinder_ = new CylinderNode(color, cylinder_height, cylinder_radius);

    cone_translator_ = new ::osg::MatrixTransform();
    cone_translator_->setMatrix(::osg::Matrix::translate(::osg::Vec3(0.f, 0.f, cylinder_height)));
    cone_translator_->addChild(cone_);
    cylinder_translator_ = new ::osg::MatrixTransform();
    cylinder_translator_->addChild(cylinder_);

    root()->addChild(cone_translator_);
    root()->addChild(cylinder_translator_);
}

ArrowNode::~ArrowNode() {}

AxisNode::AxisNode(float scale)
{
    x_ = new ArrowNode(::osg::Vec4(1.f, 0.f, 0.f, 1.f));
    y_ = new ArrowNode(::osg::Vec4(0.f, 1.f, 0.f, 1.f));
    z_ = new ArrowNode(::osg::Vec4(0.f, 0.f, 1.f, 1.f));

    x_rotation_ = new ::osg::MatrixTransform();
    x_rotation_->setMatrix(::osg::Matrix::rotate(::osg::inDegrees(90.0f), 0.f, 1.f, 0.f));
    x_rotation_->addChild(x_);
    y_rotation_ = new ::osg::MatrixTransform();
    y_rotation_->setMatrix(::osg::Matrix::rotate(::osg::inDegrees(-90.0f), 1.f, 0.f, 0.f));
    y_rotation_->addChild(y_);
    z_rotation_ = new ::osg::MatrixTransform();
    z_rotation_->addChild(z_);

    x_t_ = new osgText::Text();
    x_t_->setCharacterSize(0.05);
    x_t_->setPosition(::osg::Vec3(1.1f, 0.f, 0.f));
    x_t_->setColor(::osg::Vec4(1.0, 1.0, 1.0, 1.0));
    x_t_->setAlignment(osgText::Text::CENTER_BOTTOM);
    x_t_->setAxisAlignment(osgText::Text::SCREEN);
    x_t_->setAutoRotateToScreen(true);
    x_t_->setText("X");

    y_t_ = new osgText::Text();
    y_t_->setCharacterSize(0.05);
    y_t_->setPosition(::osg::Vec3(0.f, 1.1f, 0.f));
    y_t_->setColor(::osg::Vec4(1.0, 1.0, 1.0, 1.0));
    y_t_->setAlignment(osgText::Text::CENTER_BOTTOM);
    y_t_->setAxisAlignment(osgText::Text::SCREEN);
    y_t_->setAutoRotateToScreen(true);
    y_t_->setText("Y");

    z_t_ = new osgText::Text();
    z_t_->setCharacterSize(0.05);
    z_t_->setPosition(::osg::Vec3(0.f, 0.f, 1.1f));
    z_t_->setColor(::osg::Vec4(1.0, 1.0, 1.0, 1.0));
    z_t_->setAlignment(osgText::Text::CENTER_BOTTOM);
    z_t_->setAxisAlignment(osgText::Text::SCREEN);
    z_t_->setAutoRotateToScreen(true);
    z_t_->setText("Z");

    root()->addChild(x_rotation_);
    root()->addChild(y_rotation_);
    root()->addChild(z_rotation_);
    root()->addChild(x_t_);
    root()->addChild(y_t_);
    root()->addChild(z_t_);
}

AxisNode::~AxisNode() {}

class LineNode :public INode<LineNode>
{
public:
    explicit LineNode(::osg::Vec4 color = ::osg::Vec4(0.f, 0.f, 1.f, 1.f), float width = 1.f,
        ::osg::Vec3 begin = ::osg::Vec3(0.f, 0.f, 0.f), ::osg::Vec3 end = ::osg::Vec3(1.f, 1.f, 1.f));
    virtual ~LineNode();

private:
    ::osg::ref_ptr<::osg::Geode> geode_;
    ::osg::ref_ptr<::osg::Geometry> geometry_;
    ::osg::ref_ptr<::osg::MatrixTransform> rotation_;
};

LineNode::LineNode(::osg::Vec4 color, float width, ::osg::Vec3 begin, ::osg::Vec3 end)
{
    geometry_ = new ::osg::Geometry();
    // 1. vertices
    ::osg::ref_ptr<::osg::Vec3Array> vertices = new ::osg::Vec3Array;

    vertices->push_back(begin);
    vertices->push_back(end);
    geometry_->setVertexArray(vertices);
    // 2. color
    ::osg::ref_ptr<::osg::Vec4Array> colors = new ::osg::Vec4Array;
    colors->push_back(color);
    geometry_->setColorArray(colors);
    geometry_->setColorBinding(::osg::Geometry::BIND_OVERALL);
    // 3. 
    ::osg::ref_ptr<::osg::DrawElementsUInt> line = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
    line->push_back(0);
    line->push_back(1);
    geometry_->addPrimitiveSet(line);

    // 
    ::osg::ref_ptr<::osg::LineWidth> line_width = new ::osg::LineWidth();
    line_width->setWidth(width);

    // 4.
    geode_ = new ::osg::Geode();
    geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF);
    geode_->getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
    geode_->getOrCreateStateSet()->setRenderingHint(::osg::StateSet::TRANSPARENT_BIN);
    geode_->getOrCreateStateSet()->setAttributeAndModes(line_width.get(), ::osg::StateAttribute::ON);
    geode_->addDrawable(geometry_);
    root()->addChild(geode_);
}

LineNode::~LineNode() {}
class GridPlanNode :public INode<GridPlanNode>
{
public:
    explicit GridPlanNode(::osg::Vec4 color = ::osg::Vec4(0.3f, 0.3f, 0.3f, 0.8f),
        float row_size = 1.f, float column_size = 1.f, int row_count = 50, int column_count = 50, float height = 0.f);
    virtual ~GridPlanNode();

private:
    ::osg::ref_ptr<::osg::Geode> geode_;
    ::osg::ref_ptr<::osg::Geometry> geometry_;
};
GridPlanNode::GridPlanNode(::osg::Vec4 color, float row_size, float column_size, int row_count, int column_count, float height)
{
    if (row_count < 1) {
        row_count = 1;
    }
    if (column_count < 1) {
        column_count = 1;
    }
    geometry_ = new ::osg::Geometry();
    // 1. vertices
    float start_position_x = -1 * row_count / 2.0 * row_size;
    float start_position_y = -1 * column_count / 2.0 * column_size;

    ::osg::ref_ptr<::osg::Vec3Array> vertices = new ::osg::Vec3Array;
    for (int row_pos = 0; row_pos < row_count + 1; row_pos++) {
        for (int column_pos = 0; column_pos < column_count + 1; column_pos++) {
            vertices->push_back(::osg::Vec3(start_position_x + row_size * row_pos, start_position_y + column_size * column_pos, height));
        }
    }
    geometry_->setVertexArray(vertices);
    // 2. color
    ::osg::ref_ptr<::osg::Vec4Array> colors = new ::osg::Vec4Array;
    colors->push_back(color);
    geometry_->setColorArray(colors);
    geometry_->setColorBinding(::osg::Geometry::BIND_OVERALL);
    // 3. 
    for (int row_pos = 0; row_pos < row_count + 1; row_pos++) {
        for (int column_pos = 0; column_pos < column_count + 1; column_pos++) {
            int line_start = row_pos * (row_count + 1) + column_pos;
            int line_column = line_start + 1;
            int line_row = line_start + column_count + 1;
            //logger.debug("start: %d, column: %d, row: %d.", line_start, line_column, line_row);
            if ((line_column != (row_pos * (row_count + 1) + column_count + 1))) {
                ::osg::ref_ptr<::osg::DrawElementsUInt> column_line = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
                column_line->push_back(line_start);
                column_line->push_back(line_column);
                geometry_->addPrimitiveSet(column_line);
            }
            else {
                //logger.warnning("ignore column: %d %d", line_start, line_column);
            }
            if ((line_row != ((row_count) * (row_count + 1) + column_pos + column_count + 1))) {
                ::osg::ref_ptr<::osg::DrawElementsUInt> row_line = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
                row_line->push_back(line_start);
                row_line->push_back(line_row);
                geometry_->addPrimitiveSet(row_line);
            }
            else {
                //logger.warnning("ignore row: %d %d", line_start, line_row);
            }
        }
    }
    // 4.
    geode_ = new ::osg::Geode();
    geode_->addDrawable(geometry_);
    geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF);
    geode_->getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
    geode_->getOrCreateStateSet()->setRenderingHint(::osg::StateSet::TRANSPARENT_BIN);
    root()->addChild(geode_);
}

GridPlanNode::~GridPlanNode() {}
class PostureNode :public INode<PostureNode>
{
public:
    explicit PostureNode(float height = 0.8f, float length = 0.8f, float width = 0.8f, ::osg::Vec4 color = ::osg::Vec4(1.f, 1.f, 1.f, 0.1f));
    virtual ~PostureNode();

private:
    ::osg::ref_ptr<::osg::Geode> geode_;
    ::osg::ref_ptr<::osg::Geometry> geometry_;
};
PostureNode::PostureNode(float height, float length, float width, ::osg::Vec4 color)
{
    ::osg::Vec3 xyz(length / 2.f, width / 2.f, height / 2.f);
    ::osg::Vec3 nxyz(-length / 2.f, width / 2.f, height / 2.f);
    ::osg::Vec3 xnyz(length / 2.f, -width / 2.f, height / 2.f);
    ::osg::Vec3 xynz(length / 2.f, width / 2.f, -height / 2.f);

    ::osg::Vec3 nxnyz(-length / 2.f, -width / 2.f, height / 2.f);
    ::osg::Vec3 xnynz(length / 2.f, -width / 2.f, -height / 2.f);
    ::osg::Vec3 nxynz(-length / 2.f, width / 2.f, -height / 2.f);
    ::osg::Vec3 nxnynz(-length / 2.f, -width / 2.f, -height / 2.f);

    geometry_ = new ::osg::Geometry();
    // 1. vertices
    ::osg::ref_ptr<::osg::Vec3Array> vertices = new ::osg::Vec3Array;

    vertices->push_back(xyz);
    vertices->push_back(nxyz);
    vertices->push_back(xnyz);
    vertices->push_back(xynz);

    vertices->push_back(nxnyz);
    vertices->push_back(xnynz);
    vertices->push_back(nxynz);
    vertices->push_back(nxnynz);

    geometry_->setVertexArray(vertices);
    // 2. color
    ::osg::ref_ptr<::osg::Vec4Array> colors = new ::osg::Vec4Array;
    colors->push_back(color);
    geometry_->setColorArray(colors);
    geometry_->setColorBinding(::osg::Geometry::BIND_OVERALL);
    // 3. 
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_x = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_x->push_back(0); plan_x->push_back(2); plan_x->push_back(5); plan_x->push_back(3);
    geometry_->addPrimitiveSet(plan_x);
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_nx = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_nx->push_back(1); plan_nx->push_back(4); plan_nx->push_back(7); plan_nx->push_back(6);
    geometry_->addPrimitiveSet(plan_nx);
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_y = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_y->push_back(0); plan_y->push_back(1); plan_y->push_back(6); plan_y->push_back(3);
    geometry_->addPrimitiveSet(plan_y);
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_ny = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_ny->push_back(2); plan_ny->push_back(4); plan_ny->push_back(7); plan_ny->push_back(5);
    geometry_->addPrimitiveSet(plan_ny);
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_z = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_z->push_back(0); plan_z->push_back(1); plan_z->push_back(4); plan_z->push_back(2);
    geometry_->addPrimitiveSet(plan_z);
    ::osg::ref_ptr<::osg::DrawElementsUInt> plan_nz = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::QUADS, 0);
    plan_nz->push_back(3); plan_nz->push_back(5); plan_nz->push_back(7); plan_nz->push_back(6);
    geometry_->addPrimitiveSet(plan_nz);

    // 4.
    geode_ = new ::osg::Geode();
    geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, ::osg::StateAttribute::OFF | ::osg::StateAttribute::PROTECTED);
    geode_->getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
    geode_->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, ::osg::StateAttribute::OFF);
    geode_->getOrCreateStateSet()->setRenderingHint(::osg::StateSet::TRANSPARENT_BIN);
    geode_->getOrCreateStateSet()->setRenderBinDetails(11, "RenderBin");
    geode_->addDrawable(geometry_);
    root()->addChild(geode_);

    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xyz, xnyz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xnyz, xnynz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xnynz, xynz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xynz, xyz));

    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, nxyz, nxnyz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, nxnyz, nxnynz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, nxnynz, nxynz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, nxynz, nxyz));

    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xyz, nxyz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xynz, nxynz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xnyz, nxnyz));
    root()->addChild(new LineNode(::osg::Vec4(0.f, 0.f, 0.f, 1.f), 1.f, xnynz, nxnynz));
}

PostureNode::~PostureNode() {}

class MotionCamera: public ::osg::Group{
public:
    MotionCamera(osg::ref_ptr<osg::Camera> camera, int width, int height, cv::Mat intrinsic, cv::Mat distortion_coeffs)
        : root_camera_(camera)
        , intrinsic_(intrinsic.clone())
        , distortion_coeffs_(distortion_coeffs)
        , image_(new ::osg::Image())
        , background_(nullptr)
        , transform_(new ::osg::PositionAttitudeTransform())
    {
        this->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
        background_ = createBackground(width, height);
        this->addChild(background_);
        root_camera_->setProjectionMatrix(intrinsiceProjectionMatrix(intrinsic, width, height, 0.01, 100.));
        auto projection = root_camera_->getProjectionMatrix();
    }
    virtual ~MotionCamera(){}
public:
    void update(cv::Mat frame, const Eigen::Matrix4d &pose)
    {
        Eigen::Quaterniond rotation(pose.block<3, 3>(0, 0));
        Eigen::Vector3d translation(pose.block<3, 1>(0, 3)); 
        osg::Matrix transform = osg::Matrix::rotate(osg::Quat(rotation.x(), rotation.y(), rotation.z(), rotation.w())) *
            osg::Matrix::translate(osg::Vec3d(translation.x(), translation.y(), translation.z()));
        update(frame, transform);
        //update(frame, osg::Matrixd::lookAt(osg::Vec3d(0.02, 0.02, 0.2), osg::Vec3d(0, 0, 0), osg::Vec3d(0, 0, 1)));
    }
    void update(cv::Mat frame, const ::osg::Matrix& pose)
    {
        if (frame.empty()) {
            return;
        }
        frame_ = frame.clone();
        image_->setImage(frame_.cols, frame_.rows, 3, GL_RGB, GL_BGR, GL_UNSIGNED_BYTE, (uchar*)(frame_.data), osg::Image::AllocationMode::NO_DELETE, 1);
        ::osg::Matrix view = ::osg::Matrix::inverse(pose);
        root_camera_->setViewMatrix(view * ::osg::Matrix::rotate(osg::PI, 1, 0, 0));
    }
protected:
    ::osg::Matrix intrinsiceProjectionMatrix(cv::Mat& camera_matrix, float width, float height, float near_plane, float far_plane)
    {
        float f_x = camera_matrix.at<double>(0, 0);
        float f_y = camera_matrix.at<double>(1, 1);

        float c_x = camera_matrix.at<double>(0, 2);
        float c_y = camera_matrix.at<double>(1, 2);

        float m00 = 2 * f_x / width;
        float m11 = 2 * f_y / height;

        float m02 = 1.0f - 2 * c_x / width;
        float m12 = 2 * c_y / height - 1.0f;
        float m22 = -(far_plane + near_plane) / (far_plane - near_plane);
        float m32 = -1.0f;

        float m23 = -2.0f*far_plane*near_plane / (far_plane - near_plane);
        ::osg::Matrix projection(m00, 0., 0., 0.,
                                0., m11, 0., 0.,
                                m02, m12, m22, m32,
                                0., 0., m23, 0.);

        return projection;
    }

    osg::ref_ptr<osg::Camera> createBackground(int width, int height)
    {
        osg::ref_ptr<osg::Geometry> geometry = osg::createTexturedQuadGeometry(osg::Vec3(0.0f, 0.0f, 0.0f), osg::Vec3(width, 0.0f, 0.0f), osg::Vec3(0.0, height, 0.0), 0.0f, 1.0f, 1.0f, 0.0f);
        osg::ref_ptr<osg::Geode> node = new osg::Geode;
        node->addDrawable(geometry);
        // DISABLE SHADOW / LIGHTNING EFFECTS
        int values = osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED;
        node->getOrCreateStateSet()->setAttribute(new osg::PolygonMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::FILL), values);
        node->getOrCreateStateSet()->setMode(GL_LIGHTING, values);

        /*osg::ref_ptr<osg::Texture2D> */texture = new osg::Texture2D;
        texture->setTextureSize(width, height);
        texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
        texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
        texture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT);
        texture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT);
        texture->setResizeNonPowerOfTwoHint(false);

        texture->setImage(image_);

        // Apply texture
        osg::ref_ptr<osg::StateSet> state = geometry->getOrCreateStateSet();
        state->setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);

        //Bind texture to the quadGeometry, then use the following camera:
        osg::ref_ptr<osg::Camera> camera = new osg::Camera;
        // CAMERA SETUP
        camera->setReferenceFrame(osg::Camera::ABSOLUTE_RF);
        // use identity view matrix so that children do not get (view) transformed
        camera->setViewMatrix(osg::Matrix::identity());
        camera->setClearMask(GL_DEPTH_BUFFER_BIT);
        camera->setClearColor(osg::Vec4(0.f, 0.f, 0.f, 1.0));
        camera->setProjectionMatrixAsOrtho(0.f, width, 0.f, height, 1.0, 500.f);
        // set resize policy to fixed
        camera->setProjectionResizePolicy(osg::Camera::ProjectionResizePolicy::FIXED);
        // we don't want the camera to grab event focus from the viewers main camera(s).
        camera->setAllowEventFocus(false);
        // only clear the depth buffer
        camera->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        //camera->setViewport( 0, 0, screenWidth, screenHeight );
        camera->setRenderOrder(osg::Camera::NESTED_RENDER);
        camera->addChild(node);
        return camera;
    }

protected:
    osg::ref_ptr<osg::Camera> root_camera_;
    cv::Mat intrinsic_;
    cv::Mat distortion_coeffs_;
    osg::ref_ptr<osg::Camera> background_;
    osg::ref_ptr<osg::Texture2D> texture;
    osg::ref_ptr<osg::Image> image_;
    osg::ref_ptr<osg::PositionAttitudeTransform> transform_;
    cv::Mat frame_;
};

class PoseEstimater
{
public:
    PoseEstimater(cv::Mat intrinsic, cv::Mat distortion_coeffs)
        : intrinsic_(intrinsic.clone())
        , distortion_coeffs_(distortion_coeffs)
        , marker_half_length_(0.08/2.)
    {
        dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
        parameters_ = cv::aruco::DetectorParameters::create();
        parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
    }
    virtual ~PoseEstimater() {}
public:
    Eigen::Matrix4d process(cv::Mat frame)
    {
        Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
        std::vector<int> ids;
        std::vector<std::vector<cv::Point2f>> corners, rejected_candidates;
        cv::aruco::detectMarkers(frame, dictionary_, corners, ids, parameters_, rejected_candidates);
        if (ids.empty()) {
            return pose;
        }
        cv::aruco::drawDetectedMarkers(frame, corners, ids);
        for (auto pos = 0; pos < corners[0].size(); pos ++) {
            auto &point = corners[0].at(pos);
            cv::putText(frame, cv::format("%d", pos), point + cv::Point2f(3, 3), cv::FONT_HERSHEY_COMPLEX, 0.8, cv::Scalar(255, 0, 0, 0));
        }
        {
            std::vector<cv::Vec3d> rvecs, tvecs;
            cv::aruco::estimatePoseSingleMarkers(corners, marker_half_length_ * 2., intrinsic_, distortion_coeffs_, rvecs, tvecs);
            for (auto pos = 0; pos < ids.size(); pos++) {
                cv::aruco::drawAxis(frame, intrinsic_, distortion_coeffs_, rvecs.at(pos), tvecs.at(pos), marker_half_length_);
            }
        }
        std::vector<cv::Point3f> objects;
        objects.push_back(cv::Point3f(-marker_half_length_, marker_half_length_, 0.));
        objects.push_back(cv::Point3f(marker_half_length_, marker_half_length_, 0.));
        objects.push_back(cv::Point3f(marker_half_length_, -marker_half_length_, 0.));
        objects.push_back(cv::Point3f(-marker_half_length_, -marker_half_length_, 0.));
        cv::Mat transform = cv::Mat::eye(4, 4, CV_64F);
        {
            cv::Mat r, t, rotation;
            r = (cv::Mat_<float>(3, 1) << M_PI, 0., 0.);
            t = (cv::Mat_<float>(3, 1) << 0., 0.01, 0.05);
            std::vector<cv::Point2f> features = corners[0];
            //cv::projectPoints(objects, r, t, intrinsic_, distortion_coeffs_, features);
            if (!cv::solvePnP(objects, features, intrinsic_, distortion_coeffs_, r, t)) {
                return pose;
            }
            std::vector<cv::Point2f> reprojected_points;
            cv::projectPoints(objects, r, t, intrinsic_, distortion_coeffs_, reprojected_points);
            for (auto &point : reprojected_points) {
                cv::circle(frame, point, 3, cv::Scalar(255, 0, 0), -1);
            }
            cv::Rodrigues(r, rotation);
            cv::transpose(rotation, rotation);
            t = -rotation * t;
            rotation.copyTo(transform.rowRange(0, 3).colRange(0, 3));
            t.copyTo(transform.rowRange(0, 3).colRange(3, 4));
        }
        //std::cout << "pose: " << std::endl << transform << std::endl;
        cv::cv2eigen(transform, pose);
        return pose;
    }
protected:
    float marker_half_length_;
    cv::Mat intrinsic_;
    cv::Mat distortion_coeffs_;
    cv::Ptr<cv::aruco::Dictionary> dictionary_;
    cv::Ptr<cv::aruco::DetectorParameters> parameters_;
};

int main(int argc, const char **argv)
{
    cv::VideoCapture capture("./res/dataset.avi");
    //cv::VideoCapture capture(0);
    //cv::VideoWriter vr("dataset.avi", CV_FOURCC('M', 'J', 'P', 'G'), 30.0, cv::Size(640, 480), true);
    if (!capture.isOpened()) {
        return -1;
    }
    cv::Mat image;
    capture >> image;
    int width = image.cols;
    int height = image.rows;
    osgViewer::Viewer viewer;
    viewer.setUpViewInWindow(150, 150, width, height);
    viewer.setThreadingModel(osgViewer::Viewer::SingleThreaded);
    viewer.getCamera()->setClearColor(::osg::Vec4(0.2, 0.2, 0.2, 1.0));
    ::osg::CullStack::CullingMode cullingMode = viewer.getCamera()->getCullingMode();
    cullingMode &= ~(::osg::CullStack::SMALL_FEATURE_CULLING);
    viewer.getCamera()->setCullingMode(cullingMode);
    //viewer.setCameraManipulator(new osgGA::TrackballManipulator());

    double fx = 6.1179365025861671e+02;
    double s = 0;
    double cx = 3.2416033611966498e+02;
    double fy = 6.1179365025861671e+02;
    double cy = 2.5452265814825219e+02;

    double k1 = 1.3179761738023005e-01;
    double k2 = -2.0483007758883057e-01;
    double k3 = -2.2092233752383370e-01;
    double p1 = 2.4661308209571737e-03;
    double p2 = 8.5270505264318684e-04;

    cv::Mat intrinsic = cv::Mat::eye(3, 3, CV_64F);
    intrinsic.at<double>(0, 0)/* fx */ = fx;
    intrinsic.at<double>(0, 1)/* s  */ = s;
    intrinsic.at<double>(0, 2)/* cx */ = cx;
    intrinsic.at<double>(1, 1)/* fy */ = fy;
    intrinsic.at<double>(1, 2)/* cy */ = cy;

    cv::Mat distortion_coeffs = cv::Mat::zeros(5, 1, CV_64F);
    distortion_coeffs.at<double>(0, 0) = k1;
    distortion_coeffs.at<double>(1, 0) = k2;
    distortion_coeffs.at<double>(2, 0) = p1;
    distortion_coeffs.at<double>(3, 0) = p2;
    distortion_coeffs.at<double>(4, 0) = k3;

    PoseEstimater solver = PoseEstimater(intrinsic, distortion_coeffs);
    ::osg::ref_ptr<MotionCamera> motion_camera = new MotionCamera(viewer.getCamera(), width, height, intrinsic, distortion_coeffs);

    ::osg::ref_ptr<::osg::Group> root = new osg::Group();

    ::osg::ref_ptr<::osg::Group> objects = new osg::Group();
    //objects->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
    //objects->addChild(new AxisNode());
    auto node = ::osgDB::readNodeFile("./res/torus.osgt");
    //objects->addChild(node);
    ::osg::ref_ptr<PostureNode> cube = new PostureNode(0.08, 0.08, 0.08);
    osg::Matrix transform = osg::Matrix::rotate(osg::Quat(0., 0., 0., 1)) *
        osg::Matrix::translate(osg::Vec3d(0., 0., 0.08/2.));
    cube->setMatrix(transform);
    objects->addChild(cube);

    auto camera_node = new AxisNode();
    //objects->addChild(camera_node);

    root->addChild(objects);
    root->addChild(motion_camera);

    viewer.setSceneData(root.get());
    viewer.realize();
    while (!viewer.done())
    {
        cv::Mat frame;
        capture >> frame;
        if (frame.empty()) {
            break;
        }
        /*vr.write(frame);
        auto key = cv::waitKey(1);
        if (' ' == key) {
            vr.release();
            break;
        }
        if ('s' == key) {
            cv::imwrite(cv::format("%08d.png", cv::getTickCount()), frame);
        }*/
        Eigen::Matrix4d pose = solver.process(frame);
        {
            Eigen::Quaterniond rotation(pose.block<3, 3>(0, 0));
            Eigen::Vector3d translation(pose.block<3, 1>(0, 3));

            osg::Matrix transform = osg::Matrix::rotate(osg::Quat(rotation.x(), rotation.y(), rotation.z(), rotation.w())) *
                osg::Matrix::translate(osg::Vec3d(translation.x(), translation.y(), translation.z()));

            camera_node->setMatrix(transform);
        }
        motion_camera->update(frame, pose);
        viewer.frame();
    }
    std::system("pause");
    return 0;
}

Reference

NPL: The N-Point Linear (NPL) method of Ansar and Daniilidis [1].

EPnP: The approach of Lepitit et al. [16].

SDP: The Semi Definite Program (SDP) approach of Schweighofer and Pinz [23].

DLS: The Direct Least-Squares (DLS) solution presented in this paper. An open source implementation of DLS is available at www.umn.edu/ ̃joel

DLS-LM: Maximum-likelihood estimate, computed using iterative Levenberg-Marquardt (LM) minimization of the sum of the squared reprojection errors, initialized with DLS.

A Novel Parametrization of the Perspective-Three-Point Problem for a Direct Computation of Absolute Camera Position and Orientation

https://icwww.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf

https://blog.csdn.net/jessecw79/article/details/82945918

https://github.com/jessecw/EPnP_Eigen

Complete Solution Classification for the Perspective-Three-Point Problem

https://www.laurentkneip.com/software