void cGMM::condition(const GMM& gmm_in,const std::vector<std::size_t>& in,const std::vector<std::size_t>& out){
// std::cout<< "cGMM::condition independent x" << std::endl;
std::size_t D_c = in.size();
std::size_t K = gmm_in.K;
arma::colvec weights(K);
std::vector<arma::vec> Means(K);
std::vector<arma::mat> Covariances(K);
// std::cout<< "K: " << K << std::endl;
gaussian_c.resize(K);
// gmm_in.get_means()[0].print("gmm_in.get_means()[0]");
// gmm_in.get_covariances()[0].print("gmm_in.get_covariances()[0]");
for(std::size_t k = 0; k < gmm_in.K;k++){
// P( x_a | X_b)
gaussian_c[k].condition(gmm_in.get_means()[k],gmm_in.get_covariances()[k],in,out);
// std::cout<< "after conidtion["<<0<<"]" << std::endl;
Means[k].resize(D_c);
Covariances[k] = gaussian_c[k].Sigma_1c2;
// std::cout<< "after Covariances["<<k<<"]" << std::endl;
}
gmm_c = GMM(weights,Means,Covariances);
}
int main(int argc, char** argv)
{
CLI::ParseCommandLine(argc, argv);
if (CLI::HasParam("output_file"))
Log::Warn << "--output_file (-o) is not specified;"
<< "no results will be saved!" << endl;
if (CLI::GetParam<int>("seed") == 0)
mlpack::math::RandomSeed(time(NULL));
else
mlpack::math::RandomSeed((size_t) CLI::GetParam<int>("seed"));
if (CLI::GetParam<int>("samples") < 0)
Log::Fatal << "Parameter to --samples must be greater than 0!" << endl;
GMM gmm;
data::Load(CLI::GetParam<string>("input_model_file"), "gmm", gmm, true);
size_t length = (size_t) CLI::GetParam<int>("samples");
Log::Info << "Generating " << length << " samples..." << endl;
arma::mat samples(gmm.Dimensionality(), length);
for (size_t i = 0; i < length; ++i)
samples.col(i) = gmm.Random();
if (CLI::HasParam("output_file"))
data::Save(CLI::GetParam<string>("output_file"), samples);
}
GMM<FittingType>::GMM(const GMM<OtherFittingType>& other) :
gaussians(other.Gaussians()),
dimensionality(other.Dimensionality()),
means(other.Means()),
covariances(other.Covariances()),
weights(other.Weights()),
localFitter(FittingType()),
fitter(localFitter) { /* Nothing to do. */ }
GMM
transformGMM(const GMM & gmm) const {
GMM gmm2 = gmm;
for (unsigned int i = 0; i < gmm.n_gm(); ++i) {
gmm2.setGM(i, transformGM(gmm.getGM(i)));
}
return gmm2;
}
double Information::upper_bound_entropy(GMM &gmm) {
double HU = 0;
for(unsigned int i = 0; i < gmm.K; i++){
HU = HU - gmm.get_weigts()[i]*log(gmm.get_weigts()[i]) + 0.5*gmm.get_weigts()[i] *log(std::pow(2*M_PI*exp(1),(double)gmm.D) * arma::det(gmm.get_covariances()[i])) ;
//log(std::pow(2*M_PI*exp(1),(double)gmm.D) * arma::det(gmm.getSigma(i)))
}
HU = checkForNan(HU,qHu);
return HU;
}
/*
Assign GMMs components for each pixel.
*/
void assignGMMsComponents( const Mat& img, const Mat& mask, const GMM& bgdGMM, const GMM& fgdGMM, Mat& compIdxs )
{
Point p;
for( p.y = 0; p.y < img.rows; p.y++ )
{
for( p.x = 0; p.x < img.cols; p.x++ )
{
Vec3d color = img.at<Vec3b>(p);
compIdxs.at<int>(p) = mask.at<uchar>(p) == GC_BGD || mask.at<uchar>(p) == GC_PR_BGD ?
bgdGMM.whichComponent(color) : fgdGMM.whichComponent(color);
}
}
}
double Information::lower_bound_entropy(GMM& gmm) {
double HL = 0;
double tmp = 0;
for(unsigned int i = 0; i < gmm.K;i++){
tmp = 0;
for(unsigned int j = 0; j < gmm.K;j++){
tmp = tmp + gmm.get_weigts()[i] * mvnpdf(gmm.get_means()[i],gmm.get_means()[j],gmm.get_covariances()[i] + gmm.get_covariances()[j]);
}
tmp = log(tmp);
HL = HL + gmm.get_weigts()[i] * tmp;
}
HL = -1*HL;
HL = checkForNan(HL,qHl);
return HL;
}
static void mlpackMain()
{
RequireAtLeastOnePassed({ "output" }, false, "no results will be saved");
// Get the GMM and the points.
GMM* gmm = CLI::GetParam<GMM*>("input_model");
arma::mat dataset = std::move(CLI::GetParam<arma::mat>("input"));
// Now calculate the probabilities.
arma::rowvec probabilities(dataset.n_cols);
for (size_t i = 0; i < dataset.n_cols; ++i)
probabilities[i] = gmm->Probability(dataset.unsafe_col(i));
// And save the result.
CLI::GetParam<arma::mat>("output") = std::move(probabilities);
}
/*
Assign GMMs components for each pixel.
*/
static void assignGMMsComponents( const Mat& img, const Mat& mask, const GMM& bgGMM, const GMM& fgGMM, Mat& pixelModel )
{
//Point p;
for( int x = 0; x < img.rows; x++ )
{
for( int y = 0; y < img.cols; y++ )
{
Vec3d color = img.at<Vec3b>(x,y);
if(mask.at<uchar>(x,y) == BG || mask.at<uchar>(x,y) == PBG )
{
pixelModel.at<int>(x,y) = bgGMM.whichModel(color);
}
else
{
pixelModel.at<int>(x,y) = fgGMM.whichModel(color);
}
}
}
}
int main(int argc, char** argv)
{
CLI::ParseCommandLine(argc, argv);
// Get the GMM and the points.
GMM gmm;
data::Load(CLI::GetParam<string>("input_model_file"), "gmm", gmm);
arma::mat dataset;
data::Load(CLI::GetParam<string>("input_file"), dataset);
// Now calculate the probabilities.
arma::rowvec probabilities(dataset.n_cols);
for (size_t i = 0; i < dataset.n_cols; ++i)
probabilities[i] = gmm.Probability(dataset.unsafe_col(i));
// And save the result.
data::Save(CLI::GetParam<string>("output_file"), probabilities);
}
void cGMM::condition(const arma::colvec& x_in, const GMM &gmm_in){
// std::cout<< "cGMM::condition dependent x" << std::endl;
// x_in.print("x_in");
for(std::size_t k = 0; k < gmm_in.K;k++){
// \mu_a + Sig_12 * Sig_22^{-1} * (x - \mu_b)
gaussian_c[k].mu_condition(x_in);
gmm_c.set_mu(k,gaussian_c[k].Mean_c);
gmm_c.set_prior(k,gmm_in.get_weigts()[k] * stats::mvnpdf(x_in,gaussian_c[k].Mu_2,gaussian_c[k].invSigma22,gaussian_c[k].det_22));
}
gmm_c.set_prior( gmm_c.get_weigts() / arma::sum( gmm_c.get_weigts() + std::numeric_limits<double>::min()) );
}
static void initGMMs( const Mat& img, const Mat& mask, GMM& bgGMM, GMM& fgGMM )
{
const int kMeansNum = 10;
const int kMeansType = KMEANS_PP_CENTERS;
Mat bgClustered;
Mat fgClustered;
vector<Vec3f> bgSamples;
vector<Vec3f> fgSamples;
//for each pixel in the image, put those BG in bgsample, FG in fgsample
for( int x = 0; x < img.rows; x++ )
{
for( int y = 0; y < img.cols; y++ )
{
if( mask.at<uchar>(x,y) == BG || mask.at<uchar>(x,y) == PBG )
bgSamples.push_back( (Vec3f)img.at<Vec3b>(x,y) );
else // GC_FGD | GC_PR_FGD
fgSamples.push_back( (Vec3f)img.at<Vec3b>(x,y) );
}
}
Mat bgdSamplesMat( (int)bgSamples.size(), 3, CV_32FC1, &bgSamples[0][0] );
kmeans( bgdSamplesMat, GMM::modelNumInGMM, bgClustered, TermCriteria( CV_TERMCRIT_ITER, kMeansNum, 0.0), 0, kMeansType );
Mat fgdSamplesMat( (int)fgSamples.size(), 3, CV_32FC1, &fgSamples[0][0] );
kmeans( fgdSamplesMat, GMM::modelNumInGMM, fgClustered, TermCriteria( CV_TERMCRIT_ITER, kMeansNum, 0.0), 0, kMeansType );
bgGMM.initParameter();
for( int i = 0; i < (int)bgSamples.size(); i++ )
bgGMM.calcParameter( bgClustered.at<int>(i,0), bgSamples[i] );
bgGMM.endLearning();
fgGMM.initParameter();
for( int i = 0; i < (int)fgSamples.size(); i++ )
fgGMM.calcParameter( fgClustered.at<int>(i,0), fgSamples[i] );
fgGMM.endLearning();
}
/*
Learn GMMs parameters.
*/
void learnGMMs( const Mat& img, const Mat& mask, const Mat& compIdxs, GMM& bgdGMM, GMM& fgdGMM )
{
bgdGMM.initLearning();
fgdGMM.initLearning();
Point p;
for( int ci = 0; ci < GMM::componentsCount; ci++ )
{
for( p.y = 0; p.y < img.rows; p.y++ )
{
for( p.x = 0; p.x < img.cols; p.x++ )
{
if( compIdxs.at<int>(p) == ci )
{
if( mask.at<uchar>(p) == GC_BGD || mask.at<uchar>(p) == GC_PR_BGD )
bgdGMM.addSample( ci, img.at<Vec3b>(p) );
else
fgdGMM.addSample( ci, img.at<Vec3b>(p) );
}
}
}
}
bgdGMM.endLearning();
fgdGMM.endLearning();
}
/*
Initialize GMM background and foreground models using kmeans algorithm.
*/
void initGMMs( const Mat& img, const Mat& mask, GMM& bgdGMM, GMM& fgdGMM )
{
const int kMeansItCount = 10;
const int kMeansType = KMEANS_PP_CENTERS;
Mat bgdLabels, fgdLabels;
vector<Vec3f> bgdSamples, fgdSamples;
Point p;
for( p.y = 0; p.y < img.rows; p.y++ )
{
for( p.x = 0; p.x < img.cols; p.x++ )
{
if( mask.at<uchar>(p) == GC_BGD || mask.at<uchar>(p) == GC_PR_BGD )
bgdSamples.push_back( (Vec3f)img.at<Vec3b>(p) );
else // GC_FGD | GC_PR_FGD
fgdSamples.push_back( (Vec3f)img.at<Vec3b>(p) );
}
}
CV_Assert( !bgdSamples.empty() && !fgdSamples.empty() );
Mat _bgdSamples( (int)bgdSamples.size(), 3, CV_32FC1, &bgdSamples[0][0] );
kmeans( _bgdSamples, GMM::componentsCount, bgdLabels,
TermCriteria( CV_TERMCRIT_ITER, kMeansItCount, 0.0), 0, kMeansType, 0 );
Mat _fgdSamples( (int)fgdSamples.size(), 3, CV_32FC1, &fgdSamples[0][0] );
kmeans( _fgdSamples, GMM::componentsCount, fgdLabels,
TermCriteria( CV_TERMCRIT_ITER, kMeansItCount, 0.0), 0, kMeansType, 0 );
bgdGMM.initLearning();
for( int i = 0; i < (int)bgdSamples.size(); i++ )
bgdGMM.addSample( bgdLabels.at<int>(i,0), bgdSamples[i] );
bgdGMM.endLearning();
fgdGMM.initLearning();
for( int i = 0; i < (int)fgdSamples.size(); i++ )
fgdGMM.addSample( fgdLabels.at<int>(i,0), fgdSamples[i] );
fgdGMM.endLearning();
}
/*
Learn GMMs parameters.
*/
static void learnGMMs( const Mat& img, const Mat& mask, const Mat& pixelModel, GMM& bgGMM, GMM& fgGMM )
{
bgGMM.initParameter();
fgGMM.initParameter();
//Point p;
for( int ci = 0; ci < GMM::modelNumInGMM; ci++ )
{
for( int x = 0; x < img.rows; x++ )
{
for( int y = 0; y < img.cols; y++ )
{
if( pixelModel.at<int>(x,y) == ci )
{
if( mask.at<uchar>(x,y) == BG || mask.at<uchar>(x,y) == PBG )
bgGMM.calcParameter( ci, img.at<Vec3b>(x,y) );
else
fgGMM.calcParameter( ci, img.at<Vec3b>(x,y) );
}
}
}
}
bgGMM.endLearning();
fgGMM.endLearning();
}
float ArmObjSegmentation::getUnaryWeight(cv::Vec3f sample, GMM& color_model)
{
// return exp(-color_model.grabCutLikelihood(sample));
return color_model.probability(sample);
}
int main(int argc, char** argv){
ros::init(argc, argv, "wkmeans_test");
ros::NodeHandle node;
ros::Rate rate(100);
std::size_t K = 10;
arma::vec pi = arma::randu<arma::vec>(3);
pi = arma::normalise(pi);
std::vector<arma::vec> Mu(K);
std::vector<arma::mat> Sigma(K);
float a = -2;
float b = 2;
for(std::size_t k = 0; k < K;k++){
Mu[k] = (b - a) * arma::randu<arma::vec>(3) + a;
Sigma[k].zeros(3,3);
arma::vec tmp = 0.3 * arma::randu<arma::vec>(3) + 0.2;
Sigma[k](0,0) =tmp(0);
Sigma[k](1,1) =tmp(1);
Sigma[k](2,2) =tmp(2);
}
GMM gmm;
gmm.setParam(pi,Mu,Sigma);
std::size_t nb_samples = 10000;
arma::colvec weights;
arma::mat X(nb_samples,3);
gmm.sample(X);
arma::vec L(nb_samples);
gmm.P(X,L);
L = L / arma::max(L);
// L.elem(q1).zeros();
unsigned char rgb[3];
std::vector<std::array<float,3> > colors(nb_samples);
for(std::size_t i = 0 ; i < L.n_elem;i++){
ColorMap::jetColorMap(rgb,L(i),0,1);
colors[i][0] = ((float)rgb[0])/255;
colors[i][1] = ((float)rgb[1])/255;
colors[i][2] = ((float)rgb[2])/255;
}
arma::uvec q1 = arma::find(L > 0.6*arma::max(L));
std::vector<std::array<float,3> > colors2(q1.n_elem);
arma::mat X_W(q1.n_elem,3);
for(std::size_t i = 0; i < q1.n_elem;i++){
assert(q1(i) < colors.size());
assert(q1(i) < X.n_rows);
colors2[i] = colors[q1(i)];
X_W.row(i) = X.row(q1(i));
}
Weighted_Kmeans<double> weighted_kmeans;
weighted_kmeans.cluster(X_W.st(),L);
weighted_kmeans.centroids.print("centroids");
opti_rviz::Vis_points points(node,"centroids");
points.scale = 0.1;
points.r = 1;
points.b = 1;
arma::fmat c = arma::conv_to<arma::fmat>::from(weighted_kmeans.centroids.st());
points.initialise("world",c);
opti_rviz::Vis_point_cloud vis_point(node,"samples");
vis_point.set_display_type(opti_rviz::Vis_point_cloud::DEFAULT);
vis_point.initialise("world",X_W);
opti_rviz::Vis_gmm vis_gmm(node,"gmm");
vis_gmm.initialise("world",pi,Mu,Sigma);
while(node.ok()){
vis_gmm.update(pi,Mu,Sigma);
vis_gmm.publish();
vis_point.update(X_W,colors2,weights);
vis_point.publish();
points.update(c);
points.publish();
ros::spinOnce();
rate.sleep();
}
}
