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; }