MatrixXf featureGradient( const MatrixXf & a, const MatrixXf & b ) const {
if (ntype_ == NO_NORMALIZATION )
return kernelGradient( a, b );
else if (ntype_ == NORMALIZE_SYMMETRIC ) {
MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
}
else if (ntype_ == NORMALIZE_AFTER ) {
MatrixXf fb = lattice_.compute( b );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm2 = norm_.array()*norm_.array();
MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b );
}
else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
MatrixXf fa = lattice_.compute( a, true );
MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
VectorXf norm2 = norm_.array()*norm_.array();
MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
return -r+kernelGradient( a, b*norm_.asDiagonal() );
}
}
SeedFeature::SeedFeature( const ImageOverSegmentation & ios, const VectorXf & obj_param ) {
Image rgb_im = ios.image();
const RMatrixXs & s = ios.s();
const int Ns = ios.Ns(), W = rgb_im.W(), H = rgb_im.H();
// Initialize various values
VectorXf area = bin( s, 1, [&](int x, int y){ return 1.f; } );
VectorXf norm = (area.array()+1e-10).cwiseInverse();
pos_ = norm.asDiagonal() * bin( s, 6, [&](int i, int j){ float x=1.0*i/(W-1)-0.5,y=1.0*j/(H-1)-0.5; return makeArray<6>( x, y, x*x, y*y, fabs(x), fabs(y) ); } );
if (N_DYNAMIC_COL) {
Image lab_im;
rgb2lab( lab_im, rgb_im );
col_ = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( rgb_im(y,x, 0), rgb_im(y,x,1), rgb_im(y,x,2), lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2) ); } );
}
const int N_GEO = sizeof(EDGE_P)/sizeof(EDGE_P[0]);
for( int i=0; i<N_GEO; i++ )
gdist_.push_back( GeodesicDistance(ios.edges(),ios.edgeWeights().array().pow(EDGE_P[i])+1e-3) );
// Compute the static features
static_f_ = RMatrixXf::Zero( Ns, N_STATIC_F );
int o=0;
// Add the position features
static_f_.block( 0, o, Ns, N_STATIC_POS ) = pos_.leftCols( N_STATIC_POS );
o += N_STATIC_POS;
// Add the geodesic features
if( N_STATIC_GEO >= N_GEO ) {
RMatrixXu8 bnd = findBoundary( s );
RMatrixXf mask = (bnd.array() == 0).cast<float>()*1e10;
for( int i=0; i<N_GEO; i++ )
static_f_.col( o++ ) = gdist_[i].compute( mask );
for( int j=1; (j+1)*N_GEO<=N_STATIC_GEO; j++ ) {
mask = (bnd.array() != j).cast<float>()*1e10;
for( int i=0; i<N_GEO; i++ )
static_f_.col( o++ ) = gdist_[i].compute( mask );
}
}
if( N_STATIC_EDGE ) {
RMatrixXf edge_map = DirectedSobel().detect( ios.image() );
for( int j=0; j<s.rows(); j++ )
for( int i=0; i<s.cols(); i++ ) {
const int id = s(j,i);
int bin = edge_map(j,i)*N_STATIC_EDGE;
if ( bin < 0 ) bin = 0;
if ( bin >= N_STATIC_EDGE ) bin = N_STATIC_EDGE-1;
static_f_(id,o+bin) += norm[id];
}
o += N_STATIC_EDGE;
}
if( N_OBJ_F>1 )
static_f_.col(o++) = (computeObjFeatures(ios)*obj_param).transpose();
// Initialize the dynamic features
dynamic_f_ = RMatrixXf::Zero( Ns, N_DYNAMIC_F );
n_ = 0;
min_dist_ = RMatrixXf::Ones(Ns,5)*10;
var_ = RMatrixXf::Zero(Ns,6);
}
void filter( MatrixXf & out, const MatrixXf & in, bool transpose ) const {
// Read in the values
if( ntype_ == NORMALIZE_SYMMETRIC || (ntype_ == NORMALIZE_BEFORE && !transpose) || (ntype_ == NORMALIZE_AFTER && transpose))
out = in*norm_.asDiagonal();
else
out = in;
// Filter
if( transpose )
lattice_.compute( out, out, true );
else
lattice_.compute( out, out );
// lattice_.compute( out.data(), out.data(), out.rows() );
// Normalize again
if( ntype_ == NORMALIZE_SYMMETRIC || (ntype_ == NORMALIZE_BEFORE && transpose) || (ntype_ == NORMALIZE_AFTER && !transpose))
out = out*norm_.asDiagonal();
}
// todo: normalization factor in likelihood
MatrixXf calcCorrProb(const MatrixXf& estPts, const MatrixXf& obsPts, const VectorXf& pVis, float stdev, float pBandOutlier) {
MatrixXf sqdists = pairwiseSquareDist(estPts, obsPts);
MatrixXf pBgivenZ_unnormed = (-sqdists/(2*stdev)).array().exp();
MatrixXf pBandZ_unnormed = pVis.asDiagonal()*pBgivenZ_unnormed;
VectorXf pB_unnormed = pBandZ_unnormed.colwise().sum();
VectorXf pBorOutlier_unnormed = (pB_unnormed.array() + pBandOutlier).inverse();
MatrixXf pZgivenB = pBandZ_unnormed * pBorOutlier_unnormed.asDiagonal();
//cout << pZgivenB.row(0);
cout << stdev << endl;
return pZgivenB;
}
virtual void setParameters( const VectorXf & p ) {
if (ktype_ == DIAG_KERNEL) {
parameters_ = p;
initLattice( p.asDiagonal() * f_ );
}
else if (ktype_ == FULL_KERNEL) {
MatrixXf tmp = p;
tmp.resize( parameters_.rows(), parameters_.cols() );
parameters_ = tmp;
initLattice( tmp * f_ );
}
}
RMatrixXf SeedFeature::computeObjFeatures( const ImageOverSegmentation & ios ) {
Image rgb_im = ios.image();
const RMatrixXs & s = ios.s();
const Edges & g = ios.edges();
const int Ns = ios.Ns();
RMatrixXf r = RMatrixXf::Zero( Ns, N_OBJ_F );
if( N_OBJ_F<=1 ) return r;
VectorXf area = bin( s, 1, [&](int x, int y){ return 1.f; } );
VectorXf norm = (area.array()+1e-10).cwiseInverse();
r.col(0).setOnes();
int o = 1;
if (N_OBJ_COL>=6) {
Image lab_im;
rgb2lab( lab_im, rgb_im );
r.middleCols(o,6) = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2), lab_im(y,x,0)*lab_im(y,x,0), lab_im(y,x,1)*lab_im(y,x,1), lab_im(y,x,2)*lab_im(y,x,2) ); } );
RMatrixXf col = r.middleCols(o,3);
if( N_OBJ_COL >= 9)
r.middleCols(o+6,3) = col.array().square();
o += N_OBJ_COL;
// Add color difference features
if( N_OBJ_COL_DIFF ) {
RMatrixXf bcol = RMatrixXf::Ones( col.rows(), col.cols()+1 );
bcol.leftCols(3) = col;
for( int it=0; it*3+2<N_OBJ_COL_DIFF; it++ ) {
// Apply a box filter on the graph
RMatrixXf tmp = bcol;
for( const auto & e: g ) {
tmp.row(e.a) += bcol.row(e.b);
tmp.row(e.b) += bcol.row(e.a);
}
bcol = tmp.col(3).cwiseInverse().asDiagonal()*tmp;
r.middleCols(o,3) = (bcol.leftCols(3)-col).array().abs();
o += 3;
}
}
}
if( N_OBJ_POS >= 2 ) {
RMatrixXf xy = norm.asDiagonal() * bin( s, 2, [&](int x, int y){ return makeArray<2>( 1.0*x/(s.cols()-1)-0.5, 1.0*y/(s.rows()-1)-0.5 ); } );
r.middleCols(o,2) = xy;
o+=2;
if( N_OBJ_POS >=4 ) {
r.middleCols(o,2) = xy.array().square();
o+=2;
}
}
if( N_OBJ_EDGE ) {
RMatrixXf edge_map = DirectedSobel().detect( rgb_im );
for( int j=0; j<s.rows(); j++ )
for( int i=0; i<s.cols(); i++ ) {
const int id = s(j,i);
int bin = edge_map(j,i)*N_OBJ_EDGE;
if ( bin < 0 ) bin = 0;
if ( bin >= N_OBJ_EDGE ) bin = N_OBJ_EDGE-1;
r(id,o+bin) += norm[id];
}
o += N_OBJ_EDGE;
}
const int N_BASIC = o-1;
// Add in context features
for( int i=0; i<N_OBJ_CONTEXT; i++ ) {
const int o0 = o - N_BASIC;
// Box filter the edges
RMatrixXf f = RMatrixXf::Ones( Ns, N_BASIC+1 ), bf = RMatrixXf::Zero( Ns, N_BASIC+1 );
f.rightCols( N_BASIC ) = r.middleCols(o0,N_BASIC);
for( Edge e: g ) {
bf.row(e.a) += f.row(e.b);
bf.row(e.b) += f.row(e.a);
}
r.middleCols(o,N_BASIC) = bf.col(0).array().max(1e-10f).inverse().matrix().asDiagonal() * bf.rightCols(N_BASIC);
o += N_BASIC;
}
return r;
}
