int main(int argc, char * argv[]) {
if(argc < 5) {
cerr << "Usage: " << argv[0] << " <sizeSample> <maxDelay> <A0.txt> <A2.txt> <*.txt>" << endl;
cerr << "Using omnivision, omniposes, geom2shuda to export the geometry files. " << endl;
cerr << "Minimum 2 cameras." << endl;
return EXIT_FAILURE;
}
int sizeSample = atoi(argv[1]);
int maxDelay = atoi(argv[2]);
if (sizeSample < maxDelay) {
cerr << "sizeSample must be greater than maxDelay." << endl;
return EXIT_FAILURE;
}
vector<vector<RotaPose> > poses;
vector<vector<vector<double> > > angleRotaTab;
vector<int> numIntervals;
int numCams = argc - 3;
poses.resize(numCams);
angleRotaTab.resize(numCams);
numIntervals.resize(numCams);
vector<double> index;
for (int i = 0; i < sizeSample; ++i)
index.push_back((double)i);
// Read geometry file
for (int i = 0; i < numCams; ++i) {
// Camera i
cout << "Reading camera " << i << "...\n";
cout.flush();
string filename(argv[3+i]);
readDataCamPoses(filename, poses[i]);
div_t divresult = div(int(poses[i].size()) - 1, sizeSample);
numIntervals[i] = divresult.quot;
(angleRotaTab[i]).resize(numIntervals[i]);
int id = 0;
for (int j = 0; j < numIntervals[i]; ++j) {
for (int k = 0; k < sizeSample; ++k) {
double val = (poses[i][id]).angleOfRota(poses[i][id++]);
(angleRotaTab[i][j]).push_back(val);
}
string file, param;
file = str(format("angleRotaA%d_%d.txt") % i % j);
param = str(format("angleRotaA%d_%d") % i % j);
writeDataPlot(file, "index", param, index, angleRotaTab[i][j]);
}
}
vector<double> indexDelay;
for (int idDelay(-maxDelay); idDelay<maxDelay; idDelay++)
indexDelay.push_back((double)idDelay);
// Calculate cross-correlation
vector<vector<vector<vector<double> > > > xCorr;
xCorr.resize(numCams -1 );
vector<vector<vector<int> > > idMaxVal;
idMaxVal.resize(numCams -1 );
int nbInterval = INT_MAX;
for (int i = 0; i < numCams; ++i)
if (numIntervals[i] < nbInterval) nbInterval = numIntervals[i];
vector<double> lr(nbInterval, 0.0);
for (int i = 0; i < numCams -1; ++i) {
cout << "Ref camera " << i << " : " << endl;
cout.flush();
(xCorr[i]).resize(numCams -1 -i);
(idMaxVal[i]).resize(numCams -1 -i);
for (int j = i + 1; j < numCams; ++j) {
cout << " camera " << j << " :\n";
(xCorr[i][j-i-1]).resize(nbInterval);
(idMaxVal[i][j-i-1]).resize(nbInterval);
for (int k = 0; k < nbInterval; k++) {
double maxVal = 0;
cout << " " << k << " ";
bool check = core::xcorr1(angleRotaTab[i][k], angleRotaTab[j][k], maxDelay, xCorr[i][j-i-1][k], idMaxVal[i][j-i-1][k], maxVal);
if (!check) {
cerr << "Calculate cross-correlation fail." << endl;
return EXIT_FAILURE;
}
cout << idMaxVal[i][j-i-1][k] << " " << maxVal << " ";
int x0 = idMaxVal[i][j-i-1][k] - 1, x1 = idMaxVal[i][j-i-1][k], x2 = idMaxVal[i][j-i-1][k] + 1;
Matrix3d A; A << x0*x0,x0,1.0, x1*x1,x1,1.0, x2*x2,x2,1.0;
Vector3d b; b << xCorr[i][j-i-1][k][x0 + maxDelay], xCorr[i][j-i-1][k][x1 + maxDelay], xCorr[i][j-i-1][k][x2 + maxDelay];
Vector3d poly2nd = A.colPivHouseholderQr().solve(b); // or fullPivHouseholderQr()
double pc = - poly2nd(1) /2.0 / poly2nd(0);
cout << pc << endl;
if (j == i+1) lr[k] += pc;
if ((numCams != 2) && (i == 0) && (j == numCams - 1)) lr[k] -= pc;
string file, param;
file = str(format("crossCorrA%d%d_%d.txt") % i % j %k);
param = str(format("crossCorrA%d%d_%d") % i % j %k);
writeDataPlot(file, "index", param, indexDelay, xCorr[i][j-i-1][k]);
}
}
}
for (int k = 0; k < nbInterval; ++k) {
cout << "Interval: " << k << endl;
cout.flush();
vector<int> offsets(numCams,0);
int li = - idMaxVal[0][(idMaxVal[0]).size() -1][k];
offsets[numCams - 1] = - idMaxVal[0][(idMaxVal[0]).size() -1][k];
for (int i = 0; i < numCams - 1; ++i) {
li += idMaxVal[i][0][k];
offsets[i] = idMaxVal[i][0][k];
}
cout << "Li = " << li << "; Lr = " << lr[k] << endl;
if (numCams == 2) continue;
// Loop contraint
// Sum of offsets
//try offset around the best one (n = 1)
vector<vector<int> > setOffset;
int n = 1;//abs(li);
int nbSets = 1;
for (int i = 0; i < numCams; ++i)
nbSets *= (2 * n + 1);
setOffset.resize(nbSets);
int length = 1;
for (int i = 0; i < numCams; ++i) {
for (int j = 0; j < nbSets; ++j) {
int jj = j % (length * (2 * n + 1));
div_t divres = div(jj, length);
int kk = divres.quot;
(setOffset[j]).push_back(offsets[i] - n + kk);
}
length *= (2 * n + 1);
}
vector<bool> flagLoop(nbSets, false);
vector<double> totalZNCC(nbSets, 0.0);
double bestZNCC = 0.0;
int bestId = -1;
for (int i = 0; i < setOffset.size(); ++i) {
assert((setOffset[i]).size() == numCams);
int l = 0;
for (int j = 0; j < numCams; ++j)
// cout << setOffset[i][j] << " ";
l += setOffset[i][j];
if (l == 0) {
cout << l << " ";
for (int j = 0; j < numCams; ++j)
cout << setOffset[i][j] << " ";
flagLoop[i] = true;
for (int j = 0; j < numCams - 1; ++j) {
int id = setOffset[i][j] + maxDelay;
totalZNCC[i] += xCorr[j][0][k][id];
}
// the last pair
int id = - setOffset[i][numCams - 1] + maxDelay;
totalZNCC[i] += xCorr[0][numCams - 2][k][id];
cout << totalZNCC[i];
if (totalZNCC[i] > bestZNCC) {
bestZNCC = totalZNCC[i];
bestId = i;
}
cout << endl;
}
}
if (bestId == -1) cerr << "Failed loop contraint." << endl;
cout << "Best score: " << bestZNCC << endl;
for (int i = 0; i < numCams; ++i)
cout << setOffset[bestId][i] << " ";
cout << endl;
// the second
double secondZNCC = 0.0;
int secondId;
for (int i = 0; i < setOffset.size(); ++i) {
if ((flagLoop[i]) && (totalZNCC[i] != bestZNCC) && (totalZNCC[i] > secondZNCC)) {
secondZNCC = totalZNCC[i];
secondId = i;
}
}
cout << "Second score: " << secondZNCC << endl;
for (int i = 0; i < numCams; ++i)
cout << setOffset[secondId][i] << " ";
cout << endl << endl;
}
return EXIT_SUCCESS;
}
