Simple Pose3 graph not optimizing

[jstraub6]

I am having some troubles solving a simple Pose3 graph. Here is the code:

  gtsam::SharedNoiseModel model = gtsam::noiseModel::Diagonal::Variances(
        (gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished());

  gtsam::Values::shared_ptr initials(new gtsam::Values);
  gtsam::NonlinearFactorGraph::shared_ptr graph(new gtsam::NonlinearFactorGraph);

  // set the poses up using SE3 class 
  SE3f T_wk0;
  SE3f T_wk1(SO3f::Rx(5.*M_PI/180.), Eigen::Vector3f(0,0,1));
  SE3f dT_01(SO3f::Rx(5.*M_PI/180.), Eigen::Vector3f(0,0,1.1));

  // setup Pose3s for GTSAM
  gtsam::Pose3 poseA = gtsam::Pose3(T_wk0.matrix().cast<double>());
  gtsam::Pose3 poseB = gtsam::Pose3(T_wk1.matrix().cast<double>());
  gtsam::Pose3 poseAB = poseA.between(poseB);
  gtsam::Pose3 poseAB_obs = gtsam::Pose3(dT_01.matrix().cast<double>());

  initials->insert(0, poseA);
  initials->insert(1, poseB);

  gtsam::PriorFactor<gtsam::Pose3>::shared_ptr factor0(
      new gtsam::PriorFactor<gtsam::Pose3>(0, poseA, model));
  graph->add(factor0);
  gtsam::NonlinearFactor::shared_ptr factor01(
      new gtsam::BetweenFactor<gtsam::Pose3>(0, 1, poseAB, model));
  graph->push_back(factor01);
  gtsam::NonlinearFactor::shared_ptr factor01_obs(
      new gtsam::BetweenFactor<gtsam::Pose3>(0, 1, poseAB_obs, model));
  graph->push_back(factor01_obs);

  graph->print();

  gtsam::GaussNewtonParams params;
  params.setVerbosity("ERROR"); 
  params.setMaxIterations(10);
  gtsam::GaussNewtonOptimizer optimizer(*graph, *initials, params);
  gtsam::Values results = optimizer.optimize();

And here is the output:

NonlinearFactorGraph: size: 3

Factor 0: PriorFactor on 0
  prior mean: R:
[
        1 0 0;
        0 1 0;
        0 0 1
  ]
[0, 0, 0]';isotropic dim=6 sigma=0.1

Factor 1: BetweenFactor(0,1)
  measured: R:
[
                 1          0          0;
                 0   0.996195  0.0871557;
                 0 -0.0871557   0.996195
  ]
[0, 0, 1]';isotropic dim=6 sigma=0.1

Factor 2: BetweenFactor(0,1)
  measured: R:
[
                 1          0          0;
                 0   0.996195  0.0871557;
                 0 -0.0871557   0.996195
  ]
[0, 0, 1.1]';isotropic dim=6 sigma=0.1

Initial error: 0.5

The code enters some kind of infinte loop after that.

The issue is probably a simple misunderstanding of the way GTSAM works. I have looked through all the experiment code and I can not figure out what the issue is.

An immediate question is: How do I get the optimizer into verbose mode?

[Jing Dong]

Hi Julian, I cannot reproduce the error above, neither find obvious bug in the code. 1. Does the problem appear every time you run? Or it appears randomly? 2. Did you enable TBB in cmake during GTSAM compilation?

[jstraub6]

Hi Jing Dong,

1: Yes I get it all the time. 2: Yes it is enabled but not found in the GTSAM build. Is TBB needed for GTSAM to work?

Also: the Pose2 examples work. Do you have a .g2o file so I can try the Pose3 examples?

Thanks for taking a look. Julian

[jstraub6]

I tried disabling TBB and that did not change anything either.

I realized that I can just run ./examples/Pose3SLAMExample_g2o. That example seems to run just fine.

That makes me think the problem is in my use of GTSAM as a library from my own code. Are there any pitfalls there? Compiling and linking against GTSAM works without problems.

If I run my example in gdb and just terminate at some point and run a backtrace it seems the code is somehow stuck in updating a Hessian?!

#0  Eigen::internal::gebp_kernel<double, double, long, Eigen::internal::blas_data_mapper<double, long, 0, 0>, 4, 4, false, false>::operator() (
    this=this@entry=0x7fffffffbf1f, res=..., blockA=0x7fffffffbe10, blockB=0x7fffffffbde0, rows=0, depth=depth@entry=1, cols=1, alpha=1, strideA=1, strideA@entry=-1, 
    strideB=1, strideB@entry=-1, offsetA=offsetA@entry=0, offsetB=offsetB@entry=0) at /usr/local/include/eigen3/Eigen/src/Core/products/GeneralBlockPanelKernel.h:1160
#1  0x00000000004459e8 in Eigen::internal::general_matrix_matrix_product<long, double, 1, false, double, 0, false, 0>::run (rows=<optimized out>, cols=1, 
    depth=<optimized out>, _lhs=<optimized out>, lhsStride=<optimized out>, _rhs=<optimized out>, rhsStride=6, _res=0x6959e0, resStride=13, alpha=<optimized out>, 
    blocking=..., info=0x0) at /usr/local/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrix.h:194
#2  0x00007ffff7871b9a in gtsam::JacobianFactor::updateHessian(gtsam::FastVector<unsigned long> const&, gtsam::SymmetricBlockMatrix*) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#3  0x00007ffff7847267 in gtsam::HessianFactor::HessianFactor(gtsam::GaussianFactorGraph const&, boost::optional<gtsam::Scatter const&>) ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#4  0x00007ffff784cef5 in boost::detail::sp_if_not_array<gtsam::HessianFactor>::type boost::make_shared<gtsam::HessianFactor, gtsam::GaussianFactorGraph const&, gtsam::Scatter&>(gtsam::GaussianFactorGraph const&, gtsam::Scatter&) () from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#5  0x00007ffff78475bf in gtsam::EliminateCholesky(gtsam::GaussianFactorGraph const&, gtsam::Ordering const&) ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#6  0x00007ffff784771b in gtsam::EliminatePreferCholesky(gtsam::GaussianFactorGraph const&, gtsam::Ordering const&) ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#7  0x00007ffff781254f in boost::detail::function::function_invoker2<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (*)(gtsam::GaussianFactorGraph const&, gtsam::Ordering const&), std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> >, gtsam::GaussianFactorGraph const&, gtsam::Ordering const&>::invoke(boost::detail::function::function_buffer&, gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)
    () from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#8  0x00007ffff787c634 in gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >::EliminationPostOrderVisitor::operator()(boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> const&, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >&) () from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#9  0x00007ffff787ca25 in gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph>::eliminate(boost::function<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)> const&) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#10 0x00007ffff78895f2 in gtsam::EliminateableFactorGraph<gtsam::GaussianFactorGraph>::eliminateMultifrontal(boost::optional<gtsam::Ordering const&>, boost::function<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)> const&, boost::optional<gtsam::VariableIndex const&>, boost::optional<gtsam::Ordering::OrderingType>) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#11 0x00007ffff7889a24 in gtsam::EliminateableFactorGraph<gtsam::GaussianFactorGraph>::eliminateMultifrontal(boost::optional<gtsam::Ordering const&>, boost::function<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)> const&, boost::optional<gtsam::VariableIndex const&>, boost::optional<gtsam::Ordering::OrderingType>) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#12 0x00007ffff7885db1 in gtsam::GaussianFactorGraph::optimize(boost::optional<gtsam::Ordering const&>, boost::function<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)> const&) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#13 0x00007ffff789d613 in gtsam::NonlinearOptimizer::solve(gtsam::GaussianFactorGraph const&, gtsam::NonlinearOptimizerParams const&) const ()
   from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#14 0x00007ffff78a8fc4 in gtsam::GaussNewtonOptimizer::iterate() () from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#15 0x00007ffff789cf91 in gtsam::NonlinearOptimizer::defaultOptimize() () from /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so.4
#16 0x000000000043ee19 in optimize (this=0x7fffffffd280) at /home/jstraub/workspace/research/3rdparty/gtsam/gtsam/nonlinear/NonlinearOptimizer.h:98
#17 setupRaw_KeyframeSLAM_Test::TestBody (this=<optimized out>) at /home/jstraub/workspace/research/tdp/test/keyframe_slam.cpp:61
#18 0x0000000000464bf4 in void testing::internal::HandleSehExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*)
    ()
#19 0x000000000045fb6c in void testing::internal::HandleExceptionsInMethodIfSupported<testing::Test, void>(testing::Test*, void (testing::Test::*)(), char const*) ()
#20 0x000000000044cc55 in testing::Test::Run() ()
#21 0x000000000044d3d0 in testing::TestInfo::Run() ()
#22 0x000000000044d990 in testing::TestCase::Run() ()
#23 0x00000000004525ea in testing::internal::UnitTestImpl::RunAllTests() ()
#24 0x000000000046613e in bool testing::internal::HandleSehExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) ()
#25 0x0000000000460b18 in bool testing::internal::HandleExceptionsInMethodIfSupported<testing::internal::UnitTestImpl, bool>(testing::internal::UnitTestImpl*, bool (testing::internal::UnitTestImpl::*)(), char const*) ()
#26 0x00000000004513e7 in testing::UnitTest::Run() ()
#27 0x000000000043dddf in main (argc=1, argv=<optimized out>) at /home/jstraub/workspace/research/tdp/test/keyframe_slam.cpp:66

[jstraub6]

So yeah I just tested exactly the same code in a new experiment directly in GTSAM and everything works.

Still the same question: Any known pitfalls when linking against libgtsam?

[Jing Dong]

There is a possible pitfall when using GTSAM with your own lib, that linking two different version of Eigen together may cause conflicts. This is the case when you compile and link GTSAM is default cmake setting (use GTSAM own Eigen) and call Eigen directly in your code (use system Eigen). I have observed this conflict but I don't know how to fix, so let's avoid this conflict here.

There are two possible solutions: 1. modify GTSAM cmake setting GTSAM_USE_SYSTEM_EIGEN to ON. 2. do not link anything link to system Eigen (like system installed g2o). Directly include everything you use as 3rd lib in your project and compile from source.

The first one is easier to try, let me know whether this works on your side.

[jstraub6]

Thanks for the hint Jing Dong. I already suspected that Eigen might be messing up and set GTSAM to use the system wide one. I also verified that both my code and GTSAM use the same version of Eigen.

I am using the latest Eigen version compiled from source. But that cannot be the problem either since everything works in the GTSAM examples.

Even if I completely avoid using Eigen data structures in the test code for GTSAM in my library it does not work. And additionally all the printing of the rotation matrices and translation vectors works and shows the right values.

[Jing Dong]

That sounds strange, can you send me the whole cmake project (contains only your minimal example is great) so I can reproduce the behavior on my side?

[jstraub6]

So I prepared a minimal project for you and then the example started working.

That means something I am linking against or using in the bigger project messes with GTSAM.

One thing I already found is that I should not be using OpenMP -openmp (probably because I disabled that option in gtsam). After taking that flag out the example compiles and works in my bigger project as well.

However in the actual code where I want to use GTSAM the optimizer still gets stuck as before... Here is the compiler and the linker call:

/usr/bin/c++    -g -Wall -Wno-deprecated -O2 -finline-functions -DNDEBUG -std=c++11  -I/usr/local/cuda/include -I/home/jstraub/workspace/research/tdp/3rdparty/ann_1.1.2/include -I/home/jstraub/workspace/research/3rdparty/gtsam -I/home/jstraub/workspace/research/tdp/include -I/usr/local/include/eigen3 -I/afs/csail.mit.edu/u/j/jstraub/workspace/research/3rdparty/Pangolin/build/src/include -I/afs/csail.mit.edu/u/j/jstraub/workspace/research/3rdparty/Pangolin/build/src/../../include -I/home/jstraub/workspace/research/tdp/build/include    -o CMakeFiles/surround3D.dir/main.o -c /home/jstraub/workspace/research/tdp/experiments/surround3D/main.cpp

/usr/bin/c++   -g -Wall -Wno-deprecated -O2 -finline-functions -DNDEBUG -std=c++11     CMakeFiles/surround3D.dir/main.o  -o surround3D  -L/afs/csail.mit.edu/u/j/jstraub/workspace/research/3rdparty/Pangolin/build/src  -L/usr/lib/x86_64-linux-gnu  -L/usr/local/lib  -L/home/jstraub/workspace/research/tdp/3rdparty/ann_1.1.2/lib -rdynamic ../../src/libtdp.so /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so -lboost_serialization -lboost_system -lboost_filesystem -lboost_thread -lboost_program_options -lboost_date_time -lboost_timer -lboost_chrono -lpthread /usr/local/cuda/lib64/libcudart.so /afs/csail.mit.edu/u/j/jstraub/workspace/research/3rdparty/Pangolin/build/src/libpangolin.so -lrt -lpthread /usr/lib/x86_64-linux-gnu/libGLU.so /usr/lib/x86_64-linux-gnu/libGL.so /usr/lib/x86_64-linux-gnu/libSM.so /usr/lib/x86_64-linux-gnu/libICE.so /usr/lib/x86_64-linux-gnu/libX11.so /usr/lib/x86_64-linux-gnu/libXext.so /usr/lib/x86_64-linux-gnu/libGLEW.so /usr/lib/x86_64-linux-gnu/libSM.so /usr/lib/x86_64-linux-gnu/libICE.so /usr/lib/x86_64-linux-gnu/libX11.so /usr/lib/x86_64-linux-gnu/libXext.so /usr/lib/x86_64-linux-gnu/libGLEW.so /usr/lib/x86_64-linux-gnu/libpython2.7.so -lrealsense /usr/lib/x86_64-linux-gnu/libpng.so /usr/lib/x86_64-linux-gnu/libz.so /usr/lib/x86_64-linux-gnu/libjpeg.so -lANN -Wl,-rpath,/afs/csail.mit.edu/u/j/jstraub/workspace/research/3rdparty/Pangolin/build/src:/home/jstraub/workspace/research/tdp/build/src:/home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam:/usr/lib/x86_64-linux-gnu:/usr/local/cuda/lib64:/usr/local/lib:/home/jstraub/workspace/research/tdp/3rdparty/ann_1.1.2/lib

In the minimal example they look like:

/usr/bin/c++    -g -Wall -Wno-deprecated -O2 -finline-functions -DNDEBUG -std=c++11  -I/home/jstraub/workspace/research/3rdparty/gtsam -I/home/jstraub/workspace/research/tdp/3rdparty/gtsamTest/include -I/usr/local/include/eigen3 -I/home/jstraub/workspace/research/tdp/3rdparty/gtsamTest/build/include    -o CMakeFiles/testKfSLAM.dir/keyframe_slam.o -c /home/jstraub/workspace/research/tdp/3rdparty/gtsamTest/test/keyframe_slam.cpp

/usr/bin/c++   -g -Wall -Wno-deprecated -O2 -finline-functions -DNDEBUG -std=c++11     CMakeFiles/testKfSLAM.dir/keyframe_slam.o  -o testKfSLAM -rdynamic -Wl,-Bstatic -lgtest -lgtest_main -Wl,-Bdynamic -lpthread /home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam/libgtsam.so -lboost_serialization -lboost_system -lboost_filesystem -lboost_thread -lboost_program_options -lboost_date_time -lboost_timer -lboost_chrono -Wl,-rpath,/home/jstraub/workspace/research/3rdparty/gtsam/build/gtsam 

[Jing Dong]

Not quite sure which lib causes the problem, since there are several libs we have never tried to link against.. Since OpenMP causes the problem in your example files, how about try to enable openmp in GTSAM and then see whether the problem is gone? (probably some libs you use link to openmp? Check ldd output for your main executable?)

[Mandy Xie]

Cannot reproduce the issue, and get no more response from the issue reporter.

[Antoni Rosiñol Vidal]

Seeing same issue here. I solved it by linking everything using solely GTSAM's Eigen version. Instead of linking GTSAM with its own eigen version, and my code with the system version.

I cannot pinpoint what the exact issue is because my project is far too complex for a minimal example.

Leaving this here in case it is helpful for others.

(Note that I am working with an outdated commit, which is as for now 351 commits behind develop: c827d4cd6b11f78f3d2d9d52b335ac562a2757fc )

[Weiying Wang]

I could identify this issue and reproduce this issue steadily. The code structure for my project is hard to describe in order to address the issue. I run GaussianFactorMap.optimize() inside a ROS node and will be trapped in infinity loop. I printed out the stack trace which is nearly identical to the jstraub6 's message which indicates programs hangs at “general_matrix_matrix_product” from Eigen library.

I solved by recompiling GTSAM with using system Eigen and turn on TBB, MKL, OPENMP.

[Frank Dellaert]

Thanks for the extra intel! Some bizarre library conflict with eigen?

[Weiying Wang]

I cannot see any explicit conflicts with Eigen since I successfully compile GTSAM previously with embeded Eigen library. The GTSAM runs well solely which means I could run my main optimizer without putting it into ROS node. Once I wrap the my optimizer with a ROS node, the internal optimize() function will never end and doesn’t throw any error or exception.

Recompiling GTSAM with system Eigen library solves issues above. Hopefully you have some insights about this issue. FYI, the major part of optimizer is referring Distributed-Mapper paper.

[Frank Dellaert]

Yeah, that supports the hypothesis that ROS (or some other thing it links with) links with the system Eigen library, which then ends up conflicting with the GTSAM embedded library (what a bad idea, in retrospect! - we’ll change in a next version). Switching to system resolves this, according to this theory.