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