Pushing first draft of tsdf cpu code