void KINECT_BASE::UpdateData(VISION_DATA &data) { mKinectStruct->mStatus = mKinectStruct->mContext.WaitAndUpdateAll(); //mKinectStruct->CheckOpenNIError(mKinectStruct->mStatus, "View Cloud"); memset(&data, 0, sizeof(data)); const XnDepthPixel* pDepthMap = mKinectStruct->mDepthGenerator.GetDepthMap(); memcpy(data.depthMap, pDepthMap, 480*640*sizeof(unsigned short)); GeneratePointCloud(mKinectStruct->mDepthGenerator, pDepthMap, data); /* // Experiment Robot Matrix4f kinectAdjust; kinectAdjust<< -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; Matrix4f kinectToRobot; kinectToRobot<<0.9995, 0.0134, -0.0273, 0.0224, -0.0304, 0.5120, -0.8584, 0.2026 + 0.038, 0.0025, 0.8589, 0.5122, 0.5733, 0, 0, 0, 1; */ // Experiment Desk Matrix4f kinectAdjust; kinectAdjust<< -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; Matrix4f kinectToRobot; kinectToRobot<< 0.9989836, -0.001837780, -0.0450375377, 0.017682, -0.038951129, 0.4676363607, -0.88306231020, 0.197015+0.038, 0.02268406498, 0.88391903289, 0.46708947376, 0.561919, 0, 0, 0, 1; Matrix4f robotToWorld; robotToWorld << 1, 0, 0, 0, 0, 1, 0, 0.85, 0, 0, 1, 0, 0, 0, 0, 1; Matrix4f kinectToWorld = robotToWorld*kinectToRobot*kinectAdjust; ofstream ofs1; stringstream out1; out1<<frameNum; string dataname1 ="../PointCloud/originalcloud" + out1.str() + ".txt"; ofs1.open(dataname1,ios::trunc); ofstream ofs2; stringstream out2; out2<<frameNum; string dataname2 ="../PointCloud/transformedcloud_" + out2.str() + ".txt"; ofs2.open(dataname2,ios::trunc); ofstream ofs3; stringstream out3; out3<<frameNum; string dataname3 ="../PointCloud/grid_" + out3.str() + ".txt"; ofs3.open(dataname3,ios::trunc); for (int i = 0; i < 480; i++) { for(int j = 0; j < 640; j++) { ofs1<<data.pointCloud[i][j][0]/1000<<" "<<data.pointCloud[i][j][1]/1000<<" "<<data.pointCloud[i][j][2]/1000<<" "<<endl; } } for (int i = 0; i < 480; i++) { for(int j = 0; j < 640; j++) { // if(data.pointCloud[i][j][0] != 0&&data.pointCloud[i][j][1] != 0&&data.pointCloud[i][j][2] != 0) if(data.pointCloud[i][j][2] != 0) { Point3D tempPoint = {0, 0, 0}; tempPoint.X = kinectToWorld(0, 0)*data.pointCloud[i][j][0] + kinectToWorld(0, 1)*data.pointCloud[i][j][1] + kinectToWorld(0, 2)*data.pointCloud[i][j][2] + kinectToWorld(0, 3)*1000; tempPoint.Y = kinectToWorld(1, 0)*data.pointCloud[i][j][0] + kinectToWorld(1, 1)*data.pointCloud[i][j][1] + kinectToWorld(1, 2)*data.pointCloud[i][j][2] + kinectToWorld(1, 3)*1000; tempPoint.Z = kinectToWorld(2, 0)*data.pointCloud[i][j][0] + kinectToWorld(2, 1)*data.pointCloud[i][j][1] + kinectToWorld(2, 2)*data.pointCloud[i][j][2] + kinectToWorld(2, 3)*1000; data.pointCloud[i][j][0] = tempPoint.X/1000; data.pointCloud[i][j][1] = tempPoint.Y/1000; data.pointCloud[i][j][2] = tempPoint.Z/1000; } ofs2<<data.pointCloud[i][j][0]<<" "<<data.pointCloud[i][j][1]<<" "<<data.pointCloud[i][j][2]<<" "<<endl; } } GenerateGridMap(data); for(int i = 0; i < 60; i++) { for(int j = 0; j < 60; j++) { ofs3<<data.pGridMap[i][j].X<<" "<<data.pGridMap[i][j].Y<<" "<<data.pGridMap[i][j].Z<<endl; } } cout<<"FrameNum: "<<frameNum<<endl; ofs1.close(); ofs2.close(); ofs3.close(); }
void KINECT_BASE::updateData(VISION_DATA &data) { mKinectStruct->mStatus = mKinectStruct->mContext.WaitAndUpdateAll(); //mKinectStruct->CheckOpenNIError(mKinectStruct->mStatus, "View Cloud"); memset(&data, 0, sizeof(data)); const XnDepthPixel* pDepthMap = mKinectStruct->mDepthGenerator.GetDepthMap(); memcpy(data.depthMap, pDepthMap, 480*640*sizeof(unsigned short)); GeneratePointCloud(mKinectStruct->mDepthGenerator, pDepthMap, data); Matrix4f kinectAdjust; kinectAdjust<< 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; // Matrix4f kinectToRobot; // kinectToRobot<< 0.9989836, -0.001837780, -0.0450375377, 0.017682, // -0.038951129, 0.4676363607, -0.88306231020, 0.197015+0.038, // 0.02268406498, 0.88391903289, 0.46708947376, 0.561919, // 0, 0, 0, 1; // Matrix4f kinectToRobot; // kinectToRobot<< 1, 0, 0, 0.017682, // 0, 0.866, -0.5, 0.197015+0.038+0.85, // 0, 0.5, 0.866, 0, // 0, 0, 0, 1; //Kinect On Desk //Matrix4f kinectToRobot; //kinectToRobot<< 1, 0, 0, 0, //0, 0.891, -0.454, 0.797, //0, 0.454, 0.891, 0, //0, 0, 0, 1; //Kinect On Robot Matrix4f kinectToRobot; kinectToRobot<< 1, 0, 0, 0, 0, 0.8746, -0.4848, 0.9610, 0, 0.4848, 0.8746, 0, 0, 0, 0, 1; Matrix4f robotToWorld; robotToWorld << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; // robotToWorld << 1, 0, 0, 0, // 0, 1, 0, 0, // 0, 0, 1, 0, // 0, 0, 0, 1; Matrix4f kinectToWorld = robotToWorld*kinectToRobot*kinectAdjust; // write point cloud data // ofstream ofs1; // stringstream out1; // out1<<frameNum; // string dataname1 ="../PointCloud/originalcloud" + out1.str() + ".txt"; // ofs1.open(dataname1,ios::trunc); // ofstream ofs2; // stringstream out2; // out2<<frameNum; // string dataname2 ="../PointCloud/transformedcloud_" + out2.str() + ".txt"; // ofs2.open(dataname2,ios::trunc); // ofstream ofs3; // stringstream out3; // out3<<frameNum; // string dataname3 ="../PointCloud/grid_" + out3.str() + ".txt"; // ofs3.open(dataname3,ios::trunc); // for (int i = 0; i < 480; i++) // { // for(int j = 0; j < 640; j++) // { // ofs1<<data.pointCloud[i][j][0]/1000<<" "<<data.pointCloud[i][j][1]/1000<<" "<<data.pointCloud[i][j][2]/1000<<" "<<endl; // } // } for (int i = 0; i < 480; i++) { for(int j = 0; j < 640; j++) { if(data.pointCloud[i][j][2] != 0) { Point3D tempPoint = {0, 0, 0}; tempPoint.X = kinectToWorld(0, 0)*data.pointCloud[i][j][0] + kinectToWorld(0, 1)*data.pointCloud[i][j][1] + kinectToWorld(0, 2)*data.pointCloud[i][j][2] + kinectToWorld(0, 3)*1000; tempPoint.Y = kinectToWorld(1, 0)*data.pointCloud[i][j][0] + kinectToWorld(1, 1)*data.pointCloud[i][j][1] + kinectToWorld(1, 2)*data.pointCloud[i][j][2] + kinectToWorld(1, 3)*1000; tempPoint.Z = kinectToWorld(2, 0)*data.pointCloud[i][j][0] + kinectToWorld(2, 1)*data.pointCloud[i][j][1] + kinectToWorld(2, 2)*data.pointCloud[i][j][2] + kinectToWorld(2, 3)*1000; data.pointCloud[i][j][0] = tempPoint.X/1000; data.pointCloud[i][j][1] = tempPoint.Y/1000; data.pointCloud[i][j][2] = tempPoint.Z/1000; } // ofs2<<data.pointCloud[i][j][0]<<" "<<data.pointCloud[i][j][1]<<" "<<data.pointCloud[i][j][2]<<" "<<endl; } } GenerateGridMap(data); GenerateObstacleMap(data); // for(int i = 0; i < 60; i++) // { // for(int j = 0; j < 60; j++) // { // ofs3<<data.pGridMap[i][j].X<<" "<<data.pGridMap[i][j].Y<<" "<<data.pGridMap[i][j].Z<<endl; // } // } // cout<<"FrameNum: "<<frameNum<<endl; // ofs1.close(); // ofs2.close(); // ofs3.close(); }