Update octomat.cpp