posit.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers
#include <iostream>
#include <mrpt/utils/types_math.h>  // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "posit.h"

mrpt::vision::pnp::posit::posit(
        Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
        Eigen::MatrixXd camera_intrinsic_, int n0)
{
        obj_pts = obj_pts_;
        img_pts = img_pts_.block(0, 0, n0, 2);
        cam_intrinsic = camera_intrinsic_;
        R = Eigen::MatrixXd::Identity(3, 3);
        t = Eigen::VectorXd::Zero(3);
        f = (cam_intrinsic(0, 0) + cam_intrinsic(1, 1)) / 2;

        obj_matrix =
                (obj_pts.transpose() * obj_pts).inverse() * obj_pts.transpose();

        n = n0;

        obj_vecs = Eigen::MatrixXd::Zero(n0, 3);

        for (int i = 0; i < n; i++)
                obj_vecs.row(i) = obj_pts.row(i) - obj_pts.row(0);

        img_vecs = Eigen::MatrixXd::Zero(n0, 2);
        img_vecs_old = img_vecs;

        epsilons = Eigen::VectorXd::Zero(n);
}

void mrpt::vision::pnp::posit::POS()
{
        Eigen::Vector3d I0, J0, r1, r2, r3;
        double I0_norm, J0_norm;

        int i;
        double scale;

        for (i = 0; i < 3; i++)
        {
                I0(i) = obj_matrix.row(i).dot(img_vecs.col(0));
                J0(i) = obj_matrix.row(i).dot(img_vecs.col(1));
        }

        I0_norm = I0.norm();
        J0_norm = J0.norm();

        scale = (I0_norm + J0_norm) / 2;

        /*Computing TRANSLATION */
        t(0) = img_pts(0, 0) / scale;
        t(1) = img_pts(0, 1) / scale;
        t(2) = f / scale;

        /* Computing ROTATION */
        r1 = I0 / I0_norm;
        r2 = J0 / J0_norm;
        r3 = r1.cross(r2);

        R.row(0) = r1;
        R.row(1) = r2;
        R.row(2) = r3;
}

/**
Iterate over results obtained by the POS function;
see paper "Model-Based Object Pose in 25 Lines of Code", IJCV 15, pp. 123-141,
1995.
*/
bool mrpt::vision::pnp::posit::compute_pose(
        Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
        Eigen::FullPivLU<Eigen::MatrixXd> lu(obj_pts);
        if (lu.rank() < 3) return false;

        int i, iCount;
        long imageDiff = 1000;

        for (iCount = 0; iCount < LOOP_MAX_COUNT; iCount++)
        {
                if (iCount == 0)
                {
                        for (i = 0; i < img_vecs.rows(); i++)
                                img_vecs.row(i) = img_pts.row(i) - img_pts.row(0);
                }

                else
                {
                        // Compute new image vectors
                        epsilons.setZero();
                        for (i = 0; i < n; i++)
                        {
                                epsilons(i) += obj_vecs.row(i).dot(R.row(2));
                        }
                        epsilons /= t(2);

                        // Corrected image vectors
                        for (i = 0; i < n; i++)
                        {
                                img_vecs.row(i) =
                                        img_pts.row(i) * (1 + epsilons(i)) - img_pts.row(0);
                        }

                        imageDiff = this->get_img_diff();
                }

                img_vecs_old = img_vecs;

                this->POS();

                if (iCount > 0 && imageDiff == 0) break;

                if (iCount == LOOP_MAX_COUNT)
                {
                        std::cout << "Solution Not converged" << std::endl << std::endl;
                        break;
                }
        }
        R_ = R;
        t_ = t;

        return true;
}

long mrpt::vision::pnp::posit::get_img_diff()
{
        int i, j;
        long sumOfDiffs = 0;

        for (i = 0; i < n; i++)
        {
                for (j = 0; j < 2; j++)
                {
                        sumOfDiffs += std::abs(
                                floor(0.5 + img_vecs(i, j)) - floor(0.5 + img_vecs_old(i, j)));
                }
        }
        return sumOfDiffs;
}