diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index e44576e5f..be20ee3fa 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -196,8 +196,10 @@ namespace gtsam { Point2 reprojError(camera.project2(point, H1, H2) - measured()); b = -reprojError.vector(); } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); // TODO warn if verbose output asked for - return boost::make_shared(); } // Whiten the system if needed