GTSAM segmentation fault at evaluateError

CMakeLists.txt

cmake_minimum_required(VERSION 3.16)

project(robomap_gtsam_example LANGUAGES C CXX)

find_package(GTSAM 4.0 REQUIRED QUIET)

set(INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include")

include_directories(include
    ${INCLUDE_DIR}     # make sure your .h all inside.
    ${GTSAM_INCLUDE_DIRS}
    ${EIGEN3_INCLUDE_DIRS}
)
link_directories(
    ${GTSAM_LIBRARY_DIRS}
)

file(GLOB SOURCES ${PROJECT_SOURCE_DIR}/src/*.cpp)

add_executable(${PROJECT_NAME} ${SOURCES})

target_link_libraries(${PROJECT_NAME} 
    gtsam)

install(TARGETS ${PROJECT_NAME}
    COMPONENT linapp
    RUNTIME DESTINATION ${CMAKE_INSTALL_BIN_DIR}
    LIBRARY DESTINATION ${CMAKE_INSTALL_LIB_DIR}
    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIB_DIR}
    DESTINATION ${CMAKE_INSTALL_BIN_DIR}
)

Folder structure

├── CMakeLists.txt
├── include
│   └── GPSPose2Factor.h
├── README.md
└── src
    └── main.cpp

GPSPose2Factor.h

/**
 * @file GPSPose2Factor.h
 * @brief 2D 'GPS' like factor for Pose2
 * @date Nov 8, 2016
 * @author Jing Dong
 */

/**
 * A simple 2D 'GPS' like factor
 * The factor contains a X-Y position measurement (mx, my) for a Pose2, but no rotation information
 * The error vector will be [x-mx, y-my]'
 */

#pragma once

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose2.h>
#include <math.h>

// you can custom namespace (if needed for your project)
namespace gtsamexamples
{

    class GravityFactor : public gtsam::NoiseModelFactor2<gtsam::Pose2, gtsam::Pose2>
    {

    private:
        // measurement information
        double mx_, my_, mtheta_;

    public:
        typedef NoiseModelFactor2<gtsam::Pose2, gtsam::Pose2> Base;
        /**
         * Constructor
         * @param poseKey    associated pose varible key
         * @param model      noise model for GPS snesor, in X-Y
         * @param m          Point2 measurement
         */
        GravityFactor(
            gtsam::Key key1,
            gtsam::Key key2,
            // const gtsam::Pose2 m,
            const double theta,
            gtsam::SharedNoiseModel model) : 
                Base(model, key1, key2), 
                mx_(0), 
                my_(0),
                mtheta_(theta) {
                    
                }

        // error function
        // @param p    the pose in Pose2
        // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
        gtsam::Vector evaluateError(
            const gtsam::Pose2 &p1, 
            const gtsam::Pose2 &p2,
            boost::optional<gtsam::Matrix&> H1 = boost::none, 
            boost::optional<gtsam::Matrix&> H2 = boost::none) const
        {
            const gtsam::Rot2& R1 = p1.rotation();
            const gtsam::Rot2& R2 = p2.rotation();
            const gtsam::Point2& P1 = p1.translation();
            const gtsam::Point2& P2 = p2.translation();
            gtsam::Rot2 angle(double(-M_PI/2));
            
            gtsam::Matrix angleM = (gtsam::Matrix(2,2) <<  
                angle.c(), -angle.s(),
                angle.s(), angle.c()).finished();
            gtsam::Matrix R1_M = (gtsam::Matrix(2,2) <<  
                R1.c(), -R1.s(),
                R1.s(), R1.c()).finished();

            gtsam::Matrix R2_M = (gtsam::Matrix(2,2) <<  
                R2.c(), -R2.s(),
                R2.s(), R2.c()).finished();

            gtsam::Matrix T_M = (gtsam::Matrix(2,1) <<  
                P1.x() - P2.x(),
                P1.y() - P2.y()).finished();
        
            if (H1) (*H1) = (gtsam::Matrix(3,3) <<
               -R2_M.transpose()*R1_M, -angleM*R2_M.transpose()*T_M,
               0, 0, -1).finished();

            if (H2) (*H2) = (gtsam::Matrix(3,3) <<
                1, 0, 0,
                0, 1, 0,
                0, 0, 1).finished();
            // return error vector
            return (gtsam::Vector() << p2.theta() - p1.theta() - mtheta_).finished();
        }

        // gtsam::Vector unwhitenedError(
        //     const double ax,
        //     boost::optional<gtsam::Matrix &> H = boost::none) const
        // {
        //     return (gtsam::Vector() << 0).finished();
        // }
    };

} // namespace gtsamexamples


// ref : https://github.com/borglab/gtsam/blob/b1a822562be80040cb59323fb9d585ad15690c34/gtsam/slam/BetweenFactor.h

main.cpp

/**
 * @file Pose2GPSExample.cpp
 * @brief 2D example with custom 'GPS' factor
 * @date Nov 8, 2016
 * @author Jing Dong
 */

/**
 * A simple 2D pose-graph SLAM with 'GPS' measurement
 * The robot moves from x1 to x3, with odometry information between each pair. 
 * each step has an associated 'GPS' measurement by GPSPose2Factor
 * The graph strcuture is shown:
 * 
 *  g1   g2   g3
 *  |    |    |
 *  x1 - x2 - x3
 */

// custom GPS factor
#include "GPSPose2Factor.h"

// GTSAM headers
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>


using namespace std;
using namespace gtsam;
using namespace gtsamexamples;



//  2D LiDAR constrain
//
//
//         (delta_x, delta_y, delta_theta)
//
//                 (1,0,pi/10)             (1,0,pi/10)
//
//     ┌─────────┐            ┌─────────┐             ┌─────────┐
//     │         ├────────────┤         ├─────────────┤         │
//     │  pose1  │            │  pose2  │             │  pose3  │
//     │         ├────────────┤         ├─────────────┤         │
//     └─────────┘            └─────────┘             └─────────┘
//                  (0)                        (0)
//
//                 (delta_theta)
//
//  IMU direction constrain

// The pi/10 lidar angle is simulating the pitch/roll drift of 3D lidar pose


int main(int argc, char** argv) {

  // Create a factor graph container
  NonlinearFactorGraph graph;

  // odometry measurement noise model (covariance matrix)
  noiseModel::Diagonal::shared_ptr odomModel = 
    noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));

  // Add odometry factors
  // Create odometry (Between) factors between consecutive poses
  // robot makes 90 deg right turns at x3 - x5
  graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(1, 0, M_PI/10), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(1, 0, M_PI/10), odomModel));

  // 2D 'GPS' measurement noise model, 2-dim
  gtsam::Vector vector6(6);
  float noiseScore = 1.0;
  vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
  noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(vector6);
  // Add the GPS factors
  // note that there is NO prior factor needed at first pose, since GPS provides
  // the global positions (and rotations given more than 1 GPS measurements)
  graph.add(
      GravityFactor(
          Symbol('x', 1),
          Symbol('x', 2),
          0 /* zero angle */,
          constraintNoise));
  graph.add(
      GravityFactor(
          Symbol('x', 2),
          Symbol('x', 3),
          0 /* zero angle */,
          constraintNoise));
  // print factor graph
  graph.print("\nFactor Graph:\n"); 


  // initial varible values for the optimization
  // add random noise from ground truth values
  Values initials;
  initials.insert(Symbol('x', 1), Pose2(0, 0, 0));
  initials.insert(Symbol('x', 2), Pose2(0.951, -0.309, -0.1));
  initials.insert(Symbol('x', 3), Pose2(1.76, -0.8968, -0.2));
  
  // print initial values
  initials.print("\nInitial Values:\n"); 


  // Use Gauss-Newton method optimizes the initial values
  GaussNewtonParams parameters;
  
  // print per iteration
  parameters.setVerbosity("ERROR");
  
  // optimize!
  GaussNewtonOptimizer optimizer(graph, initials, parameters);
//   Values results = optimizer.optimize();
  
//   // print final values
//   results.print("Final Result:\n");


//   // Calculate marginal covariances for all poses
//   Marginals marginals(graph, results);
  
//   // print marginal covariances
//   cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
//   cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
//   cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;

  return 0;
}