// another simple example: invert a matrix, // returning a matrix // // [[Rcpp::export]] Eigen::MatrixXd rcppeigen_matinv(const Eigen::MatrixXd & x) { Eigen::MatrixXd Xinv(x.inverse()); return Xinv; }
// Here's the main interpolate function, using // Least Squares AutoRegression (LSAR): void InterpolateAudio(float *buffer, int len, int firstBad, int numBad) { int N = len; int i, row, col; wxASSERT(len > 0 && firstBad >= 0 && numBad < len && firstBad+numBad <= len); if(numBad >= len) return; //should never have been called! if (firstBad == 0) { // The algorithm below has a weird asymmetry in that it // performs poorly when interpolating to the left. If // we're asked to interpolate the left side of a buffer, // we just reverse the problem and try it that way. float *buffer2 = new float[len]; for(i=0; i<len; i++) buffer2[len-1-i] = buffer[i]; InterpolateAudio(buffer2, len, len-numBad, numBad); for(i=0; i<len; i++) buffer[len-1-i] = buffer2[i]; return; } Vector s(len, buffer); // Choose P, the order of the autoregression equation int P = imin(numBad * 3, 50); P = imin(P, imax(firstBad - 1, len - (firstBad + numBad) - 1)); if (P < 3) { LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } // Add a tiny amount of random noise to the input signal - // this sounds like a bad idea, but the amount we're adding // is only about 1 bit in 16-bit audio, and it's an extremely // effective way to avoid nearly-singular matrices. If users // run it more than once they get slightly different results; // this is sometimes even advantageous. for(i=0; i<N; i++) s[i] += (rand()-(RAND_MAX/2))/(RAND_MAX*10000.0); // Solve for the best autoregression coefficients // using a least-squares fit to all of the non-bad // data we have in the buffer Matrix X(P, P); Vector b(P); for(i=0; i<len-P; i++) if (i+P < firstBad || i >= (firstBad + numBad)) for(row=0; row<P; row++) { for(col=0; col<P; col++) X[row][col] += (s[i+row] * s[i+col]); b[row] += s[i+P] * s[i+row]; } Matrix Xinv(P, P); if (!InvertMatrix(X, Xinv)) { // The matrix is singular! Fall back on linear... // In practice I have never seen this happen if // we add the tiny bit of random noise. LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } // This vector now contains the autoregression coefficients Vector a = Xinv * b; // Create a matrix (a "Toeplitz" matrix, as it turns out) // which encodes the autoregressive relationship between // elements of the sequence. Matrix A(N-P, N); for(row=0; row<N-P; row++) { for(col=0; col<P; col++) A[row][row+col] = -a[col]; A[row][row+P] = 1; } // Split both the Toeplitz matrix and the signal into // two pieces. Note that this code could be made to // work even in the case where the "bad" samples are // not contiguous, but currently it assumes they are. // "u" is for unknown (bad) // "k" is for known (good) Matrix Au = MatrixSubset(A, 0, N-P, firstBad, numBad); Matrix A_left = MatrixSubset(A, 0, N-P, 0, firstBad); Matrix A_right = MatrixSubset(A, 0, N-P, firstBad+numBad, N-(firstBad+numBad)); Matrix Ak = MatrixConcatenateCols(A_left, A_right); Vector s_left = VectorSubset(s, 0, firstBad); Vector s_right = VectorSubset(s, firstBad+numBad, N-(firstBad+numBad)); Vector sk = VectorConcatenate(s_left, s_right); // Do some linear algebra to find the best possible // values that fill in the "bad" area Matrix AuT = TransposeMatrix(Au); Matrix X1 = MatrixMultiply(AuT, Au); Matrix X2(X1.Rows(), X1.Cols()); if (!InvertMatrix(X1, X2)) { // The matrix is singular! Fall back on linear... LinearInterpolateAudio(buffer, len, firstBad, numBad); return; } Matrix X2b = X2 * -1.0; Matrix X3 = MatrixMultiply(X2b, AuT); Matrix X4 = MatrixMultiply(X3, Ak); // This vector contains our best guess as to the // unknown values Vector su = X4 * sk; // Put the results into the return buffer for(i=0; i<numBad; i++) buffer[firstBad+i] = (float)su[i]; }
//----------- Begin of function LinAlg::quadratic_prog ------// //! Interior Point Quadratic Programming //! c, Q, A, b, xNames (no global or member variable access) //! //! try, by BM: //! c = { 0,0 } //! Q = { {2,0}, {0,5} } //! A = { {5,6} } //! b = { 10 } //! xNames={x1,x2} //! bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) { // init local variables int maxIteration = 20; const REAL SIGMA = 1/15.0; // 1/10.0; const REAL R = 9.0/10; int iter = 0; int n = v_c.Storage(); int m = v_b.Storage(); maxIteration *= loopCountMultiplier; #ifdef DEBUG_VC DEBUG_LOG("----------- quadratic_prog begin -----------"); // print_input(c,Q,A,b,xNames) if ( n==10 && m==12 ) { char s[500]; DEBUG_LOG("c = "); sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };", v_c(1),v_c(2),v_c(3), v_c(4),v_c(5),v_c(6), v_c(7),v_c(8),v_c(9), v_c(10)); DEBUG_LOG(s); DEBUG_LOG("Q = "); for (int i=1; i<=n; i++) { sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},", m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10) ); DEBUG_LOG(s); } DEBUG_LOG("A = "); for (i=1; i<=m; i++) { sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},", m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10) ); DEBUG_LOG(s); } DEBUG_LOG("b = "); sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,", v_b(1),v_b(2),v_b(3), v_b(4),v_b(5),v_b(6), v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12) ); DEBUG_LOG(s); /*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,", v_b(10),v_b(11),v_b(12), v_b(13),v_b(14),v_b(15), v_b(16),v_b(17),v_b(18) ); DEBUG_LOG(s); sprintf(s, "%f, %f};", v_b(19),v_b(20)); DEBUG_LOG(s);*/ } else if ( n==9 && m==20 ) { // stage 1 char s[500]; DEBUG_LOG("c = "); sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };", v_c(1),v_c(2),v_c(3), v_c(4),v_c(5),v_c(6), v_c(7),v_c(8),v_c(9)); DEBUG_LOG(s); DEBUG_LOG("Q = "); for (int i=1; i<=n; i++) { sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },", m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9) ); DEBUG_LOG(s); } DEBUG_LOG("A = "); for (i=1; i<=m; i++) { sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },", m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9) ); DEBUG_LOG(s); } DEBUG_LOG("b = "); sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ", v_b(1),v_b(2),v_b(3), v_b(4),v_b(5),v_b(6), v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12), v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20) ); DEBUG_LOG(s); } else if ( n==10 && m==22 ) { // stage 2 char s[500]; DEBUG_LOG("c = "); sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };", v_c(1),v_c(2),v_c(3), v_c(4),v_c(5),v_c(6), v_c(7),v_c(8),v_c(9),v_c(10)); DEBUG_LOG(s); DEBUG_LOG("Q = "); for (int i=1; i<=n; i++) { sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },", m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10) ); DEBUG_LOG(s); } DEBUG_LOG("A = "); for (i=1; i<=m; i++) { sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },", m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10) ); DEBUG_LOG(s); } DEBUG_LOG("b = "); sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ", v_b(1),v_b(2),v_b(3), v_b(4),v_b(5),v_b(6), v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12), v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22) ); DEBUG_LOG(s); } #endif Vector v_eN(n); v_eN = 1; Vector v_eM(m); v_eM = 1; Vector v_x(n); v_x = 1; Vector v_z(n); v_z = 1; Vector v_y(m); v_y = 1; Vector v_w(m); v_w = 1; // check m_A is nxm, m_Q is nxn err_when(m_A.Nrows() != m || m_A.Ncols() != n ); err_when(m_Q.Nrows() != n || m_Q.Ncols() != n ); // init for while loop not_converged() checking // Vector v_Road = v_b - m_A*(v_x) + v_w; Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x; REAL gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar(); REAL mute = SIGMA*gamma / (n+m); DiagonalMatrix X(n), Xinv(n); DiagonalMatrix Y(m), Yinv(m); DiagonalMatrix Z(n); DiagonalMatrix W(m); Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m); Vector KKFvec(n+m); //, v_tmp1(n), v_tmp2(m); Vector m_temp(m+n); Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m); REAL theeta; X=0; Xinv=0; Z=0; Y=0; Yinv=0; W=0; { // Matrix _t1(n,n), _t2(n,n); _t1=1; _t2=2; // _t1 = SP(_t1,_t2); } Matrix KKFmat(m+n,m+n); Matrix KKFmatInverse(m+n,m+n); REAL min=1; while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) { iter++; v_Road = v_b - m_A*v_x + v_w; v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x; gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar(); mute = SIGMA*gamma / (n+m); X.set_diagonal(v_x); Y.set_diagonal(v_y); Z.set_diagonal(v_z); W.set_diagonal(v_w); Xinv.set_diagonal(ElmDivide(v_eN, v_x)); Yinv.set_diagonal(ElmDivide(v_eM, v_y)); T1 = -(Xinv * Z + m_Q); T2 = m_A.t(); T3 = m_A; T4 = Yinv*W; //PRT_MAT << "T1: " << T1 << endl; KKFmat = (T1|T2) & (T3|T4); KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x) & (v_b - m_A*v_x + mute*Yinv*v_eM); Try { /*{ bool _f; DiagonalMatrix _dmat(n+m); _dmat=1; Matrix _inv(n+m,n+m); SVD(KKFmat, _dmat, _inv); _dmat=1; if ( KKFmat*_inv == _dmat ) _f = true; else _f = false; }*/ min = fabs(KKFmat(1,1)); for (int i=2; i<=m+n; i++) { if ( min > fabs(KKFmat(i,i)) ) // check diagonal element min = fabs(KKFmat(i,i)); } KKFmatInverse = KKFmat.i(); /* if ( KKFmat.LogDeterminant().Value() ) KKFmatInverse = KKFmat.i(); else break; */ m_temp = KKFmatInverse * KKFvec; v_dx = m_temp.Rows(1,n); v_dy = m_temp.Rows(n+1,n+m); v_dz = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx); v_dw = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy); //PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl; //PRT_MAT << "v_x: " << v_x << endl; //PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl; theeta = (R/(( ElmDivide(-v_dx,v_x) & ElmDivide(-v_dw,v_w) & ElmDivide(-v_dy,v_y) & ElmDivide(-v_dz,v_z) )).MaximumValue() ); if(theeta>1) // theeta = min(theeta,1) theeta = 1; v_x += theeta * v_dx; v_y += theeta * v_dy; v_w += theeta * v_dw; v_z += theeta * v_dz; #ifdef DEBUG_CONSOLE cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl; cout << "Ro:" << v_Road << endl; PRT_MAT15 << "v_x:" << v_x << endl; PRT_MAT15 << "v_y:" << v_y << endl; PRT_MAT15 << "v_w:" << v_w << endl; PRT_MAT15 << "v_z:" << v_z << endl; #elif defined(DEBUG_VC) char s[200]; sprintf(s, "#iter %d: %f, %f, %f", iter, v_Road.Sum(), v_Sigma.Sum(), gamma); DEBUG_LOG(s); if ( n == 9 && false ) { DEBUG_LOG("x = "); sprintf(s, " %f, %f, %f, %f, %f, %f, %f, %f, %f", v_x(1),v_x(2),v_x(3), v_x(4),v_x(5),v_x(6), v_x(7),v_x(8),v_x(9)); DEBUG_LOG(s); } #endif } CatchAll { #ifdef DEBUG_CONSOLE cout << Exception::what(); #endif #ifdef DEBUG_VC DEBUG_LOG("olinalg: failure in quad_prog"); DEBUG_LOG((char *)Exception::what()); #endif return false; } } // while v_xNames = v_x; if ( min < 0.00001 ) { // 1214 || KKFmat.LogDeterminant().Value() == 0 ) //char s[200]; //sprintf(s, "#iter %d: %f, %f, %f", // iter, v_Road.Sum(), v_Sigma.Sum(), gamma); //DEBUG_LOG(s); DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---"); } else DEBUG_LOG("----------- quad_prog normal exit -----------"); #ifdef DEBUG_CONSOLE cout << "#iter: " << iter; #endif return true; }