C++代码实现MATLAB中的hdphmm函数功能

// ============================================================
// HDP-HMM with Sticky Transitions (Fox et al., 2011)
// 完全自包含 (仅依赖 C++ 标准库, 无第三方依赖)
// 编译: g++ -std=c++14 -O2 HDPHMM.cpp -o hdpmm_demo
// 运行: ./hdpmm_demo
// ============================================================

#include <vector>
#include <cmath>
#include <random>
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <limits>

// ============================================================
// 简易矩阵类 (替代 Eigen)
// ============================================================
class Matrix {
public:
int rows, cols;
std::vector<double> data;

Matrix() : rows(0), cols(0) {}
Matrix(int r, int c, double init = 0.0)
: rows(r), cols(c), data((size_t)r * c, init) {}

double& operator()(int i, int j) { return data[(size_t)i * cols + j]; }
double operator()(int i, int j) const { return data[(size_t)i * cols + j]; }

static Matrix Zero(int r, int c) { return Matrix(r, c, 0.0); }
static Matrix Ones(int r, int c) { return Matrix(r, c, 1.0); }
static Matrix Constant(int r, int c, double v) { return Matrix(r, c, v); }
static Matrix Identity(int n) {
Matrix I(n, n, 0.0);
for (int i = 0; i < n; ++i) I(i, i) = 1.0;
return I;
}

Matrix transpose() const {
Matrix R(cols, rows);
for (int i = 0; i < rows; ++i)
for (int j = 0; j < cols; ++j)
R(j, i) = (*this)(i, j);
return R;
}

Matrix operator*(const Matrix& B) const {
Matrix R(rows, B.cols, 0.0);
for (int i = 0; i < rows; ++i)
for (int k = 0; k < cols; ++k) {
double a = (*this)(i, k);
if (a == 0.0) continue;
for (int j = 0; j < B.cols; ++j)
R(i, j) += a * B(k, j);
}
return R;
}

Matrix operator+(const Matrix& B) const {
Matrix R(rows, cols);
for (size_t i = 0; i < data.size(); ++i) R.data[i] = data[i] + B.data[i];
return R;
}
Matrix operator–(const Matrix& B) const {
Matrix R(rows, cols);
for (size_t i = 0; i < data.size(); ++i) R.data[i] = data[i] – B.data[i];
return R;
}
Matrix operator*(double s) const {
Matrix R(rows, cols);
for (size_t i = 0; i < data.size(); ++i) R.data[i] = data[i] * s;
return R;
}
Matrix operator/(double s) const { return (*this) * (1.0 / s); }
Matrix& operator+=(const Matrix& B) {
for (size_t i = 0; i < data.size(); ++i) data[i] += B.data[i];
return *this;
}
Matrix& operator/=(double s) {
for (double& v : data) v /= s;
return *this;
}

double sum() const {
double s = 0;
for (double v : data) s += v;
return s;
}
double norm() const {
double s = 0;
for (double v : data) s += v * v;
return std::sqrt(s);
}

// LU 分解求行列式
double determinant() const {
int n = rows;
Matrix M = *this;
double det = 1.0;
for (int i = 0; i < n; ++i) {
int pivot = i;
for (int k = i + 1; k < n; ++k)
if (std::fabs(M(k, i)) > std::fabs(M(pivot, i))) pivot = k;
if (std::fabs(M(pivot, i)) < 1e-12) return 0.0;
if (pivot != i) {
for (int j = 0; j < n; ++j) std::swap(M(i, j), M(pivot, j));
det = –det;
}
det *= M(i, i);
for (int k = i + 1; k < n; ++k) {
double f = M(k, i) / M(i, i);
for (int j = i; j < n; ++j) M(k, j) -= f * M(i, j);
}
}
return det;
}

// 高斯-约旦消元求逆
Matrix inverse() const {
int n = rows;
Matrix M = *this;
Matrix I = Matrix::Identity(n);
for (int i = 0; i < n; ++i) {
int pivot = i;
for (int k = i + 1; k < n; ++k)
if (std::fabs(M(k, i)) > std::fabs(M(pivot, i))) pivot = k;
if (pivot != i) {
for (int j = 0; j < n; ++j) {
std::swap(M(i, j), M(pivot, j));
std::swap(I(i, j), I(pivot, j));
}
}
double pv = M(i, i);
if (std::fabs(pv) < 1e-300) pv = 1e-300;
for (int j = 0; j < n; ++j) { M(i, j) /= pv; I(i, j) /= pv; }
for (int k = 0; k < n; ++k) {
if (k == i) continue;
double f = M(k, i);
for (int j = 0; j < n; ++j) {
M(k, j) -= f * M(i, j);
I(k, j) -= f * I(i, j);
}
}
}
return I;
}

// Cholesky: 返回下三角 L, 满足 L * L^T = A
Matrix cholesky() const {
int n = rows;
Matrix Lm(n, n, 0.0);
for (int i = 0; i < n; ++i) {
for (int j = 0; j <= i; ++j) {
double s = (*this)(i, j);
for (int k = 0; k < j; ++k) s -= Lm(i, k) * Lm(j, k);
if (i == j) {
if (s < 1e-12) s = 1e-12;
Lm(i, j) = std::sqrt(s);
} else {
Lm(i, j) = s / Lm(j, j);
}
}
}
return Lm;
}

// 取第 i 行 (1 x cols)
Matrix row(int i) const {
Matrix R(1, cols);
for (int j = 0; j < cols; ++j) R(0, j) = (*this)(i, j);
return R;
}
};

inline Matrix operator*(double s, const Matrix& M) { return M * s; }

// ============================================================
// HDP-HMM 类 (Sticky HDP-HMM, Fox et al. 2011)
// ============================================================
class HDPHMM {
public:
double alpha, gamma, kappa;
int L;

Matrix beta; // L x 1 全局转移基分布
Matrix A; // L x L 转移矩阵
std::vector<Matrix> theta; // 每个 D x 1
std::vector<Matrix> Sigma; // 每个 D x D

Matrix observations; // T x D
int T, D;
std::vector<int> z;

Matrix Nmat; // L x L
Matrix Ni; // L x 1

std::mt19937 rng;

HDPHMM(int L_, double a_, double g_, double k_, unsigned seed = 42u)
: alpha(a_), gamma(g_), kappa(k_), L(L_), rng(seed) {}

void initialize(const Matrix& data) {
observations = data;
T = data.rows;
D = data.cols;

z.resize(T);
std::uniform_int_distribution<int> dist(0, L – 1);
for (int t = 0; t < T; ++t) z[t] = dist(rng);

beta = Matrix::Constant(L, 1, 1.0 / L);
A = Matrix::Constant(L, L, 1.0 / L);
Nmat = Matrix::Zero(L, L);
Ni = Matrix::Zero(L, 1);

theta.resize(L);
Sigma.resize(L);
for (int k = 0; k < L; ++k) {
theta[k] = Matrix::Zero(D, 1);
Sigma[k] = Matrix::Identity(D);
}
updateCounts();
}

void updateCounts() {
Nmat = Matrix::Zero(L, L);
Ni = Matrix::Zero(L, 1);
for (int t = 0; t < T – 1; ++t) {
int i = z[t], j = z[t + 1];
Nmat(i, j) += 1;
Ni(i, 0) += 1;
}
}

// —– 采样转移矩阵 A 和全局 beta —–
void sampleTransitions() {
std::vector<std::vector<int>> m(L, std::vector<int>(L, 0));
std::uniform_real_distribution<double> u01(0, 1);
for (int i = 0; i < L; ++i) {
for (int j = 0; j < L; ++j) {
double p = (Nmat(i, j) * beta(j, 0)) /
(Nmat(i, j) + alpha * beta(j, 0)
+ (i == j ? kappa : 0.0) + 1e-300);
if (u01(rng) < p) m[i][j] = 1;
}
}

Matrix beta_post(L, 1, gamma / L);
for (int j = 0; j < L; ++j)
for (int i = 0; i < L; ++i)
beta_post(j, 0) += m[i][j];
beta = sampleDirichlet(beta_post);

for (int i = 0; i < L; ++i) {
Matrix alpha_post(L, 1);
for (int j = 0; j < L; ++j)
alpha_post(j, 0) = alpha * beta(j, 0);
alpha_post(i, 0) += kappa;
for (int j = 0; j < L; ++j)
alpha_post(j, 0) += Nmat(i, j);

Matrix row_probs = sampleDirichlet(alpha_post);
for (int j = 0; j < L; ++j)
A(i, j) = row_probs(j, 0);
}
}

// —– 前向-后向采样隐状态 —–
void sampleStates() {
Matrix logLik(T, L);
for (int t = 0; t < T; ++t) {
Matrix xrow = observations.row(t); // 1 x D
for (int k = 0; k < L; ++k)
logLik(t, k) = logEmissionProb(xrow, k);
}

Matrix logAlpha(T, L);
for (int k = 0; k < L; ++k)
logAlpha(0, k) = std::log(beta(k, 0) + 1e-300) + logLik(0, k);

for (int t = 1; t < T; ++t) {
for (int k = 0; k < L; ++k) {
double mx = –std::numeric_limits<double>::infinity();
for (int j = 0; j < L; ++j) {
double v = logAlpha(t – 1, j) + std::log(A(j, k) + 1e-300);
if (v > mx) mx = v;
}
logAlpha(t, k) = mx + logLik(t, k);
}
}

std::vector<int> z_new(T);

// 采样最后一个状态
Matrix probs_last(L, 1);
double pmax = –std::numeric_limits<double>::infinity();
for (int k = 0; k < L; ++k)
if (logAlpha(T – 1, k) > pmax) pmax = logAlpha(T – 1, k);
double psum = 0;
for (int k = 0; k < L; ++k) {
probs_last(k, 0) = std::exp(logAlpha(T – 1, k) – pmax);
psum += probs_last(k, 0);
}
for (int k = 0; k < L; ++k) probs_last(k, 0) /= psum;
z_new[T – 1] = sampleDiscrete(probs_last);

// 后向采样
for (int t = T – 2; t >= 0; —t) {
int nxt = z_new[t + 1];
Matrix probs(L, 1);
double pmx = –std::numeric_limits<double>::infinity();
for (int k = 0; k < L; ++k) {
double v = logAlpha(t, k) + std::log(A(k, nxt) + 1e-300);
if (v > pmx) pmx = v;
}
double ps = 0;
for (int k = 0; k < L; ++k) {
double v = logAlpha(t, k) + std::log(A(k, nxt) + 1e-300);
probs(k, 0) = std::exp(v – pmx);
ps += probs(k, 0);
}
for (int k = 0; k < L; ++k) probs(k, 0) /= ps;
z_new[t] = sampleDiscrete(probs);
}

z = z_new;
updateCounts();
}

// —– 采样发射参数 (高斯分布) —–
void sampleEmissions() {
double nu0 = D + 2;
Matrix Psi0 = Matrix::Identity(D);

for (int k = 0; k < L; ++k) {
std::vector<int> idx;
for (int t = 0; t < T; ++t)
if (z[t] == k) idx.push_back(t);

int nk = (int)idx.size();
if (nk == 0) {
theta[k] = sampleGaussian(Matrix::Zero(D, 1), Matrix::Identity(D));
Sigma[k] = sampleInvWishart(nu0, Psi0);
continue;
}

Matrix mean = Matrix::Zero(D, 1);
for (int t : idx)
for (int d = 0; d < D; ++d)
mean(d, 0) += observations(t, d);
for (int d = 0; d < D; ++d) mean(d, 0) /= nk;

Matrix scatter = Matrix::Zero(D, D);
for (int t : idx) {
Matrix diff(D, 1);
for (int d = 0; d < D; ++d)
diff(d, 0) = observations(t, d) – mean(d, 0);
scatter += diff * diff.transpose();
}

Matrix Psi_n = Psi0 + scatter
+ (mean * mean.transpose()) * ((double)nk / (nk + 1.0));
double nu_n = nu0 + nk;
Sigma[k] = sampleInvWishart(nu_n, Psi_n);

Matrix cov_theta = Sigma[k] / (double)nk;
theta[k] = sampleGaussian(mean, cov_theta);
}
}

void gibbsStep() {
sampleTransitions();
sampleStates();
sampleEmissions();
}

void run(int nIter) {
for (int iter = 0; iter < nIter; ++iter) {
gibbsStep();
if (iter % 50 == 0) {
std::cout << "Iter " << std::setw(3) << iter
<< " | active states: " << countActiveStates()
<< std::endl;
}
}
}

int countActiveStates() const {
std::vector<bool> used(L, false);
for (int s : z) used[s] = true;
int cnt = 0;
for (bool b : used) if (b) cnt++;
return cnt;
}

void printTransitionMatrix() const {
std::cout << "Transition Matrix A:" << std::endl;
std::cout << std::fixed << std::setprecision(4);
for (int i = 0; i < L; ++i) {
std::cout << " ";
for (int j = 0; j < L; ++j)
std::cout << std::setw(8) << A(i, j);
std::cout << std::endl;
}
}

private:
double logEmissionProb(const Matrix& x_row, int k) const {
Matrix x = x_row.transpose(); // D x 1
Matrix diff = x – theta[k];
double logDet = std::log(Sigma[k].determinant() + 1e-300);
Matrix quad = diff.transpose() * Sigma[k].inverse() * diff;
return –0.5 * (D * std::log(2 * M_PI) + logDet + quad(0, 0));
}

Matrix sampleDirichlet(const Matrix& alpha) {
int K = alpha.rows;
Matrix samples(K, 1);
for (int i = 0; i < K; ++i) {
std::gamma_distribution<double> g(alpha(i, 0), 1.0);
samples(i, 0) = g(rng);
}
double s = samples.sum();
if (s < 1e-300) s = 1e-300;
samples /= s;
return samples;
}

int sampleDiscrete(const Matrix& probs) {
double total = probs.sum();
if (total <= 0.0) total = 1e-300;
std::uniform_real_distribution<double> u(0, total);
double r = u(rng);
double c = 0;
for (int i = 0; i < probs.rows; ++i) {
c += probs(i, 0);
if (r <= c) return i;
}
return probs.rows – 1;
}

// 从 N(mean, cov) 采样 (Cholesky 法)
Matrix sampleGaussian(const Matrix& mean, const Matrix& cov) {
int d = mean.rows;
Matrix zvec(d, 1);
std::normal_distribution<double> nd(0, 1);
for (int i = 0; i < d; ++i) zvec(i, 0) = nd(rng);
Matrix Lm = cov.cholesky();
return mean + Lm * zvec;
}

// InvWishart(nu, Psi) 采样 (Bartlett 分解)
Matrix sampleInvWishart(double nu, const Matrix& Psi) {
int d = Psi.rows;
Matrix S(d, d, 0.0);
std::normal_distribution<double> nd(0, 1);
for (int i = 0; i < d; ++i) {
std::chi_squared_distribution<double> chi(nu – i);
S(i, i) = std::sqrt(chi(rng));
for (int j = 0; j < i; ++j)
S(i, j) = nd(rng);
}
Matrix W = S * S.transpose(); // ~ Wishart(nu, I)
Matrix Lm = Psi.cholesky(); // Psi = Lm * Lm^T
// X ~ InvWishart(nu, Psi) => X = Lm * W^{-1} * Lm^T
return Lm * W.inverse() * Lm.transpose();
}
};

// ============================================================
// main
// ============================================================
int main() {
int T = 1000, D = 2, trueK = 3;
Matrix data(T, D);

std::vector<std::vector<double>> trueMeans = {
{ 0.0, 0.0},
{ 5.0, 5.0},
{–5.0, 5.0}
};

// 生成合成数据 (3 状态高斯 HMM, 自转移 0.9) – 固定种子保证可复现
std::mt19937 rng(42);
std::normal_distribution<double> noise(0, 1);
std::uniform_real_distribution<double> u01(0, 1);
int state = 0;
for (int t = 0; t < T; ++t) {
if (u01(rng) < 0.1) state = (state + 1) % trueK;
for (int d = 0; d < D; ++d)
data(t, d) = trueMeans[state][d] + noise(rng);
}

// 构造 HDP-HMM (固定种子, 便于复现)
HDPHMM model(/*L=*/10, /*alpha=*/1.0, /*gamma=*/1.0, /*kappa=*/10.0,
/*seed=*/42u);
model.initialize(data);

std::cout << "=== HDP-HMM Gibbs Sampling ===" << std::endl;
model.run(/*nIter=*/500);

std::cout << "\\nFinal active states: "
<< model.countActiveStates() << std::endl;
model.printTransitionMatrix();

std::cout << "\\nEstimated emission means:" << std::endl;
std::cout << std::fixed << std::setprecision(4);
std::vector<bool> used(model.L, false);
for (int s : model.z) used[s] = true;
for (int k = 0; k < model.L; ++k) {
if (used[k]) {
std::cout << "State " << k << ": ";
for (int d = 0; d < model.D; ++d)
std::cout << std::setw(9) << model.theta[k](d, 0);
std::cout << std::endl;
}
}

return 0;
}