Refactor some malloc/free to new/delete
Refactor MatMap