ppnp.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 "ppnp.h"

mrpt::vision::pnp::ppnp::ppnp(const Eigen::MatrixXd& obj_pts, const Eigen::MatrixXd& img_pts, const Eigen::MatrixXd& cam_intrinsic)
{
        P = img_pts;
        S = obj_pts;
        C = cam_intrinsic;
}

bool mrpt::vision::pnp::ppnp::compute_pose(Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
{
        double tol=0.00001;
        
        Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n, n), A(n,n), Y(n,3), E(n,3), E_old(n,3), U,V, I3 =Eigen::MatrixXd::Identity(3, 3), PR, Z=Eigen::MatrixXd::Zero(n, n);
        Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);
        
        e.fill(1);
        II.fill(1.0/((double)n));
        
        A=I-e*e.transpose()/n;
        
        double err=std::numeric_limits<double>::infinity();
        
        E_old.fill(1000);
        
    int cnt =0;

        while(err>tol)
        {
                
                Eigen::JacobiSVD<Eigen::MatrixXd> svd(P.transpose()*Z*A*S, Eigen::ComputeThinU | Eigen::ComputeThinV);
                U=svd.matrixU();
                V=svd.matrixV();
                
                I3(2,2) = (U*V.transpose()).determinant();
                R=U*I3*V.transpose();
                PR = P*R;
                
                c=(S-Z*PR).transpose()*II;

                Y=S-e*c.transpose();

                Zmindiag=((PR*Y.transpose()).diagonal()).array() / ((P.array()*P.array()).rowwise().sum()).array();
                
                for (int i = 0; i < n; i++)
                        if (Zmindiag(i) < 0)
                                Zmindiag(i) = 0;


                Z=Zmindiag.asDiagonal();
                
                E=Y-Z*PR;

                err=(E-E_old).norm();

                E_old=E;
        
        cnt ++;
    
        }

        t=-R*c;
        
        return 1;
        
        
}