diff --git a/g2o/examples/bal/bal_example.cpp b/g2o/examples/bal/bal_example.cpp index c97967f78..0f8b02f6e 100644 --- a/g2o/examples/bal/bal_example.cpp +++ b/g2o/examples/bal/bal_example.cpp @@ -88,7 +88,7 @@ class VertexCameraBAL : public BaseVertex<9, Eigen::VectorXd> virtual void oplusImpl(const double* update) { - Eigen::VectorXd::ConstMapType v(update, 9); + Eigen::VectorXd::ConstMapType v(update, VertexCameraBAL::Dimension); _estimate += v; } }; @@ -258,18 +258,24 @@ class EdgeObservationBAL : public BaseBinaryEdge<2, Vector2d, VertexCameraBAL, V const VertexCameraBAL* cam = static_cast(vertex(0)); const VertexPointBAL* point = static_cast(vertex(1)); - typedef ceres::internal::AutoDiff BalAutoDiff; + typedef ceres::internal::AutoDiff BalAutoDiff; - Matrix dError_dCamera; - Matrix dError_dPoint; + Matrix dError_dCamera; + Matrix dError_dPoint; double *parameters[] = { const_cast(cam->estimate().data()), const_cast(point->estimate().data()) }; double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() }; - double value[2]; - BalAutoDiff::Differentiate(*this, parameters, 2, value, jacobians); + double value[Dimension]; + bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, value, jacobians); // copy over the Jacobians (convert row-major -> column-major) - _jacobianOplusXi = dError_dCamera; - _jacobianOplusXj = dError_dPoint; + if (diffState) { + _jacobianOplusXi = dError_dCamera; + _jacobianOplusXj = dError_dPoint; + } else { + assert(0 && "Error while differentiating"); + _jacobianOplusXi.setZero(); + _jacobianOplusXi.setZero(); + } } };