GMMExpectationMaximization::uint GMMExpectationMaximization::execute(const MatrixX & dataset)
{
const uint data_count = dataset.rows();
const uint num_gaussians = m_means.size();
const uint dim = dataset.cols();
MatrixX pxi(data_count,num_gaussians);
MatrixX pix(data_count,num_gaussians);
VectorX pxidatatot(data_count);
VectorX weights(num_gaussians);
VectorX ex(data_count);
MatrixX ts(dim,dim);
VectorX dif(dim);
Real prev_log_likelyhood = 1.0;
uint it_num;
for (it_num = 0; it_num < m_max_iterations; it_num++)
{
for (uint g = 0; g < num_gaussians; g++)
weights[g] = m_weights[g];
for (uint d = 0; d < data_count; d++)
for (uint g = 0; g < num_gaussians; g++)
pxi(d,g) = gauss(m_means[g],m_covs[g],dataset.row(d).transpose());
pxidatatot = pxi * weights;
Real log_likelyhood = pxidatatot.array().log().sum() / Real(data_count);
if (it_num != 0 && (std::abs(log_likelyhood / prev_log_likelyhood - 1.0) < m_termination_threshold))
break;
prev_log_likelyhood = log_likelyhood;
for (uint d = 0; d < data_count; d++)
pix.row(d) = (pxi.row(d).transpose().array() * weights.array()).transpose() / pxidatatot[d];
ex = pix.colwise().sum();
for(uint g = 0; g < num_gaussians; g++)
{
m_weights[g] = ex[g] / Real(data_count);
m_means[g] = (dataset.transpose() * pix.col(g)) / ex[g];
ts = MatrixX::Zero(dim,dim);
for (uint d = 0; d < data_count; d++)
{
dif = dataset.row(d).transpose() - m_means[g];
ts.noalias() += (dif * dif.transpose()) * pix(d,g);
}
m_covs[g] = (ts / ex[g]) + MatrixX::Identity(dim,dim) * m_epsilon;
}
// interruption point here
if (m_termination_handler && m_termination_handler->isTerminated())
return it_num;
}
return it_num;
}
RecSkinWidget::RecSkinWidget(QWidget *parent)
: QWidget(parent)
, ui(new Ui::RecSkinWidget)
, nowPixNum(-1)
, m_rowMaxPix(15)
{
ui->setupUi(this);
setFixedSize(490,255);
QStringList l = MOption::instance()->option(OPTION_Recommand, OPTION_GROUP_Theme).toStringList();
QString appPath = QCoreApplication::applicationDirPath() + "/";
for(int i = 0; i < 4; ++i)
{
RecSkinPushButton *SkinPushButton = new RecSkinPushButton(this);
connect(SkinPushButton,SIGNAL(currentPixmap(QString,QPixmap,QColor)),
this,SIGNAL(currentPixmap(QString,QPixmap,QColor)));
connect(SkinPushButton,SIGNAL(clickNum(int)),
this,SLOT(onClickNum(int)));
if(i <= l.size() - 1)
{
QString path = appPath + l.at(i);
qDebug() << path;
QPixmap pix(path);
SkinPushButton->setToolTip("");
if(i == 0)
{
QString themeType = MOption::instance()->option("WindowBGPixmapType", "theme").toString();
QImage skinImage(pix.toImage());
int num = 0;
int red = 0;
int green = 0;
int blue = 0;
for(int ii = 0; ii < skinImage.width(); ++ii)
{
for(int j = 0; j < skinImage.height(); ++j)
{
++num;
QRgb rgbValue(skinImage.pixel(ii, j));
red += qRed(rgbValue);
green += qGreen(rgbValue);
blue += qBlue(rgbValue);
}
}
if(num != 0)
{
red = red/num;
green = green/num;
blue = blue/num;
}
oneFileName = path;
onePix = pix;
oneColor = QColor(red,green,blue,255);
if(themeType.isEmpty())
{
SkinPushButton->setPixmap(path,pix,oneColor,120,true);
QTimer::singleShot(1000, this, SLOT(getRgb()));
}
else
{
SkinPushButton->setPixmap(path,pix,QColor(255,255,255,255),120);
}
}
else
{
SkinPushButton->setPixmap(path,pix,QColor(255,255,255,255),120);
}
SkinPushButton->setSkin(true);
}
ui->horizontalLayoutDefault->addWidget(SkinPushButton);
}
QPixmap addPix(":/add.png");
QPixmap historyPix(":/skinHistoryImage.png");
RecSkinPushButton *button = new RecSkinPushButton(this);
button->setPixmap("",addPix, QColor(255,255,255,255),60,false);
connect(button,SIGNAL(clickResult()),
this,SIGNAL(definePixmapResult()));
ui->horizontalLayoutHistoryBotton->insertWidget(0,button);
QString historyPath = QCoreApplication::applicationDirPath() + "/theme/saved";
// TODO MEM leak
QDir *dir = new QDir(historyPath);
QStringList filter;
QList<QFileInfo> *fileInfo=new QList<QFileInfo>(dir->entryInfoList(filter));
QStringList fileNames;
for(int i = 0; i < fileInfo->count(); ++ i)
{
if(fileInfo->at(i).isDir())
continue;
fileNames << fileInfo->at(i).fileName();
}
fileNames.sort();
int fileSize = fileNames.size();
if(fileSize > m_rowMaxPix)
fileSize = m_rowMaxPix;
for(int i = 0; i < m_rowMaxPix - fileSize; ++i)
{
addItem("",historyPix, QColor(255,255,255,255),false,false);
}
for(int i = 0; i < fileSize; ++i)
{
QString imagePathName = historyPath + "/" + fileNames.value(i);
QPixmap pxi(imagePathName);
addItem(imagePathName,pxi, QColor(255,255,255,255),false,true);
}
QString themeType = MOption::instance()->option("WindowBGPixmapType", "theme").toString();
if(themeType == "bitmap")
{
QString fileName = MOption::instance()->option("WindowBGPixmap", "theme").toString();
for(int i = 0; i < buttons.size(); ++i)
{
if(buttons.value(i)->getFileName() == fileName)
nowPixNum = i;
}
}
}
bool Matcher::findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Feature& ref_ftr,
const double d_estimate,
const double d_min,
const double d_max,
double& depth)
{
SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
// Compute start and end of epipolar line in old_kf for match search, on unit plane!
Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
epi_dir_ = A - B;
// Compute affine warp matrix
warp::getWarpMatrixAffine(
*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
// feature pre-selection
reject_ = false;
if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering)
{
const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
if(cosangle < options_.epi_search_edgelet_max_angle) {
reject_ = true;
return false;
}
}
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
// Find length of search range on epipolar line
Vector2d px_A(cur_frame.cam_->world2cam(A));
Vector2d px_B(cur_frame.cam_->world2cam(B));
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
// Warp reference patch at ref_level
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
if(epi_length_ < 2.0)
{
px_cur_ = (px_A+px_B)/2.0;
Vector2d px_scaled(px_cur_/(1<<search_level_));
bool res;
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true;
}
return false;
}
size_t n_steps = epi_length_/0.7; // one step per pixel
Vector2d step = epi_dir_/n_steps;
if(n_steps > options_.max_epi_search_steps)
{
printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
n_steps, epi_length_, d_min, d_max);
return false;
}
// for matching, precompute sum and sum2 of warped reference patch
int pixel_sum = 0;
int pixel_sum_square = 0;
PatchScore patch_score(patch_);
// now we sample along the epipolar line
Vector2d uv = B-step;
Vector2i last_checked_pxi(0,0);
++n_steps;
for(size_t i=0; i<n_steps; ++i, uv+=step)
{
Vector2d px(cur_frame.cam_->world2cam(uv));
Vector2i pxi(px[0]/(1<<search_level_)+0.5,
px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
if(pxi == last_checked_pxi)
continue;
last_checked_pxi = pxi;
// check if the patch is full within the new frame
if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
continue;
// TODO interpolation would probably be a good idea
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+ (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+ (pxi[0]-halfpatch_size_);
int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
if(zmssd < zmssd_best) {
zmssd_best = zmssd;
uv_best = uv;
}
}
if(zmssd_best < PatchScore::threshold())
{
if(options_.subpix_refinement)
{
px_cur_ = cur_frame.cam_->world2cam(uv_best);
Vector2d px_scaled(px_cur_/(1<<search_level_));
bool res;
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true;
}
return false;
}
px_cur_ = cur_frame.cam_->world2cam(uv_best);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
return true;
}
return false;
}
