//Run MAP inference with QPBO
void runQPBO(GraphicalModelType* gm,std::string ofname)
{
QPBO qpbo(*gm);
std::cout<<"Running Inference"<<std::endl;
qpbo.infer();
std::cout << "MAP Result: " << qpbo.value() << " Bound: "<<qpbo.bound()<<std::endl;
std::vector<LabelType> result;
qpbo.arg(result);
//Write Result
writeResult(result,ofname);
}
void SecondOrderOptimizeFusionMove::fusionMove(Depth &p1, const Depth &p2) const {
//create problem
int nPix = width * height;
kolmogorov::qpbo::QPBO<EnergyTypeT> qpbo(nPix*10, nPix*20);
const double& MRFRatio = model->MRFRatio;
//construct graph
auto addTripleToGraph = [&](int p, int q, int r, double w) {
double vp1 = p1[p], vp2 = p2[p], vq1 = p1[q], vq2 = p2[q], vr1 = p1[r], vr2 = p2[r];
double lam = w * model->weight_smooth;
// if (refSeg[p] == refSeg[q] && refSeg[p] == refSeg[r])
// lam = lamh;
// else
// lam = laml;
EnergyTypeT A = (EnergyTypeT)(lapE(vp1, vq1, vr1) * lam * MRFRatio);
EnergyTypeT B = (EnergyTypeT)(lapE(vp1, vq1, vr2) * lam * MRFRatio);
EnergyTypeT C = (EnergyTypeT)(lapE(vp1, vq2, vr1) * lam * MRFRatio);
EnergyTypeT D = (EnergyTypeT)(lapE(vp1, vq2, vr2) * lam * MRFRatio);
EnergyTypeT E = (EnergyTypeT)(lapE(vp2, vq1, vr1) * lam * MRFRatio);
EnergyTypeT F = (EnergyTypeT)(lapE(vp2, vq1, vr2) * lam * MRFRatio);
EnergyTypeT G = (EnergyTypeT)(lapE(vp2, vq2, vr1) * lam * MRFRatio);
EnergyTypeT H = (EnergyTypeT)(lapE(vp2, vq2, vr2) * lam * MRFRatio);
// printf("=========================================================\n");
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp1,vq1,vr1,lam,lapE(vp1,vq1,vr1),A);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp1,vq1,vr2,lam,lapE(vp1,vq1,vr2),B);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp1,vq2,vr1,lam,lapE(vp1,vq2,vr1),C);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp1,vq2,vr2,lam,lapE(vp1,vq2,vr2),D);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp2,vq1,vr1,lam,lapE(vp2,vq1,vr1),E);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp2,vq1,vr2,lam,lapE(vp2,vq1,vr2),F);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp2,vq2,vr1,lam,lapE(vp2,vq2,vr1),G);
// printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", vp2,vq2,vr2,lam,lapE(vp2,vq2,vr2),H);
double pi = (A + D + F + G) - (B + C + E + H);
if(pi >= 0){
qpbo.AddPairwiseTerm(p,q,0,C-A,0,G-E);
qpbo.AddPairwiseTerm(p,r,0,0,E-A,F-B);
qpbo.AddPairwiseTerm(q,r,0,B-A,0,D-C);
if(pi > 0) {
int w = qpbo.AddNode();
qpbo.AddUnaryTerm(w, A, A - (EnergyTypeT)pi);
qpbo.AddPairwiseTerm(p, w, 0, (EnergyTypeT)pi, 0, 0);
qpbo.AddPairwiseTerm(q, w, 0, (EnergyTypeT)pi, 0, 0);
qpbo.AddPairwiseTerm(r, w, 0, (EnergyTypeT)pi, 0, 0);
}
}else{
qpbo.AddPairwiseTerm(p,q,B-D,0,F-H,0);
qpbo.AddPairwiseTerm(p,r,C-G,D-H,0,0);
qpbo.AddPairwiseTerm(q,r,E-F,0,G-H,0);
int w = qpbo.AddNode();
qpbo.AddUnaryTerm(w,H+(EnergyTypeT)pi,H);
qpbo.AddPairwiseTerm(p, w, 0, 0, -1 * (EnergyTypeT)pi, 0);
qpbo.AddPairwiseTerm(q, w, 0, 0, -1 * (EnergyTypeT)pi, 0);
qpbo.AddPairwiseTerm(r, w, 0, 0, -1 * (EnergyTypeT)pi, 0);
}
};
qpbo.AddNode(nPix);
for(auto i=0; i<nPix; ++i) {
qpbo.AddUnaryTerm(i, (EnergyTypeT)model->operator()(i, (int)p1[i]), (EnergyTypeT)model->operator()(i, (int)p2[i]));
}
for(auto y=1; y<height-1; ++y){
for(auto x=1; x<width-1; ++x) {
addTripleToGraph(y * width + x - 1, y * width + x, y * width + x + 1, model->hCue[y*width+x]);
addTripleToGraph((y - 1) * width + x, y * width + x, (y + 1) * width + x, model->vCue[y*width+x]);
}
}
//solve
float t = (float) getTickCount();
qpbo.MergeParallelEdges();
qpbo.Solve();
qpbo.ComputeWeakPersistencies();
//qpbo.Improve();
//fusion
float unlabeled = 0.0;
float changed = 0.0;
Depth orip1;
orip1.initialize(width, height, -1);
for(auto i=0; i<width * height; ++i)
orip1.setDepthAtInd(i, p1[i]);
for (auto i = 0; i < width * height; ++i) {
int l = qpbo.GetLabel(i);
double disp1 = orip1.getDepthAtInd(i);
double disp2 = p2.getDepthAtInd(i);
if (l == 0)
p1.setDepthAtInd(i, disp1);
else if (l < 0) {
p1.setDepthAtInd(i, disp1);
unlabeled += 1.0;
}
else {
p1.setDepthAtInd(i, disp2);
changed += 1.0;
}
}
printf("Unlabeled pixels: %.2f, ratio: %.2f; label changed: %.2f, ratio: %.2f\n", unlabeled,
unlabeled / (float)nPix,
changed, changed / (float)nPix);
}
real PseudoBoolean<real>::minimize_reduction(vector<label>& x, int& nlabelled) const
{
index nVars = index( x.size() );
// Here it is important that all indices appear as pairs
// in aij or ai
ASSERT_STR( nvars() <= nVars , "x too small");
PBF<real, 4> hocr;
map<int,bool> var_used;
for (auto itr=ai.begin(); itr != ai.end(); ++itr) {
int i = itr->first;
real a = itr->second;
hocr.AddUnaryTerm(i, 0, a);
var_used[i] = true;
}
for (auto itr=aij.begin(); itr != aij.end(); ++itr) {
int i = get_i(itr->first);
int j = get_j(itr->first);
real a = itr->second;
hocr.AddPairwiseTerm(i,j, 0,0,0, a);
var_used[i] = true;
var_used[j] = true;
}
for (auto itr=aijk.begin(); itr != aijk.end(); ++itr) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
real a = itr->second;
int ind[] = {i,j,k};
real E[8] = {0,0,0,0, 0,0,0,0};
E[7] = a;
hocr.AddHigherTerm(3, ind, E);
var_used[i] = true;
var_used[j] = true;
var_used[k] = true;
}
for (auto itr=aijkl.begin(); itr != aijkl.end(); ++itr) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
int l = get_l(itr->first);
real a = itr->second;
int ind[] = {i,j,k,l};
real E[16] = {0,0,0,0, 0,0,0,0,
0,0,0,0, 0,0,0,0};
E[15] = a;
hocr.AddHigherTerm(4, ind, E);
var_used[i] = true;
var_used[j] = true;
var_used[k] = true;
var_used[l] = true;
}
index nOptimizedVars = hocr.maxID() + 1;
PBF<real,2> qpbf;
hocr.toQuadratic(qpbf);
hocr.clear();
QPBO<real> qpbo(nVars, qpbf.size(), err_function_qpbo);
convert(qpbo, qpbf);
qpbo.MergeParallelEdges();
qpbo.Solve();
qpbo.ComputeWeakPersistencies();
nlabelled = 0;
for (int i=0; i < nOptimizedVars; ++i) {
if (var_used[i] || x.at(i)<0) {
x[i] = qpbo.GetLabel(i);
}
if (x[i] >= 0) {
nlabelled++;
}
}
// These variables were not part of the minimization
ASSERT(nVars >= nOptimizedVars);
nlabelled += nVars - nOptimizedVars;
real energy = constant + qpbo.ComputeTwiceLowerBound()/2;
//double energy = eval(x); //Only when x is fully labelled
return energy;
}
int main(int argc, char** argv) {
if (argc < 3) {
printf("Usage: %s img1.png img2.png ... imgN.png\n", argv[0]);
exit(1);
}
const int imageCount = argc - 1;
CImg<float> initImg;
CImg<uint8_t> initImgGray;
CImg<float> curImg;
CImg<uint8_t> curImgGray;
// Load a grayscale image from RGB
initImg = CImg<float>::get_load(argv[1]).RGBtoLab();
initImgGray = initImg.get_shared_channel(0);
int originalWidth = initImg.width();
int originalHeight = initImg.height();
const int workingWidth = originalWidth;
const int workingHeight = originalHeight;
const Eigen::Vector2d imageCenter(workingWidth / 2.0, workingHeight / 2.0);
const double imageSize = max(workingWidth / 2.0, workingHeight / 2.0);
printf("Image size = %d x %d\n", workingWidth, workingHeight);
const int numPoints = 20000;
const int numMainPoints = 3000;
const int windowSize = 31;
CVOpticalFlow klt(windowSize, 15);
float minDistance = min(workingWidth, workingHeight) * 1.0 / sqrt((float) numPoints);
minDistance = max(5.0f, minDistance);
klt.init(initImgGray, numPoints, minDistance);
const int numGoodPoints = klt.sortFeatures(min(workingWidth, workingHeight) * 1.0 / sqrt(numMainPoints));
printf("Feature count = %d\n", klt.featureCount());
DepthReconstruction reconstruct;
CVFundamentalMatrixEstimator fundMatEst;
reconstruct.init(imageCount - 1, klt.featureCount(), numGoodPoints);
vector<Eigen::Vector2f> keypoints;
for (int pointI = 0; pointI < klt.featureCount(); pointI++) {
Eigen::Vector2f match0;
Eigen::Vector2f matchOther;
float error;
klt.getMatch(pointI, match0, matchOther, error);
keypoints.push_back(match0);
match0 -= imageCenter.cast<float>();
match0 /= imageSize;
reconstruct.setKeypoint(pointI, match0.cast<double>());
}
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Processing image #%d\n", imgI);
curImg = CImg<float>::get_load(argv[1 + imgI]).RGBtoLab();
curImgGray = curImg.get_shared_channel(0);
assert(curImg.width() == originalWidth);
assert(curImg.height() == originalHeight);
printf("Computing KLT\n");
klt.compute(curImgGray);
printf("Done\n");
for (int pointI = 0; pointI < klt.featureCount(); pointI++) {
Eigen::Vector2f match0;
Eigen::Vector2f matchOther;
float error;
klt.getMatch(pointI, match0, matchOther, error);
matchOther -= imageCenter.cast<float>();
matchOther /= imageSize;
reconstruct.addObservation(imgI - 1, pointI, matchOther.cast<double>());
}
}
reconstruct.solve();
// Visualize the result
if (true) {
CImg<float> depthVis(workingWidth * 0.25, workingHeight * 0.25);
reconstruct.visualize(depthVis, 1, 0.99, 1.0, false);
depthVis.normalize(0, 255).save_png("results/depth_estimate.png");
depthVis.display();
}
{
const vector<double> rDepths = reconstruct.getDepths();
// double minDepth = *(min_element(&(rDepths.front()), &(rDepths[numMainPoints])));
// double maxDepth = *(max_element(&(rDepths.front()), &(rDepths[numMainPoints])));
// printf("depth range = [%f, %f]\n", minDepth, maxDepth);
vector<double> depth;
TriQPBO qpbo(initImg, keypoints);
// qpbo.addCandidateVertexDepths(rDepths);
for (int camI = 0; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
depth.clear();
reconstruct.getAllDepthSamples(camI, depth);
qpbo.addCandidateVertexDepths(depth);
}
}
printf("initializing qpbo\n");
qpbo.init();
printf("done\n");
CImg<float> colorVis(workingWidth, workingHeight, 1, 3);
colorVis.fill(0);
qpbo.visualizeTriangulation(colorVis);
colorVis.display();
colorVis.normalize(0, 255).save_png("results/delaunay.png");
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/initial_triangle_depth.png");
// (initImg, depthVis).display();
}
printf("Enter unary cost factor...\n");
float unaryCostFactor = 0;
// cin >> unaryCostFactor;
// unaryCostFactor << cin;
// qpbo.solveAlphaExpansion(minDepth, maxDepth, 32, 2, unaryCostFactor);
qpbo.solve(2, unaryCostFactor);
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/qpbo_triangle_depth.png");
}
// (initImg, depthVis).display();
for (int i = 0; i < 1; i++) {
qpbo.smoothAvg();
}
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/qpbo_smoothed_triangle_depth.png");
}
qpbo.solveSmoothHack();
// Output wrl file
{
vector<array<Eigen::Vector3d, 3>> triangles;
qpbo.getSmoothTriangles(triangles);
fstream wrl;
wrl.open("results/model.wrl", std::ios_base::out);
vector<double> depths;
for (const auto& tri : triangles) {
for (const auto& v : tri) {
depths.push_back(v.z());
}
}
sort(depths.begin(), depths.end());
double maxDepth = depths[depths.size() * 0.75];
wrl << "#VRML V2.0 utf8" << endl;
wrl << "Transform { children [" << endl;
wrl << "WorldInfo {" << endl;
wrl << "info [\"Input Format: pol\"]" << endl;
wrl << "}" << endl;
wrl << "Shape {" << endl;
wrl << "\tappearance Appearance { material" << endl;
wrl << "Material {" << endl;
wrl << "diffuseColor 1.0 1.0 1.0" << endl;
wrl << "transparency 0" << endl;
wrl << "}" << endl;
wrl << "texture ImageTexture {" << endl;
wrl << "url [\"" << argv[1] << "\"]" << endl;
wrl << "}}" << endl;
wrl << "geometry IndexedFaceSet {" << endl;
wrl << "coord\nCoordinate {" << endl;
wrl << "point [" << endl;
size_t vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
double depth = -1.0 * (min(tri[i].z(), maxDepth) / maxDepth);
wrl <<
" " << (depth - 1) * ((tri[i].x() / initImg.width()) - 0.5) <<
" " << (depth - 1) * ((tri[i].y() / initImg.height()) - 0.5) <<
// " " << -1.0 * log(1.0 + tri[i].z()) << endl;
" " << depth << endl;
}
vertexIndex++;
}
wrl << "]" << endl;
wrl << "}" << endl;
wrl << "colorPerVertex FALSE" << endl;
wrl << "normal Normal { vector [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl <<
" " << 0 <<
" " << 0 <<
" " << 1 << endl;
}
vertexIndex++;
}
wrl << "]}" << endl;
wrl << "texCoord TextureCoordinate {" << endl;
wrl << "point [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl <<
" " << tri[i].x() / initImg.width() <<
" " << tri[i].y() / initImg.height() << endl;
}
vertexIndex++;
}
wrl << "]" << endl;
wrl << "}" << endl;
wrl << "coordIndex [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl << " " << vertexIndex;
vertexIndex++;
}
wrl << ", -1 " << endl;
}
wrl << "]" << endl;
wrl << "}}]}" << endl;
wrl.close();
}
/*
for (int camI = 0; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
vector<double> depth; // reconstruct.getDepths();
reconstruct.getAllDepthSamples(camI, depth);
printf("initializing qpbo\n");
TriQPBO qpbo(initImg, keypoints, depth);
printf("done\n");
// CImg<float> colorVis(workingWidth, workingHeight, 1, 3);
// colorVis.fill(0);
// qpbo.visualizeTriangulation(colorVis);
// colorVis.display();
qpbo.solve();
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 10);
depthVis.max(0.0);
depthVis.display();
}
}
*/
/*
for (int camI = -1; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
TriQPBO qpbo(initImg, keypoints);
vector<double> depth(keypoints.size());
if (camI < 0) {
depth = reconstruct.getDepths();
} else {
reconstruct.getAllDepthSamples(camI, depth);
}
qpbo.addCandidateVertexDepths(depth, false);
qpbo.solve();
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 10);
depthVis.display();
}
}
*/
}
#if false
const bool display_daisy_stereo = false;
if (display_daisy_stereo) {
SparseDaisyStereo daisyStereo;
daisyStereo.init(initImg.get_shared_channel(0));
vector<Eigen::Vector2f> pointSamples;
for (int y = 0; y < 128; y++) {
for (int x = 0; x < 128; x++) {
pointSamples.push_back(Eigen::Vector2f(
x * workingWidth / 128.0f,
y * workingHeight / 128.0f));
}
}
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Rectifying image #%d\n", imgI);
curImg = CImg<float>::get_load(argv[1 + imgI]).RGBtoLab();
PolarFundamentalMatrix polarF;
bool rectificationPossible = reconstruct.getPolarFundamentalMatrix(
imgI - 1,
Eigen::Vector2d(workingWidth / 2.0, workingHeight / 2.0),
max(workingWidth / 2.0, workingHeight / 2.0),
polarF);
if (!rectificationPossible) {
printf("Rectification not possible, epipoles at infinity.\n");
continue;
}
vector<Eigen::Vector2f> matches(pointSamples.size());
vector<float> matchDistances(pointSamples.size());
daisyStereo.match(curImg.get_shared_channel(0), polarF, pointSamples, matches,
matchDistances);
}
}
#endif
#if false
bool display_dense_interp = false;
if (display_dense_interp) {
float scaleFactor = 256.0f / max(originalWidth, originalHeight);
int denseInterpWidth = scaleFactor * originalWidth;
int denseInterpHeight = scaleFactor * originalHeight;
SparseInterp<double> interp(denseInterpWidth, denseInterpHeight);
interp.init(reconstruct.getPointCount(), 1.0);
const int minInlierCount = 1;
for (size_t i = 0; i < reconstruct.getPointCount(); i++) {
double depth;
size_t inlierC;
const Eigen::Vector2d& pt = reconstruct.getDepthSample(i, depth, inlierC);
if (depth > 0) {
Eigen::Vector2d ptImg = (pt * max(denseInterpWidth, denseInterpHeight) / 2.0);
ptImg.x() += denseInterpWidth / 2.0;
ptImg.y() += denseInterpHeight / 2.0;
interp.insertSample(i, ptImg.x() + 0.5, ptImg.y() + 0.5, depth);
}
}
interp.solve();
}
#endif
#if false
bool display_dense_flow = false;
if (display_dense_flow) {
CImg<uint8_t> initDown = initImgGray;
CImg<uint8_t> curDown;
float scaleFactor = 1024.0f / max(originalWidth, originalHeight);
int scaledWidth = scaleFactor * originalWidth;
int scaledHeight = scaleFactor * originalHeight;
// Resize with moving-average interpolation
initDown.resize(scaledWidth, scaledHeight, -100, -100, 2);
CVDenseOpticalFlow denseFlow;
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Computing dense flow for image #%d\n", imgI);
curDown = CImg<uint8_t>::get_load(argv[1 + imgI]).RGBtoLab().channel(0);
curDown.resize(scaledWidth, scaledHeight, -100, -100, 2);
denseFlow.compute(initDown, curDown);
CImg<float> flow(scaledWidth, scaledHeight, 2);
cimg_forXY(flow, x, y) {
denseFlow.getRelativeFlow(x, y, flow(x, y, 0), flow(x, y, 1));
}
(flow.get_shared_slice(0), flow.get_shared_slice(1)).display();
}
}
real PseudoBoolean<real>::minimize_reduction_fixetal(vector<label>& x, int& nlabelled) const
{
int n = index( x.size() );
// Work with a copy of the coefficients
map<triple, real> aijk = this->aijk;
//map<pair, real> aij = this->aij;
//map<int, real> ai = this->ai;
//real constant = this->constant;
// The quadratic function we are reducing to
int nedges = int( aij.size() + aijk.size() + aijkl.size() + 1000 );
int nvars = int( n + aijkl.size() + aijk.size() + 1000 );
QPBO<real> qpbo(nvars, nedges, err_function_qpbo);
qpbo.AddNode(n);
map<int,bool> var_used;
//
// Step 1: reduce all positive higher-degree terms
//
// Go through the variables one by one and perform the
// reductions of the quartic terms
//for (int ind=0; ind<n; ++ind) {
// // Collect all terms with positive coefficients
// // containing variable i
// real alpha_sum = 0;
// // Holds new var
// int y = -1;
// for (auto itr=aijkl.begin(); itr != aijkl.end(); ++itr) {
// int i = get_i(itr->first);
// int j = get_j(itr->first);
// int k = get_k(itr->first);
// int l = get_l(itr->first);
// real a = itr->second;
// // We only have to test for ind==i because of the
// // order we process the indices
// if (ind==i && a > 0) {
// alpha_sum += a;
// // Add term of degree 3
// aijk[ make_triple(j,k,l) ] += a;
// // Add negative term of degree 4
// // -a*y*xj*xk*xl
// if (y<0) y = qpbo.AddNode();
// int z = qpbo.AddNode();
// qpbo.AddUnaryTerm(z, 0, 3*a);
// qpbo.AddPairwiseTerm(z,y, 0,0,0, -a);
// qpbo.AddPairwiseTerm(z,j, 0,0,0, -a);
// qpbo.AddPairwiseTerm(z,k, 0,0,0, -a);
// qpbo.AddPairwiseTerm(z,l, 0,0,0, -a);
// }
// }
// for (auto itr=aijk.begin(); itr != aijk.end(); ++itr) {
// int i = get_i(itr->first);
// int j = get_j(itr->first);
// int k = get_k(itr->first);
// real a = itr->second;
// // We only have to test for ind==i because of the
// // order we process the indices
// if (ind==i && a > 0) {
// alpha_sum += a;
// // Add term of degree 2
// qpbo.AddPairwiseTerm(j,k, 0,0,0, a);
// // Add negative term of degree 3
// // -a*y*xj*xk
// if (y<0) y = qpbo.AddNode();
// int z = qpbo.AddNode();
// qpbo.AddUnaryTerm(z, 0, 2*a);
// qpbo.AddPairwiseTerm(z,y, 0,0,0, -a);
// qpbo.AddPairwiseTerm(z,j, 0,0,0, -a);
// qpbo.AddPairwiseTerm(z,k, 0,0,0, -a);
// }
// }
// if (alpha_sum > 0) {
// // Add the new quadratic term
// qpbo.AddPairwiseTerm(y,ind, 0,0,0, alpha_sum);
// }
//}
//
// This code should be equivalent to the commented
// block above, but faster
//
vector<real> alpha_sum(n, 0);
vector<int> y(n, -1);
for (auto itr=aijkl.begin(); itr != aijkl.end(); ++itr) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
int l = get_l(itr->first);
real a = itr->second;
if (a > 0) {
alpha_sum[i] += a;
// Add term of degree 3
aijk[ make_triple(j,k,l) ] += a;
// Add negative term of degree 4
// -a*y*xj*xk*xl
if (y[i]<0) y[i] = qpbo.AddNode();
int z = qpbo.AddNode();
qpbo.AddUnaryTerm(z, 0, 3*a);
qpbo.AddPairwiseTerm(z,y[i], 0,0,0, -a);
qpbo.AddPairwiseTerm(z,j, 0,0,0, -a);
qpbo.AddPairwiseTerm(z,k, 0,0,0, -a);
qpbo.AddPairwiseTerm(z,l, 0,0,0, -a);
}
var_used[i] = true;
var_used[j] = true;
var_used[k] = true;
var_used[l] = true;
}
for (auto itr=aijk.begin(); itr != aijk.end(); ++itr) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
real a = itr->second;
if (a > 0) {
alpha_sum[i] += a;
// Add term of degree 2
qpbo.AddPairwiseTerm(j,k, 0,0,0, a);
//aij[ make_pair(j,k) ] += a;
// Add negative term of degree 3
// -a*y*xj*xk
if (y[i]<0) y[i] = qpbo.AddNode();
/*int z = qpbo.AddNode();
qpbo.AddUnaryTerm(z, 0, 2*a);
qpbo.AddPairwiseTerm(z,y[i], 0,0,0, -a);
qpbo.AddPairwiseTerm(z,j, 0,0,0, -a);
qpbo.AddPairwiseTerm(z,k, 0,0,0, -a);*/
aijk[ make_triple(y[i],j,k) ] += -a;
}
var_used[i] = true;
var_used[j] = true;
var_used[k] = true;
}
// No need to continue with the lower degree terms
//for (auto itr=aij.begin(); itr != aij.end(); ++itr) {
// int i = get_i(itr->first);
// int j = get_j(itr->first);
// real& a = itr->second;
// if (a > 0) {
// alpha_sum[i] += a;
// // Add term of degree 1
// ai[ j ] += a;
// // Add negative term of degree 2
// // -a*y*xj
// if (y[i]<0) y[i] = qpbo.AddNode();
// aij[ make_pair(y[i],j) ] += -a;
// // Now remove this term
// a = 0;
// }
//}
//for (auto itr=ai.begin(); itr != ai.end(); ++itr) {
// int i =itr->first;
// real& a = itr->second;
// if (a > 0) {
// alpha_sum[i] += a;
// // Add term of degree 0
// constant += a;
// // Add negative term of degree 1
// // -a*y*xj
// if (y[i]<0) y[i] = qpbo.AddNode();
// ai[ y[i] ] += -a;
// // Now remove this term
// a = 0;
// }
//}
for (int i=0;i<n;++i) {
if (alpha_sum[i] > 0) {
// Add the new quadratic term
qpbo.AddPairwiseTerm(y[i],i, 0,0,0, alpha_sum[i]);
}
}
//
// Done with reducing all positive higher-degree terms
//
// Add all negative quartic terms
for (auto itr=aijkl.begin(); itr != aijkl.end(); ++itr) {
real a = itr->second;
if (a < 0) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
int l = get_l(itr->first);
int z = qpbo.AddNode();
qpbo.AddUnaryTerm(z, 0, -3*a);
qpbo.AddPairwiseTerm(z,i, 0,0,0, a);
qpbo.AddPairwiseTerm(z,j, 0,0,0, a);
qpbo.AddPairwiseTerm(z,k, 0,0,0, a);
qpbo.AddPairwiseTerm(z,l, 0,0,0, a);
}
}
// Add all negative cubic terms
for (auto itr=aijk.begin(); itr != aijk.end(); ++itr) {
real a = itr->second;
if (a < 0) {
int i = get_i(itr->first);
int j = get_j(itr->first);
int k = get_k(itr->first);
int z = qpbo.AddNode();
qpbo.AddUnaryTerm(z, 0, -2*a);
qpbo.AddPairwiseTerm(z,i, 0,0,0, a);
qpbo.AddPairwiseTerm(z,j, 0,0,0, a);
qpbo.AddPairwiseTerm(z,k, 0,0,0, a);
}
}
// Add all quadratic terms
for (auto itr=aij.begin(); itr != aij.end(); ++itr) {
real a = itr->second;
int i = get_i(itr->first);
int j = get_j(itr->first);
qpbo.AddPairwiseTerm(i,j, 0,0,0, a);
var_used[i] = true;
var_used[j] = true;
}
// Add all linear terms
for (auto itr=ai.begin(); itr != ai.end(); ++itr) {
real a = itr->second;
int i = itr->first;
qpbo.AddUnaryTerm(i, 0, a);
var_used[i] = true;
}
qpbo.MergeParallelEdges();
qpbo.Solve();
qpbo.ComputeWeakPersistencies();
nlabelled = 0;
for (int i=0; i<n; ++i) {
if (var_used[i] || x.at(i)<0) {
x[i] = qpbo.GetLabel(i);
}
if (x[i] >= 0) {
nlabelled++;
}
}
real energy = constant + qpbo.ComputeTwiceLowerBound()/2;
return energy;
}
