///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// const int MAX_SAMPLE = 20000;
const bool fast = true, very_fast = true;
// Remove the DC (Y : N x M)
RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// RMatrixXf sY = dY;
// if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// std::sort( samples.data(), samples.data()+samples.size() );
// sY = RMatrixXf( samples.size(), dY.cols() );
// for( int i=0; i<samples.size(); i++ )
// sY.row(i) = dY.row( samples[i] );
// }
// ... and use (pc > 0)
VectorXf lbl = VectorXf::Zero( Y.rows() );
// Find the largest PC of (dY.T * dY) and project onto it
if( very_fast ) {
// Find the largest PC using poweriterations
VectorXf U = VectorXf::Random( dY.cols() );
U = U.array() / U.norm()+std::numeric_limits<float>::min();
for( int it=0; it<20; it++ ){
// Normalize
VectorXf s = dY.transpose()*(dY*U);
s.array() /= s.norm()+std::numeric_limits<float>::min();
if ( (U-s).norm() < 1e-6 )
break;
U = s;
}
// Project onto the PC
lbl = dY*U;
}
else if(fast) {
// Compute the eigen values of the covariance (and project onto the largest eigenvector)
MatrixXf cov = dY.transpose()*dY;
SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
MatrixXf ev = eigensolver.eigenvectors();
lbl = dY * ev.col( ev.cols()-1 );
}
else {
// Use the SVD
JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
// Project onto the largest PC
lbl = svd.matrixU().col(0) * svd.singularValues()[0];
}
// Find the representative label
if( rep_label )
dY.array().square().rowwise().sum().minCoeff( rep_label );
return (lbl.array() < 0).cast<float>();
}
Hamming::Hamming( const VectorXs & gt, float class_weight_pow ):gt_( gt ) {
int M=0,N=gt.rows();;
for( int i=0; i<N; i++ )
if( gt[i] >= M )
M = gt[i]+1;
VectorXf cnt = VectorXf::Zero( M );
for( int i=0; i<N; i++ )
if( gt[i] >= 0 )
cnt[gt[i]] += 1;
class_weight_ = cnt.array() / cnt.array().sum();
class_weight_ = class_weight_.array().pow( -class_weight_pow );
class_weight_ = class_weight_.array() / (cnt.array()*class_weight_.array()).sum();
}
void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
VectorXf b = in.col(i);
VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
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);
}
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() );
}
}
VectorXf PBSeqWeightEstimator::calc_residue_weight(const vector<string>& msa, const int& idx) const {
VectorXf s = VectorXf::Zero(dim); // number of times a particular residue appears
char c;
for (vector<string>::const_iterator pos = msa.begin(); pos != msa.end(); ++pos) {
c = (*pos)[idx];
if (is_allowed(c)) s(abc_idx(c)) += 1;
}
double r = (double) (s.array() > 0).count(); // number of different residues
VectorXf wt(dim);
for (int k = 0; k < dim; ++k) {
if (s(k) > 0) wt(k) = 1. / (r * s(k));
else wt(k) = 0;
}
return wt;
}
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;
}
bool multiModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
int blockSize = 10;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int h = 0;
MatrixXf Hs(M, 9);
MatrixXf inx(M, psize);
MatrixXf res(dataSize, M);
MatrixXi resIndex(dataSize, M);
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount++;
if (m < blockSize)
RandomSampling(psize, dataSize, sample);
else
WeightedSampling(psize, dataSize, resIndex, sample, h);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
for (int i = 0; i < psize; i++)
inx(m, i) = sample[i];
Matrix3f temp_H;
MatrixXf temp_A, localResidue;
fitHomography(pts1, pts2, temp_H, temp_A);
computeHomographyResidue(x1, x2, temp_H, localResidue);
Hs.row(m) = unrollMatrix3f(temp_H);
res.col(m) = localResidue;
if (m >= (blockSize-1) && (m+1)%blockSize == 0) {
h = round(0.1f*m);
sortResidueForIndex(res, (m/blockSize)*blockSize, ((m+1)/blockSize)*blockSize, resIndex);
}
}
VectorXf bestModel(M);
bestModel.setZero();
int bestIndex = 0;
int bestCount = -1;
for (int i = 0; i < M; i++) {
for (int j = 0; j < dataSize; j++)
if (res(j, i) < THRESHOLD)
bestModel(i) += 1;
if (bestModel(i) > bestCount) {
bestIndex = i;
bestCount = bestModel(i);
}
}
VectorXf bestModelRes = res.col(bestIndex);
int inlierCount = (bestModelRes.array() < THRESHOLD).count();
inlier.resize(inlierCount, data.cols());
int runningIdx = 0;
for (int i = 0; i < dataSize; i++)
if (bestModelRes(i) < THRESHOLD) {
inlier.row(runningIdx) = data.row(i);
runningIdx ++;
}
return true;
}
void CloudProjector::_compute() {
assert(orienter_);
assert(orienter_->getOutputCloud());
assert(!depth_projection_);
assert(!intensity_projection_);
MatrixXf& oriented = *orienter_->getOutputCloud();
VectorXf& intensities = *orienter_->getOutputIntensity();
// -- Get a copy of the projected points.
MatrixXf projected(oriented.rows(), 2);
int c=0;
for(int i=0; i<3; ++i) {
if(i == axis_of_projection_)
continue;
projected.col(c) = oriented.col(i);
++c;
}
// -- Transform into pixel units. projected is currently in meters, centered at 0.
//projected *= pixels_per_meter_;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
// u is the col number in the image plane, v is the row number.
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Translate to coordinate system where (0,0) is the upper right of the image.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= min_u;
projected(i, 1) = max_v - projected(i, 1);
}
// -- Get the max depth.
float max_depth = -FLT_MAX;
float min_depth = FLT_MAX;
for(int i=0; i<oriented.rows(); ++i) {
if(oriented(i, axis_of_projection_) > max_depth)
max_depth = oriented(i, axis_of_projection_);
if(oriented(i, axis_of_projection_) < min_depth)
min_depth = oriented(i, axis_of_projection_);
}
// -- Compute the normalized depths. Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest.
VectorXf depths = oriented.col(axis_of_projection_);
if(axis_of_projection_ == 1)
depths = -depths;
depths = (depths.array() - depths.minCoeff()).matrix();
depths = depths / depths.maxCoeff();
// -- Fill the IplImages.
assert(sizeof(float) == 4);
CvSize size = cvSize(ceil(max_u - min_u) + 1, ceil(max_v - min_v) + 1);
float pad_width = 0;
if(min_width_ > 0 && size.width < min_width_) {
pad_width = (float)(min_width_ - size.width) / 2.;
size.width = min_width_;
}
float pad_height = 0;
if(min_height_ > 0 && size.height < min_height_) {
pad_height = (float)(min_height_ - size.height) / 2.;
size.height = min_height_;
}
IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1);
intensity_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
depth_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(depth_projection_);
cvSetZero(intensity_projection_);
assert(intensities.rows() == projected.rows());
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1) + pad_height);
int col = floor(projected(i, 0) + pad_width);
assert(row < size.height && row >= 0 && col < size.width && col >= 0);
// Update accumulator.
((float*)(acc->imageData + row * acc->widthStep))[col]++;
// Update intensity values.
float val = intensities(i) / 255.0 * (3.0 / 4.0) + 0.25;
assert(val <= 1.0 && val >= 0.0);
((float*)(intensity_projection_->imageData + row * intensity_projection_->widthStep))[col] += val;
// Update depth values.
((float*)(depth_projection_->imageData + row * depth_projection_->widthStep))[col] += depths(i); //
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<acc->height; ++v) {
float* intensity_ptr = (float*)(intensity_projection_->imageData + v * intensity_projection_->widthStep);
float* depth_ptr = (float*)(depth_projection_->imageData + v * depth_projection_->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<acc->width; ++u) {
if(*acc_ptr == 0) {
*intensity_ptr = 0;
*depth_ptr = 0;
}
else {
*intensity_ptr = *intensity_ptr / *acc_ptr;
*depth_ptr = *depth_ptr / *acc_ptr;
}
intensity_ptr++;
depth_ptr++;
acc_ptr++;
}
}
// -- Blur the images. TODO: depth too?
cvSmooth(intensity_projection_, intensity_projection_, CV_GAUSSIAN, smoothing_, smoothing_);
// -- Clean up.
cvReleaseImage(&acc);
}
