void nl_shinji_kneip_ransac(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter,
const POINT_T thre_3d_, const POINT_T thre_2d_, const POINT_T nl_thre, int& Iter, POINT_T confidence = 0.99){
typedef Matrix<POINT_T, Dynamic, Dynamic> MX;
typedef Matrix<POINT_T, 3, 1> P3;
typedef SE3Group<POSE_T> RT;
POSE_T cos_thr = cos(atan(thre_2d_ / adapter.getFocal()));
POINT_T cos_nl_thre = cos(nl_thre);
RandomElements<int> re((int)adapter.getNumberCorrespondences());
const int K = 3;
MX Xw(3, K + 1), Xc(3, K + 1), bv(3, K + 1);
MX Nw(3, K + 1), Nc(3, K + 1);
Matrix<short, Dynamic, Dynamic> inliers(adapter.getNumberCorrespondences(), 3);
adapter.setMaxVotes(-1);
for (int ii = 0; ii < Iter; ii++) {
//randomly select K candidates
RT solution_kneip, solution_shinji, solution_nl;
vector<RT> v_solutions;
vector<int> selected_cols;
re.run(K + 1, &selected_cols);
if (assign_sample<POSE_T, POINT_T>(adapter, selected_cols, &Xw, &Nw, &Xc, &Nc, &bv)){
solution_shinji = shinji<POSE_T, POINT_T>(Xw, Xc, K);
v_solutions.push_back(solution_shinji);
}
if (kneip<POSE_T, POINT_T>(Xw, bv, &solution_kneip)){
v_solutions.push_back(solution_kneip);
}
nl_2p<POSE_T, POINT_T>(Xc.col(0), Nc.col(0), Xc.col(1), Xw.col(0), Nw.col(0), Xw.col(1), &solution_nl);
v_solutions.push_back(solution_nl);
for (typename vector<RT>::iterator itr = v_solutions.begin(); itr != v_solutions.end(); ++itr) {
//collect votes
int votes = 0;
inliers.setZero();
P3 eivE; P3 pc; POINT_T cos_a;
for (int c = 0; c < adapter.getNumberCorrespondences(); c++) {
if (adapter.isValid(c)){
//with normal data
POINT_T cos_alpha = adapter.getNormalCurr(c).dot(itr->so3().template cast<POINT_T>() * adapter.getNormalGlob(c));
if (cos_alpha > cos_nl_thre){
inliers(c, 2) = 1;
votes++;
}
//with 3d data
eivE = adapter.getPointCurr(c) - (itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>());
if (eivE.norm() < thre_3d_){
inliers(c, 1) = 1;
votes++;
}
}
//with 2d
pc = itr->so3().template cast<POINT_T>() * adapter.getPointGlob(c) + itr->translation().template cast<POINT_T>();// transform pw into pc
pc = pc / pc.norm(); //normalize pc
//compute the score
cos_a = pc.dot( adapter.getBearingVector(c) );
if (cos_a > cos_thr){
inliers(c, 0) = 1;
votes++;
}
}
//cout << endl;
if (votes > adapter.getMaxVotes() ){
assert(votes == inliers.sum());
adapter.setMaxVotes(votes);
adapter.setRcw(itr->so3());
adapter.sett(itr->translation());
adapter.setInlier(inliers);
//cout << inliers.inverse() << endl << endl;
//adapter.printInlier();
//Iter = RANSACUpdateNumIters(confidence, (POINT_T)(adapter.getNumberCorrespondences() * 3 - votes) / adapter.getNumberCorrespondences() / 3, K, Iter);
}
}//for(vector<RT>::iterator itr = v_solutions.begin() ...
}//for(int ii = 0; ii < Iter; ii++)
PnPPoseAdapter<POSE_T, POINT_T>* pAdapterPnP = &adapter;
pAdapterPnP->cvtInlier();
AOPoseAdapter<POSE_T, POINT_T>* pAdapterAO = &adapter;
pAdapterAO->cvtInlier();
adapter.cvtInlier();
return;
}
