Mat_<double> worldHomToCameraHom(
Mat_<double> const worldPtsHom,
Mat_<double> const rotMatrix,
Mat_<double> const translation)
{
assert(worldPtsHom.cols == 4);
assert(rotMatrix.rows == 3);
assert(rotMatrix.cols == 3);
assert(translation.rows == 3);
assert(translation.cols == 1);
// Convert rotMatrix + translation into a linear transformation in
// homogeneous coordinates.
Mat_<double> rigidMotion = Mat_<double>::zeros(4, 4);
rotMatrix.copyTo(rigidMotion(Range(0, 3), Range(0, 3)));
translation.copyTo(rigidMotion(Range(0, 3), Range(3, 4)));
rigidMotion(3, 3) = 1;
// cout << "rigidMotion: " << rigidMotion << endl;
// Assuming camera calibration matrix is identity.
// Note that OpenCV treats size as "[cols rows]", so matrix multiplication
// has to be done backwards (transpose everything).
Mat_<double> projection = Mat_<double>::eye(3, 4);
Mat result = projection * rigidMotion * worldPtsHom.t();
return result.t();
}
double ConjGradSolverImpl::minimize(InputOutputArray x){
CV_Assert(_Function.empty()==false);
dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon));
Mat x_mat=x.getMat();
CV_Assert(MIN(x_mat.rows,x_mat.cols)==1);
int ndim=MAX(x_mat.rows,x_mat.cols);
CV_Assert(x_mat.type()==CV_64FC1);
if(d.cols!=ndim){
d.create(1,ndim);
r.create(1,ndim);
r_old.create(1,ndim);
minimizeOnTheLine_buf1.create(1,ndim);
minimizeOnTheLine_buf2.create(1,ndim);
}
Mat_<double> proxy_x;
if(x_mat.rows>1){
buf_x.create(1,ndim);
Mat_<double> proxy(ndim,1,buf_x.ptr<double>());
x_mat.copyTo(proxy);
proxy_x=buf_x;
}else{
proxy_x=x_mat;
}
_Function->getGradient(proxy_x.ptr<double>(),d.ptr<double>());
d*=-1.0;
d.copyTo(r);
//here everything goes. check that everything is setted properly
dprintf(("proxy_x\n"));print_matrix(proxy_x);
dprintf(("d first time\n"));print_matrix(d);
dprintf(("r\n"));print_matrix(r);
for(int count=0;count<_termcrit.maxCount;count++){
minimizeOnTheLine(_Function,proxy_x,d,minimizeOnTheLine_buf1,minimizeOnTheLine_buf2);
r.copyTo(r_old);
_Function->getGradient(proxy_x.ptr<double>(),r.ptr<double>());
r*=-1.0;
double r_norm_sq=norm(r);
if(_termcrit.type==(TermCriteria::MAX_ITER+TermCriteria::EPS) && r_norm_sq<_termcrit.epsilon){
break;
}
r_norm_sq=r_norm_sq*r_norm_sq;
double beta=MAX(0.0,(r_norm_sq-r.dot(r_old))/r_norm_sq);
d=r+beta*d;
}
if(x_mat.rows>1){
Mat(ndim, 1, CV_64F, proxy_x.ptr<double>()).copyTo(x);
}
return _Function->calc(proxy_x.ptr<double>());
}
/**
* @author JIA Pei
* @version 2010-02-05
* @brief Calculate statistics for all profiles; Computer all landmarks' mean prof and covariance matrix
* @return void
*/
void VO_ASMNDProfiles::VO_CalcStatistics4AllProfiles()
{
// Calcuate Inverse of Sg
for(unsigned int i = 0; i < this->m_iNbOfPyramidLevels; i++)
{
Mat_<float> allProfiles = Mat_<float>::zeros( this->m_iNbOfSamples, this->m_vvvNormalizedProfiles[0][i][0].GetProfileLength() );
Mat_<float> Covar = Mat_<float>::zeros(this->m_vvvNormalizedProfiles[0][i][0].GetProfileLength(),
this->m_vvvNormalizedProfiles[0][i][0].GetProfileLength() );
Mat_<float> Mean = Mat_<float>::zeros(1, this->m_vvvNormalizedProfiles[0][i][0].GetProfileLength() );
for(unsigned int j = 0; j < this->m_iNbOfPoints; j++)
{
for(unsigned int k = 0; k < this->m_iNbOfProfileDim; k++)
{
for(unsigned int l = 0; l < this->m_iNbOfSamples; l++)
{
Mat_<float> tmpRow = allProfiles.row(l);
Mat_<float> tmp = this->m_vvvNormalizedProfiles[l][i][j].GetTheProfile().col(k).t();
tmp.copyTo(tmpRow);
}
// OK! Now We Calculate the Covariance Matrix of prof for Landmark iPoint
cv::calcCovarMatrix( allProfiles, Covar, Mean, CV_COVAR_NORMAL+CV_COVAR_ROWS+CV_COVAR_SCALE, CV_32F);
// cv::calcCovarMatrix( allProfiles, Covar, Mean, CV_COVAR_SCRAMBLED+CV_COVAR_ROWS, CV_32F);
this->m_vvMeanNormalizedProfile[i][j].Set1DimProfile(Mean.t(), k);
// Explained by YAO Wei, 2008-1-29.
// Actually Covariance Matrix is semi-positive definite. But I am not sure
// whether it is invertible or not!!!
// In my opinion it is un-invert, since C = X.t() * X!!!
cv::invert(Covar, this->m_vvvCVMInverseOfSg[i][j][k], DECOMP_SVD);
}
}
}
}
Mat_<double> cartToHom(Mat_<double> const cart)
{
Mat_<double> hom(cart.rows, cart.cols + 1, 1.0);
Mat_<double> tmp = hom(Range::all(), Range(0, cart.cols));
cart.copyTo(tmp);
return hom;
}
void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x)
{
int iter = 0;
int m = A.size().height;
int n = A.size().width;
Mat_<double> AT = A.t();
double error = 1e-8;
Mat_<double> H = AT*A;
Mat_<double> f = -AT*b;
Mat_<double> x_old = Mat_<double>::zeros(n,1);
Mat_<double> x_new = Mat_<double>::zeros(n,1);
Mat_<double> mu_old = Mat_<double>::zeros(n,1);
Mat_<double> mu_new = Mat_<double>::zeros(n,1);
Mat_<double> r = Mat_<double>::zeros(n,1);
f.copyTo(mu_old);
while(iter < NNLS_MAX_ITER)
{
iter++;
for(int k=0;k<n;k++)
{
x_old.copyTo(x_new);
x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) );
if(x_new(k,0) != x_old(k,0))
{
r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k);
r.copyTo(mu_new);
}
x_new.copyTo(x_old);
mu_new.copyTo(mu_old);
}
if(eKKT(H,f,x_new,error) == true)
{
break;
}
}
x_new.copyTo(x);
}
MatCluster::MatCluster(const cv::Mat_<unsigned char> &wholeOrPart,
int count,
const cv::Rect &boundsInWhole,
std::vector<cv::Point> deepest,
int smoothing, bool copy, bool matIsWhole)
: Cluster(count, boundsInWhole, deepest, smoothing)
{
Mat_<unsigned char> part = matIsWhole ? wholeOrPart(boundsInWhole) : wholeOrPart;
if (copy) part.copyTo(mat);
else mat = part;
}
void readMat(cv::Mat_<T>& mat, char* file){
ifstream* infile = new ifstream(file,ifstream::in|ifstream::binary);
int rows = 0,cols = 0,type=0,size=0;
(*infile)>>rows;
(*infile)>>cols;
(*infile)>>type;
(*infile)>>size;
char* data = new char[size];
infile->read(data,size);
infile->close();
int sizes[2] = {rows,cols};
Mat_<T> temp = Mat(2,sizes,type,data);
temp.copyTo(mat);
delete[] data;
}
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar())
{
FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0);
if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) {
Size wholeSize;
Point ofs;
src.locateROI(wholeSize, ofs);
int dtop = std::min(ofs.y, top);
int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom);
int dleft = std::min(ofs.x, left);
int dright = std::min(wholeSize.width - src.cols - ofs.x, right);
src.adjustROI(dtop, dbottom, dleft, dright);
top -= dtop;
left -= dleft;
bottom -= dbottom;
right -= dright;
}
if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) {
dst.release();
dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right);
}
if (top == 0 && left == 0 && bottom == 0 && right == 0) {
if (src.data != dst.data || src.step != dst.step)
src.copyTo(dst);
return 0;
}
borderType &= ~BORDER_ISOLATED;
if (borderType != BORDER_CONSTANT) {
copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType);
} else {
int cn = src.channels, cn1 = cn;
AutoBuffer<double> buf(cn);
scalarToRawData<_Tp, chs>(value, buf, cn);
copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf);
}
return 0;
}
/**
* @author JIA Pei
* @version 2010-04-03
* @brief Calculate modified steepest descent image for template face - project out appearance variation
* @return void
*/
void VO_AAMInverseIA::VO_CalcModifiedSDI()
{
//project out appearance variation i.e. modify the steepest descent image
this->m_MatSteepestDescentImages = Mat_<float>::zeros(this->m_iNbOfTextures, this->m_iNbOfShapeEigens+4);
this->m_MatModifiedSteepestDescentImages = Mat_<float>::zeros(this->m_iNbOfTextures, this->m_iNbOfShapeEigens+4);
for (unsigned int i = 0; i < this->m_iNbOfTextures; i++)
{
// AAM Revisited (63)
for (unsigned int j = 0; j < 4; j++)
{
this->m_MatSteepestDescentImages(i, j) = this->m_MatSteepestDescentImages4GlobalShapeNormalization(i, j);
}
// AAM Revisited (64)
for (unsigned int j = 0; j < this->m_iNbOfShapeEigens; j++)
{
this->m_MatSteepestDescentImages(i, 4+j) = this->m_MatSteepestDescentImages4ShapeModel(i, j);
}
}
Mat_<float> oneCol = Mat_<float>::zeros(this->m_iNbOfTextures, 1);
Mat_<float> spanedsum = Mat_<float>::zeros(this->m_iNbOfTextures, 1);
Mat_<float> modifiedoneCol = Mat_<float>::zeros(this->m_iNbOfTextures, 1);
Mat_<float> oneSpanRowTranspose = Mat_<float>::zeros(this->m_iNbOfTextures, 1);
for (unsigned int i = 0; i < this->m_MatSteepestDescentImages.cols; i++)
{
spanedsum = Mat_<float>::zeros(this->m_iNbOfTextures, 1);
oneCol = this->m_MatSteepestDescentImages.col(i);
for (unsigned int j = 0; j < this->m_iNbOfTextureEigens; j++)
{
oneSpanRowTranspose = this->m_PCANormalizedTexture.eigenvectors.row(j).t();
double weight = oneSpanRowTranspose.dot(oneCol);
// dst(I)=src1(I)*alpha+src2(I)*beta+gamma
cv::addWeighted( spanedsum, 1.0, oneSpanRowTranspose, weight, 0.0, spanedsum );
}
cv::subtract(oneCol, spanedsum, modifiedoneCol);
Mat_<float> tmpCol = this->m_MatModifiedSteepestDescentImages.col(i);
modifiedoneCol.copyTo(tmpCol);
}
}
Mat_<float> HistogramFeatures::derivateHistogram(Mat_<float> &Histogram){
float tmp1, tmp2;
Mat_<float> firstDer;
Histogram.copyTo(firstDer);
firstDer = cv::Mat::zeros(Histogram.rows,Histogram.cols,Histogram.type());
firstDer(0,0) = Histogram(0,1) - Histogram(0,0);
for(int i = 1; i < Histogram.cols; ++i){
tmp1 = Histogram(0,i) - Histogram(0,i-1);
tmp2 = Histogram(0,i+1) - Histogram(0,i);
firstDer(0,i) = (tmp2 + tmp1)/2;
}
firstDer(0,Histogram.cols-1) = Histogram(0,Histogram.cols-1) - Histogram(0,Histogram.cols-2);
return firstDer;
// int i;
// float tmp1, tmp2;
// //compute derivative first index
// firstderv[0] = data[1]-data[0];
// for(i=1; i<nx-2; i++) //compute first derivative
// {
// tmp1 = data[i]-data[i-1];
// tmp2 = data[i+1]-data[i];
// firstderv[i] = (tmp2+tmp1)/2;
// }
// //compute derivative last index
// firstderv[nx-1] = data[nx-1]-data[nx-2];
}
void CThinPlateSpline::computeSplineCoeffs(std::vector<Point>& iPIn, std::vector<Point>& iiPIn, float lambda,const TPS_INTERPOLATION tpsInter)
{
std::vector<Point>* iP = NULL;
std::vector<Point>* iiP = NULL;
if(tpsInter == FORWARD_WARP)
{
iP = &iPIn;
iiP = &iiPIn;
// keep information which coefficients are set
FLAG_COEFFS_BACK_WARP_SET = true;
FLAG_COEFFS_FORWARD_WARP_SET = false;
}
else if(tpsInter == BACK_WARP)
{
iP = &iiPIn;
iiP = &iPIn;
// keep information which coefficients are set
FLAG_COEFFS_BACK_WARP_SET = false;
FLAG_COEFFS_FORWARD_WARP_SET = true;
}
//get number of corresponding points
int dim = 2;
int n = iP->size();
//Initialize mathematical datastructures
Mat_<float> V(dim,n+dim+1,0.0);
Mat_<float> P(n,dim+1,1.0);
Mat_<float> K = (K.eye(n,n)*lambda);
Mat_<float> L(n+dim+1,n+dim+1,0.0);
// fill up K und P matrix
std::vector<Point>::iterator itY;
std::vector<Point>::iterator itX;
int y = 0;
for (itY = iP->begin(); itY != iP->end(); ++itY, y++) {
int x = 0;
for (itX = iP->begin(); itX != iP->end(); ++itX, x++) {
if (x != y) {
K(y, x) = (float)fktU(*itY, *itX);
}
}
P(y,1) = (float)itY->y;
P(y,2) = (float)itY->x;
}
Mat Pt;
transpose(P,Pt);
// insert K into L
Rect range = Rect(0, 0, n, n);
Mat Lr(L,range);
K.copyTo(Lr);
// insert P into L
range = Rect(n, 0, dim + 1, n);
Lr = Mat(L,range);
P.copyTo(Lr);
// insert Pt into L
range = Rect(0,n,n,dim+1);
Lr = Mat(L,range);
Pt.copyTo(Lr);
// fill V array
std::vector<Point>::iterator it;
int u = 0;
for(it = iiP->begin(); it != iiP->end(); ++it)
{
V(0,u) = (float)it->y;
V(1,u) = (float)it->x;
u++;
}
// transpose V
Mat Vt;
transpose(V,Vt);
cMatrix = Mat_<float>(n+dim+1,dim,0.0);
// invert L
Mat invL;
invert(L,invL,DECOMP_LU);
//multiply(invL,Vt,cMatrix);
cMatrix = invL * Vt;
//compensate for rounding errors
for(int row = 0; row < cMatrix.rows; row++)
{
for(int col = 0; col < cMatrix.cols; col++)
{
double v = cMatrix(row,col);
if(v > (-1.0e-006) && v < (1.0e-006) )
{
cMatrix(row,col) = 0.0;
}
}
}
}
void spm_bp::init_label_super(Mat_<Vec2f>& label_super_k, Mat_<float>& dCost_super_k) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved)
{
printf("==================================================\n");
printf("Initiating particles...Done!\n");
vector<Vec2f> label_vec;
Mat_<float> localDataCost;
for (int sp = 0; sp < numOfSP1; ++sp) {
int id = repPixels1[sp];
int y = subRange1[id][0];
int x = subRange1[id][1];
int h = subRange1[id][3] - subRange1[id][1] + 1;
int w = subRange1[id][2] - subRange1[id][0] + 1;
label_vec.clear();
int k = 0;
while (k < NUM_TOP_K) {
float du = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_u;
float dv = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_v;
du = floor(du * upScale + 0.5) / upScale;
dv = floor(dv * upScale + 0.5) / upScale;
if (du >= -disp_range_u && du <= disp_range_u && dv >= -disp_range_v && dv <= disp_range_v) {
int index_tp = 1;
for (int k1 = 0; k1 < k; ++k1) {
if (checkEqual_PMF_PMBP(label_super_k[repPixels1[sp]][k1], Vec2f(du, dv)))
index_tp = 0;
}
if (index_tp == 1) {
for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii)
label_super_k[superpixelsList1[sp][ii]][k] = Vec2f(du, dv);
label_vec.push_back(Vec2f(du, dv));
++k;
}
}
}
#if USE_CLMF0_TO_AGGREGATE_COST
cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
leftCombinedCrossMap.create(h, w);
subCrossMap1[sp].copyTo(leftCombinedCrossMap);
CFFilter cff;
#endif
int vec_size = label_vec.size();
localDataCost.create(h, w * vec_size);
localDataCost.setTo(0);
#pragma omp parallel for num_threads(NTHREADS)
for (int i = 0; i < vec_size; ++i) {
int kx = i * w;
Mat_<float> rawCost;
getLocalDataCostPerlabel(sp, label_vec[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
rawCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
}
//getLocalDataCost( sp, label_vec, localDataCost);
int pt, px, py, kx;
for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii) {
//cout<<ii<<endl;
pt = superpixelsList1[sp][ii];
px = pt / width1;
py = pt % width1;
for (int kk = 0; kk < NUM_TOP_K; kk++) {
kx = kk * w;
const Mat_<float>& local = localDataCost(cv::Rect(kx, 0, w, h));
dCost_super_k[pt][kk] = local[px - x][py - y];
}
}
}
printf("==================================================\n");
}
void spm_bp::runspm_bp(cv::Mat_<cv::Vec2f>& flowResult)
{
clock_t start_disp, finish_disp;
start_disp = clock();
printf("==================================================\n");
printf("SPM-BP begins\n");
Mat_<Vec2f> label_k(height1 * width1, NUM_TOP_K);
Mat_<float> dcost_k(height1 * width1, NUM_TOP_K);
#ifdef _WIN32
printf("Running on Windows\n");
// Dummy code that speeds up program on windows considerably
// Save label
vector<vector<Vec2f> > label_saved(numOfSP1);
//label_saved.reserve(10000);
vector<vector<Mat_<float> > > dcost_saved(numOfSP1);
//dcost_saved.reserve(10000);
for (int i = 0; i < numOfSP1; i++) {
label_saved[i].reserve(100000);
dcost_saved[i].reserve(100000);
}
#endif
//initiate labels
init_label_super(label_k, dcost_k);
//MESSAGE 0:left, 1:right, 2:up, 3:down
Mat_<Vec<float, NUM_TOP_K> > message(height1 * width1, 4);
message.setTo(0);
//precompute smooth weight
float omega[256];
for (int i = 0; i < 256; ++i)
omega[i] = lambda_smooth * std::exp(-float(i) / 20);
// TODO: Change to Mat4f
Mat_<Vec4f> smoothWt(height1, width1);
smoothWt.setTo(lambda_smooth);
// # pragma omp parallel for
for (int i = 1; i < height1 - 1; ++i)
{
for (int j = 1; j < width1 - 1; ++j) {
const Vec3f &ref = im1f[i][j];
// TODO: Don't need abs here since norm >= 0
// smoothWt[i][j][0] = omega[int(abs(norm(ref - im1f[i][j - 1])))];
// smoothWt[i][j][1] = omega[int(abs(norm(ref - im1f[i][j + 1])))];
// smoothWt[i][j][2] = omega[int(abs(norm(ref - im1f[i - 1][j])))];
// smoothWt[i][j][3] = omega[int(abs(norm(ref - im1f[i + 1][j])))];
smoothWt[i][j][0] = omega[int(norm(ref - im1f[i][j - 1]))];
smoothWt[i][j][1] = omega[int(norm(ref - im1f[i][j + 1]))];
smoothWt[i][j][2] = omega[int(norm(ref - im1f[i - 1][j]))];
smoothWt[i][j][3] = omega[int(norm(ref - im1f[i + 1][j]))];
}
}
float dis_belief_l[NUM_TOP_K];
float dis_belief_r[NUM_TOP_K];
float dis_belief_u[NUM_TOP_K];
float dis_belief_d[NUM_TOP_K];
const int BUFSIZE = NUM_TOP_K * 50;
vector<Vec2f> vec_label(BUFSIZE), vec_label_nei(BUFSIZE);
vector<float> vec_mes_l(BUFSIZE),
vec_mes_r(BUFSIZE),
vec_mes_u(BUFSIZE),
vec_mes_d(BUFSIZE),
vec_belief(BUFSIZE),
vec_d_cost(BUFSIZE);
Mat_<float> DataCost_nei;
start_disp = clock();
double wall_timer = omp_get_wtime();
if (display)
Show_WTA_Flow(-1, label_k, dcost_k, message, flowResult);
int spBegin, spEnd, spStep;
for (int iter = 0; iter < iterNum; iter++) {
if (iter % 2) {
spBegin = numOfSP1 - 1, spEnd = -1, spStep = -1;
}
else {
spBegin = 0, spEnd = numOfSP1, spStep = 1;
}
for (int sp = spBegin; sp != spEnd; sp += spStep) {
int curSPP = superpixelsList1[sp][0];
Vec4i curRange = subRange1[curSPP];
int y = curRange[0];
int x = curRange[1];
int w = curRange[2] - curRange[0] + 1;
int h = curRange[3] - curRange[1] + 1;
// rand neighbor pixel and store
vec_label_nei.clear();
std::set<int>::iterator sIt;
std::set<int>& sAdj = spGraph1[iter % 2].adjList[sp];
for (sIt = sAdj.begin(); sIt != sAdj.end(); ++sIt) {
repPixels1[*sIt] = superpixelsList1[*sIt][rand() % superpixelsList1[*sIt].size()];
Vec2f test_label;
//vector<Vec2f> & cur_label_k = label_k[repPixels1[*sIt]];
for (int k = 0; k < NUM_TOP_K; k++) {
//test_label= cur_label_k[k];
test_label = label_k[repPixels1[*sIt]][k];
if (isNewLabel_PMF_PMBP(vec_label_nei, test_label) == -1)
vec_label_nei.push_back(test_label);
}
}
repPixels1[sp] = superpixelsList1[sp][rand() % superpixelsList1[sp].size()];
for (int k = 0; k < NUM_TOP_K; k++) {
Vec2f test_label = label_k[repPixels1[sp]][k];
float mag = min(disp_range_u, disp_range_v) / 8;
for (; mag >= (1.0 / upScale); mag /= 2.0) {
Vec2f test_label_random;
float tmpVerLabel = test_label[0] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;
float tmpHorLabel = test_label[1] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;
test_label_random[0] = floor(tmpVerLabel * upScale + 0.5) / upScale;
test_label_random[1] = floor(tmpHorLabel * upScale + 0.5) / upScale;
if (test_label_random[0] >= -disp_range_u && test_label_random[0] <= disp_range_u
&& test_label_random[1] >= -disp_range_v && test_label_random[1] <= disp_range_v
&& isNewLabel_PMF_PMBP(vec_label_nei, test_label_random) == -1) //here
{
vec_label_nei.push_back(test_label_random);
}
}
}
const int vec_size = vec_label_nei.size();
// DataCost_nei.release();
DataCost_nei.create(h, w * vec_size);
DataCost_nei.setTo(0);
#if USE_CLMF0_TO_AGGREGATE_COST
cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
leftCombinedCrossMap.create(h, w);
subCrossMap1[sp].copyTo(leftCombinedCrossMap);
CFFilter cff;
#endif
// Compute matching costs for each candidate particles
// printf("Computing matching costs start\n");
#pragma omp parallel for num_threads(NTHREADS)
for (int i = 0; i < vec_size; ++i) {
int kx = i * w;
Mat_<float> rawCost;
getLocalDataCostPerlabel(sp, vec_label_nei[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
rawCost.copyTo(DataCost_nei(cv::Rect(kx, 0, w, h)));
}
//getLocalDataCost(sp, vec_label_nei, DataCost_nei);
// printf("Computing matching costs end\n");
Vec4i curRange_s = spRange1[curSPP];
// int spw = curRange_s[2] - curRange_s[0] + 1;
// int sph = curRange_s[3] - curRange_s[1] + 1;
int spy_s, spy_e, spx_s, spx_e, spx_step, spy_step;
spy_s = curRange_s[0];
spy_e = curRange_s[2] + 1;
spx_s = curRange_s[1];
spx_e = curRange_s[3] + 1;
spx_step = 1;
spy_step = 1;
if (iter % 4 == 1) {
spy_s = curRange_s[2];
spy_e = curRange_s[0] - 1;
spx_s = curRange_s[3];
spx_e = curRange_s[1] - 1;
spx_step = -1;
spy_step = -1;
}
else if (iter % 4 == 2) {
spy_s = curRange_s[2];
spy_e = curRange_s[0] - 1;
spy_step = -1;
spx_s = curRange_s[1];
spx_e = curRange_s[3] + 1;
spx_step = 1;
}
else if (iter % 4 == 3)
{
spy_s = curRange_s[0];
spy_e = curRange_s[2] + 1;
spy_step = 1;
spx_s = curRange_s[3];
spx_e = curRange_s[1] - 1;
spx_step = -1;
}
for (int bi = spx_s; bi != spx_e; bi = bi + spx_step)
for (int bj = spy_s; bj != spy_e; bj = bj + spy_step) {
int p1 = bi * width1 + bj;
int pl = bi * width1 + (bj - 1);
int pu = (bi - 1) * width1 + bj;
int pr = bi * width1 + (bj + 1);
int pd = (bi + 1) * width1 + bj;
//Compute disbelief: three incoming message + data_cost
for (int k = 0; k < NUM_TOP_K; ++k) {
if (bj != 0)
dis_belief_l[k] = message[pl][0][k] + message[pl][2][k] + message[pl][3][k] + dcost_k[pl][k];
if (bj != width1 - 1)
dis_belief_r[k] = message[pr][1][k] + message[pr][2][k] + message[pr][3][k] + dcost_k[pr][k];
if (bi != 0)
dis_belief_u[k] = message[pu][0][k] + message[pu][1][k] + message[pu][2][k] + dcost_k[pu][k];
if (bi != height1 - 1)
dis_belief_d[k] = message[pd][0][k] + message[pd][1][k] + message[pd][3][k] + dcost_k[pd][k];
}
vec_label.clear();
vec_mes_l.clear();
vec_mes_r.clear();
vec_mes_u.clear();
vec_mes_d.clear();
vec_belief.clear();
vec_d_cost.clear();
// Update and save messages with current reference pixel's labels
Vec4f wt_s = smoothWt[bi][bj];
for (int k = 0; k < NUM_TOP_K; ++k) {
Vec2f test_label = label_k[p1][k];
vec_label.push_back(test_label);
float dcost = dcost_k[p1][k];
vec_d_cost.push_back(dcost);
//start_disp = clock();
float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
if (bj != 0) {
_mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
vec_mes_l.push_back(_mes_l);
}
if (bj != width1 - 1) {
_mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
vec_mes_r.push_back(_mes_r);
}
if (bi != 0) {
_mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
vec_mes_u.push_back(_mes_u);
}
if (bi != height1 - 1) {
_mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
vec_mes_d.push_back(_mes_d);
}
vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
}
//propagation + random search
int kx;
for (int test_id = 0; test_id < vec_label_nei.size(); ++test_id) {
Vec2f test_label = vec_label_nei[test_id];
//if(isNewLabel_PMF_PMBP(vec_label,test_label)==-1)
{
vec_label.push_back(test_label);
kx = test_id * w;
const Mat_<float>& local = DataCost_nei(cv::Rect(kx, 0, w, h));
float dcost = local[bi - x][bj - y];
vec_d_cost.push_back(dcost);
//start_disp = clock();
float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
if (bj != 0) {
_mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
vec_mes_l.push_back(_mes_l);
}
if (bj != width1 - 1) {
_mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
vec_mes_r.push_back(_mes_r);
}
if (bi != 0) {
_mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
vec_mes_u.push_back(_mes_u);
}
if (bi != height1 - 1) {
_mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
vec_mes_d.push_back(_mes_d);
}
vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
}
}
Compute_top_k(vec_belief, vec_label, vec_mes_l, vec_mes_r, vec_mes_u, vec_mes_d, vec_d_cost, message, label_k, dcost_k, p1, NUM_TOP_K);
//cout<<label_k[p1][0]<<endl;
Message_normalization_PMF_PMBP(message, p1, NUM_TOP_K);
}
//cout<<"Message "<<clock()-start_disp<<endl;
// propagation end
} //superpixel scan end
if (display)
Show_WTA_Flow(iter, label_k, dcost_k, message, flowResult);
finish_disp = clock();
std::cout << " time on clock(): " << (double)(clock() - start_disp) / CLOCKS_PER_SEC
<< " time on wall: " << omp_get_wtime() - wall_timer << "\n";
} //iteration
Show_WTA_Flow(iterNum, label_k, dcost_k, message, flowResult);
printf("==================================================\n");
printf("SPM-BP finished\n==================================================\n");
}
int MultipleGraphsClassifier::predict(DisjointSetForest &segmentation, const Mat_<Vec3b> &image, const Mat_<float> &mask) {
int maxGraphSize = max(segmentation.getNumberOfComponents(), this->maxTrainingGraphSize);
int minGraphSize = min(segmentation.getNumberOfComponents(), this->minTrainingGraphSize);
if (minGraphSize <= this->k) {
cout<<"the smallest graph has size "<<minGraphSize<<", cannot compute "<<this->k<<" eigenvectors"<<endl;
exit(EXIT_FAILURE);
}
// compute each feature graph for the test sample
vector<WeightedGraph> featureGraphs;
featureGraphs.reserve(this->features.size());
for (int i = 0; i < (int)this->features.size(); i++) {
featureGraphs.push_back(this->computeFeatureGraph(i, segmentation, image, mask));
}
// order graphs by feature, to compute pattern vectors feature by
// feature.
vector<vector<WeightedGraph> > graphsByFeatures(this->features.size());
// add feature graphs for the test sample at index 0
for (int i = 0; i < (int)this->features.size(); i++) {
graphsByFeatures[i].reserve(this->trainingFeatureGraphs.size() + 1);
graphsByFeatures[i].push_back(featureGraphs[i]);
}
// add feature graphs for each training sample
for (int i = 0; i < (int)this->trainingFeatureGraphs.size(); i++) {
for (int j = 0; j < (int)this->features.size(); j++) {
graphsByFeatures[j].push_back(get<0>(this->trainingFeatureGraphs[i])[j]);
}
}
// compute the corresponding pattern vectors
vector<vector<VectorXd> > patternsByFeatures(this->features.size());
for (int i = 0; i < (int)this->features.size(); i++) {
patternsByFeatures[i] = patternVectors(graphsByFeatures[i], this->k, maxGraphSize);
}
// concatenate pattern vectors by image, converting to opencv format
// to work with cv's ML module.
Mat_<float> longPatterns = Mat_<float>::zeros(this->trainingFeatureGraphs.size() + 1, maxGraphSize * k * this->features.size());
for (int i = 0; i < (int)this->trainingFeatureGraphs.size() + 1; i++) {
VectorXd longPattern(maxGraphSize * k * this->features.size());
for (int j = 0; j < this->features.size(); j++) {
longPattern.block(j * maxGraphSize * k, 0, maxGraphSize * k, 1) = patternsByFeatures[j][i];
}
Mat_<double> cvLongPattern;
eigenToCv(longPattern, cvLongPattern);
Mat_<float> castLongPattern = Mat_<float>(cvLongPattern);
castLongPattern.copyTo(longPatterns.row(i));
}
// classification of long patterns using SVM
CvKNearest svmClassifier;
Mat_<int> classes(this->trainingFeatureGraphs.size(), 1);
for (int i = 0; i < (int)this->trainingFeatureGraphs.size(); i++) {
classes(i,0) = get<1>(this->trainingFeatureGraphs[i]);
}
svmClassifier.train(longPatterns.rowRange(1, longPatterns.rows), classes);
return (int)svmClassifier.find_nearest(longPatterns.row(0), 1);
}
/*!
* @brief Import DIP Image from given path
* @param impath : path to the image
* @param[out] I : output image
*/
template<typename T> void readDipImage(const char * impath, DipImage<T> & I)
{
Mat_<T> J = imread(impath, 0);
J.copyTo(I);
};
Mat_<double> estimateRotTransl(
Mat_<double> const worldPts,
Mat_<double> const imagePts)
{
assert(imagePts.cols == 2);
assert(worldPts.cols == 3);
assert(imagePts.rows == worldPts.rows);
// TODO verify all worldPts have z=0
// See "pose estimation" section in the paper.
// Set up linear system of equations.
int const n = imagePts.rows;
Mat_<double> F(2 * n, 9);
for(int i = 0; i < n; i++)
{
F(2 * i, 0) = worldPts(i, 0);
F(2 * i, 1) = 0;
F(2 * i, 2) = -worldPts(i, 0) * imagePts(i, 0);
F(2 * i, 3) = worldPts(i, 1);
F(2 * i, 4) = 0;
F(2 * i, 5) = -worldPts(i, 1) * imagePts(i, 0);
F(2 * i, 6) = 1;
F(2 * i, 7) = 0;
F(2 * i, 8) = -imagePts(i, 0);
F(2 * i + 1, 0) = 0;
F(2 * i + 1, 1) = worldPts(i, 0);
F(2 * i + 1, 2) = -worldPts(i, 0) * imagePts(i, 1);
F(2 * i + 1, 3) = 0;
F(2 * i + 1, 4) = worldPts(i, 1);
F(2 * i + 1, 5) = -worldPts(i, 1) * imagePts(i, 1);
F(2 * i + 1, 6) = 0;
F(2 * i + 1, 7) = 1;
F(2 * i + 1, 8) = -imagePts(i, 1);
}
// Find least-squares estimate of rotation + translation.
SVD svd(F);
Mat_<double> rrp = svd.vt.row(8);
rrp = rrp.clone().reshape(0, 3).t();
if(rrp(2, 2) < 0) {
rrp *= -1; // make sure depth is positive
}
// cout << "rrp: " << rrp << endl;
Mat_<double> transl = \
2 * rrp.col(2) / (norm(rrp.col(0)) + norm(rrp.col(1)));
// cout << "transl: " << transl << endl;
Mat_<double> rot = Mat_<double>::zeros(3, 3);
rrp.col(0).copyTo(rot.col(0));
rrp.col(1).copyTo(rot.col(1));
SVD svd2(rot);
rot = svd2.u * svd2.vt;
if(determinant(rot) < 0) {
rot.col(2) *= -1; // make sure it's a valid rotation matrix
}
if(abs(determinant(rot) - 1) > 1e-10) {
cerr << "Warning: rotation matrix has determinant " \
<< determinant(rot) << " where expected 1." << endl;
}
// cout << "rot: " << rot << endl;
Mat_<double> rotTransl(3, 4);
rot.col(0).copyTo(rotTransl.col(0));
rot.col(1).copyTo(rotTransl.col(1));
rot.col(2).copyTo(rotTransl.col(2));
transl.copyTo(rotTransl.col(3));
return rotTransl;
}
