Thank you for the great project. My questions is, how to write own cost function with c++? I have visual and inertial data.