diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 96ea9b182..ff059ef78 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -65,8 +65,8 @@ class AdaptAutoDiff { // Convert from row-major to columnn-major // TODO: if this is a bottleneck (probably not!) fix Autodiff to be // Column-Major - *H1 = Eigen::Map(rowMajor1); - *H2 = Eigen::Map(rowMajor2); + if (H1) *H1 = Eigen::Map(rowMajor1); + if (H2) *H2 = Eigen::Map(rowMajor2); } else { // Apply the mapping, to get result diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index edfd420ef..3a9b5297a 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -57,16 +57,19 @@ int main() { f1 = boost::make_shared >(z, model, 1, 2); time("GeneralSFMFactor : ", f1, values); - // AdaptAutoDiff - typedef AdaptAutoDiff AdaptedSnavely; - Point2_ expression(AdaptedSnavely(), camera, point); - f2 = boost::make_shared >(model, z, expression); - time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); - // ExpressionFactor Point2_ expression2(camera, &Camera::project2, point); f3 = boost::make_shared >(model, z, expression2); time("Point2_(camera, &Camera::project, point): ", f3, values); + // AdaptAutoDiff + values.clear(); + values.insert(1,Vector9(Vector9::Zero())); + values.insert(2,Vector3(0,0,1)); + typedef AdaptAutoDiff AdaptedSnavely; + Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); + f2 = boost::make_shared >(model, z.vector(), expression); + time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); + return 0; }