// sx: x standard deviation
// sy: y standard deviation
// sr: red standard deviation
// sg: green standard deviation
// sb: blue standard deviation
// im: image (as a vector)
// w: weight
// function:
void DenseCRF2D::addPairwiseBilateral ( float sx, float sy, float sr, float sg, float sb, const unsigned char* im, float w, const SemiMetricFunction * function ) {
float * feature = new float [N_*5];
for( int j=0; j<H_; j++ )
for( int i=0; i<W_; i++ ){
feature[(j*W_+i)*5+0] = i / sx;
feature[(j*W_+i)*5+1] = j / sy;
feature[(j*W_+i)*5+2] = im[(i+j*W_)*3+0] / sr;
feature[(j*W_+i)*5+3] = im[(i+j*W_)*3+1] / sg;
feature[(j*W_+i)*5+4] = im[(i+j*W_)*3+2] / sb;
}
addPairwiseEnergy( feature, 5, w, function );
delete [] feature;
}
void DenseCRF2D::addPairwiseColorGaussian(float sr, float sg, float sb, const unsigned char * im, float w, const SemiMetricFunction * function)
{
float * feature = new float[N_ * 3];
for (int j = 0; j < H_; j++)
for (int i = 0; i < W_; i++){
feature[(j*W_ + i) * 3 + 0] = im[(i + j*W_) * 3 + 0] / sr;
feature[(j*W_ + i) * 3 + 1] = im[(i + j*W_) * 3 + 1] / sg;
feature[(j*W_ + i) * 3 + 2] = im[(i + j*W_) * 3 + 2] / sb;
}
addPairwiseEnergy(feature, 3, w, function);
delete[] feature;
}
void
pcl::DenseCrf::addPairwiseNormals (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &coord,
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals,
float sx, float sy, float sz,
float snx, float sny, float snz,
float w)
{
std::cout << coord.size () << std::endl;
std::cout << normals.size () << std::endl;
// create feature vector
std::vector<float> feature;
// reserve space for the six-dimensional Gaussian kernel
feature.resize (N_ * 6);
// fill the feature vector
for (size_t i = 0; i < coord.size (); i++)
{
if (pcl_isnan (normals[i].x ()))
{
if (i > 0)
{
normals[i].x () = normals[i-1].x ();
normals[i].y () = normals[i-1].y ();
normals[i].z () = normals[i-1].z ();
}
//std::cout << "NaN" << std::endl;
}
feature[i * 6 ] = static_cast<float> (coord[i].x ()) / sx;
feature[i * 6 + 1] = static_cast<float> (coord[i].y ()) / sy;
feature[i * 6 + 2] = static_cast<float> (coord[i].z ()) / sz;
feature[i * 6 + 3] = static_cast<float> (normals[i].x ()) / snx;
feature[i * 6 + 4] = static_cast<float> (normals[i].y ()) / sny;
feature[i * 6 + 5] = static_cast<float> (normals[i].z ()) / snz;
}
// add kernel
std::cout << "TEEEEST" << std::endl;
addPairwiseEnergy (feature, 6, w);
std::cout << "TEEEEST2" << std::endl;
}
void
pcl::DenseCrf::addPairwiseGaussian (float sx, float sy, float sz, float w)
{
// create feature vector
std::vector<float> feature;
// reserve space for the three-dimensional Gaussian kernel
feature.resize (N_ * 3);
// fill the feature vector
for (size_t i = 0; i < data_.size (); i++)
{
feature[i * 3 ] = static_cast<float> (data_[i].x ()) / sx;
feature[i * 3 + 1] = static_cast<float> (data_[i].y ()) / sy;
feature[i * 3 + 2] = static_cast<float> (data_[i].z ()) / sz;
}
// add kernel
addPairwiseEnergy (feature, 3, w);
}
void
pcl::DenseCrf::addPairwiseBilateral (float sx, float sy, float sz,
float sr, float sg, float sb,
float w)
{
// create feature vector
std::vector<float> feature;
// reserve space for the six-dimensional Gaussian kernel
feature.resize (N_ * 6);
// fill the feature vector
for (size_t i = 0; i < color_.size (); i++)
{
feature[i * 6 ] = static_cast<float> (data_[i].x ()) / sx;
feature[i * 6 + 1] = static_cast<float> (data_[i].y ()) / sy;
feature[i * 6 + 2] = static_cast<float> (data_[i].z ()) / sz;
feature[i * 6 + 3] = static_cast<float> (color_[i].x ()) / sr;
feature[i * 6 + 4] = static_cast<float> (color_[i].y ()) / sg;
feature[i * 6 + 5] = static_cast<float> (color_[i].z ()) / sb;
}
// add kernel
addPairwiseEnergy (feature, 6, w);
}
/////////////////////////////////
///// Pairwise Potentials /////
/////////////////////////////////
void DenseCRF::addPairwiseEnergy (const MatrixXf & features, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type) {
assert( features.cols() == N_ );
addPairwiseEnergy( new PairwisePotential( features, function, kernel_type, normalization_type ) );
}
/////////////////////////////////
///// Pairwise Potentials /////
/////////////////////////////////
void DenseCRF::addPairwiseEnergy (const float* features, int D, float w, const SemiMetricFunction * function) {
if (function)
addPairwiseEnergy( new SemiMetricPotential( features, D, N_, w, function ) );
else
addPairwiseEnergy( new PottsPotential( features, D, N_, w ) );
}
