コード例 #1
0
int FeatureTransformationEstimator::consensus3D(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d T, double thresh, Eigen::Array<bool, 1, Eigen::Dynamic> &consensusSet)
{
    int consensus = 0;

    P = T * P.colwise().homogeneous();
    Eigen::MatrixXd norms = (P - Q).colwise().norm();
    consensusSet = norms.array() < thresh;
    consensus = consensusSet.count();

    return consensus;
}
コード例 #2
0
int FeatureTransformationEstimator::consensusReprojection(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d T, double thresh, Eigen::Array<bool, 1, Eigen::Dynamic> &consensusSet)
{
    Eigen::MatrixXd Pproj(2, P.cols());
    Eigen::MatrixXd Qproj(2, P.cols());
    for (int i = 0; i < P.cols(); i++) {
        Eigen::Vector3d PT = T * P.col(i).homogeneous();
        Pproj(0,i) = pnp.uc + pnp.fu * PT(0) / PT(2);
        Pproj(1,i) = pnp.vc + pnp.fv * PT(1) / PT(2);

        Qproj(0,i) = pnp.uc + pnp.fu * Q(0,i) / Q(2,i);
        Qproj(1,i) = pnp.vc + pnp.fv * Q(1,i) / Q(2,i);
    }

    int consensus = 0;
    Eigen::MatrixXd norms = (Pproj - Qproj).colwise().norm();
    consensusSet = norms.array() < thresh;
    consensus = consensusSet.count();

    return consensus;
}