Now that I see this: 

 * SGMatrix has the () operator implemented, so no need for this hard to read indexing
 * Never copy matrices element by element, inefficient, rather use memcpy (if possible). You can either copy vector wise if their dimension changed, and if not, you can do one memcpy call for the whole matrix