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/gtsam/XYFactor.h

@@ -43,7 +43,7 @@ public:
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
   gtsam::Vector evaluateError(const VALUE& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -59,5 +59,5 @@ public:
 
 };
 
-} // namespace gtsamexamples
+} // namespace rtabmap