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