void SecondOrderOptimizeFusionMove::optimize(Depth &result, const int max_iter) const {
vector<Depth> proposals;
genProposal(proposals);
//proposals.push_back(noisyDisp);
char buffer[1024] = {};
//initialize by random
result.initialize(width, height, -1);
const int nLabel = model->nLabel;
std::default_random_engine generator;
std::uniform_int_distribution<int> distribution(0, nLabel - 1);
for (auto i = 0; i < width * height; ++i) {
//result.setDepthAtInd(i, (double) distribution(generator));
//result.setDepthAtInd(i, noisyDisp[i]);
result[i] = 0;
}
sprintf(buffer, "%s/temp/init_result.jpg", file_io.getDirectory().c_str());
result.saveImage(buffer, 256.0 / (double)nLabel);
list<double> diffE;
double lastEnergy = evaluateEnergy(result);
double initialEnergy = lastEnergy;
int iter = 0;
const double termination = 0.1;
float timming = (float) getTickCount();
const int smoothInterval = 5;
while (true) {
if (iter == max_iter)
break;
cout << "======================" << endl;
Depth newProposal;
// if (iter > 0 && iter % smoothInterval == 0) {
// Depth orip1;
// newProposal.initialize(width, height, -1);
// orip1.initialize(width, height, -1);
// for (auto i = 0; i < width * height; ++i) {
// orip1.setDepthAtInd(i, result[i]);
// newProposal.setDepthAtInd(i, result[i]);
// }
// int direction = iter / smoothInterval;
// if (direction % 2 == 0) {
// //horizontally
// for (auto y = 0; y < height; ++y) {
// for (auto x = 1; x < width - 1; ++x)
// newProposal.setDepthAtInt(x, y, (orip1(x + 1, y) + orip1(x - 1, y)) / 2);
// newProposal.setDepthAtInt(width - 1, y, orip1(width - 1, y));
// }
// } else {
// //vertically
// for (auto x = 0; x < width; ++x) {
// for (auto y = 1; y < height - 1; ++y)
// newProposal.setDepthAtInt(x, y, (orip1(x, y + 1) + orip1(x, y - 1)) / 2);
// newProposal.setDepthAtInt(x, height - 1, orip1(x, height - 1));
// }
// }
// cout << "Iteration " << iter << " using smoothing proposal " << endl;
// } else {
// }
newProposal = proposals[iter % (proposals.size())];
printf("Fusing with proposal %d\n", (int)(iter % proposals.size()));
//after several iteration, smooth the dispartiy
fusionMove(result, newProposal);
double e = evaluateEnergy(result);
double energyDiff = lastEnergy - e;
if (diffE.size() >= average_over)
diffE.pop_front();
diffE.push_back(energyDiff);
double average_diffe = std::accumulate(diffE.begin(), diffE.end(), 0.0) / (double) diffE.size();
printf("Done. Final energy: %.5f, energy decrease: %.5f average decrease: %.5f\n", e, energyDiff,
average_diffe);
lastEnergy = e;
sprintf(buffer, "%s/temp/fusionmove_iter%05d.jpg", file_io.getDirectory().c_str(), iter);
result.saveImage(buffer, 256.0 / (double) nLabel);
if (iter > proposals.size() * 2 && average_diffe < termination) {
cout << "Converge!" << endl;
break;
}
iter++;
}
timming = ((float) getTickCount() - timming) / (float) getTickFrequency();
printf("All done. Initial energy: %.5f, final energy: %.5f, time usage: %.2fs\n", initialEnergy, lastEnergy,
timming);
}
void DynamicConfidence::run(const int anchor, Depth &confidence) {
const int framenum = file_io.getTotalNum();
CHECK_LT(anchor, framenum);
const int startid = anchor - max_tWindow / 2 >= 0 ? anchor - max_tWindow / 2 : 0;
const int endid = anchor + max_tWindow / 2 < framenum ? anchor + max_tWindow / 2 : framenum - 1;
char buffer[1024] = {};
vector<FlowFrame> flow_forward((size_t) endid - startid + 1);
vector<FlowFrame> flow_backward((size_t) endid - startid + 1);
printf("=====================\nFrame: %d\n", anchor);
cout << "Reading optical flow..." << endl;
for (auto i = startid; i <= endid; ++i) {
cout << '.' << flush;
if (i < file_io.getTotalNum() - 1)
flow_forward[i - startid].readFlowFile(file_io.getOpticalFlow_forward(i));
if (i > 0)
flow_backward[i - startid].readFlowFile(file_io.getOpticalFlow_backward(i));
}
cout << endl;
const int width = (int)(flow_forward[0].width() / downsample);
const int height = (int)(flow_forward[0].height() / downsample);
const int widthFull = flow_forward[0].width();
const int heightFull = flow_forward[0].height();
Depth confidence_down(width, height, -1);
confidence.initialize(widthFull, heightFull, 0);
const int min_interval = 0;
const double kth_ratio = 0.8;
const size_t min_length = 5;
double min_depth, max_depth;
cout << "Computing min-max depth" << endl;
computeMinMaxDepth(anchor, min_depth, max_depth);
const int id = anchor - startid;
const theia::Camera& refCam = reconstruction.View(orderedId[anchor].second)->Camera();
cout << "Computing confidence..." << endl;
const int unit = width * height / 10;
const int testx = 1596 / downsample;
const int testy = 472 / downsample;
int startx = 0, endx = width-1, starty = 0, endy = height-1;
if(testx >=0 && testy>=0){
printf("Debug mode: %d, %d\n", testx, testy);
startx = testx;
endx = testx;
starty = testy;
endy = testy;
}
double max_line_length = 100;
for (auto y = starty; y<= endy; ++y) {
for (auto x = startx; x <= endx; ++x) {
if((y*width+x) % unit == 0)
cout << '.' << flush;
vector<double> epiErr;
Vector2d locL((double) x, (double) y);
Vector3d ray = refCam.PixelToUnitDepthRay(locL * downsample);
Vector3d minpt = refCam.GetPosition() + ray * min_depth;
Vector3d maxpt = refCam.GetPosition() + ray * max_depth;
if(testx >= 0 && testy >= 0){
printf("min depth: %.3f, max depth: %.3f\n", min_depth, max_depth);
}
for (auto i = 0; i < flow_forward.size(); ++i) {
const theia::Camera& cam2 = reconstruction.View(orderedId[i+startid].second)->Camera();
if(i == id){
Mat img = imread(file_io.getImage(i+startid));
cv::circle(img, cv::Point(locL[0], locL[1]), 2, cv::Scalar(0,0,255), 2);
sprintf(buffer, "%s/temp/conf_ref%05d_%05d.jpg", file_io.getDirectory().c_str(), anchor, i+startid);
imwrite(buffer, img);
continue;
}
if (std::abs(id - i) < min_interval)
continue;
Vector2d locR;
if (id < i) {
if (!flow_util::trackPoint(locL * downsample, flow_forward, id, i, locR))
continue;
} else {
if (!flow_util::trackPoint(locL * downsample, flow_backward, id, i, locR))
continue;
}
Vector2d spt, ept;
cam2.ProjectPoint(minpt.homogeneous(), &spt);
cam2.ProjectPoint(maxpt.homogeneous(), &ept);
// Vector2d dir = spt - ept;
// dir.normalize();
// spt = ept + dir * max_line_length;
if(x == testx && y == testy){
Mat img = imread(file_io.getImage(i+startid));
cv::circle(img, cv::Point(locR[0], locR[1]), 2, cv::Scalar(0,0,255), 2);
printf("---------------------\nFrame %d, spt:(%.2f,%.2f), ept:(%.2f,%.2f)\n", i+startid, spt[0], spt[1], ept[0], ept[1]);
cv::line(img, cv::Point(spt[0], spt[1]), cv::Point(ept[0], ept[1]), cv::Scalar(255,0,0), 2);
sprintf(buffer, "%s/temp/conf_ref%05d_%05d.jpg", file_io.getDirectory().c_str(), anchor, i+startid);
imwrite(buffer, img);
theia::Matrix3x4d pMatrix;
cam2.GetProjectionMatrix(&pMatrix);
cout << "Projection matrix:" << endl << pMatrix << endl;
}
epiErr.push_back(geometry_util::distanceToLineSegment<2>(locR, spt, ept));
}
if (epiErr.size() < min_length) {
confidence(x, y) = 0.0;
continue;
}
const size_t kth = (size_t) (epiErr.size() * kth_ratio);
nth_element(epiErr.begin(), epiErr.begin() + kth, epiErr.end());
confidence_down.setDepthAtInt(x, y, epiErr[kth]);
}
}
cout << endl;
//upsample to original resolution
for(auto x=0; x<widthFull-downsample; ++x){
for(auto y=0; y<heightFull-downsample; ++y)
confidence(x,y) = confidence_down.getDepthAt(Vector2d((double)x/downsample, (double)y/downsample));
}
confidence.updateStatics();
}