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