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;
}
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;
}