Correct the Jacobian calculation of GravityFactor (#1692)

* correct the Jacobian calculation of GravityFactor

* reimplement GravityFactor's analytical Jacobian

* adopt some minor changes from the GTSAM mainline

* use GTSAM's native AttitudeFactor instead of GravityFactor

* add missing header file and namespace prefix

* Removed GravityFactor. Fixed build with latest GTSAM version from source. Require c++17 when building with GTSAM>=4.3.0.

* Fixed cmake error when gtsam is not installed

---------

Co-authored-by: matlabbe <[email protected]>

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -13,6 +13,7 @@
 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
 
@@ -77,8 +78,8 @@ namespace vertigo {
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		OptionalMatrixType H1=OptionalNone,
-		OptionalMatrixType H2=OptionalNone) const {
+		gtsam::OptionalMatrixType H1=OptionalNone,
+		gtsam::OptionalMatrixType H2=OptionalNone) const {
 #else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {