//--------------------------PDA与ICP计算------------------------------
ofMatrix3x3 getRotationMatrix(const ofMatrix4x4& Tmaxtrix)
{
ofMatrix3x3 Rmaxtrix = ofMatrix3x3(Tmaxtrix(0,0),Tmaxtrix(0,1),Tmaxtrix(0,2),
Tmaxtrix(1,0),Tmaxtrix(1,1),Tmaxtrix(1,2),
Tmaxtrix(2,0),Tmaxtrix(2,1),Tmaxtrix(2,2));
return Rmaxtrix;
}
ofMatrix3x3 EnvironmentMapping::mat4ToMat3(ofMatrix4x4 mat4)
{
return ofMatrix3x3(mat4._mat[0][0], mat4._mat[0][1], mat4._mat[0][2], mat4._mat[1][0], mat4._mat[1][1], mat4._mat[1][2], mat4._mat[2][0], mat4._mat[2][1], mat4._mat[2][2]);
}
ofMatrix3x3 CubeMapping::mat4ToMat3(ofMatrix4x4 mat4)
{
return ofMatrix3x3(mat4._mat[0][0], mat4._mat[0][1], mat4._mat[0][2], mat4._mat[1][0], mat4._mat[1][1], mat4._mat[1][2], mat4._mat[2][0], mat4._mat[2][1], mat4._mat[2][2]);
}
ofMatrix3x3 ofApp::mat4ToMat3(ofMatrix4x4 mat4)
{
return ofMatrix3x3(mat4._mat[0][0], mat4._mat[0][1], mat4._mat[0][2], mat4._mat[1][0], mat4._mat[1][1], mat4._mat[1][2], mat4._mat[2][0], mat4._mat[2][1], mat4._mat[2][2]);
}
//--------------------------------------------------------------
void testApp::setup() {
// 试验用函数
testh();
ofSetLogLevel(OF_LOG_VERBOSE);
// 初始化
int count = 0;
m_nui = NULL;
bKinectInitSucessful = false;
HRESULT hr = NuiGetSensorCount(&count);
if(count <= 0)
{
std::cout<<"No kinect sensor was found!!"<<endl;
goto FINAL;
}
hr = NuiCreateSensorByIndex(0,&m_nui);
if(FAILED(hr))
{
std::cout<<"create failed"<<endl;
goto FINAL;
}
hr=m_nui->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH);
//hr=m_nui->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX);//NUI_INITIALIZE_FLAG_USES_DEPTH);
if(FAILED(hr))
{
std::cout<<"NuiInitialize failed"<<endl;
goto FINAL;
}
depthStreamHandle = NULL;
hr = m_nui->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH,NUI_IMAGE_RESOLUTION_640x480,0,2,NULL,&depthStreamHandle);
//hr=m_nui->NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX,NUI_IMAGE_RESOLUTION_320x240,0,2,NULL,&depthStreamHandle);
if(FAILED(hr))
{
std::cout<<"Could not open color image stream video"<<endl;
goto FINAL;
}
nearThreshold = 255;
farThreshold = 220;
bThreshWithOpenCV = true;
ofSetFrameRate(60);
// 加入cvwindow
cv::namedWindow("orginal");
// start from the front
bDrawPointCloud = false;
// 我的改变
test=true; // Mat存取控制
test2=false;// 实验区控制
test3=true; // depthfloat存入文件控制
countnum = 0;// 融合帧数量
photonum = 0;// 读入照片数量（无用）
finalPointNum = 0;// 光线投影点云数量
tsdfFirstTime = true;// tsdf初始控制
draww=false;// 法向绘制控制（n键）
// 摄像机内参
the_K_cam=ofMatrix3x3(589.3,0,323,0,590.4,244.6,0,0,1);
invKcam = the_K_cam.inverse(the_K_cam);
std::cout<<invKcam<<endl;
//m_points=(ofVec3f*)malloc(sizeof(ofVec3f)*245*325);
depth_float= new float[KINECT_WIDTH*KINECT_HEIGHT];
tsdfWeightData = new short2[TSDF_SIZE*TSDF_SIZE*TSDF_SIZE];
// ICP得到的变换矩阵
final_A = Eigen::MatrixXf::Zero(6,6);
final_B = Eigen::MatrixXf::Zero(6,1);
clock_t t1=clock();
//readglobalfile("data.txt",tsdfData);
//readglobalfile("0.txt",depth_float);
//readFileToArry(pointsmap_orignal,"testdata1.txt");
//readFileToArry(pointsmap_final,"testdata1.txt");
//testTheIcp(pointsmap_orignal,pointsmap_final);
clock_t t2=clock();
double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
//cout<<ti<<endl;
bKinectInitSucessful = true;
// 摄像机位置控制
x_back = 0;
y_back = 0;
z_back = -650;
FINAL:
if(FAILED(hr))
{
std::cout<<"待做撤销工作"<<endl;
if(m_nui != NULL)
{
m_nui->NuiShutdown();
m_nui->Release();
m_nui = NULL;
}
}
}
