C++ typenamevector::so3方法代码示例

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

//.........这里部分代码省略.........
	//cout << q_g_f_w << endl;
	ROTATION R_g_f_w(q_g_f_w);
	//cout << R_g_f_w << endl;

	V3 nl_x = R_g_f_w * nl1_w.template cast<POSE_T>();
	axis.normalized();

	V3 c_c = pt1_c.template cast<POSE_T>();
	POSE_T beta = acos(nl1_c(0)); //rotation nl1_w to x axis (1,0,0)
	V3 axis2( 0, nl1_c(2), -nl1_c(1) ); //rotation axis between nl1_m to x axis (1,0,0) i.e. cross( nl1_w, x );
	axis2.normalized();

	Quaternion<POSE_T> q_gp_f_c(AngleAxis<POSE_T>(beta, axis2));
	//cout << q_gp_f_c << endl;
	ROTATION R_gp_f_c(q_gp_f_c);
	//cout << R_gp_f_c << endl;
	//{
	//	Vector3 nl_x = R_gp_f_c * nl1_c;
	//	print<T, Vector3>(nl_x);
	//}

	V3 pt2_g = R_g_f_w * (pt2_w.template cast<POSE_T>() - c_w); pt2_g(0) = POINT_T(0);  pt2_g.normalized();
	V3 pt2_gp = R_gp_f_c * (pt2_c.template cast<POSE_T>() - c_c); pt2_gp(0) = POINT_T(0);  pt2_gp.normalized();

	POSE_T gamma = acos(pt2_g.dot(pt2_gp)); //rotate pt2_g to pt2_gp;
	V3 axis3(1,0,0); 

	Quaternion< POSE_T > q_gp_f_g(AngleAxis<POSE_T>(gamma, axis3));
	//cout << q_gp_f_g << endl;
	ROTATION R_gp_f_g ( q_gp_f_g );
	//cout << R_gp_f_g << endl;

	ROTATION R_c_f_gp = R_gp_f_c.inverse();
	p_solution->so3() = R_c_f_gp * R_gp_f_g * R_g_f_w;
	//{
	//	T3 pt = *R_cfw * (pt2_w - c_w) + c_c;
	//	cout << norm<T, T3>( pt - pt2_c ) << endl;
	//}
	//{
	//	cout << norm<T, T3>(nl1_w) << endl;
	//	cout << norm<T, T3>(nl1_c) << endl;
	//	cout << norm<T, T3>(*R_cfw * nl1_w) << endl;
	//	cout << norm<T, T3>(nl1_c - *R_cfw * nl1_w) << endl;
	//}
	p_solution->translation() = c_c - p_solution->so3() * c_w;

	return;
}


template< typename POSE_T, typename POINT_T > /*Matrix<float,-1,-1,0,-1,-1> = MatrixXf*/
SE3Group<POSE_T>  nl_shinji_ls(const Matrix<POINT_T, Dynamic, Dynamic> & Xw_, const Matrix<POINT_T, Dynamic, Dynamic>&  Xc_,
	const Matrix<POINT_T, Dynamic, Dynamic> & Nw_, const Matrix<POINT_T, Dynamic, Dynamic>&  Nc_, const int K) {
	typedef SE3Group<POSE_T> RT;

	assert(Xw_.rows() == 3);

	//Compute the centroid of each point set
	Matrix<POINT_T, 3, 1> Cw(0, 0, 0), Cc(0, 0, 0); //Matrix<float,3,1,0,3,1> = Vector3f
	for (int nCount = 0; nCount < K; nCount++){
		Cw += Xw_.col(nCount);
		Cc += Xc_.col(nCount);
	}
	Cw /= (POINT_T)K;
	Cc /= (POINT_T)K;