C++ Matrix3d::diagonal方法代码示例

void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
	//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
	using namespace Eigen;

	Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
	Matrix3d EEt = E * E.transpose();
	Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
	Vector3d b1,b2;

#if 1
	//Method 1
	Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
	Vector3d bbt_diag = bbt.diagonal();
	if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
		b1 = bbt.row(0) / sqrt(bbt_diag(0));
		b2 = -b1;
	} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
		b1 = bbt.row(1) / sqrt(bbt_diag(1));
		b2 = -b1;
	} else {
		b1 = bbt.row(2) / sqrt(bbt_diag(2));
		b2 = -b1;
	}
#else
	//Method 2
	if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
		b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
		b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else {
		b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	}
#endif
	
	//Horn90 (19)
	Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
	cofactors.transposeInPlace();
	
	//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
	Matrix3d B1; B1 <<	0,-b1(2),b1(1),
						b1(2),0,-b1(0),
						-b1(1),b1(0),0;
	Matrix3d B2; B2 <<	0,-b2(2),b2(1),
						b2(2),0,-b2(0),
						-b2(1),b2(0),0;

	Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);

	//Horn90 (24)
	R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2); 
	t1 = b1; t2 = b2;
	
	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}

void CFullRelPose::writeEdge(std::ostream & toroGraphFile, const bool b2d) const
{
	if(IS_DEBUG) CHECK(!hasPosition(), "Node is disconnected?? Or too bad");

	const double dLength = length();
	//double dVar = sqr(dLength/4);// scale.scaleVarHacked();
	double dVar = variance();
	dVar = std::max<double>(1e-5, dVar);

	normalisedPose.scale(dLength).write(toroGraphFile, b2d);

	Vector3d t, x_axis; x_axis.setZero();
	normalisedPose.t.asVector(t);

	if(b2d)
	{
		//double inf_ff, inf_fs, inf_ss, inf_rr, inf_fr, inf_sr;
		Vector2d t2d(t(0), t(2));
		Vector2d t2d_perp(t(2), -t(0));

		Matrix2d Q, LAMBDA;
		LAMBDA.setZero();

		Q << t2d, t2d_perp;

		LAMBDA(0,0) = dVar; //dVar may be zero
		LAMBDA(1,1) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

		CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

		const Matrix2d C = Q * LAMBDA * Q.transpose();
		const Matrix2d & I = C.inverse();
		//cout << I << endl;

		//THROW("Not complete...")
		double dRotInf = 1.0/sqr(normalisedPose.SD.relOrientationSD());
		toroGraphFile << I(0,0) << ' ' << I(1,0) << ' ' << I(1,1) << ' ' << dRotInf << " 0 0";

		return;
	}

	if(t(0) < 0.9) //check t isn't x axis aligned (x is arbitrary)
		x_axis(0) = 1;
	else
		x_axis(1) = 1;

	Matrix3d Q;

	if(t.sum() == 0) //Pure rotation. Should be arbitrary
	{
		Q.setIdentity();
	}
	else
	{
		Vector3d tperp1 = t.cross(x_axis);
		tperp1.normalize();

		Vector3d tperp2 = tperp1.cross(t);

		Q << t, tperp1, tperp2;
	}

	CHECK(isnan(Q.sum()), "writeEdge: nan")

	Matrix3d LAMBDA; LAMBDA.setZero();
	LAMBDA(0,0) = dVar; //dVar may be zero
	LAMBDA(1,1) = LAMBDA(2,2) = dVar * sqr(normalisedPose.SD.cameraMotionAngleSD());

	CHECK(isnan(LAMBDA.sum()), "writeEdge: nan")

	const Matrix3d C = Q * LAMBDA * Q.transpose();

	//std::cout << "Covariance: " << std::endl << C << std::endl;
	CHECK(isnan(C.sum()), "writeEdge: nan")

	Matrix3d Crpy; Crpy.setZero();
	Crpy.diagonal().setConstant(sqr(normalisedPose.SD.relOrientationSD()));
	Matrix<double, 6, 6> Cfull; Cfull << C, Matrix3d::Zero(), Matrix3d::Zero(), Crpy;

	CHECK(isnan(Cfull.sum()), "writeEdge: nan")

	if(Cfull.trace() < 0.0001)
	{
		std::cout << "Warning: Covariance matrix near-singular, adjusting diagonal\n";
		Cfull.diagonal().array() += 0.0001;
	}

	const Matrix<double, 6, 6> & Ifull = Cfull.inverse();
	//std::cout << "Information: " << std::endl << Ifull << std::endl;

	//cout << "Warning: Not inverting information matrix\n"; Yes the information mat does work a little better.
	//Possibly ok for 1 edge...if(IS_DEBUG) CHECK(Cfull.determinant()<0.0001, "CFullRelPose::writeEdge: Singular covariance matrix");

	CHECK(isnan(Ifull.sum()), "writeEdge: nan")

	for (int nRow = 0; nRow <6; nRow++)
	{
		for(int nCol = nRow; nCol < 6; nCol++)
			toroGraphFile << Ifull(nRow, nCol) << ' ';
	}
//.........这里部分代码省略.........