double LightplaneCalibrator::Calibrate() { GeneratePointCloud(); cv::Mat pcloud(light_point_cloud_.size(), 3, CV_64F); cv::Mat light_plane_wcs(1,4,CV_64F); double x_ave, y_ave, z_ave; for (int i = 0; i < light_point_cloud_.size(); i++) { pcloud.row(i) = light_point_cloud_[i].t(); } x_ave = mean(pcloud.col(0))(0); y_ave = mean(pcloud.col(1))(0); z_ave = mean(pcloud.col(2))(0); pcloud.col(0) = pcloud.col(0) - x_ave; pcloud.col(1) = pcloud.col(1) - y_ave; pcloud.col(2) = pcloud.col(2) - z_ave; cv::Mat w, u, vt; cv::SVD::compute(pcloud, w, u, vt); vt.row(2).copyTo(light_plane_wcs.colRange(0, 3)); light_plane_wcs.at<double>(0, 3) = - light_plane_wcs.at<double>(0, 0) * x_ave - light_plane_wcs.at<double>(0, 1) * y_ave - light_plane_wcs.at<double>(0, 2) * z_ave; cam_->light_plane_ = light_plane_wcs * ref_pose_[0].inv(); return 0.0; }
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(); }