C++ ofMatrix4x4::getInverse方法代码示例

void testApp::compute_pda(const ofMatrix4x4 &preTmatrix,int timeZ,ofMatrix4x4 &newTmatrix)//,const ofVec3f* pointmapNow,const ofVec3f* pointmap_pre)
{
	ofMatrix4x4 nowTmatrix;
	if(timeZ==0)
		nowTmatrix = preTmatrix;
	else
	{
		// 迭代
		nowTmatrix = newTmatrix;
		//cout<<"z is big than 0ne"<<endl;
	}
	//ofMatrix4x4 invPreT = nowTmatrix.getInverse();
	ofMatrix4x4 invPreT = preTmatrix.getInverse();
	ofMatrix3x3 nowRmatrix = getRotationMatrix(nowTmatrix);
	ofMatrix3x3 preRmatrix = getRotationMatrix(preTmatrix);

	int i=0,j=0;
	int dnum = 0;
	double total_b = 0;
	double total_bb = 0;

	final_A = Eigen::MatrixXf::Zero(6,6);
	final_B = Eigen::MatrixXf::Zero(6,1);

	// 找到对应点对
	for(j=0;j<KINECT_HEIGHT;j++)
	for(i=0;i<KINECT_WIDTH;i++)
	{
		int num = j*KINECT_WIDTH+i;

		if(pointsmap_orignal[num].z>0)//isvalid_orignal[num])
		{
			ofVec4f pointNow = ofVec4f(pointsmap_orignal[num].x,pointsmap_orignal[num].y,pointsmap_orignal[num].z,1);
			ofVec4f spacePointNow = nowTmatrix*pointNow;
			ofVec3f spacenormalNow = nmmul(nowRmatrix,normalmap_orignal[num]);
			//cout<<i<<" "<<j<<endl;

			ofVec4f pointPreC = invPreT*spacePointNow;
			ofVec3f dpointPreC = ofVec3f(pointPreC.x,pointPreC.y,pointPreC.z);
			ofVec3f pointPreZ = nmmul(the_K_cam,dpointPreC);
			ofVec2f pointPreU = ofVec2f(pointPreZ.x/pointPreZ.z,pointPreZ.y/pointPreZ.z);
			//cout<<endl;

			int num_p = (int)(pointPreU.x+0.5)+(int)(pointPreU.y+0.5)*KINECT_WIDTH;
			int kk = num;
			if(pointsmap_final[num_p].z>0)
			{
				ofVec4f pointPre = ofVec4f(pointsmap_final[num_p].x,pointsmap_final[num_p].y,pointsmap_final[num_p].z,1);
				ofVec4f spacePointPre = pointPre;
				//ofVec4f spacePointPre = preTmatrix*pointPre;
				ofVec3f spacenormalPre = normalmap_final[num_p];
				//ofVec3f spacenormalPre = nmmul(preRmatrix,normalmap_final[num_p]);
				float pointDistance = spacePointNow.distance(spacePointPre);
				float pointAngle = spacenormalNow.angle(spacenormalPre);
				if(pointDistance<=THRESHOLD_D&&pointAngle<=THRESHOLD_A)
				{
					ofVec3f dspacePointNow = ofVec3f(spacePointNow.x,spacePointNow.y,spacePointNow.z);
					ofVec3f dspacePointPre = ofVec3f(spacePointPre.x,spacePointPre.y,spacePointPre.z);
					ofVec3f minsPoint = (dspacePointPre - dspacePointNow);
					float b =  minsPoint.dot(spacenormalPre);
					if(b > 0)
						total_b += b;
					else
						total_bb += b;
					compute_icp(b,dspacePointNow,spacenormalPre);
					++dnum;
				}
			}
		}
	}
	cout<<"dnum = "<<dnum<<" total_b = "<<total_b<<" total_bb = "<<total_bb<<endl;

	//double deta = final_A.determinant();
	//if(deta>0) // 条件待修改
		Eigen::Matrix<float,6,1> result = final_A.llt().solve(final_B).cast<float>();
		
	cout<<"result = "<<endl;
	cout<<result<<endl;

	float alpha = result(0);
	float beta = result(1);
	float gamma = result(2);

	Eigen::Matrix3f rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma,Eigen::Vector3f::UnitZ())*Eigen::AngleAxisf(beta,Eigen::Vector3f::UnitY())*Eigen::AngleAxisf(alpha,Eigen::Vector3f::UnitX());
	ofMatrix4x4 changeMatrix = ofMatrix4x4(rinc(0,0),rinc(0,1),rinc(0,2),result(3),
		                                   rinc(1,0),rinc(1,1),rinc(1,2),result(4),
		                                   rinc(2,0),rinc(2,1),rinc(2,2),result(5),
										   0,0,0,1);
	cout<<"change = "<<endl;
	cout<<changeMatrix<<endl;

	newTmatrix = changeMatrix * newTmatrix;
	cout<<"newTM = "<<endl;
	cout<<newTmatrix<<endl;
}

//--------------------------tsdf计算-----------------------------
void testApp::compute_tsdf(const ofMatrix4x4 &t_g,cv::Mat &depthimage,const int g_size,const ofVec4f &camP)
{
	//ofVec3f iy = nmmul(invKcam,ofVec3f(0,0,1));
	
	// 写入文件
	//ofstream out;
	//out.open("data.txt",ios::trunc);

	// 显示截平面的tsdf数据
	cv::Mat color(g_size,g_size,CV_8UC1);
	cv::Mat bigcol;//(KINECT_HEIGHT,KINECT_WIDTH,CV_8UC1);
	cv::Mat bbcol;

	ofMatrix4x4 invTrans = t_g.getInverse();
	float changenum = 0.0;
	float dist = 0.0;
	float tsdf = 0.0;
	int wight = 0;
	float pretsdf = 0.0;
	int preweight = 0;

	int i,j,k;
	int num = 0;
	for(k=0;k<g_size;k++)
	{
		for(j=0;j<g_size;j++)
		for(i=0;i<g_size;i++)
		{
			ofVec4f spacePostion = ofVec4f(-2.0+i*SIZE_TRIDE,-2.0+j*SIZE_TRIDE,0.5+k*SIZE_TRIDE,1.0);		
			ofVec4f camPostion = invTrans*spacePostion;
			ofVec3f dcamPostion = ofVec3f(camPostion.x,camPostion.y,camPostion.z);
			ofVec3f picPosition = nmmul(the_K_cam,dcamPostion);
			ofVec2f depthPostion = ofVec2f(picPosition.x/picPosition.z,picPosition.y/picPosition.z);
			float minus = 0;

			int x = (int)(max(minus,depthPostion.x)+0.5);
			int y = (int)(max(minus,depthPostion.y)+0.5);
			//cout<<x<<" "<<y<<endl;

			x = min(x,KINECT_WIDTH-1);
			y = min(y,KINECT_HEIGHT-1);
			//cout<<x<<" "<<y<<endl;

			ofVec3f rdepthPostion = ofVec3f(x,y,1);
			changenum = nmmul(invKcam,rdepthPostion).length();
			dist = spacePostion.distance(camP);
			float dd = depth_float[x+y*KINECT_WIDTH];
			if(dd>0)
				tsdf =-(dist/changenum-depth_float[x+y*KINECT_WIDTH]);
			else
				tsdf = 100;
			if(tsdf>0)
			{
				tsdf = min(1.0,tsdf/(SIZE_TRIDE*2));
			}
			else
			{
				tsdf = max(-1.0,tsdf/(SIZE_TRIDE*2));
			}
			// 写入文件与存入内存
			//out<<tsdf<<" ";
			//tsdfData[num] = tsdf;
			num++;

			const int nowweight = 1;
			float newtsdf;
			if(tsdfFirstTime)
			{
				newtsdf = tsdf;
				wight = 1;
			}
			else
			{
				unpack_tsdf(pretsdf,preweight,num);
				wight = max(128,preweight+nowweight);
				newtsdf = (pretsdf*preweight+tsdf)/(wight,nowweight);
			}
			pack_tsdf(newtsdf,wight,num);
		}
	}
	//out.close();
	tsdfFirstTime = false;
	cv::imshow("zzz",color);
}