//get the edge with max probability and then create initialize the hessian
//memory if the nodes have changed
void EdgeSE2PointXYMixture::computeError()
{
int best = -1;
double minError = numeric_limits<double>::max();
for(unsigned int i=0;i<numberComponents;i++){
//allEdges[i]->computeError();
double thisNegLogProb = getNegLogProb(i);
if(minError>thisNegLogProb){
best = i;
minError = thisNegLogProb;
}
}
//if(best!=bestComponent){
bestComponent = best;
UpdateBelief(bestComponent);
//}
g2o::EdgeSE2PointXY::computeError();
}
static cv::Mat RunLoopyBPOnGrideGraph(std::string rootFolder, MCImg<float> &unaryCosts, cv::Mat &imL)
{
struct LoopyBPOnGridGraphEvalParams {
MCImg<float> *allMessages;
MCImg<float> *allBeliefs;
MCImg<float> *unaryCosts;
LoopyBPOnGridGraphEvalParams() : allMessages(0), allBeliefs(0), unaryCosts(0) {}
};
int numRows = unaryCosts.h, numCols = unaryCosts.w, numDisps = unaryCosts.n;
std::vector<cv::Point2i> pixelList(numRows * numCols);
for (int y = 0, id = 0; y < numRows; y++) {
for (int x = 0; x < numCols; x++, id++) {
pixelList[id] = cv::Point2i(x, y);
}
}
MCImg<float> oldMessages(numRows, numCols, 4 * numDisps);
MCImg<float> newMessages(numRows, numCols, 4 * numDisps);
MCImg<float> allBeliefs(numRows, numCols, numDisps);
// Step 1 - Initialize messages
memset(oldMessages.data, 0, 4 * numRows * numCols * numDisps * sizeof(float));
memset(newMessages.data, 0, 4 * numRows * numCols * numDisps * sizeof(float));
for (int i = 0; i < numRows * numCols * numDisps; i++) {
allBeliefs.data[i] = 1.f / numDisps;
}
// Step 2 - Update messages
float maxBeliefDiff = FLT_MAX;
const int maxBPRound = 100;
for (int round = 0; round < maxBPRound && maxBeliefDiff > 1e-7; round++) {
int tic = clock();
if (round % 1 == 0) {
cv::Mat dispMap = DecodeDisparityFromBeliefs(allBeliefs);
//std::vector<std::pair<std::string, void*>> auxParams;
//auxParams.push_back(std::pair<std::string, void*>("allMessages", &oldMessages));
//auxParams.push_back(std::pair<std::string, void*>("allBeliefs", &allBeliefs));
//auxParams.push_back(std::pair<std::string, void*>("unaryCosts", &unaryCosts));
//EvaluateDisparity(rootFolder, dispMap, 1.f, auxParams, "OnMouseLoopyBPOnGridGraph");
LoopyBPOnGridGraphEvalParams evalParams;
evalParams.allMessages = &oldMessages;
evalParams.allBeliefs = &allBeliefs;
evalParams.unaryCosts = &unaryCosts;
EvaluateDisparity(rootFolder, dispMap, 1.f, &evalParams, "OnMouseLoopyBPOnGridGraph");
}
printf("Doing round %d ...\n", round);
std::random_shuffle(pixelList.begin(), pixelList.end());
//printf("pixelList.size() = %d\n", pixelList.size());
//printf("numDisps = %d\n", numDisps);
int numPixels = pixelList.size();
// update messages
#pragma omp parallel for
for (int i = 0; i < numPixels; i++) {
if (i % 1000 == 0) {
//printf("i = %d\n", i);
}
cv::Point2i s = pixelList[i];
for (int k = 0; k < 4; k++) {
cv::Point2i t = s + dirDelta[k];
UpdateMessage(s, t, oldMessages, newMessages, unaryCosts, imL);
}
}
memcpy(oldMessages.data, newMessages.data, 4 * numRows * numCols * numDisps * sizeof(float));
// update beliefs
maxBeliefDiff = -1;
for (int i = 0; i < pixelList.size(); i++) {
cv::Point2i t = pixelList[i];
float maxDiff = UpdateBelief(t, allBeliefs, oldMessages, unaryCosts);
maxBeliefDiff = std::max(maxBeliefDiff, maxDiff);
}
printf("maxBeliefDiff = %f\n", maxBeliefDiff);
printf("%.2fs\n", (clock() - tic) / 1000.f);
cv::Mat dispMap = DecodeDisparityFromBeliefs(allBeliefs);
cv::cvtColor(dispMap, dispMap, CV_GRAY2BGR);
int maxDisp, visualizeScale;
SetupStereoParameters(rootFolder, numDisps, maxDisp, visualizeScale);
dispMap.convertTo(dispMap, CV_8UC3, visualizeScale);
char filePath[1024];
sprintf(filePath, "d:/data/tmpResults/iter=%d.png", round);
cv::imwrite(filePath, dispMap);
}
// Step 3 - Collect Beliefs
cv::Mat dispMap = DecodeDisparityFromBeliefs(allBeliefs);
EvaluateDisparity(rootFolder, dispMap);
return dispMap;
}
