Conjugate gradient implementation converging on wrong values

I'm trying to write a simple conjugate gradient method function in C++ - pretty simple right? But it literally can't solve any systems of equations. It converges but onto the completely wrong values.

I've written the function out here and provided the main function where I generate positive definite matrix A using symmetric matrix M. I've provided all the code to see if anyone can spot anything wrong. Please don't roast me for doing this - it's a well understand algorithm and it's pointless not providing all the code right? Please assume that the ApplyLHS and innerProduct functions work properly.

template <typename TData>
void DoConjGrad(
std::vector<TData> &in,
std::vector<TData> &out,
std::vector<TData> &A
)
{
TData tol = 10e-8;
size_t N = in.size();
std::vector<TData> r(N);
std::vector<TData> p(N);
std::vector<TData> w(N);
size_t k = 0;
TData rTr;
TData rTr_prev;
TData pTw;
TData alpha;
TData beta;
// initial guess for x0
std::fill(out.begin(), out.end(), 0);
// r0 = b - Ax0
ApplyLHS(out, r, A);
std::transform(in.cbegin(), in.cend(), r.cbegin(), r.begin(), 
[](const TData &bElem, const TData &rElem){ return bElem - rElem; } );
// p0 := r0
std::copy(r.cbegin(), r.cend(), p.begin());
// calculate (rTr)0
rTr = innerProduct(r, r);
for (int i = 0; i < 100; ++i)
{
ApplyLHS(p, w, A);
pTw = innerProduct(p, w);
alpha = rTr / pTw;
std::transform(out.cbegin(), out.cend(), p.cbegin(), out.begin(),
[alpha](const TData &xElem, const TData &pElem)
{
return xElem + alpha*pElem;
});
std::transform(r.cbegin(), r.cend(), w.cbegin(), r.begin(),
[alpha](const TData &rElem, const TData &wElem)
{
return rElem - alpha*wElem;
});
if (rTr < tol)
break;
rTr_prev = rTr;
rTr = innerProduct(r, r);
beta = rTr / rTr_prev;
std::transform(r.cbegin(), r.cend(), p.cbegin(), p.begin(),
[beta](const TData &rElem, const TData &pElem)
{
return rElem + beta*pElem;
});
}
}

Main:

int main()
{
srand(100);
size_t N = 2;
std::vector<double> A(N * N);
std::vector<double> M(N * N);
std::vector<double> x_true(N);
std::vector<double> x_calc(N);
std::vector<double> b(N);
for (int i = 0; i < N; ++i)
for (int j = 0; j <= i; ++j)
{
double r = 2*((double)rand())/((double)RAND_MAX) - 1;
M[i*N + j] = r;
M[j*N + i] = r;
}
// get positive semi-definite matrix = M.T @ M
for (int i = 0; i < N; ++i)
for (int j = 0; j < N; ++j)
for (int k = 0; k < N; ++k)
A[i*N + j] = M[k*N + i] * M[k*N + j];
// generate random x
for (int i = 0; i < N; ++i)
x_true[i] = 2*((double)rand())/((double)RAND_MAX) - 1;
// calculate b
ApplyLHS(x_true, b, A);
// solve for x calc
DoConjGrad<double>(b, x_calc, A);
}

Helper functions:

template <typename TData>
void printMat(
size_t nr,
size_t nc,
std::vector<TData> &mat
)
{
for (int i = 0; i < nr; ++i)
{
for (int j = 0; j < nc; ++j)
{
std::cout.precision(5);
std::cout.width(10);
std::cout << mat[i*nc + j] << "\t";
}
std::cout << "\n";
}
}
template <typename TData>
void ApplyLHS(
std::vector<TData> &in,
std::vector<TData> &out,
std::vector<TData> &A
)
{
size_t N = in.size();
std::fill(out.begin(), out.end(), 0);
for (int i = 0; i < N; ++i)
for (int j = 0; j < N; ++j)
out[i] += in[j] * A[i*N + j];
}
template <typename TData>
TData innerProduct(
std::vector<TData> &a,
std::vector<TData> &b
)
{
TData result = 0;
for (int i = 0; i < a.size(); ++i)
{
result += a[i] * b[i];
}
return result;
}

So, the good news first: there appears to be NOTHING WRONG with your conjugate-gradient algorithm! (Well, nothing causing it to fail anyway. Personally, I would make the tolerance smaller, and move the loop exit condition after the point where you calculate rTr.)

If you calculate the determinant of A from your code ... you will find that det(A)=0 every time, regardless of the value of N. (So A is singular; there is some non-zero x such that Ax=0 and hence xT.A.x=0: thus A isn't strictly positive definite either, which is a condition for the CG algorithm).

But a major clue is that the determinant of M is not (usually) zero ... and yet det(A) should be det(M) squared.

So, look at your matrix multiplication for A = M^TM. It is not correct; (each element should be initialised and then formed from a sum). Change this part to the following and you are good to go.

// get positive semi-definite matrix = Mtranspose * M
for (int i = 0; i < N; ++i)
{
for (int j = 0; j < N; ++j)
{
A[i*N+j] = 0;                                     // CAREFUL!!
for (int k = 0; k < N; ++k)                       // ""  
A[i*N + j] += M[k*N + i] * M[k*N + j];        // ""
}
}