// Gradient computations
double DenseCRF::gradient( int n_iterations, const ObjectiveFunction & objective, VectorXf * unary_grad, VectorXf * lbl_cmp_grad, VectorXf * kernel_grad) const {
// Run inference
std::vector< MatrixXf > Q(n_iterations+1);
MatrixXf tmp1, unary( M_, N_ ), tmp2;
unary.fill(0);
if( unary_ )
unary = unary_->get();
expAndNormalize( Q[0], -unary );
for( int it=0; it<n_iterations; it++ ) {
tmp1 = -unary;
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q[it] );
tmp1 -= tmp2;
}
expAndNormalize( Q[it+1], tmp1 );
}
// Compute the objective value
MatrixXf b( M_, N_ );
double r = objective.evaluate( b, Q[n_iterations] );
sumAndNormalize( b, b, Q[n_iterations] );
// Compute the gradient
if(unary_grad && unary_)
*unary_grad = unary_->gradient( b );
if( lbl_cmp_grad )
*lbl_cmp_grad = 0*labelCompatibilityParameters();
if( kernel_grad )
*kernel_grad = 0*kernelParameters();
for( int it=n_iterations-1; it>=0; it-- ) {
// Do the inverse message passing
tmp1.fill(0);
int ip = 0, ik = 0;
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
// Compute the pairwise gradient expression
if( lbl_cmp_grad ) {
VectorXf pg = pairwise_[k]->gradient( b, Q[it] );
lbl_cmp_grad->segment( ip, pg.rows() ) += pg;
ip += pg.rows();
}
// Compute the kernel gradient expression
if( kernel_grad ) {
VectorXf pg = pairwise_[k]->kernelGradient( b, Q[it] );
kernel_grad->segment( ik, pg.rows() ) += pg;
ik += pg.rows();
}
// Compute the new b
pairwise_[k]->applyTranspose( tmp2, b );
tmp1 += tmp2;
}
sumAndNormalize( b, tmp1.array()*Q[it].array(), Q[it] );
// Add the gradient
if(unary_grad && unary_)
*unary_grad += unary_->gradient( b );
}
return r;
}
MatrixXf DenseCRF::inference ( int n_iterations ) const {
MatrixXf Q( M_, N_ ), tmp1, unary( M_, N_ ), tmp2;
unary.fill(0);
if( unary_ )
unary = unary_->get();
expAndNormalize( Q, -unary );
for( int it=0; it<n_iterations; it++ ) {
tmp1 = -unary;
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q );
tmp1 -= tmp2;
}
expAndNormalize( Q, tmp1 );
}
return Q;
}
MatrixXf DenseCRF::startInference() const{
MatrixXf Q( M_, N_ );
Q.fill(0);
// Initialize using the unary energies
if( unary_ )
expAndNormalize( Q, -unary_->get() );
return Q;
}
void
pcl::DenseCrf::runInference (float relax)
{
// set the unary potentials
for (size_t i = 0; i < unary_.size (); i++)
next_[i] = -unary_[i];
// Add up all pairwise potentials
for( unsigned int i = 0; i < pairwise_potential_.size(); i++ )
pairwise_potential_[i]->compute( next_, current_, tmp_, M_ );
// Exponentiate and normalize
expAndNormalize( current_, next_, 1.0, relax );
}
void
pcl::DenseCrf::mapInference (int n_iterations, std::vector<int> &result, float relax)
{
// Start inference
// Initialize using the unary energies
expAndNormalize (current_, unary_, -1);
for (int i = 0; i < n_iterations; i++)
{
runInference (relax);
std::cout << "iteration: " << i+1 << " - DONE" << std::endl;
}
// Find the map
for (int i = 0; i < N_; i++)
{
const int prob_idx = i * M_;
// Find the max
float p_label = current_[prob_idx];
int idx = 0;
for (int j = 1; j < M_; j++)
{
if (p_label < current_[prob_idx + j])
{
p_label = current_[prob_idx + j];
idx = j;
}
}
result[i] = idx;
}
/*
for( int i = 0; i < N_; i++ ){
const float * p = prob + i*M_;
// Find the max and subtract it so that the exp doesn't explode
float mx = p[0];
int imx = 0;
for( int j=1; j<M_; j++ )
if( mx < p[j] ){
mx = p[j];
imx = j;
}
result[i] = imx
}
*/
}
void DenseCRF::stepInference( MatrixXf & Q, MatrixXf & tmp1, MatrixXf & tmp2 ) const{
tmp1.resize( Q.rows(), Q.cols() );
tmp1.fill(0);
if( unary_ )
tmp1 -= unary_->get();
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q );
tmp1 -= tmp2;
}
// Exponentiate and normalize
expAndNormalize( Q, tmp1 );
}
void
pcl::DenseCrf::inference (int n_iterations, std::vector<float> &result, float relax)
{
// Start inference
// Initialize using the unary energies
expAndNormalize (current_, unary_, -1);
for (int i = 0; i < n_iterations; i++)
{
runInference (relax);
std::cout << "iteration: " << i+1 << " - DONE" << std::endl;
}
// Copy the data into the result vector
result = current_;
}
void DenseCRF::stepInference( float relax ){
#ifdef SSE_DENSE_CRF
__m128 * sse_next_ = (__m128*)next_;
__m128 * sse_unary_ = (__m128*)unary_;
__m128 * sse_additional_unary_ = (__m128*)additional_unary_;
#endif
// Set the unary potential
#ifdef SSE_DENSE_CRF
for( int i=0; i<(N_*M_-1)/4+1; i++ )
sse_next_[i] = - sse_unary_[i] - sse_additional_unary_[i];
#else
for( int i=0; i<N_*M_; i++ )
next_[i] = - unary_[i] - additional_unary_[i];
#endif
// Add up all pairwise potentials
for( unsigned int i=0; i<pairwise_.size(); i++ )
pairwise_[i]->apply( next_, current_, tmp_, M_ );
// Exponentiate and normalize
expAndNormalize( current_, next_, 1.0, relax );
}
void DenseCRF::startInference(){
// Initialize using the unary energies
expAndNormalize( current_, unary_, -1 );
}
