Skip to content

Commit

Permalink
set up saving frames
Browse files Browse the repository at this point in the history
  • Loading branch information
@Loeing
Loeing committed Mar 23, 2016
1 parent e3e7aca commit 76aaeca
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 106 deletions.
57 changes: 0 additions & 57 deletions KeyFrameTrajectory.txt
View file
Original file line number Diff line number Diff line change
@@ -1,57 +0,0 @@
<<<<<<< HEAD
1425127704.678000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000
1425127705.078000 0.0009969 -0.0144543 0.0566856 0.0037247 -0.0147869 0.0017574 0.9998822
1425127705.244667 0.0006868 -0.0183527 0.0826381 0.0031011 -0.0249783 0.0042908 0.9996740
1425127705.411333 0.0002030 -0.0223148 0.1101719 0.0048738 -0.0378387 0.0011121 0.9992713
1425127705.611333 -0.0007276 -0.0298328 0.1431321 0.0112049 -0.0528653 -0.0178835 0.9983786
1425127705.878000 -0.0014852 -0.0391333 0.1809480 0.0132674 -0.0704378 -0.0349333 0.9968160
1425127706.211333 -0.0022624 -0.0473917 0.2222326 0.0101363 -0.0820006 -0.0375392 0.9958735
1425127706.411333 -0.0030016 -0.0529473 0.2447886 0.0137021 -0.0868292 -0.0355459 0.9954945
1425127706.644666 -0.0032210 -0.0585713 0.2694314 0.0078124 -0.0926492 -0.0315422 0.9951684
1425127706.878000 -0.0043253 -0.0638582 0.2966880 0.0100303 -0.1027384 -0.0302428 0.9941980
1425127707.178000 -0.0104588 -0.0740838 0.3327004 0.0160940 -0.1233721 -0.0535456 0.9907841
1425127707.711333 -0.0117254 -0.0869244 0.3804245 0.0147960 -0.1304126 -0.0554221 0.9897990
1425127707.978000 -0.0115039 -0.0931805 0.4010625 0.0139567 -0.1334431 -0.0458597 0.9898965
1425127708.278000 -0.0152445 -0.1004068 0.4298636 0.0173961 -0.1478225 -0.0509896 0.9875454
1425127708.611333 -0.0283420 -0.1094146 0.4725224 0.0259634 -0.1862459 -0.1071449 0.9762983
1425127708.911333 -0.0427929 -0.1161387 0.5122981 0.0171004 -0.2164794 -0.1192699 0.9688235
1425127709.211333 -0.0598253 -0.1206552 0.5473996 0.0147882 -0.2469048 -0.1301369 0.9601477
1425127709.511333 -0.0709822 -0.1271640 0.5724392 0.0300024 -0.2563762 -0.1355878 0.9565495
1425127709.811333 -0.0837077 -0.1323146 0.5973798 0.0319418 -0.2695656 -0.1330736 0.9532080
1425127710.111333 -0.0993693 -0.1380325 0.6206532 0.0444736 -0.2809991 -0.1499438 0.9468783
1425127710.444666 -0.1142555 -0.1409320 0.6389078 0.0349652 -0.2734735 -0.1345338 0.9517827
1425127710.911333 -0.1487574 -0.1451638 0.6738245 0.0380955 -0.2933710 -0.1377543 0.9452544
1425127711.344666 -0.1812644 -0.1496134 0.7102752 0.0433410 -0.3117349 -0.1600062 0.9355966
1425127711.678000 -0.2115816 -0.1517484 0.7395014 0.0485248 -0.3352044 -0.1803958 0.9234396
1425127711.944666 -0.2325112 -0.1531309 0.7592614 0.0489761 -0.3463612 -0.1866920 0.9180312
1425127712.378000 -0.2728901 -0.1551143 0.7907720 0.0462371 -0.3752355 -0.1943030 0.9051557
1425127712.611333 -0.2965874 -0.1547948 0.8080280 0.0512635 -0.3995271 -0.2042850 0.8921983
1425127712.844666 -0.3197309 -0.1552144 0.8241311 0.0524273 -0.4295111 -0.2208745 0.8740630
1425127713.078000 -0.3369819 -0.1559951 0.8359599 0.0561714 -0.4434492 -0.2384043 0.8621838
1425127713.311333 -0.3582837 -0.1544009 0.8468951 0.0602098 -0.4649810 -0.2490666 0.8474274
1425127713.544667 -0.3789352 -0.1538314 0.8563112 0.0631482 -0.4859186 -0.2593826 0.8322356
1425127713.778000 -0.3980846 -0.1534726 0.8649901 0.0507079 -0.5068394 -0.2586730 0.8207501
1425127714.178000 -0.4294189 -0.1539187 0.8771867 0.0496365 -0.5458387 -0.2894384 0.7847431
1425127714.378000 -0.4407573 -0.1538574 0.8818229 0.0611978 -0.5577777 -0.3071154 0.7686475
1425127714.578000 -0.4494452 -0.1540183 0.8862600 0.0577061 -0.5617457 -0.3057444 0.7665717
1425127714.744666 -0.4576592 -0.1539516 0.8898489 0.0500217 -0.5680141 -0.3008585 0.7644227
1425127714.944666 -0.4687458 -0.1536780 0.8934107 0.0429560 -0.5782529 -0.2953191 0.7593188
1425127715.211333 -0.4823866 -0.1536427 0.8985714 0.0327674 -0.5879110 -0.2916619 0.7538038
1425127715.678000 -0.5116579 -0.1487873 0.9071342 0.0398542 -0.6177297 -0.2996516 0.7259687
1425127715.944666 -0.5229688 -0.1488402 0.9107274 0.0431756 -0.6234514 -0.3050559 0.7185994
1425127716.144666 -0.5325135 -0.1475040 0.9119203 0.0386505 -0.6290085 -0.2983858 0.7168126
1425127716.578000 -0.5510626 -0.1457915 0.9132203 0.0412691 -0.6347198 -0.2975707 0.7119545
1425127717.178000 -0.5818200 -0.1392307 0.9167885 0.0161345 -0.6548398 -0.2932295 0.6963770
1425127717.678000 -0.6118475 -0.1347768 0.9177986 0.0070542 -0.6866263 -0.3182642 0.6536072
1425127718.144666 -0.6355821 -0.1285619 0.9171220 -0.0029465 -0.7156309 -0.3371550 0.6117109
1425127718.544666 -0.6537900 -0.1251610 0.9183440 -0.0296782 -0.7348275 -0.3640463 0.5715050
1425127718.744666 -0.6626555 -0.1228659 0.9210250 -0.0358260 -0.7521340 -0.3730331 0.5420861
1425127719.011333 -0.6736398 -0.1214565 0.9224738 0.0124936 0.7743112 0.3927708 -0.4960012
=======
1456217450.068751 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000
1456217451.935417 -0.0379150 0.0013713 -0.0053628 -0.0068628 -0.0095710 -0.0036868 0.9999238
1456217452.268751 -0.0533119 0.0023223 -0.0001109 -0.0189581 0.0038693 -0.0000645 0.9998128
1456217453.368751 -0.0854905 0.0000622 0.0084415 -0.0155981 -0.0077460 -0.0030123 0.9998438
1456217453.602084 -0.0876852 -0.0017143 0.0071558 -0.0115689 -0.0079131 -0.0026918 0.9998981
1456217454.068751 -0.0917969 -0.0012258 0.0025253 -0.0061691 -0.0198678 -0.0021861 0.9997812
>>>>>>> 0412eb271ac61504be761a5e33e70f2010ce807a
114 changes: 65 additions & 49 deletions src/ProbabilityMapping.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@

#define DEBUG 1
#define DBG(do_something) if (DEBUG) { do_something; }
#define SAVE_IMAGES 1
#define SAVE(do_something) if (SAVE_IMAGES) { do_something; }

ProbabilityMapping::ProbabilityMapping() {}

Expand All @@ -47,43 +49,44 @@ void ProbabilityMapping::FirstLoop(ORB_SLAM::KeyFrame *kf, std::vector<std::vect

cv::Mat gradx, grady, gradmag, gradth, really;
cv::Mat image = kf->GetImage();

SAVE(imwrite("/home/josh/Workspace/Scanner3D/mainImage.png",image))
GetGradientMagAndOri(image, &gradx, &grady, &gradmag, &gradth, &really);
DBG(cout << "Got it!\n";
cout << "Generating Depth Hypotheses...\n")

std::vector<depthHo> depth_ho;

std::vector<std::vector<depthHo> > temp_ho (image.rows, std::vector<depthHo>(image.cols, depthHo()) );
for(int x = 0; x < image.rows; x++){
for(int y = 0; y < image.cols; y++){
DBG(cout << "Pixel gradient magnitude: " << gradmag.at<float>(x,y) << " lambdaG: " << lambdaG <<"\n")
if(gradmag.at<float>(x,y) < lambdaG){
continue;
}
depth_ho.clear();
DBG(cout << "Closest Matches size:"<< closestMatches.size() << "\n")

for(size_t i=0; i<closestMatches.size(); i++){
ORB_SLAM::KeyFrame* kf2 = closestMatches[i];
SAVE(imwrite("/home/josh/Workspace/Scanner3D/math"+ boost::lexical_cast<std::string>(i) +".png",image))
for(int x = 0; x < image.rows; x++){
for(int y = 0; y < image.cols; y++){
DBG(cout << "Pixel gradient magnitude: " << gradmag.at<float>(x,y) << " lambdaG: " << lambdaG <<"\n")
if(gradmag.at<float>(x,y) < lambdaG){
continue;
}

depth_ho.clear();
DBG(cout << "Closest Matches size:"<< closestMatches.size() << "\n")
for(size_t i=0; i<closestMatches.size(); i++){
ORB_SLAM::KeyFrame* kf2 = closestMatches[i];

depthHo dh;
float pixel = image.at<uchar>(x,y); //maybe it should be cv::Mat
EpipolarSearch(kf, kf2, x, y, pixel, gradx, grady, gradmag, min_depth, max_depth, &dh);
DBG(cout << "Depth: " << dh.depth << "\n")
if (dh.supported)
depth_ho.push_back(dh);
}

DBG(printf("FirstLoop: found a set of %d hypotheseses for pixel %d,%d\n", (int)(depth_ho.size()), x, y))
if (depth_ho.size()) {
depthHo dh;
DBG(cout << "Calculating Inverse Depth Hypothesis\n")
InverseDepthHypothesisFusion(depth_ho, &dh);
dh.supported = true;
temp_ho[x][y] = dh;
} else {
temp_ho[x][y].supported = false;
DBG(printf("FirstLoop: found a set of %d hypotheseses for pixel %d,%d\n", (int)(depth_ho.size()), x, y))
if (depth_ho.size()) {
depthHo dh;
DBG(cout << "Calculating Inverse Depth Hypothesis\n")
InverseDepthHypothesisFusion(depth_ho, &dh);
dh.supported = true;
temp_ho[x][y] = dh;
} else {
temp_ho[x][y].supported = false;
}
}
}
}
Expand Down Expand Up @@ -138,32 +141,19 @@ void ProbabilityMapping::EpipolarSearch(ORB_SLAM::KeyFrame* kf1, ORB_SLAM::KeyFr
}
DBG(cout << "IN LOOP\n";
cout << "Pixel value2:" << static_cast<unsigned>(image.at<uchar>(uj,vj)) << endl;
string r;
uchar d1 = grad2mag.type() & CV_MAT_DEPTH_MASK;
uchar d2 = grad2th.type() & CV_MAT_DEPTH_MASK;
switch ( d1 ) {
case CV_8U: r = "8U"; break;
case CV_8S: r = "8S"; break;
case CV_16U: r = "16U"; break;
case CV_16S: r = "16S"; break;
case CV_32S: r = "32S"; break;
case CV_32F: r = "32F"; break;
case CV_64F: r = "64F"; break;
default: r = "User"; break;
}
cout << "Depths: " << r << endl;
cout << "Grad2mag:" << grad2mag.at<float>(uj,vj) << endl;
cout << "manual magnitude:" << really2.at<float>(uj,vj) << endl)
cout << "Grad2mag:" << grad2mag.at<float>(uj,vj) << endl)
//cout << "manual magnitude:" << really2.at<float>(uj,vj) << endl)

DBG(cout << "calculating epipolar line angle\n");
float th_epipolar_line = cv::fastAtan2(uj,vj);
DBG(cout << "theta epipolar line: " << th_epipolar_line << endl)
DBG(cout << "grad2th: " << grad2th.at<float>(uj,vj) << endl)

if(grad2mag.at<float>(uj,vj) < lambdaG){
DBG(cout << "low gradient\n")
continue;
}

float th_epipolar_line = cv::fastAtan2(uj,vj);
DBG(cout << "grad2th: " << grad2th.at<float>(uj,vj) << endl;
cout << "theta epipolar line: " << th_epipolar_line << endl)


//FIXME ASAP
if(abs(grad2th.at<float>(uj,vj) - th_epipolar_line + 180) < lambdaL){
cout << "low angle\n";
Expand Down Expand Up @@ -208,7 +198,7 @@ void ProbabilityMapping::EpipolarSearch(ORB_SLAM::KeyFrame* kf1, ORB_SLAM::KeyFr
float q = (grad2mag.at<float>(uj_plus, vj_plus) - grad2mag.at<float>(uj_minus, vj_plus))/2.0;

float ustar = best_pixel + (g*best_photometric_err + (1/0.23)*q*best_gradient_modulo_err)/(g*g + (1/0.23)*q*q);
float ustar_var = (2*image_stddev.at<uchar>(best_pixel,best_vj)*image_stddev.at<uchar>(best_pixel,best_vj)/(g*g + (1/0.23)*q*q));
float ustar_var = (2*image_stddev.at<float>(best_pixel,best_vj)*image_stddev.at<float>(best_pixel,best_vj)/(g*g + (1/0.23)*q*q));

DBG(cout << "Computing Inverse Depth Hypothesis\n")
ComputeInvDepthHypothesis(kf1, best_pixel, ustar, ustar_var, a, b, c, dh);
Expand Down Expand Up @@ -496,13 +486,39 @@ void ProbabilityMapping::GetGradientMagAndOri(const cv::Mat& image, cv::Mat* gra
cv::magnitude(*gradx,*grady,*mag);
cv::phase(*gradx,*grady,*ori,true);


cout << "printing results of grad mag and theta" << endl;
std::vector<FILE*> files;
FILE* fmag_cv = fopen("/home/josh/Workspace/Scanner3D/cv_grad_mag.csv", "w");
FILE* ftheta_cv = fopen("/home/josh/Workspace/Scanner3D/cv_grad_theta.csv", "w");
files.push_back(fmag_cv);
files.push_back(ftheta_cv);
for (int k=0; k < files.size(); k++) {
if (files[k]) {
cv::Mat* m;
switch (k) {
case 1: m = mag; break; // cv magnitude
default: m = ori; break; // cv theta
}
for (int i=0; i < m->rows; i++) {
for (int j=0; j < m->cols; j++) {
float f = m->at<float>(i,j);
fprintf(files[k], "%3.3f, ", f);
}
fprintf(files[k], "\n");
}
fclose(files[k]);
}
}
//For manual version

cv::Mat absgradx2, absgrady2, sum;
cv::pow(*gradx, 2.0, absgradx2);
cv::pow(*grady, 2.0, absgrady2);
cv::addWeighted(absgradx2, 1.0, absgrady2, 1.0, 0, sum);
cv::sqrt(sum, *really);
//cv::Mat gradx2 = cv::Mat::zeros(image.rows, image.cols, CV_32F);
//cv::Mat grady2 = cv::Mat::zeros(image.rows, image.cols, CV_32F);
//cv::Mat sum = cv::Mat::zeros(image.rows, image.cols, CV_32F);
//cv::pow(*gradx, 2.0, gradx2);
//cv::pow(*grady, 2.0, grady2);
//cv::addWeighted(gradx2, 1.0, grady2, 1.0, 0, sum);
//cv::sqrt(sum, *really);

//cv::Scharr(image, *gradx, CV_16S, 1, 0);
//cv::Scharr(image, *grady, CV_16S, 0, 1);
Expand Down

0 comments on commit 76aaeca

Please sign in to comment.