247 lines
8.6 KiB
C++
247 lines
8.6 KiB
C++
/***
|
||
* @Date: 2020-09-12 19:01:56
|
||
* @Author: Qing Shuai
|
||
* @LastEditors: Qing Shuai
|
||
* @LastEditTime: 2022-07-29 22:38:40
|
||
* @FilePath: /EasyMocapPublic/library/pymatch/include/matchSVT.hpp
|
||
*/
|
||
#pragma once
|
||
#include "base.h"
|
||
#include "projfunc.hpp"
|
||
#include "Timer.hpp"
|
||
#include "visualize.hpp"
|
||
|
||
namespace match
|
||
{
|
||
struct Config
|
||
{
|
||
bool debug;
|
||
int max_iter;
|
||
float tol;
|
||
float w_rank;
|
||
float w_sparse;
|
||
Config(Control& control){
|
||
debug = (control["debug"] > 0.);
|
||
max_iter = int(control["maxIter"]);
|
||
tol = control["tol"];
|
||
w_rank = control["w_rank"];
|
||
w_sparse = control["w_sparse"];
|
||
}
|
||
};
|
||
|
||
// dimGroups: [0, nF1, nF1 + nF2, ...]
|
||
ListList getBlocksFromDimGroups(const List& dimGroups){
|
||
ListList block;
|
||
for(int i=0;i<dimGroups.size() - 1;i++){
|
||
// 这个视角没有找到人的情况
|
||
if(dimGroups[i] == dimGroups[i+1])continue;
|
||
for(int j=0;j<dimGroups.size()-1;j++){
|
||
if(i==j)continue;
|
||
if(dimGroups[j] == dimGroups[j+1])continue;
|
||
block.push_back({dimGroups[i], dimGroups[i+1] - dimGroups[i],
|
||
dimGroups[j], dimGroups[j+1] - dimGroups[j]});
|
||
}
|
||
}
|
||
return block;
|
||
}
|
||
// matchSVT with constraint and observation
|
||
// M_aff: (N, N): affinity matrix
|
||
// M_constr: =0, when (i, j) cannot be the same person
|
||
// if not consider this, set to 1(N, N)
|
||
// M_obs: =0, when (i, j) cannot be observed
|
||
// if not consider this, set to 1(N, N)
|
||
Mat matchSVT(Mat M_aff, List dimGroups, Mat M_constr, Mat M_obs, Control control)
|
||
{
|
||
bool debug = (control["debug"] > 0.);
|
||
int max_iter = int(control["maxIter"]);
|
||
float tol = control["tol"];
|
||
|
||
int N = M_aff.rows();
|
||
auto dual_blocks = getBlocksFromDimGroups(dimGroups);
|
||
// 对角线约束
|
||
for(int i=0;i<dimGroups.size() - 1;i++){
|
||
M_constr.block(dimGroups[i], dimGroups[i], dimGroups[i+1] - dimGroups[i], dimGroups[i+1]-dimGroups[i]).setZero();
|
||
}
|
||
M_constr.diagonal().setOnes();
|
||
// 将affinity乘一下constraint，保证满足约束
|
||
M_aff = (M_aff.array() * M_constr.array()).matrix();
|
||
// check一下所有区块，如果最大值和最小值差异过小的，直接认为是错误观测
|
||
for (auto block : dual_blocks)
|
||
{
|
||
Mat mat = M_aff.block(block[0], block[2], block[1], block[3]);
|
||
if(debug){
|
||
std::cout << "(" << block[0] << ", " << block[2] << "), ";
|
||
std::cout << "min: " << mat.minCoeff() << ", max: " << mat.maxCoeff() << std::endl;
|
||
}
|
||
if(mat.minCoeff() > 0.7 && block[1] > 1 && block[3] > 1){
|
||
// 如果大于0.9,说明区分度不够高啊，认为观测是虚假的
|
||
M_obs.block(block[0], block[2], block[1], block[3]).setZero();
|
||
}
|
||
}
|
||
// set the diag of M_aff to zeros
|
||
M_aff.diagonal().setConstant(0);
|
||
Mat X = M_aff;
|
||
Mat Y = Mat::Zero(N, N);
|
||
Mat Q = M_aff;
|
||
Mat W = (control["w_sparse"] - M_aff.array()).matrix();
|
||
float mu = 64;
|
||
Timer timer;
|
||
timer.tic();
|
||
for (int iter = 0; iter < max_iter; iter++)
|
||
{
|
||
Mat X0 = X;
|
||
// update Q with SVT
|
||
Q = 1.0 / mu * Y + X;
|
||
Eigen::BDCSVD<Mat> UDV(Q.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV));
|
||
Array Ds(Dsoft(UDV.singularValues(), control["w_rank"] / mu));
|
||
Mat Qnew(UDV.matrixU() * Ds.matrix().asDiagonal() * UDV.matrixV().adjoint());
|
||
Q = Qnew;
|
||
// update X
|
||
X = Q - (M_obs.array() * W.array() + Y.array()).matrix() / mu;
|
||
X = (X.array() * M_constr.array()).matrix();
|
||
// set the diagonal
|
||
X.diagonal().setOnes();
|
||
// 注意这个min,max
|
||
X = X.cwiseMin(1.f).cwiseMax(0.f);
|
||
#pragma omp parallel for
|
||
for(int i=0;i<dual_blocks.size();i++)
|
||
{
|
||
auto& block = dual_blocks[i];
|
||
X.block(block[0], block[2], block[1], block[3]) = myproj2dpam(X.block(block[0], block[2], block[1], block[3]), 1e-2);
|
||
}
|
||
X = (X + X.transpose().eval()) / 2;
|
||
Y = Y + mu * (X - Q);
|
||
float pRes = (X - Q).norm() / N;
|
||
float dRes = mu * (X - X0).norm() / N;
|
||
if(debug){
|
||
#ifdef _USE_OPENCV_
|
||
cv::imshow("Q", eigen2mat(Q));
|
||
cv::imshow("X", eigen2mat(X));
|
||
cv::waitKey(100);
|
||
#endif
|
||
std::cout << "Iter " << iter << ", Res = (" << pRes << ", " << dRes << "), mu = " << mu << std::endl;
|
||
}
|
||
if (pRes < tol && dRes < tol)
|
||
{
|
||
std::cout << "End " << iter << ", Res = (" << pRes << ", " << dRes << "), mu = " << mu << std::endl;
|
||
break;
|
||
}
|
||
|
||
if (pRes > 10 * dRes)
|
||
{
|
||
mu *= 2;
|
||
}
|
||
else if (dRes > 10 * pRes)
|
||
{
|
||
mu /= 2;
|
||
}
|
||
}
|
||
if(debug){
|
||
#ifdef _USE_OPENCV_
|
||
timer.toc("solving svt");
|
||
cv::imshow("X", eigen2mat(X));
|
||
cv::imshow("Q", eigen2mat(Q));
|
||
cv::waitKey(0);
|
||
#endif
|
||
}
|
||
return X;
|
||
}
|
||
|
||
Mat matchALS(Mat M_aff, List dimGroups, Mat M_constr, Mat M_obs, Control control)
|
||
{
|
||
// This function is to solve
|
||
// min <W, X> + alpha||x||_* + beta||x||_1, st. X \in C
|
||
// <beta - W, AB^T> + alpha/2||A||^2 + alpha/2||B||^2
|
||
// st AB^T = Z, Z\in \Omega
|
||
const Config cfg(control);
|
||
Mat W = (M_aff + M_aff.transpose())/2;
|
||
// set the diag of W to zeros
|
||
for(int i=0;i<W.rows();i++){
|
||
W(i, i) = 0;
|
||
}
|
||
Mat X = W;
|
||
Mat Z = W;
|
||
Mat Y = W;
|
||
Y.setZero();
|
||
int mu = 64;
|
||
int n = X.rows();
|
||
int maxRank = 0;
|
||
for(size_t i=0;i<dimGroups.size() - 1;i++){
|
||
if(dimGroups[i+1]-dimGroups[i] > maxRank){
|
||
maxRank = dimGroups[i+1]-dimGroups[i];
|
||
}
|
||
}
|
||
std::cout << "[matchALS] set the max rank = " << maxRank << std::endl;
|
||
Mat eyeRank = Mat::Identity(maxRank, maxRank);
|
||
// initial value
|
||
Mat A = Mat::Random(n, maxRank);
|
||
Mat B;
|
||
for(int iter=0;iter<cfg.max_iter;iter++){
|
||
Mat X0 = X;
|
||
X = Z - (((Y - W).array() + cfg.w_sparse)/mu).matrix();
|
||
B = ((A.transpose() * A + cfg.w_rank/mu * eyeRank).ldlt().solve(A.transpose() * X)).transpose();
|
||
A = ((B.transpose() * B + cfg.w_rank/mu * eyeRank).ldlt().solve(B.transpose() * X.transpose())).transpose();
|
||
|
||
X = A * B.transpose();
|
||
Z = X + Y/mu;
|
||
for(int i=0;i<dimGroups.size() - 1;i++){
|
||
int start = dimGroups[i];
|
||
int end = dimGroups[i+1];
|
||
Z.block(start, start, end-start, end-start).setIdentity();
|
||
}
|
||
// 注意这个min,max
|
||
Z = Z.cwiseMin(1.f).cwiseMax(0.f);
|
||
Y = Y + mu*(X - Z);
|
||
|
||
float pRes = (X - Z).norm()/n;
|
||
float dRes = mu*(X - X0).norm()/n;
|
||
|
||
if(cfg.debug){
|
||
#ifdef _USE_OPENCV_
|
||
cv::imshow("Z", eigen2mat(Z));
|
||
cv::imshow("X", eigen2mat(X));
|
||
cv::waitKey(10);
|
||
#endif
|
||
std::cout << "Iter " << iter << ", Res = (" << pRes << ", " << dRes << "), mu = " << mu << std::endl;
|
||
}
|
||
|
||
if (pRes < cfg.tol && dRes < cfg.tol)
|
||
{
|
||
std::cout << "End " << iter << ", Res = (" << pRes << ", " << dRes << "), mu = " << mu << std::endl;
|
||
break;
|
||
}
|
||
|
||
if(pRes > 10*dRes){
|
||
mu *= 2;
|
||
}else if(dRes > 10*pRes){
|
||
mu /= 2;
|
||
}
|
||
}
|
||
X = (X + X.transpose()) / 2;
|
||
return X;
|
||
}
|
||
|
||
Vec<int> getViewsFromDim(List& dimGroups){
|
||
Vec<int> lists(dimGroups.back(), -1);
|
||
for(int i=0;i<dimGroups.size() - 1;i++){
|
||
for(int c=dimGroups[i];c<dimGroups[i+1];c++){
|
||
lists[c] = i;
|
||
}
|
||
}
|
||
return lists;
|
||
}
|
||
|
||
Vec<int> getDimsFromViews(List& views){
|
||
Vec<int> dims = {0};
|
||
int startview = 0;
|
||
for(int i=0;i<views.size();i++){
|
||
if(views[i] != startview){
|
||
dims.push_back(i);
|
||
startview = views[i];
|
||
}
|
||
}
|
||
dims.push_back(views.size());
|
||
return dims;
|
||
}
|
||
} // namespace match
|