int main(int argc, char * argv[]) { int width = 640; int height = 480; parse_argument(argc, argv, "-w", width); parse_argument(argc, argv, "-h", height); Resolution::getInstance(width, height); Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2]; IplImage * deCompImage = 0; std::string logFile; assert(parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file"); RawLogReader logReader(decompressionBuffer, deCompImage, logFile, find_argument(argc, argv, "-f") != -1); cv::Mat1b tmp(height, width); cv::Mat3b depthImg(height, width); while(logReader.hasMore()) { logReader.getNext(); cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData); cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]); cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0); cv::cvtColor(tmp, depthImg, CV_GRAY2RGB); cv::imshow("RGB", rgbImg); cv::imshow("Depth", depthImg); char key = cv::waitKey(1); if(key == 'q') break; else if(key == ' ') key = cv::waitKey(0); if(key == 'q') break; } delete [] decompressionBuffer; if(deCompImage) { cvReleaseImage(&deCompImage); } return 0; }
int TrcReader::Main(int iArgc, char ** ppArgv) { RegisterSigHandler(SIGUSER1, TrcReader::SignalHandler); RegisterSigHandler(SIGINTR, TrcReader::SignalHandler); LogReader logReader(logBuffer); logReader.Open(); InternalWait(); }
void readTeamsFromFile(string filePath, map<string, int> &ARMarkersList, map<string, bool> &ARMarkersState, map<string, float> &ARMarkersScale, map<string, float> &ARMarkersRotation, map<string, string> &ARMarkersMesh) { ifstream logReader(filePath.c_str(), ofstream::in); string line; int i = 0; while (getline(logReader, line)) { // end of the file if (line.find("##") != string::npos) break; // comment else if (line.find("#") != string::npos) ; else { vector<string> data = split(line, ';'); string id = data[0]; // add index ARMarkersList[id] = i; // add scale ARMarkersScale[id] = atof(data[1].c_str()); // add rotation ARMarkersRotation[id] = atof(data[2].c_str()); // add mesh name ARMarkersMesh[id] = data[3]; cout << "Adding id: " << id << " - mesh: " << data[3] << " at index: " << i << " - Scale: " << data[1].c_str() << " - Rot: " << data[2].c_str() << endl; i++; } } }
void menuGuest(char *dir){ FILE *fp; khachHang temp; fp = fopen(dir,"rb"); fread(&temp,sizeof(temp),1,fp); // doc cac thong tin can thiet tu file fclose(fp); int choice; // bien lua chon switch case do{ printf("\n======================================"); printf("\n| Xin Chao %-20s |",temp.tenTk); printf("\n| |"); printf("\n| ---1.Kiem Tra So Du Tai Khoan |"); printf("\n| |"); printf("\n| ---2.Rut Tien |"); printf("\n| |"); printf("\n| ---3.Chuyen Khoan |"); printf("\n| |"); printf("\n| ---4.Doi Ma Pin |"); printf("\n| |"); printf("\n| ---5.Xem Lich Su Giao Dich |"); printf("\n| |"); printf("\n| ---6.Thoat |"); printf("\n| |"); printf("\n======================================\n"); printf("\n#Chon : "); fflush(stdin); scanf("%d",&choice); switch (choice){ case 1 : system("cls"); kttk(dir); printf("\n Ban Co Muon Thuc Hien Tiep Giao Dich (Y/N) \n"); if(yesNo() == 1){ system("cls"); choice = 0; // de quay ve menu, thoa dieu kien vong lap continue; } else{ printf("\n Xin Chao Va Hen Gap Lai\n"); getch(); exit(0); //thoat chuong trinh break; } case 2 : system("cls"); rutTien(dir); printf("\n Ban Co Muon Thuc Hien Tiep Giao Dich (Y/N) \n"); if(yesNo() == 1){ system("cls"); choice = 0; // de quay ve menu continue; } else{ printf("\n Xin Chao Va Hen Gap Lai\n"); getch(); exit(0); //thoat chuong trinh break; } case 3 : system("cls"); chuyenKhoan(dir); printf("\n Ban Co Muon Thuc Hien Tiep Giao Dich (Y/N) \n"); if(yesNo() == 1){ system("cls"); choice = 0; // de quay ve menu continue; } else{ printf("\n Xin Chao Va Hen Gap Lai\n"); getch(); exit(0); //thoat chuong trinh break; } case 4 : system("cls"); doiMaPin(dir); printf("\n Ban Co Muon Thuc Hien Tiep Giao Dich (Y/N) \n"); if(yesNo() == 1){ system("cls"); choice = 0; // de quay ve menu continue; } else{ printf("\n Xin Chao Va Hen Gap Lai\n"); getch(); exit(0); //thoat chuong trinh break; } case 5 : system("cls"); logReader(1,dir); printf("\n Ban Co Muon Thuc Hien Tiep Giao Dich (Y/N) \n"); system("cls"); choice = 0; // de quay ve menu continue; case 6 : printf("\nXin Chao Va Hen Gap Lai\n"); getch(); exit(0); //thoat chuong trinh break; default: system("cls"); // xoa man hinh, nhap lai break; } } while (choice > 6 || choice < 1); }
void ATMmanager(){ FILE *fp; khachHang temp; int choice; // bien lua chon switch case do{ printf("\n======================================"); printf("\n| Xin Chao ATM Manager |"); printf("\n| |"); printf("\n| ---1.Tao Tai Khoan |"); printf("\n| |"); printf("\n| ---2.Sua Tai Khoan |"); printf("\n| |"); printf("\n| ---3.Nap Tien |"); printf("\n| |"); printf("\n| ---4.Active Account |"); printf("\n| |"); printf("\n| ---5.Xem File Log |"); printf("\n| |"); printf("\n| ---6.Thoat |"); printf("\n| |"); printf("\n======================================\n"); printf("\n#Chon : "); fflush(stdin); scanf("%d",&choice); switch (choice){ case 1 : system("cls"); createAcc(); system("cls"); choice = 0; // de quay ve menu, thoa dieu kien vong lap continue; case 2 : editAcc(); system("cls"); choice = 0; // de quay ve menu, thoa dieu kien vong lap continue; case 3 : system("cls"); napTien(); system("cls"); choice = 0; continue; case 4 : system("cls"); active(); system("cls"); choice = 0; continue; case 5 : system("cls"); logReader(0,"Dung Quan Tam Toi Cai Nay =]]"); system("cls"); choice = 0; continue; case 6 : exit(0); //thoat chuong trinh break; default: system("cls"); // xoa man hinh, nhap lai break; } } while (choice > 6 || choice < 1); }
int main(int argc, char * argv[]) { int width = 640; int height = 480; Resolution::getInstance(width, height); Intrinsics::getInstance(528, 528, 320, 240); cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F); intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx(); intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy(); intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx(); intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy(); intrinsicMatrix.at<double>(0,1) =0; intrinsicMatrix.at<double>(1,0) =0; intrinsicMatrix.at<double>(2,0) =0; intrinsicMatrix.at<double>(2,1) =0; intrinsicMatrix.at<double>(2,2) =1; Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2]; IplImage * deCompImage = 0; std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg"; // assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file"); RawLogReader logReader(decompressionBuffer, deCompImage, logFile, true); cv::Mat1b tmp(height, width); cv::Mat3b depthImg(height, width); PlaceRecognition placeRecognition(&intrinsicMatrix); iSAMInterface iSAM; //Keyframes KeyframeMap map(true); Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero(); Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity(); int64_t lastTime = 0; OdometryProvider * odom = 0; //int frame_index = 0; // uint64_t timestamp; /*if(true) { odom = new FOVISOdometry; if(logReader.hasMore()) { logReader.getNext(); Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity(); Eigen::Vector3f tcurr = Eigen::Vector3f::Zero(); odom->getIncrementalTransformation(tcurr, Rcurr, logReader.timestamp, (unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); } }*/ //else // { odom = new DVOdometry; if(logReader.hasMore()) { logReader.getNext(); DVOdometry * dvo = static_cast<DVOdometry *>(odom); dvo->firstRun((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); } //} ofstream fout1("camera_pose_DVOMarch28.txt"); ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt"); ofstream fout3("loop_closure_transformationMarch28.txt"); ofstream fout4("camera_pose_after_optimizationMarch28.txt"); ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt"); ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt"); /* pcl::visualization::PCLVisualizer cloudViewer; cloudViewer.setBackgroundColor(1, 1, 1); cloudViewer.initCameraParameters(); cloudViewer.addCoordinateSystem(0.1, 0, 0, 0); */ //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared()); //cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer"); int loopClosureCount=0; while(logReader.hasMore()) { logReader.getNext(); cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData); cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]); cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0); cv::cvtColor(tmp, depthImg, CV_GRAY2RGB); cv::imshow("RGB", rgbImg); cv::imshow("Depth", depthImg); char key = cv::waitKey(1); if(key == 'q') { break; } else if(key == ' ') { key = cv::waitKey(0); } if(key == 'q') { break; } Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity(); Eigen::Vector3f tcurr = Eigen::Vector3f::Zero(); // #1 odom->getIncrementalTransformation(tcurr, Rcurr, logReader.timestamp, (unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl; Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot; Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans; //Eigen::MatrixXd covariance = odom->getCovariance(); //Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3; if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1) { Eigen::MatrixXd covariance = odom->getCovariance(); iSAM.addCameraCameraConstraint(lastTime, logReader.timestamp, lastPlaceRecognitionRot, lastPlaceRecognitionTrans, Rcurr, tcurr); //covariance); printCovariance(fout5, covariance); lastTime = logReader.timestamp; lastPlaceRecognitionRot = Rcurr; lastPlaceRecognitionTrans = tcurr; cout<<"before add keyframe"<<endl; // #2 map.addKeyframe((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0], Rcurr, tcurr, logReader.timestamp); fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl; /* //Save keyframe { cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData); cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]); //save keyframe depth char fileName[1024] = {NULL}; sprintf(fileName, "keyframe_depth_%06d.png", frame_index); cv::imwrite(fileName, depthImgKeyframe); //save keyframe rgb sprintf(fileName, "keyframe_rgb_%06d.png", frame_index); cv::imwrite(fileName, rgbImgKeyframe); frame_index ++; } */ int64_t matchTime; Eigen::Matrix4d transformation; // Eigen::MatrixXd cov(6,6); //isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity())) Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity(); cout<<"map.addKeyframe is OK"<<endl; // #3 if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0], logReader.timestamp, matchTime, transformation, cov, loopClosureCount)) { //printCovariance(fout6, cov); cout<<"logReader.timestamp "<<logReader.timestamp<<endl; cout<<"matchTime "<<matchTime<<endl; /* transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601, 0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587, 0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276, 0, 0, 0, 0.999999999999998; */ /* transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689, -0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094, 0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125, 0, 0, 0, 1; */ iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov); fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl; loopClosureCount++; } } if(loopClosureCount>=1) { break; } } /* for(int i=0; i<loopClosureCount;i++) { iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1, placeRecognition.loopClosureConstraints.at(i)->time2, placeRecognition.loopClosureConstraints.at(i)->constraint); }*/ std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore; iSAM.getCameraPoses(posesBefore); cout<<"It works good before optimization"<<endl; // #4 double residual =iSAM.optimise(); cout<<"It works good after optimize and before map.applyPoses"<<endl; // map.applyPoses(isam); //cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl; /* pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap(); // Write it back to disk under a different name. // Another possibility would be "savePCDFileBinary()". cout<<"before storing the point cloud map"<<endl; pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud); cout << "Saved data points to outputMap.pcd." << std::endl; cout<<"copy data into octomap..."<<endl; octomap::ColorOcTree tree( 0.05 ); for (size_t i=0; i<(*cloud).points.size(); i++) { // 将点云里的点插入到octomap中 tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true ); } for (size_t i=0; i<(*cloud).points.size(); i++) { tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b); } tree.updateInnerOccupancy(); tree.write("OctomapColorLab03DVODensity005.ot"); cout<<"please see the done."<<endl; */ //pcl::visualization::PCLVisualizer cloudViewer; // cloudViewer.setBackgroundColor(1, 1, 1); //cloudViewer.initCameraParameters(); // cloudViewer.addCoordinateSystem(0.1, 0, 0, 0); //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared()); //cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer"); std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph; iSAM.getCameraPoses(newPoseGraph); /* for(unsigned int i = 0; i < newPoseGraph.size(); i++) { // file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " "; Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1); Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3); fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " "; Eigen::Quaternionf currentCameraRotation(rot); //file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n"; }*/ for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++) { Eigen::Matrix3f Roptimized; Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2), ite->second(1,0), ite->second(1,1), ite->second(1,2), ite->second(2,0), ite->second(2,1), ite->second(2,2); Eigen::Quaternionf quatOptimized(Roptimized); fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl; } cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl; // drawPoses(poses, cloudViewer, 1.0, 0, 0); //drawPoses(posesBefore, cloudViewer, 0, 0, 1.0); //cloudViewer.spin(); delete [] decompressionBuffer; return 0; }
void readTeamsFromFile(string filePath, map<string, string> &team1, map<string, string> &team2, bool debug) throw(cv::Exception) { ifstream logReader(filePath.c_str(), ofstream::in); string line; while (getline(logReader,line)) { // First Team if (line.find("team_1") != string::npos) { vector<string> team = split(line, '='); team1["name"] = team[1]; if(debug) cout << "Team1: " << team1["name"] << "\n"; } else // Second Team if (line.find("team_2") != string::npos) { vector<string> team = split(line, '='); team2["name"] = team[1]; if(debug) cout << "Team2: " << team2["name"] << "\n"; } else // First Team's colour if (line.find("colour_1") != string::npos) { vector<string> colour = split(line, '='); team1["colour"] = colour[1]; if(debug) cout << "Team1 colour: " << team1["colour"] << "\n"; } else // Second Team's colour if (line.find("colour_2") != string::npos) { vector<string> colour = split(line, '='); team2["colour"] = colour[1]; if(debug) cout << "Team2 colour: " << team2["colour"] << "\n"; } else // First Team's short name if (line.find("short_1") != string::npos) { vector<string> s_name = split(line, '='); team1["s_name"] = s_name[1]; if(debug) cout << "Team1 short name: " << team1["s_name"] << "\n"; } else // Second Team's short name if (line.find("short_2") != string::npos) { vector<string> s_name = split(line, '='); team2["s_name"] = s_name[1]; if(debug) cout << "Team2 short name: " << team2["s_name"] << "\n"; } else // First Team's players if (line.find("players_1") != string::npos) { vector<string> players = split(split(line, '=')[1],';'); uint i; for(i = 0; i < players.size(); i++) { vector<string> p = split(players[i], ','); team1[p[0]] = p[1]; } } else // Second Team's players if (line.find("players_2") != string::npos) { vector<string> players = split(split(line, '=')[1],';'); uint i; for(i = 0; i < players.size(); i++) { vector<string> p = split(players[i], ','); team2[p[0]] = p[1]; } } // End of the file else if (line.find("##") != string::npos) break; } }