Undo unnecessary changes; Add schemes to tree
Fix Eigen alignment issues; pass more things by reference.