Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Incremental Smoothing with Plane Factor occasionally fails #561

Closed
mcamurri opened this issue Oct 10, 2020 · 15 comments
Closed

Incremental Smoothing with Plane Factor occasionally fails #561

mcamurri opened this issue Oct 10, 2020 · 15 comments

Comments

@mcamurri
Copy link

Hi all,
I'm experiencing some problems with isam2 optimization with plane factors.
Occasionally, the system throws an gtsam::IndeterminantLinearSystemException when two planes are present. But if I comment one of them, the system converges.

Description

Let's consider the following minimal example (or at least as minimal as I could get to reproduce the error).

The code instantiates a prior pose, two planes, and the priors for the planes.

All the values used are perfect: the measurements and the priors have no errors from the ground truth, so the system should converge immediately to the solution. However, with two planes the system throws a gtsam::IndeterminantLinearSystemException. If I comment one of the two planes, no exception is thrown.


// GTSAM
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

using namespace gtsam;

// Create unique keys for each state.
using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y)
using symbol_shorthand::P; //< Planes

int main(void){
 // Typedefs
  using Plane       = OrientedPlane3;
  using PlaneFactor = gtsam::OrientedPlane3Factor;

  // Setup graph & object
  FastMap<char, Vector> thresholds;
  thresholds['x'] = (Vector(6) << 0.05, 0.05, 0.05, 0.03, 0.03, 0.03).finished();
  thresholds['p'] = Vector3{0.03, 0.03, 0.1};

  ISAM2Params isam2params;
  isam2params.setRelinearizeThreshold(thresholds);
  isam2params.setRelinearizeSkip(3);
  isam2params.findUnusedFactorSlots = true;
  double lag = 1;
  gtsam::IncrementalFixedLagSmoother smoother(lag, isam2params);

  NonlinearFactorGraph graph;
  Values initialEstimate;
  std::map<gtsam::Key, double> timestampMap;

  // Initial values
  Plane p849(0.211098835, 0.214292752, 0.95368543, 26.4269514);
  Plane p897(0.301901811, 0.151751467, 0.941183717, 33.4388229);

  Pose3 x1527(Rot3(0.799903913, -0.564527097,  0.203624376, 0.552537226,   0.82520195,  0.117236322, -0.234214312, 0.0187322547,  0.972004505), Vector3{-91.7500013,-0.47569366,-4.61067606});

  // Setup priors
  initialEstimate.insert(P(849) , p849 );
  initialEstimate.insert(P(897) , p897 );
  initialEstimate.insert(X(1527), x1527);

  // Setup prior factors
  Pose3 x1527_prior(Rot3(0.799903913, -0.564527097,  0.203624376, 0.552537226,   0.82520195,  0.117236322, -0.234214312, 0.0187322547,  0.972004505), Vector3{-91.7500013, -0.47569366, -4.61067606});
  auto x1527_noise = gtsam::noiseModel::Isotropic::Sigma(6, 0.01);

  graph.emplace_shared<PriorFactor<Pose3>>(X(1527), x1527_prior, x1527_noise);

  Plane p849_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514);
  Plane p897_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229);


  auto p849_noise  = gtsam::noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
  auto p897_noise  = gtsam::noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});

  graph.emplace_shared<PriorFactor<Plane>>(P(849), p849_prior, p849_noise);
  graph.emplace_shared<PriorFactor<Plane>>(P(897), p897_prior, p897_noise);

  Plane p849_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073);
  Plane p897_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088);

  // Setup other factors

  const auto x1527_p897_noise = noiseModel::Isotropic::Sigma(3, 0.0322889);

  graph.emplace_shared<PlaneFactor>(p897_meas.planeCoefficients(), x1527_p897_noise, X(1527), P(897));

  const auto x1527_p849_noise = noiseModel::Isotropic::Sigma(3, 0.0451801);

  // OFFENDING LINE BELOW:
  //graph.emplace_shared<PlaneFactor>(p849_meas.planeCoefficients(), x1527_p849_noise, X(1527), P(849));

  // Setup timestamps
  // Update the timestamps of all referenced nodes
  for (const auto & factor: graph) {
    auto keys = factor->keys();
    for (const Key & key: keys) {
      if (timestampMap.count(key) == 0) {
        timestampMap[key] = 156485.0213; // seconds
      }
    }
  }

  // Optimize
  try {
    smoother.update(graph, initialEstimate, timestampMap);
    std::cerr << "ALL OK" << std::endl;
  } catch(gtsam::IndeterminantLinearSystemException e){
    std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl;
  }

  smoother.getDelta().print();
  smoother.calculateEstimate().print();
}

Steps to reproduce

  1. Uncomment the offending line
  2. Compile and launch the small main file

Expected behavior

The system should converge to the same values of the priors

Environment

Ubuntu 18.04 equipped with GTSAM 4.0.3 stock.
Please note though that the problem persists even when using the branch that fixes the derivatives by @dwisth here.

Additional information

  • The two planes being added are not degenerate.
  • Changing the thresholds or other parameters for iSAM does not help
  • The system converges when using QR decomposition instead of Cholesky (but we want to use Cholesky)
  • The exception seems to be thrown from gtsam/nonlinear/ISAM2Clique.cpp
  • The issue persist when randomly changing the plane coefficients
@dellaert
Copy link
Member

This is a great example! Before o look at it in detail: does the optimization work if you just do gauss-Newton optimization rather than iSAM?

@mcamurri
Copy link
Author

This is a great example! Before o look at it in detail: does the optimization work if you just do gauss-Newton optimization rather than iSAM?

Yes, exact same behavior with Gauss-Newton. Here is the listing:


// GTSAM
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>

using namespace gtsam;

// Create unique keys for each state.
using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y)
using symbol_shorthand::P; //< Planes

int main(void){
 // Typedefs
  using Plane       = OrientedPlane3;
  using PlaneFactor = gtsam::OrientedPlane3Factor;





  // Setup graph & object
  FastMap<char, Vector> thresholds;
  thresholds['x'] = (Vector(6) << 0.05, 0.05, 0.05, 0.03, 0.03, 0.03).finished();
  thresholds['p'] = Vector3{0.03, 0.03, 0.1};



  NonlinearFactorGraph graph;
  Values initialEstimate;
  std::map<gtsam::Key, double> timestampMap;

  // Initial values
  Plane p849(0.211098835, 0.214292752, 0.95368543, 26.4269514);
  Plane p897(0.301901811, 0.151751467, 0.941183717, 33.4388229);

  Pose3 x1527(Rot3(0.799903913, -0.564527097,  0.203624376, 0.552537226,   0.82520195,  0.117236322, -0.234214312, 0.0187322547,  0.972004505), Vector3{-91.7500013,-0.47569366,-2.2});

  // Setup priors
  initialEstimate.insert(P(849) , p849 );
  initialEstimate.insert(P(897) , p897 );
  initialEstimate.insert(X(1527), x1527);

  // Setup prior factors
  Pose3 x1527_prior(Rot3(0.799903913, -0.564527097,  0.203624376, 0.552537226,   0.82520195,  0.117236322, -0.234214312, 0.0187322547,  0.972004505), Vector3{-91.7500013, -0.47569366, -2.2});
  auto x1527_noise = gtsam::noiseModel::Isotropic::Sigma(6, 0.01);

  graph.emplace_shared<PriorFactor<Pose3>>(X(1527), x1527_prior, x1527_noise);

  Plane p849_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514);
  Plane p897_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229);


  auto p849_noise  = gtsam::noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});
  auto p897_noise  = gtsam::noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5});

  graph.emplace_shared<PriorFactor<Plane>>(P(849), p849_prior, p849_noise);
  graph.emplace_shared<PriorFactor<Plane>>(P(897), p897_prior, p897_noise);

  Plane p849_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073);
  Plane p897_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088);

  // Setup other factors

  const auto x1527_p897_noise = noiseModel::Isotropic::Sigma(3, 0.0322889);

  graph.emplace_shared<PlaneFactor>(p897_meas.planeCoefficients(), x1527_p897_noise, X(1527), P(897));

  const auto x1527_p849_noise = noiseModel::Isotropic::Sigma(3, 0.0451801);

  // OFFENDING LINE BELOW:
  graph.emplace_shared<PlaneFactor>(p849_meas.planeCoefficients(), x1527_p849_noise, X(1527), P(849));

  // Setup timestamps
  // Update the timestamps of all referenced nodes
  for (const auto & factor: graph) {
    auto keys = factor->keys();
    for (const Key & key: keys) {
      if (timestampMap.count(key) == 0) {
        timestampMap[key] = 156485.0213; // seconds
      }
    }
  }

  gtsam::GaussNewtonParams params;
  params.setVerbosity("TERMINATION");  // show info about stopping conditions
  gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, params);

  // Optimize
  try {
    Values result = optimizer.optimize();
    std::cout << "Optimization complete" << std::endl;

    std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
    std::cout << "final error=" << graph.error(result) << std::endl;

    std::cerr << "ALL OK" << std::endl;
  } catch(gtsam::IndeterminantLinearSystemException e){
    std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl;
  }

}

@dellaert dellaert mentioned this issue Oct 12, 2020
Merged
@dellaert
Copy link
Member

Marco, I added your test in #564
There is definitely an issue, but it's weirder than I even imagined. Take a look. Could be a subtle memory issue.
Running out of time but could you try a valgrind on the example?

@mcamurri
Copy link
Author

Here is the output of the second listing (Gauss-Newton) with the offending line through the command:
valgrind --tool=memcheck --leak-check=yes --show-reachable=yes --num-callers=20 --track-fds=yes -v ./indet_system

Output:

==24449== Memcheck, a memory error detector
==24449== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==24449== Using Valgrind-3.13.0 and LibVEX; rerun with -h for copyright info
==24449== Command: ./indet_system
==24449== 
--24449-- Valgrind options:
--24449--    --tool=memcheck
--24449--    --leak-check=yes
--24449--    --show-reachable=yes
--24449--    --num-callers=20
--24449--    --track-fds=yes
--24449--    -v
--24449-- Contents of /proc/version:
--24449--   Linux version 5.4.0-47-generic (buildd@lgw01-amd64-038) (gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04)) #51~18.04.1-Ubuntu SMP Sat Sep 5 14:35:50 UTC 2020
--24449-- 
--24449-- Arch and hwcaps: AMD64, LittleEndian, amd64-cx16-lzcnt-rdtscp-sse3-avx-avx2-bmi
--24449-- Page sizes: currently 4096, max supported 4096
--24449-- Valgrind library directory: /usr/lib/valgrind
--24449-- Reading syms from /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system
--24449-- Reading syms from /lib/x86_64-linux-gnu/ld-2.27.so
--24449--   Considering /lib/x86_64-linux-gnu/ld-2.27.so ..
--24449--   .. CRC mismatch (computed ac9397f6 wanted d0d82632)
--24449--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/ld-2.27.so ..
--24449--   .. CRC is valid
--24449-- Reading syms from /usr/lib/valgrind/memcheck-amd64-linux
--24449--   Considering /usr/lib/valgrind/memcheck-amd64-linux ..
--24449--   .. CRC mismatch (computed 41ddb025 wanted 9972f546)
--24449--    object doesn't have a symbol table
--24449--    object doesn't have a dynamic symbol table
--24449-- Scheduler: using generic scheduler lock implementation.
--24449-- Reading suppressions file: /usr/lib/valgrind/default.supp
==24449== embedded gdbserver: reading from /tmp/vgdb-pipe-from-vgdb-to-24449-by-mcamurri-on-???
==24449== embedded gdbserver: writing to   /tmp/vgdb-pipe-to-vgdb-from-24449-by-mcamurri-on-???
==24449== embedded gdbserver: shared mem   /tmp/vgdb-pipe-shared-mem-vgdb-24449-by-mcamurri-on-???
==24449== 
==24449== TO CONTROL THIS PROCESS USING vgdb (which you probably
==24449== don't want to do, unless you know exactly what you're doing,
==24449== or are doing some strange experiment):
==24449==   /usr/lib/valgrind/../../bin/vgdb --pid=24449 ...command...
==24449== 
==24449== TO DEBUG THIS PROCESS USING GDB: start GDB like this
==24449==   /path/to/gdb ./indet_system
==24449== and then give GDB the following command
==24449==   target remote | /usr/lib/valgrind/../../bin/vgdb --pid=24449
==24449== --pid is optional if only one valgrind process is running
==24449== 
--24449-- REDIR: 0x401f320 (ld-linux-x86-64.so.2:strlen) redirected to 0x580608c1 (???)
--24449-- REDIR: 0x401f100 (ld-linux-x86-64.so.2:index) redirected to 0x580608db (???)
--24449-- Reading syms from /usr/lib/valgrind/vgpreload_core-amd64-linux.so
--24449--   Considering /usr/lib/valgrind/vgpreload_core-amd64-linux.so ..
--24449--   .. CRC mismatch (computed 50df1b30 wanted 4800a4cf)
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so
--24449--   Considering /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so ..
--24449--   .. CRC mismatch (computed f893b962 wanted 95ee359e)
--24449--    object doesn't have a symbol table
==24449== WARNING: new redirection conflicts with existing -- ignoring it
--24449--     old: 0x0401f320 (strlen              ) R-> (0000.0) 0x580608c1 ???
--24449--     new: 0x0401f320 (strlen              ) R-> (2007.0) 0x04c32db0 strlen
--24449-- REDIR: 0x401d390 (ld-linux-x86-64.so.2:strcmp) redirected to 0x4c33ee0 (strcmp)
--24449-- REDIR: 0x401f860 (ld-linux-x86-64.so.2:mempcpy) redirected to 0x4c374f0 (mempcpy)
--24449-- Reading syms from /usr/local/lib/libgtsam.so.4.0.3
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libtbb.so.2
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /lib/x86_64-linux-gnu/libgcc_s.so.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /lib/x86_64-linux-gnu/libc-2.27.so
--24449--   Considering /lib/x86_64-linux-gnu/libc-2.27.so ..
--24449--   .. CRC mismatch (computed c2c067b2 wanted d73adc7f)
--24449--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libc-2.27.so ..
--24449--   .. CRC is valid
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_serialization.so.1.65.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_filesystem.so.1.65.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_timer.so.1.65.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libtbbmalloc.so.2
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /usr/local/lib/libmetis-gtsam.so
--24449-- Reading syms from /lib/x86_64-linux-gnu/libm-2.27.so
--24449--   Considering /lib/x86_64-linux-gnu/libm-2.27.so ..
--24449--   .. CRC mismatch (computed 4cce394c wanted e22acdb4)
--24449--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libm-2.27.so ..
--24449--   .. CRC is valid
--24449-- Reading syms from /lib/x86_64-linux-gnu/libdl-2.27.so
--24449--   Considering /lib/x86_64-linux-gnu/libdl-2.27.so ..
--24449--   .. CRC mismatch (computed 415bd007 wanted 4d6f4a12)
--24449--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libdl-2.27.so ..
--24449--   .. CRC is valid
--24449-- Reading syms from /lib/x86_64-linux-gnu/libpthread-2.27.so
--24449--   Considering /usr/lib/debug/.build-id/bc/3c06107774266c5f7db3f1f380a3da68af90fa.debug ..
--24449--   .. build-id is valid
--24449-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_chrono.so.1.65.1
--24449--    object doesn't have a symbol table
--24449-- Reading syms from /lib/x86_64-linux-gnu/librt-2.27.so
--24449--   Considering /lib/x86_64-linux-gnu/librt-2.27.so ..
--24449--   .. CRC mismatch (computed c8cf706c wanted b5c8ed65)
--24449--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/librt-2.27.so ..
--24449--   .. CRC is valid
--24449-- REDIR: 0x5d2bce0 (libc.so.6:memmove) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2adb0 (libc.so.6:strncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bfc0 (libc.so.6:strcasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2a800 (libc.so.6:strcat) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2ade0 (libc.so.6:rindex) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2d830 (libc.so.6:rawmemchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2be50 (libc.so.6:mempcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bc80 (libc.so.6:bcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2ad70 (libc.so.6:strncmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2a870 (libc.so.6:strcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bdb0 (libc.so.6:memset) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d49160 (libc.so.6:wcschr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2ad10 (libc.so.6:strnlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2a8e0 (libc.so.6:strcspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2c010 (libc.so.6:strncasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2a8b0 (libc.so.6:strcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2c150 (libc.so.6:memcpy@@GLIBC_2.14) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2ae10 (libc.so.6:strpbrk) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2a830 (libc.so.6:index) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2ace0 (libc.so.6:strlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d35730 (libc.so.6:memrchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2c060 (libc.so.6:strcasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bc50 (libc.so.6:memchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d49f20 (libc.so.6:wcslen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2b0c0 (libc.so.6:strspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bf90 (libc.so.6:stpncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2bf60 (libc.so.6:stpcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2d860 (libc.so.6:strchrnul) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5d2c0b0 (libc.so.6:strncasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24449-- REDIR: 0x5e1b630 (libc.so.6:__strrchr_avx2) redirected to 0x4c32730 (rindex)
--24449-- REDIR: 0x5d240e0 (libc.so.6:malloc) redirected to 0x4c2faa0 (malloc)
--24449-- REDIR: 0x5d270a0 (libc.so.6:calloc) redirected to 0x4c31a70 (calloc)
--24449-- REDIR: 0x5e1b800 (libc.so.6:__strlen_avx2) redirected to 0x4c32cf0 (strlen)
--24449-- REDIR: 0x5e17e10 (libc.so.6:__memcmp_avx2_movbe) redirected to 0x4c35e00 (bcmp)
--24449-- REDIR: 0x5df6fd0 (libc.so.6:__strcmp_ssse3) redirected to 0x4c33da0 (strcmp)
--24449-- REDIR: 0x577f280 (libstdc++.so.6:operator new(unsigned long)) redirected to 0x4c30110 (operator new(unsigned long))
--24449-- REDIR: 0x5e0a370 (libc.so.6:__strncpy_ssse3) redirected to 0x4c32fb0 (strncpy)
--24449-- REDIR: 0x5e1bd40 (libc.so.6:__memcpy_avx_unaligned_erms) redirected to 0x4c366e0 (memmove)
--24449-- REDIR: 0x5e1c1c0 (libc.so.6:__memset_avx2_unaligned_erms) redirected to 0x4c365d0 (memset)
--24449-- REDIR: 0x5d2b600 (libc.so.6:__GI_strstr) redirected to 0x4c37760 (__strstr_sse2)
--24449-- REDIR: 0x5e17690 (libc.so.6:__memchr_avx2) redirected to 0x4c33f80 (memchr)
--24449-- REDIR: 0x5e17960 (libc.so.6:__rawmemchr_avx2) redirected to 0x4c37050 (rawmemchr)
--24449-- REDIR: 0x5d249c0 (libc.so.6:free) redirected to 0x4c30cd0 (free)
--24449-- REDIR: 0x577d390 (libstdc++.so.6:operator delete(void*)) redirected to 0x4c311d0 (operator delete(void*))
--24449-- REDIR: 0x577f340 (libstdc++.so.6:operator new[](unsigned long, std::nothrow_t const&)) redirected to 0x4c30ac0 (operator new[](unsigned long, std::nothrow_t const&))
--24449-- REDIR: 0x5e0cef0 (libc.so.6:__stpcpy_ssse3) redirected to 0x4c35f60 (stpcpy)
--24449-- REDIR: 0x5e1bd20 (libc.so.6:__mempcpy_avx_unaligned_erms) redirected to 0x4c37130 (mempcpy)
--24449-- REDIR: 0x5e1b440 (libc.so.6:__strchrnul_avx2) redirected to 0x4c37020 (strchrnul)
--24449-- REDIR: 0x5d25ca0 (libc.so.6:realloc) redirected to 0x4c31cb0 (realloc)
--24449-- REDIR: 0x577f330 (libstdc++.so.6:operator new[](unsigned long)) redirected to 0x4c30830 (operator new[](unsigned long))
--24449-- REDIR: 0x577d3c0 (libstdc++.so.6:operator delete[](void*)) redirected to 0x4c316d0 (operator delete[](void*))
CAPTURED THE EXCEPTION: 8070450532247929681
==24449== 
==24449== FILE DESCRIPTORS: 3 open at exit.
==24449== Open file descriptor 2: /dev/pts/11
==24449==    <inherited from parent>
==24449== 
==24449== Open file descriptor 1: /dev/pts/11
==24449==    <inherited from parent>
==24449== 
==24449== Open file descriptor 0: /dev/pts/11
==24449==    <inherited from parent>
==24449== 
==24449== 
==24449== HEAP SUMMARY:
==24449==     in use at exit: 8,680 bytes in 9 blocks
==24449==   total heap usage: 430 allocs, 421 frees, 120,913 bytes allocated
==24449== 
==24449== Searching for pointers to 9 not-freed blocks
==24449== Checked 11,333,984 bytes
==24449== 
==24449== 32 bytes in 1 blocks are still reachable in loss record 1 of 6
==24449==    at 0x4C31B25: calloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x712F7F4: _dlerror_run (dlerror.c:140)
==24449==    by 0x712F050: dlopen@@GLIBC_2.2.5 (dlopen.c:87)
==24449==    by 0x68E5F94: ??? (in /usr/lib/x86_64-linux-gnu/libtbbmalloc.so.2)
==24449==    by 0x4010782: call_init (dl-init.c:72)
==24449==    by 0x4010782: _dl_init (dl-init.c:119)
==24449==    by 0x40010C9: ??? (in /lib/x86_64-linux-gnu/ld-2.27.so)
==24449== 
==24449== 128 bytes in 1 blocks are still reachable in loss record 2 of 6
==24449==    at 0x4C3089F: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x54CB73B: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54C217B: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CB97D: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54D7F74: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CD8E6: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CD928: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CBB3C: tbb::internal::allocate_root_with_context_proxy::allocate(unsigned long) const (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x51901CC: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x1159E6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24449== 
==24449== 192 bytes in 1 blocks are possibly lost in loss record 3 of 6
==24449==    at 0x4C2FB0F: malloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x577D8FF: __cxa_allocate_exception (in /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25)
==24449==    by 0x50DD83B: gtsam::HessianFactor::eliminateCholesky(gtsam::Ordering const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50DED72: gtsam::EliminateCholesky(gtsam::GaussianFactorGraph const&, gtsam::Ordering const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50DEECA: gtsam::EliminatePreferCholesky(gtsam::GaussianFactorGraph const&, gtsam::Ordering const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50A35D2: 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&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50D1E2C: 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> >&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50D2400: gtsam::treeTraversal::internal::PreOrderTask<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> > (boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> const&, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >&), gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >::EliminationPostOrderVisitor>::processNodeRecursively(boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> const&, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50D2672: gtsam::treeTraversal::internal::PreOrderTask<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> > (boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> const&, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >&), gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >::EliminationPostOrderVisitor>::execute() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x54D6B45: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x50D1BA4: gtsam::treeTraversal::internal::RootTask<std::vector<boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster>, tbb::tbb_allocator<boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> > >, gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> > (boost::shared_ptr<gtsam::ClusterTree<gtsam::GaussianFactorGraph>::Cluster> const&, gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >&), gtsam::EliminationData<gtsam::EliminatableClusterTree<gtsam::GaussianBayesTree, gtsam::GaussianFactorGraph> >::EliminationPostOrderVisitor>::execute() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x54D6B45: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54D378F: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x50CDB47: 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 (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50CE41B: gtsam::EliminateableFactorGraph<gtsam::GaussianFactorGraph>::eliminateMultifrontal(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&>) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50CE556: gtsam::EliminateableFactorGraph<gtsam::GaussianFactorGraph>::eliminateMultifrontal(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&>) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x50C956F: gtsam::GaussianFactorGraph::optimize(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 (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x519555F: gtsam::NonlinearOptimizer::solve(gtsam::GaussianFactorGraph const&, gtsam::NonlinearOptimizerParams const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x5150AEC: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449== 
==24449== 608 bytes in 2 blocks are possibly lost in loss record 4 of 6
==24449==    at 0x4C31B25: calloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x40134F6: allocate_dtv (dl-tls.c:286)
==24449==    by 0x40134F6: _dl_allocate_tls (dl-tls.c:530)
==24449==    by 0x733A227: allocate_stack (allocatestack.c:627)
==24449==    by 0x733A227: pthread_create@@GLIBC_2.2.5 (pthread_create.c:644)
==24449==    by 0x54CA3C9: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54D3034: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x518EC25: tbb::interface9::internal::start_for<tbb::blocked_range<unsigned long>, gtsam::(anonymous namespace)::_LinearizeOneFactor, tbb::auto_partitioner const>::execute() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x54D6B45: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54D378F: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x519022E: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x1159E6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24449== 
==24449== 1,552 bytes in 1 blocks are still reachable in loss record 5 of 6
==24449==    at 0x4C30B2F: operator new[](unsigned long, std::nothrow_t const&) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x519D8AE: boost::pool<boost::default_user_allocator_new_delete>::malloc_need_resize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x519C568: gtsam::Values::tryInsert(unsigned long, gtsam::Value const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x519C77C: gtsam::Values::insert(unsigned long, gtsam::Value const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x118B7B: void gtsam::Values::insert<gtsam::OrientedPlane3>(unsigned long, gtsam::OrientedPlane3 const&) (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24449==    by 0x11514D: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24449== 
==24449== 6,168 bytes in 3 blocks are still reachable in loss record 6 of 6
==24449==    at 0x4C3089F: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24449==    by 0x54D1752: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54D1A6F: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CEAFB: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CD992: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x54CBD60: tbb::internal::get_initial_auto_partitioner_divisor() (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24449==    by 0x5190207: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24449==    by 0x1159E6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24449== 
==24449== LEAK SUMMARY:
==24449==    definitely lost: 0 bytes in 0 blocks
==24449==    indirectly lost: 0 bytes in 0 blocks
==24449==      possibly lost: 800 bytes in 3 blocks
==24449==    still reachable: 7,880 bytes in 6 blocks
==24449==                       of which reachable via heuristic:
==24449==                         newarray           : 6,168 bytes in 3 blocks
==24449==         suppressed: 0 bytes in 0 blocks
==24449== 
==24449== ERROR SUMMARY: 2 errors from 2 contexts (suppressed: 0 from 0)
==24449== ERROR SUMMARY: 2 errors from 2 contexts (suppressed: 0 from 0)

Without the offending line, the error count goes to 1:

==24841== Memcheck, a memory error detector
==24841== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==24841== Using Valgrind-3.13.0 and LibVEX; rerun with -h for copyright info
==24841== Command: ./indet_system
==24841== 
--24841-- Valgrind options:
--24841--    --tool=memcheck
--24841--    --leak-check=yes
--24841--    --show-reachable=yes
--24841--    --num-callers=20
--24841--    --track-fds=yes
--24841--    -v
--24841-- Contents of /proc/version:
--24841--   Linux version 5.4.0-47-generic (buildd@lgw01-amd64-038) (gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04)) #51~18.04.1-Ubuntu SMP Sat Sep 5 14:35:50 UTC 2020
--24841-- 
--24841-- Arch and hwcaps: AMD64, LittleEndian, amd64-cx16-lzcnt-rdtscp-sse3-avx-avx2-bmi
--24841-- Page sizes: currently 4096, max supported 4096
--24841-- Valgrind library directory: /usr/lib/valgrind
--24841-- Reading syms from /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system
--24841-- Reading syms from /lib/x86_64-linux-gnu/ld-2.27.so
--24841--   Considering /lib/x86_64-linux-gnu/ld-2.27.so ..
--24841--   .. CRC mismatch (computed ac9397f6 wanted d0d82632)
--24841--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/ld-2.27.so ..
--24841--   .. CRC is valid
--24841-- Reading syms from /usr/lib/valgrind/memcheck-amd64-linux
--24841--   Considering /usr/lib/valgrind/memcheck-amd64-linux ..
--24841--   .. CRC mismatch (computed 41ddb025 wanted 9972f546)
--24841--    object doesn't have a symbol table
--24841--    object doesn't have a dynamic symbol table
--24841-- Scheduler: using generic scheduler lock implementation.
--24841-- Reading suppressions file: /usr/lib/valgrind/default.supp
==24841== embedded gdbserver: reading from /tmp/vgdb-pipe-from-vgdb-to-24841-by-mcamurri-on-???
==24841== embedded gdbserver: writing to   /tmp/vgdb-pipe-to-vgdb-from-24841-by-mcamurri-on-???
==24841== embedded gdbserver: shared mem   /tmp/vgdb-pipe-shared-mem-vgdb-24841-by-mcamurri-on-???
==24841== 
==24841== TO CONTROL THIS PROCESS USING vgdb (which you probably
==24841== don't want to do, unless you know exactly what you're doing,
==24841== or are doing some strange experiment):
==24841==   /usr/lib/valgrind/../../bin/vgdb --pid=24841 ...command...
==24841== 
==24841== TO DEBUG THIS PROCESS USING GDB: start GDB like this
==24841==   /path/to/gdb ./indet_system
==24841== and then give GDB the following command
==24841==   target remote | /usr/lib/valgrind/../../bin/vgdb --pid=24841
==24841== --pid is optional if only one valgrind process is running
==24841== 
--24841-- REDIR: 0x401f320 (ld-linux-x86-64.so.2:strlen) redirected to 0x580608c1 (???)
--24841-- REDIR: 0x401f100 (ld-linux-x86-64.so.2:index) redirected to 0x580608db (???)
--24841-- Reading syms from /usr/lib/valgrind/vgpreload_core-amd64-linux.so
--24841--   Considering /usr/lib/valgrind/vgpreload_core-amd64-linux.so ..
--24841--   .. CRC mismatch (computed 50df1b30 wanted 4800a4cf)
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so
--24841--   Considering /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so ..
--24841--   .. CRC mismatch (computed f893b962 wanted 95ee359e)
--24841--    object doesn't have a symbol table
==24841== WARNING: new redirection conflicts with existing -- ignoring it
--24841--     old: 0x0401f320 (strlen              ) R-> (0000.0) 0x580608c1 ???
--24841--     new: 0x0401f320 (strlen              ) R-> (2007.0) 0x04c32db0 strlen
--24841-- REDIR: 0x401d390 (ld-linux-x86-64.so.2:strcmp) redirected to 0x4c33ee0 (strcmp)
--24841-- REDIR: 0x401f860 (ld-linux-x86-64.so.2:mempcpy) redirected to 0x4c374f0 (mempcpy)
--24841-- Reading syms from /usr/local/lib/libgtsam.so.4.0.3
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libtbb.so.2
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /lib/x86_64-linux-gnu/libgcc_s.so.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /lib/x86_64-linux-gnu/libc-2.27.so
--24841--   Considering /lib/x86_64-linux-gnu/libc-2.27.so ..
--24841--   .. CRC mismatch (computed c2c067b2 wanted d73adc7f)
--24841--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libc-2.27.so ..
--24841--   .. CRC is valid
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_serialization.so.1.65.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_filesystem.so.1.65.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_timer.so.1.65.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libtbbmalloc.so.2
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /usr/local/lib/libmetis-gtsam.so
--24841-- Reading syms from /lib/x86_64-linux-gnu/libm-2.27.so
--24841--   Considering /lib/x86_64-linux-gnu/libm-2.27.so ..
--24841--   .. CRC mismatch (computed 4cce394c wanted e22acdb4)
--24841--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libm-2.27.so ..
--24841--   .. CRC is valid
--24841-- Reading syms from /lib/x86_64-linux-gnu/libdl-2.27.so
--24841--   Considering /lib/x86_64-linux-gnu/libdl-2.27.so ..
--24841--   .. CRC mismatch (computed 415bd007 wanted 4d6f4a12)
--24841--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libdl-2.27.so ..
--24841--   .. CRC is valid
--24841-- Reading syms from /lib/x86_64-linux-gnu/libpthread-2.27.so
--24841--   Considering /usr/lib/debug/.build-id/bc/3c06107774266c5f7db3f1f380a3da68af90fa.debug ..
--24841--   .. build-id is valid
--24841-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_chrono.so.1.65.1
--24841--    object doesn't have a symbol table
--24841-- Reading syms from /lib/x86_64-linux-gnu/librt-2.27.so
--24841--   Considering /lib/x86_64-linux-gnu/librt-2.27.so ..
--24841--   .. CRC mismatch (computed c8cf706c wanted b5c8ed65)
--24841--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/librt-2.27.so ..
--24841--   .. CRC is valid
--24841-- REDIR: 0x5d2bce0 (libc.so.6:memmove) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2adb0 (libc.so.6:strncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bfc0 (libc.so.6:strcasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2a800 (libc.so.6:strcat) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2ade0 (libc.so.6:rindex) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2d830 (libc.so.6:rawmemchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2be50 (libc.so.6:mempcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bc80 (libc.so.6:bcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2ad70 (libc.so.6:strncmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2a870 (libc.so.6:strcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bdb0 (libc.so.6:memset) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d49160 (libc.so.6:wcschr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2ad10 (libc.so.6:strnlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2a8e0 (libc.so.6:strcspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2c010 (libc.so.6:strncasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2a8b0 (libc.so.6:strcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2c150 (libc.so.6:memcpy@@GLIBC_2.14) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2ae10 (libc.so.6:strpbrk) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2a830 (libc.so.6:index) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2ace0 (libc.so.6:strlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d35730 (libc.so.6:memrchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2c060 (libc.so.6:strcasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bc50 (libc.so.6:memchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d49f20 (libc.so.6:wcslen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2b0c0 (libc.so.6:strspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bf90 (libc.so.6:stpncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2bf60 (libc.so.6:stpcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2d860 (libc.so.6:strchrnul) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5d2c0b0 (libc.so.6:strncasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--24841-- REDIR: 0x5e1b630 (libc.so.6:__strrchr_avx2) redirected to 0x4c32730 (rindex)
--24841-- REDIR: 0x5d240e0 (libc.so.6:malloc) redirected to 0x4c2faa0 (malloc)
--24841-- REDIR: 0x5d270a0 (libc.so.6:calloc) redirected to 0x4c31a70 (calloc)
--24841-- REDIR: 0x5e1b800 (libc.so.6:__strlen_avx2) redirected to 0x4c32cf0 (strlen)
--24841-- REDIR: 0x5e17e10 (libc.so.6:__memcmp_avx2_movbe) redirected to 0x4c35e00 (bcmp)
--24841-- REDIR: 0x5df6fd0 (libc.so.6:__strcmp_ssse3) redirected to 0x4c33da0 (strcmp)
--24841-- REDIR: 0x577f280 (libstdc++.so.6:operator new(unsigned long)) redirected to 0x4c30110 (operator new(unsigned long))
--24841-- REDIR: 0x5e0a370 (libc.so.6:__strncpy_ssse3) redirected to 0x4c32fb0 (strncpy)
--24841-- REDIR: 0x5e1bd40 (libc.so.6:__memcpy_avx_unaligned_erms) redirected to 0x4c366e0 (memmove)
--24841-- REDIR: 0x5e1c1c0 (libc.so.6:__memset_avx2_unaligned_erms) redirected to 0x4c365d0 (memset)
--24841-- REDIR: 0x5d2b600 (libc.so.6:__GI_strstr) redirected to 0x4c37760 (__strstr_sse2)
--24841-- REDIR: 0x5e17690 (libc.so.6:__memchr_avx2) redirected to 0x4c33f80 (memchr)
--24841-- REDIR: 0x5e17960 (libc.so.6:__rawmemchr_avx2) redirected to 0x4c37050 (rawmemchr)
--24841-- REDIR: 0x5d249c0 (libc.so.6:free) redirected to 0x4c30cd0 (free)
--24841-- REDIR: 0x577d390 (libstdc++.so.6:operator delete(void*)) redirected to 0x4c311d0 (operator delete(void*))
--24841-- REDIR: 0x577f340 (libstdc++.so.6:operator new[](unsigned long, std::nothrow_t const&)) redirected to 0x4c30ac0 (operator new[](unsigned long, std::nothrow_t const&))
--24841-- REDIR: 0x5e0cef0 (libc.so.6:__stpcpy_ssse3) redirected to 0x4c35f60 (stpcpy)
--24841-- REDIR: 0x5e1bd20 (libc.so.6:__mempcpy_avx_unaligned_erms) redirected to 0x4c37130 (mempcpy)
--24841-- REDIR: 0x5e1b440 (libc.so.6:__strchrnul_avx2) redirected to 0x4c37020 (strchrnul)
--24841-- REDIR: 0x5d25ca0 (libc.so.6:realloc) redirected to 0x4c31cb0 (realloc)
--24841-- REDIR: 0x577f330 (libstdc++.so.6:operator new[](unsigned long)) redirected to 0x4c30830 (operator new[](unsigned long))
--24841-- REDIR: 0x577d3c0 (libstdc++.so.6:operator delete[](void*)) redirected to 0x4c316d0 (operator delete[](void*))
converged
errorThreshold: 0.0766692 <? 0
absoluteDecrease: 1.72362124573e-13 <? 1e-05
relativeDecrease: 2.24812844221e-12 <? 1e-05
iterations: 3 >? 100
Optimization complete
initial error=2468.82346403
final error=0.0766691623738
ALL OK
==24841== 
==24841== FILE DESCRIPTORS: 3 open at exit.
==24841== Open file descriptor 2: /dev/pts/11
==24841==    <inherited from parent>
==24841== 
==24841== Open file descriptor 1: /dev/pts/11
==24841==    <inherited from parent>
==24841== 
==24841== Open file descriptor 0: /dev/pts/11
==24841==    <inherited from parent>
==24841== 
==24841== 
==24841== HEAP SUMMARY:
==24841==     in use at exit: 8,488 bytes in 8 blocks
==24841==   total heap usage: 670 allocs, 662 frees, 148,277 bytes allocated
==24841== 
==24841== Searching for pointers to 8 not-freed blocks
==24841== Checked 11,334,080 bytes
==24841== 
==24841== 32 bytes in 1 blocks are still reachable in loss record 1 of 5
==24841==    at 0x4C31B25: calloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24841==    by 0x712F7F4: _dlerror_run (dlerror.c:140)
==24841==    by 0x712F050: dlopen@@GLIBC_2.2.5 (dlopen.c:87)
==24841==    by 0x68E5F94: ??? (in /usr/lib/x86_64-linux-gnu/libtbbmalloc.so.2)
==24841==    by 0x4010782: call_init (dl-init.c:72)
==24841==    by 0x4010782: _dl_init (dl-init.c:119)
==24841==    by 0x40010C9: ??? (in /lib/x86_64-linux-gnu/ld-2.27.so)
==24841== 
==24841== 128 bytes in 1 blocks are still reachable in loss record 2 of 5
==24841==    at 0x4C3089F: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24841==    by 0x54CB73B: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54C217B: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CB97D: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54D7F74: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CD8E6: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CD928: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CBB3C: tbb::internal::allocate_root_with_context_proxy::allocate(unsigned long) const (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x51901CC: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x1158B6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24841== 
==24841== 608 bytes in 2 blocks are possibly lost in loss record 3 of 5
==24841==    at 0x4C31B25: calloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24841==    by 0x40134F6: allocate_dtv (dl-tls.c:286)
==24841==    by 0x40134F6: _dl_allocate_tls (dl-tls.c:530)
==24841==    by 0x733A227: allocate_stack (allocatestack.c:627)
==24841==    by 0x733A227: pthread_create@@GLIBC_2.2.5 (pthread_create.c:644)
==24841==    by 0x54CA3C9: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54D3034: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x518EC25: tbb::interface9::internal::start_for<tbb::blocked_range<unsigned long>, gtsam::(anonymous namespace)::_LinearizeOneFactor, tbb::auto_partitioner const>::execute() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x54D6B45: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54D378F: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x519022E: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x1158B6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24841== 
==24841== 1,552 bytes in 1 blocks are still reachable in loss record 4 of 5
==24841==    at 0x4C30B2F: operator new[](unsigned long, std::nothrow_t const&) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24841==    by 0x519D8AE: boost::pool<boost::default_user_allocator_new_delete>::malloc_need_resize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x519C568: gtsam::Values::tryInsert(unsigned long, gtsam::Value const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x519C77C: gtsam::Values::insert(unsigned long, gtsam::Value const&) (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x1189EB: void gtsam::Values::insert<gtsam::OrientedPlane3>(unsigned long, gtsam::OrientedPlane3 const&) (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24841==    by 0x114FF3: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24841== 
==24841== 6,168 bytes in 3 blocks are still reachable in loss record 5 of 5
==24841==    at 0x4C3089F: operator new[](unsigned long) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==24841==    by 0x54D1752: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54D1A6F: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CEAFB: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CD992: ??? (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x54CBD60: tbb::internal::get_initial_auto_partitioner_divisor() (in /usr/lib/x86_64-linux-gnu/libtbb.so.2)
==24841==    by 0x5190207: gtsam::NonlinearFactorGraph::linearize(gtsam::Values const&) const (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x5150AD7: gtsam::GaussNewtonOptimizer::iterate() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x51945DE: gtsam::NonlinearOptimizer::defaultOptimize() (in /usr/local/lib/libgtsam.so.4.0.3)
==24841==    by 0x1158B6: main (in /home/mcamurri/git/drs_ros_packages/vilens/vilens/build/devel/lib/vilens/indet_system)
==24841== 
==24841== LEAK SUMMARY:
==24841==    definitely lost: 0 bytes in 0 blocks
==24841==    indirectly lost: 0 bytes in 0 blocks
==24841==      possibly lost: 608 bytes in 2 blocks
==24841==    still reachable: 7,880 bytes in 6 blocks
==24841==                       of which reachable via heuristic:
==24841==                         newarray           : 6,168 bytes in 3 blocks
==24841==         suppressed: 0 bytes in 0 blocks
==24841== 
==24841== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)
==24841== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)

Note that I wasn't able to reproduce the success by commenting out both the plane priors. I will try later on with the test you have made. This was done on my small main but it shouldn't make much difference.

@dellaert
Copy link
Member

Hmmm. I'm not an expert on debugging memory errors but don't see crazy things here at first glance. Still, the fact that you can't get error by commenting out prior again points to non-determinism. I'll take another look after teaching but please pore over the code as well?

My bet is on some subtle bug in OrientedPlane3 or OrientedPlane3Factor, and non-determinism points to either uninitialized memory or worse, overwriting some nearby memory. I did use Debug, however, and I think that adds bounds checking in Eigen - but nothing triggered.

@ProfFan
Copy link
Collaborator

ProfFan commented Oct 12, 2020

TBB is famous for generating false positives with Valgrind. Could you try reproducing without TBB? That would make the stacktrace a bit cleaner.

@mcamurri
Copy link
Author

@ProfFan you are right. I've recompiled gtsam without TBB and run the test @dellaert made. The stack is clear of errors:

==5045== Memcheck, a memory error detector
==5045== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==5045== Using Valgrind-3.13.0 and LibVEX; rerun with -h for copyright info
==5045== Command: ./testOrientedPlane3Factor
==5045== 
--5045-- Valgrind options:
--5045--    --tool=memcheck
--5045--    --leak-check=yes
--5045--    --show-reachable=yes
--5045--    --num-callers=20
--5045--    --track-fds=yes
--5045--    -v
--5045-- Contents of /proc/version:
--5045--   Linux version 5.4.0-47-generic (buildd@lgw01-amd64-038) (gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04)) #51~18.04.1-Ubuntu SMP Sat Sep 5 14:35:50 UTC 2020
--5045-- 
--5045-- Arch and hwcaps: AMD64, LittleEndian, amd64-cx16-lzcnt-rdtscp-sse3-avx-avx2-bmi
--5045-- Page sizes: currently 4096, max supported 4096
--5045-- Valgrind library directory: /usr/lib/valgrind
--5045-- Reading syms from /home/mcamurri/git/libraries/gtsam/build/gtsam/slam/tests/testOrientedPlane3Factor
--5045-- Reading syms from /lib/x86_64-linux-gnu/ld-2.27.so
--5045--   Considering /lib/x86_64-linux-gnu/ld-2.27.so ..
--5045--   .. CRC mismatch (computed ac9397f6 wanted d0d82632)
--5045--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/ld-2.27.so ..
--5045--   .. CRC is valid
--5045-- Reading syms from /usr/lib/valgrind/memcheck-amd64-linux
--5045--   Considering /usr/lib/valgrind/memcheck-amd64-linux ..
--5045--   .. CRC mismatch (computed 41ddb025 wanted 9972f546)
--5045--    object doesn't have a symbol table
--5045--    object doesn't have a dynamic symbol table
--5045-- Scheduler: using generic scheduler lock implementation.
--5045-- Reading suppressions file: /usr/lib/valgrind/default.supp
==5045== embedded gdbserver: reading from /tmp/vgdb-pipe-from-vgdb-to-5045-by-mcamurri-on-???
==5045== embedded gdbserver: writing to   /tmp/vgdb-pipe-to-vgdb-from-5045-by-mcamurri-on-???
==5045== embedded gdbserver: shared mem   /tmp/vgdb-pipe-shared-mem-vgdb-5045-by-mcamurri-on-???
==5045== 
==5045== TO CONTROL THIS PROCESS USING vgdb (which you probably
==5045== don't want to do, unless you know exactly what you're doing,
==5045== or are doing some strange experiment):
==5045==   /usr/lib/valgrind/../../bin/vgdb --pid=5045 ...command...
==5045== 
==5045== TO DEBUG THIS PROCESS USING GDB: start GDB like this
==5045==   /path/to/gdb ./testOrientedPlane3Factor
==5045== and then give GDB the following command
==5045==   target remote | /usr/lib/valgrind/../../bin/vgdb --pid=5045
==5045== --pid is optional if only one valgrind process is running
==5045== 
--5045-- REDIR: 0x401f320 (ld-linux-x86-64.so.2:strlen) redirected to 0x580608c1 (???)
--5045-- REDIR: 0x401f100 (ld-linux-x86-64.so.2:index) redirected to 0x580608db (???)
--5045-- Reading syms from /usr/lib/valgrind/vgpreload_core-amd64-linux.so
--5045--   Considering /usr/lib/valgrind/vgpreload_core-amd64-linux.so ..
--5045--   .. CRC mismatch (computed 50df1b30 wanted 4800a4cf)
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so
--5045--   Considering /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so ..
--5045--   .. CRC mismatch (computed f893b962 wanted 95ee359e)
--5045--    object doesn't have a symbol table
==5045== WARNING: new redirection conflicts with existing -- ignoring it
--5045--     old: 0x0401f320 (strlen              ) R-> (0000.0) 0x580608c1 ???
--5045--     new: 0x0401f320 (strlen              ) R-> (2007.0) 0x04c32db0 strlen
--5045-- REDIR: 0x401d390 (ld-linux-x86-64.so.2:strcmp) redirected to 0x4c33ee0 (strcmp)
--5045-- REDIR: 0x401f860 (ld-linux-x86-64.so.2:mempcpy) redirected to 0x4c374f0 (mempcpy)
--5045-- Reading syms from /home/mcamurri/git/libraries/gtsam/build/gtsam/libgtsam.so.4.1.0
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.25
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /lib/x86_64-linux-gnu/libgcc_s.so.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /lib/x86_64-linux-gnu/libc-2.27.so
--5045--   Considering /lib/x86_64-linux-gnu/libc-2.27.so ..
--5045--   .. CRC mismatch (computed c2c067b2 wanted d73adc7f)
--5045--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libc-2.27.so ..
--5045--   .. CRC is valid
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_serialization.so.1.65.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_filesystem.so.1.65.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_timer.so.1.65.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /home/mcamurri/git/libraries/gtsam/build/gtsam/3rdparty/metis/libmetis/libmetis-gtsam.so
--5045-- Reading syms from /lib/x86_64-linux-gnu/libm-2.27.so
--5045--   Considering /lib/x86_64-linux-gnu/libm-2.27.so ..
--5045--   .. CRC mismatch (computed 4cce394c wanted e22acdb4)
--5045--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/libm-2.27.so ..
--5045--   .. CRC is valid
--5045-- Reading syms from /lib/x86_64-linux-gnu/libpthread-2.27.so
--5045--   Considering /usr/lib/debug/.build-id/bc/3c06107774266c5f7db3f1f380a3da68af90fa.debug ..
--5045--   .. build-id is valid
--5045-- Reading syms from /usr/lib/x86_64-linux-gnu/libboost_chrono.so.1.65.1
--5045--    object doesn't have a symbol table
--5045-- Reading syms from /lib/x86_64-linux-gnu/librt-2.27.so
--5045--   Considering /lib/x86_64-linux-gnu/librt-2.27.so ..
--5045--   .. CRC mismatch (computed c8cf706c wanted b5c8ed65)
--5045--   Considering /usr/lib/debug/lib/x86_64-linux-gnu/librt-2.27.so ..
--5045--   .. CRC is valid
--5045-- REDIR: 0x5b27ce0 (libc.so.6:memmove) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26db0 (libc.so.6:strncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27fc0 (libc.so.6:strcasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26800 (libc.so.6:strcat) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26de0 (libc.so.6:rindex) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b29830 (libc.so.6:rawmemchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27e50 (libc.so.6:mempcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27c80 (libc.so.6:bcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26d70 (libc.so.6:strncmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26870 (libc.so.6:strcmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27db0 (libc.so.6:memset) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b45160 (libc.so.6:wcschr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26d10 (libc.so.6:strnlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b268e0 (libc.so.6:strcspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b28010 (libc.so.6:strncasecmp) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b268b0 (libc.so.6:strcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b28150 (libc.so.6:memcpy@@GLIBC_2.14) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26e10 (libc.so.6:strpbrk) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26830 (libc.so.6:index) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b26ce0 (libc.so.6:strlen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b31730 (libc.so.6:memrchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b28060 (libc.so.6:strcasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27c50 (libc.so.6:memchr) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b45f20 (libc.so.6:wcslen) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b270c0 (libc.so.6:strspn) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27f90 (libc.so.6:stpncpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b27f60 (libc.so.6:stpcpy) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b29860 (libc.so.6:strchrnul) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5b280b0 (libc.so.6:strncasecmp_l) redirected to 0x4a2a6e0 (_vgnU_ifunc_wrapper)
--5045-- REDIR: 0x5c17630 (libc.so.6:__strrchr_avx2) redirected to 0x4c32730 (rindex)
--5045-- REDIR: 0x5b200e0 (libc.so.6:malloc) redirected to 0x4c2faa0 (malloc)
--5045-- REDIR: 0x5c17800 (libc.so.6:__strlen_avx2) redirected to 0x4c32cf0 (strlen)
--5045-- REDIR: 0x5c13e10 (libc.so.6:__memcmp_avx2_movbe) redirected to 0x4c35e00 (bcmp)
--5045-- REDIR: 0x5bf2fd0 (libc.so.6:__strcmp_ssse3) redirected to 0x4c33da0 (strcmp)
--5045-- REDIR: 0x557b280 (libstdc++.so.6:operator new(unsigned long)) redirected to 0x4c30110 (operator new(unsigned long))
--5045-- REDIR: 0x5c17d40 (libc.so.6:__memcpy_avx_unaligned_erms) redirected to 0x4c366e0 (memmove)
--5045-- REDIR: 0x5b230a0 (libc.so.6:calloc) redirected to 0x4c31a70 (calloc)
--5045-- REDIR: 0x5579390 (libstdc++.so.6:operator delete(void*)) redirected to 0x4c311d0 (operator delete(void*))
--5045-- REDIR: 0x5b209c0 (libc.so.6:free) redirected to 0x4c30cd0 (free)
--5045-- REDIR: 0x557b340 (libstdc++.so.6:operator new[](unsigned long, std::nothrow_t const&)) redirected to 0x4c30ac0 (operator new[](unsigned long, std::nothrow_t const&))
--5045-- REDIR: 0x5c181c0 (libc.so.6:__memset_avx2_unaligned_erms) redirected to 0x4c365d0 (memset)
--5045-- REDIR: 0x5c17d20 (libc.so.6:__mempcpy_avx_unaligned_erms) redirected to 0x4c37130 (mempcpy)
graphsize: 4

--5045-- REDIR: 0x5c13690 (libc.so.6:__memchr_avx2) redirected to 0x4c33f80 (memchr)
Factor 0: PriorFactor on x0
  prior mean:  R: [
--5045-- REDIR: 0x5c17440 (libc.so.6:__strchrnul_avx2) redirected to 0x4c37020 (strchrnul)
	0.799904, -0.564527, 0.203624;
	0.552537, 0.825202, 0.117236;
	-0.234214, 0.0187323, 0.972005
]
t:    -91.75 -0.475694      -2.2
isotropic dim=6 sigma=0.01

Factor 1: PriorFactor on p2
  prior mean:  : 0.301902 0.151751 0.941184  33.4388
  noise model: diagonal sigmas[0.785398163; 0.785398163; 5];

OrientedPlane3Factor Factor on p1
Measured Plane : 0.0638967294 0.0755284627  0.995094297   2.55956073
isotropic dim=3 sigma=0.0451801

OrientedPlane3Factor Factor on p2
Measured Plane :   0.104902077 -0.0275756528   0.994100165    1.32765088
isotropic dim=3 sigma=0.0322889

CAPTURED THE EXCEPTION: 8070450532247928833
There were no test failures
==5045== 
==5045== FILE DESCRIPTORS: 3 open at exit.
==5045== Open file descriptor 2: /dev/pts/0
==5045==    <inherited from parent>
==5045== 
==5045== Open file descriptor 1: /dev/pts/0
==5045==    <inherited from parent>
==5045== 
==5045== Open file descriptor 0: /dev/pts/0
==5045==    <inherited from parent>
==5045== 
==5045== 
==5045== HEAP SUMMARY:
==5045==     in use at exit: 1,552 bytes in 1 blocks
==5045==   total heap usage: 1,170 allocs, 1,169 frees, 154,427 bytes allocated
==5045== 
==5045== Searching for pointers to 1 not-freed blocks
==5045== Checked 335,480 bytes
==5045== 
==5045== 1,552 bytes in 1 blocks are still reachable in loss record 1 of 1
==5045==    at 0x4C30B2F: operator new[](unsigned long, std::nothrow_t const&) (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==5045==    by 0x518DCEE: boost::pool<boost::default_user_allocator_new_delete>::malloc_need_resize() (in /home/mcamurri/git/libraries/gtsam/build/gtsam/libgtsam.so.4.1.0)
==5045==    by 0x518C8A8: gtsam::Values::tryInsert(unsigned long, gtsam::Value const&) (in /home/mcamurri/git/libraries/gtsam/build/gtsam/libgtsam.so.4.1.0)
==5045==    by 0x518CABC: gtsam::Values::insert(unsigned long, gtsam::Value const&) (in /home/mcamurri/git/libraries/gtsam/build/gtsam/libgtsam.so.4.1.0)
==5045==    by 0x123137: void gtsam::Values::insert<gtsam::Pose3>(unsigned long, gtsam::Pose3 const&) (in /home/mcamurri/git/libraries/gtsam/build/gtsam/slam/tests/testOrientedPlane3Factor)
==5045==    by 0x11E539: OrientedPlane3Factorlm_translation_errorTest::run(TestResult&) (in /home/mcamurri/git/libraries/gtsam/build/gtsam/slam/tests/testOrientedPlane3Factor)
==5045==    by 0x12A821: TestRegistry::run(TestResult&) (in /home/mcamurri/git/libraries/gtsam/build/gtsam/slam/tests/testOrientedPlane3Factor)
==5045==    by 0x11AF45: main (in /home/mcamurri/git/libraries/gtsam/build/gtsam/slam/tests/testOrientedPlane3Factor)
==5045== 
==5045== LEAK SUMMARY:
==5045==    definitely lost: 0 bytes in 0 blocks
==5045==    indirectly lost: 0 bytes in 0 blocks
==5045==      possibly lost: 0 bytes in 0 blocks
==5045==    still reachable: 1,552 bytes in 1 blocks
==5045==         suppressed: 0 bytes in 0 blocks
==5045== 
==5045== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)
==5045== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)

If I recompile with TBB, then I get 3 errors (which I spare you).

@ProfFan
Copy link
Collaborator

ProfFan commented Oct 12, 2020

Eigen's Debug mode run time bound checks are not helpful here because the code path is completely different under Release. I would suggest an uninitialized variable (which is the nastiest since both valgrind and ASAN cannot detect). I have work on my hand so I can't investigate so this is a pointer for anyone who's working on this.

@dellaert
Copy link
Member

It fails under Debug, too, though... Maybe that's a hint?

@jlblancoc
Copy link
Member

Running the new test in a debug build (u18.04, g++7) with valgrind gives no suspicious memory access. Neither as is in git, nor uncommenting the p2_prior factor. In that latter case the test failed, of course, but it looks more like a logic or math problem than an uninitialized/corrupted memory issue.

@jlblancoc
Copy link
Member

The augmented information matrix on p1-x0 has large (but not absurdly large) negative eigenvalues, btw...

eig(I)
ans =

  -2.2134e+01
  -1.3863e+01
  -1.5577e+00
  -6.6390e-13
  -5.6631e-15
   2.1241e-16
   6.6629e-16
   2.2134e+01
   3.5339e+01
   1.9767e+03

@dellaert
Copy link
Member

Hmmm. Marco, I propose you print out the singular values of the entire information matrix: it should be positive definite. If not, we should figure out why. Don’t use the augmented part if you use the method @jlblancoc refers to, just the top-left block, omitting the last row and column.

@dwisth
Copy link
Contributor

dwisth commented Oct 18, 2020

Hi Frank,

Thanks again for the feedback.

I added the following code to your example to calculate the singular values (I didn't have permission to push to the branch):

Matrix info = graph.linearizeToHessianFactor(initialEstimate)->information();
Eigen::JacobiSVD<Matrix> svd(info, Eigen::ComputeThinU);
std::cout << "Singular Values:\n" << svd.singularValues().transpose() << std::endl;
std::cout << "Matrix U:\n" << svd.matrixU() << std::endl;

I also added the GaussNewtonParams to the optimizer as you mentioned here.

Please let me know if this isn't the best way to calculate this an I can run it again.

Without the priors on the planes the output is:

errorThreshold: 1.34864e-26 <? 0
absoluteDecrease: -6.93331028744e-27 <? 1e-05
relativeDecrease: -1.05802556953 <? 1e-05
iterations: 2 >? 100
Singular Values:
  7227856.57593   3900046.66513    11554.603727   11447.8952301   10004.1893614           10000     9999.992414   9997.42007424   876.885178384   463.629436563  0.117115818303 0.0584303566143
Matrix U:
 3.05749155202e-08  0.000203902807992     0.043944479922  -0.00133561731752   0.00327181364483 -8.13856900573e-14 -1.86582853059e-05 -3.16663230232e-06     0.106971160401     0.993284210451  1.48677042265e-05 -1.61163266599e-06
 0.000150569143325     0.999874039535 -1.04508237136e-05  7.61870498253e-06  1.96616620193e-05 -5.28274821756e-13 -0.000639944543154   -0.0112168372323 -2.19274190229e-05 -0.000202567354402   0.00101435534489   -0.0111617400842
 1.68764240573e-06    0.0112070304694 -1.54012737984e-05 -0.000478990846787  2.14200557276e-05 -2.35713441824e-15 -6.83693024657e-06 -0.000125797549625 -1.13078314566e-05  1.85379398803e-06   -0.0904992302243     0.995833342313
    0.998750564567 -0.000279062238723  -0.00425225577754  6.80738691313e-05  0.000173737506859  5.12382147205e-13  0.000621406264083   -0.0115045141176    0.0468019236689  -0.00485248849205   -0.0114597193946     -0.00104093244
  -0.0472386502513  1.31983933644e-05   -0.0899197346123   0.00123405396733   0.00322719142037  7.02600019382e-14 -4.84575065675e-05   0.00054336790131       0.9895172693    -0.102596268418  0.000540122053905  5.96455652937e-05
   0.0115189560064 -3.21851829849e-06 -1.36086058062e-05 -0.000964913738444 -2.21415804757e-05  5.60199194881e-15  6.83580815522e-06  -0.00013260883655 -5.94154734593e-07 -1.53174371527e-05     0.995829503712    0.0904984175442
  7.7307419102e-05  7.43572915294e-05     0.813574265572      0.56837703658    0.0920197314187 -1.66783091651e-12 -0.000641683245406   0.00681276961955    0.0684733214383   -0.0429068723044  0.000533004251426  0.000333304664153
 0.000107986943198  0.000100831065696    -0.568317890614     0.820638180575   0.00663674256352  9.05842138274e-13 -0.000200109211003   0.00924311059846   -0.0494189235086    0.0315470359615  0.000746807763752  0.000453070406121
-5.16349440162e-06 -1.24301092223e-05   -0.0712522635495   -0.0580178575669     0.995706232728 -4.60585274124e-12  -0.00573296628998   0.00104517090576  -0.00956546816723  0.000824554774567  -2.9987268781e-05 -5.30283529548e-05
  0.00121014453295  0.000717594935947 -1.79246945802e-05  -0.00104230353744  -0.00227821376876     0.925362353338    -0.369407708441    0.0850541215878  7.54385827748e-08  7.23604838035e-08 -1.15573562131e-06 -4.27268452632e-07
 -0.00031795517522  0.000848714700754 -5.85658021976e-06 -7.56605307881e-05    0.0053213985507     0.368867026621     0.929190529936    0.0226479865482  8.05147599797e-08 -2.73422722084e-08  4.01664135017e-07 -4.24897565619e-07
    0.011468542568    0.0111775125185 -0.000214457843987   -0.0113527533003  -0.00166198509197   -0.0874164269234    0.0104300936341     0.995922398508  1.13831276344e-06  6.50610106653e-07 -1.05393644222e-05  -6.3158476992e-06

With a prior only on p1:

converged
errorThreshold: 0.0630133707 <? 0
absoluteDecrease: 1.08871245352e-13 <? 1e-05
relativeDecrease: 1.72774832075e-12 <? 1e-05
iterations: 3 >? 100
Singular Values:
 7227856.57593  3900048.28587  11554.6068581   11447.895233  10004.1893787          10000  9999.99241466  9997.42027821  876.903800868  465.228801703 0.118107841678 0.097636936477
Matrix U:
  3.0574930447e-08  0.000203902808013    0.0439508961498  -0.00133577499349   0.00327236228026 -9.17320066784e-14 -1.86613148709e-05  -3.1665143594e-06     0.107386723055     0.993239082816  1.48655755096e-05  1.07834713144e-06
 0.000150569216665     0.999874039638 -1.04521337561e-05  7.61875257792e-06  1.96621800382e-05 -8.80834660302e-13 -0.000639995035471   -0.0112168298853  -2.2012160763e-05 -0.000202558084258   0.00300614452676    -0.010797052687
 1.68764285858e-06    0.0112070259271 -1.54008947176e-05  -0.00047899254574  2.14201111227e-05 -6.11941717528e-15 -6.83636036435e-06 -0.000125777584024 -1.13077011445e-05  1.85637452258e-06    -0.268203721303     0.963296901437
    0.998750564567 -0.000279062258366   -0.0042522533701  6.80700019074e-05  0.000173738994954  8.60528572836e-13  0.000621354215108   -0.0115045169203    0.0467998922282  -0.00487204416798   -0.0110853912383  -0.00308590464483
  -0.0472386502513   1.3198394294e-05   -0.0899196835801   0.00123397216971   0.00322720977533  4.31728603128e-14 -4.84550602443e-05  0.000543368296285     0.989474319248    -0.103009714795  0.000520576887842  0.000155857944773
   0.0115189560064 -3.21851852506e-06 -1.36077564132e-05 -0.000964913750218 -2.21415767839e-05  9.43369315503e-15  6.83520758479e-06 -0.000132608868809 -5.97663041291e-07 -1.52665262073e-05     0.963293271661     0.268202231189
  7.7307419105e-05  7.43572605427e-05      0.81357354761     0.568377768558    0.0920195920786 -2.02654104557e-12 -0.000641649017657   0.00681277769308    0.0684547864806   -0.0429406494304  0.000464334894702  0.000423770267717
 0.000107986943202  0.000100831023677    -0.568318455404     0.820637668631   0.00663684588997  7.92409850119e-13 -0.000200067785724   0.00924311193524   -0.0494052946495    0.0315714941146  0.000653096069585  0.000580050748984
-5.16349440211e-06 -1.24301040434e-05   -0.0712520636577   -0.0580179259469     0.995706243123 -7.79520704277e-12  -0.00573293329634   0.00104525191259   -0.0095651346898  0.000828450376876 -1.99563658608e-05 -5.75586996912e-05
  0.00121014453298  0.000717594636806 -1.79237957276e-05  -0.00104230355817  -0.00227820796801     0.925362353133    -0.369407324212     0.085055792746  7.54717451094e-08  7.23541449287e-08 -1.05965350122e-06 -6.29471514346e-07
-0.000317955175187  0.000848714347192 -5.85650136757e-06 -7.56605432021e-05   0.00532137086733     0.368867027137     0.929190632338    0.0226437829842  8.05026446805e-08 -2.73816196514e-08  4.71963816561e-07 -3.47137775699e-07
   0.0114685425685      0.01117750786 -0.000214447996293   -0.0113527535721  -0.00166204050134   -0.0874164269179      0.01043459941     0.995922351269  1.13861268671e-06  6.50376973562e-07 -9.22562952533e-06 -8.12818531459e-06

With a prior on both p1 and p2:

converged
errorThreshold: 0.136903693 <? 0
absoluteDecrease: 2.94042568072e-13 <? 1e-05
relativeDecrease: 2.1478059549e-12 <? 1e-05
iterations: 3 >? 100
/home/david/git/gtsam/gtsam/slam/tests/testOrientedPlane3Factor.cpp:240: Failure: "expected 0 but was: 0.13690369346500605" 
Singular Values:
  7227858.19665   3900048.28587   11554.6199972   11447.8952356   10004.1893956           10000   9999.99241529   9997.42049326   878.494607302   465.245975706  0.157325516372 0.0986290479126
Matrix U:
 3.05749086567e-08  0.000203902808013    0.0439507914704  -0.00133569922174   0.00327238046304 -8.84373407917e-14 -1.86613463037e-05 -3.16641055135e-06     0.106989278445     0.993281977841  1.48179572848e-05 -1.61249565849e-06
 0.000150569109526     0.999874039638 -1.04521298542e-05   7.6187360439e-06  1.96628462572e-05 -7.70381835845e-13 -0.000639944308141   -0.0112168327793 -2.19313596594e-05 -0.000202566831135   0.00101416696134   -0.0111617524172
  1.6876416577e-06    0.0112070259271  -1.5400069078e-05 -0.000478992571968  2.14201153885e-05 -4.94257902279e-15 -6.83579111928e-06 -0.000125777613802 -1.12893257227e-05  1.84808714787e-06   -0.0904824397435     0.995834868107
    0.998750564626 -0.000279062122488  -0.00425289384695  6.80717321311e-05  0.000173765252851   7.4792136335e-13  0.000621405968666   -0.0115045117644    0.0468017832124  -0.00485328346898   -0.0114597339476  -0.00104073962426
  -0.0472386502541   1.3198387867e-05   -0.0899332244992     0.001234007912   0.00322775093607  5.18613717093e-14 -4.84606423781e-05  0.000543368026424     0.989514300024    -0.102613063915  0.000540128626446  5.96178426612e-05
   0.0115189534872 -3.21851565308e-06 -1.36061678914e-05  -0.00096491716022 -2.21416967848e-05  8.19249379072e-15  6.83466921356e-06 -0.000132587826896 -5.93287005414e-07 -1.52677435235e-05     0.995831029468    0.0904816271279
  7.7307401743e-05  7.43572605623e-05     0.813571573724     0.568379157644    0.0920198780335 -1.91076346742e-12 -0.000641679794524   0.00681278021421    0.0684828465973   -0.0429142975077  0.000533013714559   0.00033329546665
  0.00010798691895  0.000100831023704    -0.568319132496     0.820636698274   0.00663663887277   8.2963460539e-13 -0.000200108254506   0.00924311158016   -0.0494257961254     0.031552477011  0.000746816304715  0.000453060102503
-5.16349324192e-06 -1.24301040447e-05   -0.0712521378995   -0.0580180437144     0.995706216264 -6.77714344402e-12  -0.00573291923612   0.00104528444433  -0.00956698616148  0.000824768609232 -2.99882940619e-05 -5.30278554026e-05
   0.0012101442612  0.000717594637112 -1.79219972615e-05  -0.00104230360176   -0.0022782060513     0.925362353198    -0.369407708743    0.0850541220102  7.54642914084e-08  7.23822983471e-08 -1.15782410858e-06 -4.28713338982e-07
-0.000317955103855  0.000848714347112  -5.8563302783e-06 -7.56605503577e-05   0.00532135215213     0.368867026973     0.929190530089    0.0226479854579  8.05407923734e-08 -2.73520097114e-08  4.02370554863e-07 -4.26347525692e-07
   0.0114685399925    0.0111775078629 -0.000214428236363   -0.0113527540638  -0.00166209918326   -0.0874164269195    0.0104300954127     0.995922398378  1.13869475423e-06  6.50799939747e-07  -1.0558491634e-05 -6.33726113327e-06

@dellaert
Copy link
Member

dellaert commented Oct 19, 2020 via email

@dwisth dwisth mentioned this issue Jan 19, 2021
Merged
@dwisth
Copy link
Contributor

dwisth commented Apr 5, 2021

Just realised this issue was still open - it was resolved in #680.

@dwisth dwisth closed this as completed Apr 5, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
5 participants
@ProfFan @jlblancoc @dwisth @dellaert @mcamurri