Bundle Adjustment with OpenCV and PCL