PoseSolverWrapper.cpp 2.46 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11
//
//  PoseSolverWrapper.cpp
//  VADSDelegates
//
//  Created by JiangZhping on 2017/2/1.
//  Copyright © 2017年 JiangZhiping. All rights reserved.
//

#include "PoseSolverWrapper.h"
#include "RPP.h"

12
std::tuple<cv::Vec3d, cv::Vec3d, cv::Matx33d, cv::Matx44d> PoseSolverWrapper::solvePnP(const std::vector<cv::Point2f> & imagePoints, const std::vector<cv::Point3f> & modelPoints, PoseEstimationSolver solveMethod, const cv::Matx33d & cameraMatrix, const cv::Vec5d distortionEfficient) {
13
    
14
    cv::Vec3d rvec, tvec;
15
    cv::Matx33d pose;
16
    cv::Matx44d transform;
17
    if (solveMethod == PnPSolverMethodIterative) {
18
        cv::solvePnP(modelPoints, imagePoints, cameraMatrix, distortionEfficient, rvec, tvec, false, 0); // cv::SOLVEPNP_ITERATIVE
19
    } else if(solveMethod == PnPSolverMethodEPnP) {
20
        cv::solvePnP(modelPoints, imagePoints, cameraMatrix, distortionEfficient, rvec, tvec, false, 1); // cv::SOLVEPNP_EPNP
21
    } else if(solveMethod == PnPSolverMethodDLS) {
22
        cv::solvePnP(modelPoints, imagePoints, cameraMatrix, distortionEfficient, rvec, tvec, false, 3); // cv::SOLVEPNP_DLS
23
    } else if(solveMethod == PnPSolverMethodUPnP) {
24
        cv::solvePnP(modelPoints, imagePoints, cameraMatrix, distortionEfficient, rvec, tvec, false, 4); // cv::SOLVEPNP_UPNP
25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40
    } else if(solveMethod == PnPSolverMethodRPP) {
        cv::Mat modelPointsMat = cv::Mat(modelPoints).reshape(1).t();
        cv::Mat imagePointsMat = cv::Mat(imagePoints).reshape(1).t();
        modelPointsMat.convertTo(modelPointsMat, CV_64F);
        imagePointsMat.convertTo(imagePointsMat, CV_64F);
        cv::Mat1d normalizedImagePointsMat = cv::Mat1d::zeros(3,imagePoints.size());
        normalizedImagePointsMat.row(2).setTo(cv::Scalar(1.0));
        imagePointsMat.copyTo(normalizedImagePointsMat.rowRange(0, 2));
        for (int i = 0 ; i < imagePoints.size(); i ++) {
            cv::Mat1d normalizedP = cv::Mat1d(cameraMatrix.inv()) * normalizedImagePointsMat.col(i);
            normalizedP.copyTo (normalizedImagePointsMat.col(i));
        }
        int iteration;
        double objectError;
        double imageError;
        cv::Mat1d rm;
41 42
        cv::Mat1d tvecMat;
        RPP::Rpp(modelPointsMat, normalizedImagePointsMat, rm, tvecMat, iteration, objectError, imageError);
43
        cv::Rodrigues(rm, rvec);
44
        tvecMat.copyTo(tvec);
45 46
    }
    cv::Rodrigues(rvec, pose);
47
    transform = cve::matx::joinRotationMatAndTranslationVec(pose, tvec);
48
    
49
    return std::make_tuple(rvec, tvec, pose, transform);
50
}