C++ Matrix3f::Display方法代码示例

void rectify::combineMatrices()
{
 firstHomography = euclideanTrans * projectiveTrans;
 secondHomography = euclideanTransDash * projectiveTransDash;
 firstHomography.Display();
 secondHomography.Display();


 std::vector<vector3d>checkVec;
 checkVec.clear();
 checkVec.reserve(6);
 checkVec.push_back(vector3d(0, 0, 1));
 checkVec.push_back(vector3d(800, 0, 1));
 checkVec.push_back(vector3d(0, 600, 1));
 checkVec.push_back(vector3d(400, 300, 1));
 checkVec.push_back(vector3d(800, 600, 1));
 checkVec.push_back(vector3d(500, 500, 1));

 vector3d first, second;
 for(int i = 0; i < checkVec.size(); i++)
 {
	first = firstHomography * checkVec[i];
	second = secondHomography * checkVec[i];
	first.Display("first");
	second.Display("second");
 }

 int dataID;
 for(int i = 0; i < global::inliersRecord.size(); i++)
 {
  global::secondImagePoints[dataID];
  dataID = inliersRecord[i];
  first = global::firstImagePoints[dataID];
  first = firstHomography * first;
  first.change(first.x/first.z, first.y/first.z, 1.0);
  first.Display("fiC");

  second = global::secondImagePoints[dataID];
  second = secondHomography * second;
  second.change(second.x/second.z, second.y/second.z, 1.0);
  second.Display("secChan");

  std::cout<<" disparity "<<fabs(first.x - second.x)<<std::endl;
 }

 first = firstHomography * global::epipoleA;
 first.Display("f");
 second = secondHomography * global::epipoleB;
  second.Display("s");


vector3d inf = vector3d(1, 0, 0);
Matrix3f infX;
//infX.AsymmetricMatrix(inf);
infX.m[2][1] = 1;
infX.m[1][2] = -1;
(secondHomography.ReturnTranspose() * infX * firstHomography).Display("Fun");
Matrix3f fund = global::fundamentalMatrix;
 fund = fund * (1/fund.m[2][1]);
fund.Display("DAm");

}

//.........这里部分代码省略.........

 solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
 homographyMat = Matrix3f(outputMat);
 matD.Inverse();
 homographyMat = matD * homographyMat * mat;
 //homographyMat.Display();
 ransacItr++;							//counter increased
//check accuracy of homographyMat with other points
 for(int i = 0; i < totalPointSize; i++)
 {
	first = homographyMat * firstImagePoints[i];
	first.change(first.x/first.z, first.y/first.z, 1.0);
	second = secondImagePoints[i];
	errorDeviation[i] = second.distancePointsSquared(first);
	std::cout<<" index  "<<i<<" and S.D.  "<<sqrt(errorDeviation[i])<<"   ";
	//first.Display("f");	second.Display("s");
	threshold += errorDeviation[i];
	if( i % 2 == 0) std::cout<<std::endl;
 }
	threshold /= totalPointSize;
	threshold = sqrt(threshold) * 0.8;
std::cout<<" thres "<<threshold<<std::endl;

 oldSampleSize = sampleInliers.size();				//'s' is the sample Size, or old num_pts
 sampleInliers.clear();
 sampleInliers.reserve(totalPointSize);

 std::cout<<std::endl;
 for(int i = 0; i < totalPointSize; i++)
 {
	 if(sqrt(errorDeviation[i]) <= threshold)
	 {
		std::cout<<" Pindex "<<i<<" deviation "<<sqrt(errorDeviation[i])<<"   ";
		if( i % 2 == 0) std::cout<<std::endl;
		sampleInliers.push_back(i);			//'Si' is the consensus set
	 }
 }
 oldSampleSize = sampleInliers.size() - oldSampleSize;
 std::cout<<std::endl<<" sampleInliersPushed  "<<oldSampleSize<<"  "<<std::endl;

 num_pts = sampleInliers.size();
 omega = (float)num_pts / totalPointSize;			//probability of an inlier
 omega = log10(1 - pow(omega, oldSampleSize));
 capitalN = (-2)/omega;
	std::cout<<std::endl<<" omega  "<<omega<<" capitalN "<<capitalN<<std::endl;
 if(capitalN <= ransacItr || ransacItr > 50) ransacRepeat = false;

//need to check whether omega was converging
}

 std::cout<<std::endl;
//final re-estimation of the model
 normalization(sampleInliers, mat, matD);
 for(int i = 0; i < num_pts; i++)
 {	
	dataID = sampleInliers[i];
	first = global::firstImagePoints[dataID];
	second = global::secondImagePoints[dataID];
	first = mat * first;
	second = matD * second;
	coeffMat[2 * i][0] = 0;
	coeffMat[2 * i][1] = 0;
	coeffMat[2 * i][2] = 0;
	coeffMat[2 * i][3] = -second.z * first.x;
	coeffMat[2 * i][4] = -second.z * first.y;
	coeffMat[2 * i][5] = -second.z * first.z;
	coeffMat[2 * i][6] = second.y * first.x;
	coeffMat[2 * i][7] = second.y * first.y;
	coeffMat[2 * i][8] = second.y * first.z;

	coeffMat[2 * i + 1][0] = second.z * first.x;
	coeffMat[2 * i + 1][1] = second.z * first.y;
	coeffMat[2 * i + 1][2] = second.z * first.z;
	coeffMat[2 * i + 1][3] = 0;
	coeffMat[2 * i + 1][4] = 0;
	coeffMat[2 * i + 1][5] = 0;
	coeffMat[2 * i + 1][6] = -second.x * first.x;
	coeffMat[2 * i + 1][7] = -second.x * first.y;
	coeffMat[2 * i + 1][8] = -second.x * first.z;
 }

 solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
 std::cout<<" answer "<<std::endl;
	for(int j = 0; j < 9; j++)
 std::cout<<" "<<outputMat[j]<<"  ";

 homographyMat = Matrix3f(outputMat);
 homographyMat.Display("homoSmall");
 matD.Inverse();
 homographyMat = matD * homographyMat * mat;
 homographyMat.Display("homography");

global::inliersRecord.clear();
global::inliersRecord.reserve(sampleInliers.size());
for(int i = 0; i < sampleInliers.size(); i++)
	global::inliersRecord.push_back(sampleInliers[i]);

 //release memory
 delete[] coeffMat;
}