david-m-rosen / se-sync Goto Github PK

An implementation of the SE-Sync algorithm for synchronization over the special Euclidean group.

License: GNU Lesser General Public License v3.0

MATLAB 6.01% CMake 1.37% C++ 14.59% Shell 0.11% Jupyter Notebook 77.91% Mathematica 0.01%

se-sync pose-graph-optimization pose-synchronization global-optimization semidefinite-programming riemannian-optimization pose-graph-slam graph-slam slam certifiable-robustness

se-sync's People

Contributors

Stargazers

Watchers

Forkers

efernandez olzhas kanster romateng mattgiamou jbriales hhmaizi konanrobot vbillys cindyrella weiqiao peterzhousz lajoiepy hrheydarian pantonante xiehongle keevindoherty fantaosha simple-aibluefisher yunzc xlicz lufberry jlblancoc zhangaigh marineroboticsgroup capriciouszihao pablospe daobilige-su shaolinbit doctorwk007 pipigenius plkms xiaojake rantengsky tonioteran digiamm soumics chengwei920412 hsh88 kimsoohwan dongyanchaotj dioram rqbrother mfkiwl eagleflag88 windbicycle namdinhrobotics patilnabhi entityless hkust-swarm ultrakunp qianqian121 borsh metavai neu-ral moderngangster 222233tommy leongoretzkatju huliangbing summer2474 solee7650 utiasasrl jerrymin-lzu yetingxiao neoques yangyiqiao gfchen01 qiqzhang mediumcore yjf0602 pangchenglin luanjinlu u-mac eeqmcc lurvelly ericzzj1989 kimbundu

se-sync's Issues

Memory Free issue on save

Hello David,

I've be able to run the example code, but each time on exit, the problem tries to free unallocated memory. I'm not sure if this is a big deal, or not, but some relevant information is posted below. Thanks!

Here is the result of running lldb ./C++/build/bin/SE-Sync data/CSAIL.g2o

===== END SE-SYNC =====

Saving final poses to file: poses.txt
SE-Sync(24330,0x7fff98f93380) malloc: *** error for object 0xbfd00f9944bcb218: pointer being freed was not allocated
*** set a breakpoint in malloc_error_break to debug
Process 24330 stopped
* thread #1, queue = 'com.apple.main-thread', stop reason = signal SIGABRT
    frame #0: 0x00007fff60ab6b6e libsystem_kernel.dylib`__pthread_kill + 10
libsystem_kernel.dylib`__pthread_kill:
->  0x7fff60ab6b6e <+10>: jae    0x7fff60ab6b78            ; <+20>
    0x7fff60ab6b70 <+12>: movq   %rax, %rdi
    0x7fff60ab6b73 <+15>: jmp    0x7fff60aadb00            ; cerror_nocancel
    0x7fff60ab6b78 <+20>: retq   
Target 0: (SE-Sync) stopped.
(lldb) bt
* thread #1, queue = 'com.apple.main-thread', stop reason = signal SIGABRT
  * frame #0: 0x00007fff60ab6b6e libsystem_kernel.dylib`__pthread_kill + 10
    frame #1: 0x00007fff60c81080 libsystem_pthread.dylib`pthread_kill + 333
    frame #2: 0x00007fff60a121ae libsystem_c.dylib`abort + 127
    frame #3: 0x00007fff60b10822 libsystem_malloc.dylib`free + 521
    frame #4: 0x000000010000432d SE-Sync`main [inlined] Eigen::internal::handmade_aligned_free(void*) at Memory.h:98 [opt]
    frame #5: 0x000000010000431f SE-Sync`main [inlined] Eigen::internal::aligned_free(void*) at Memory.h:179 [opt]
    frame #6: 0x000000010000431f SE-Sync`main [inlined] void Eigen::internal::conditional_aligned_free<true>(void*) at Memory.h:230 [opt]
    frame #7: 0x000000010000431f SE-Sync`main [inlined] void Eigen::internal::conditional_aligned_delete_auto<double, true>(ptr=<unavailable>) at Memory.h:416 [opt]
    frame #8: 0x000000010000431f SE-Sync`main [inlined] Eigen::DenseStorage<double, -1, -1, 1, 0>::~DenseStorage() at DenseStorage.h:542 [opt]
    frame #9: 0x000000010000431b SE-Sync`main [inlined] Eigen::DenseStorage<double, -1, -1, 1, 0>::~DenseStorage() at DenseStorage.h:542 [opt]
    frame #10: 0x000000010000431b SE-Sync`main [inlined] Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >::~PlainObjectBase() at PlainObjectBase.h:98 [opt]
    frame #11: 0x000000010000431b SE-Sync`main [inlined] Eigen::Matrix<double, -1, 1, 0, -1, 1>::~Matrix() at Matrix.h:178 [opt]
    frame #12: 0x000000010000431b SE-Sync`main [inlined] Eigen::Matrix<double, -1, 1, 0, -1, 1>::~Matrix() at Matrix.h:178 [opt]
    frame #13: 0x000000010000431b SE-Sync`main [inlined] SESync::RelativePoseMeasurement::~RelativePoseMeasurement(this=<unavailable>) at RelativePoseMeasurement.h:17 [opt]
    frame #14: 0x000000010000431b SE-Sync`main [inlined] SESync::RelativePoseMeasurement::~RelativePoseMeasurement(this=<unavailable>) at RelativePoseMeasurement.h:17 [opt]
    frame #15: 0x000000010000431b SE-Sync`main [inlined] std::__1::allocator<SESync::RelativePoseMeasurement>::destroy(__p=<unavailable>) at memory:1838 [opt]
    frame #16: 0x000000010000431b SE-Sync`main [inlined] void std::__1::allocator_traits<std::__1::allocator<SESync::RelativePoseMeasurement> >::__destroy<SESync::RelativePoseMeasurement>(__p=<unavailable>) at memory:1706 [opt]
    frame #17: 0x000000010000431b SE-Sync`main [inlined] void std::__1::allocator_traits<std::__1::allocator<SESync::RelativePoseMeasurement> >::destroy<SESync::RelativePoseMeasurement>(__p=<unavailable>) at memory:1574 [opt]
    frame #18: 0x000000010000431b SE-Sync`main [inlined] std::__1::__vector_base<SESync::RelativePoseMeasurement, std::__1::allocator<SESync::RelativePoseMeasurement> >::__destruct_at_end(__new_last=0x0000000100357000) at vector:417 [opt]
    frame #19: 0x00000001000042fe SE-Sync`main [inlined] std::__1::__vector_base<SESync::RelativePoseMeasurement, std::__1::allocator<SESync::RelativePoseMeasurement> >::clear() at vector:361 [opt]
    frame #20: 0x00000001000042fe SE-Sync`main [inlined] std::__1::__vector_base<SESync::RelativePoseMeasurement, std::__1::allocator<SESync::RelativePoseMeasurement> >::~__vector_base() at vector:444 [opt]
    frame #21: 0x00000001000042f2 SE-Sync`main [inlined] std::__1::vector<SESync::RelativePoseMeasurement, std::__1::allocator<SESync::RelativePoseMeasurement> >::~vector() at vector:450 [opt]
    frame #22: 0x00000001000042f2 SE-Sync`main [inlined] std::__1::vector<SESync::RelativePoseMeasurement, std::__1::allocator<SESync::RelativePoseMeasurement> >::~vector() at vector:450 [opt]
    frame #23: 0x00000001000042f2 SE-Sync`main(argc=<unavailable>, argv=<unavailable>) at main.cpp:40 [opt]
    frame #24: 0x00007fff60966015 libdyld.dylib`start + 1
    frame #25: 0x00007fff60966015 libdyld.dylib`start + 1

From a previous build, running lldb ./C++/build/bin/SE-Sync data/kitti_00.g2o:

* * thread #1, queue = 'com.apple.main-thread', stop reason = EXC_BAD_ACCESS (code=1, address=0x101a52ff8)
    frame #0: 0x0000000100005b2f SE-Sync`SESync::SESyncResult::~SESyncResult() [inlined] Eigen::internal::handmade_aligned_free(void*) at Memory.h:98 [opt]
   95  	/** \internal Frees memory allocated with handmade_aligned_malloc */
   96  	inline void handmade_aligned_free(void *ptr)
   97  	{
-> 98  	  if (ptr) std::free(*(reinterpret_cast<void**>(ptr) - 1));
   99  	}
   100 	
   101 	/** \internal

System: macOS 10.13.4
Compiler: clang --version

clang version 4.0.1 (tags/RELEASE_401/final)
Target: x86_64-apple-darwin17.5.0
Thread model: posix
InstalledDir: /usr/local/opt/llvm@4/bin

Debugger: lldb --version

lldb-902.0.79.2
  Swift-4.1

Where can I find the optimized pose?

I built this in vs2019 and tried running it.
I want to visualize the result through the PCL,
but I don't know how to obtain an optimized pose for each node.
How can I get an optimized transformation matrix?

my code is here

SESync::measurements_t measurements;

// add edges					
for (int p = 1; p < kfrm_storage.size(); p++)
{
	SESync::RelativePoseMeasurement measurement;
	measurement.i = p - 1;
	measurement.j = p;
	measurement.R = kfrm_storage[p].dG.block(0, 0, 3, 3);
	measurement.t = kfrm_storage[p].dG.block(0, 3, 3, 1);
	measurement.kappa = toRad(1.0);
	measurement.tau = 0.01;
	measurements.push_back(measurement);
}

// add lc
for (int p = 0; p < lc_info.size(); p++)
{
	SESync::RelativePoseMeasurement measurement;
	measurement.i = lc_info[p].idx0;
	measurement.j = lc_info[p].idx1;
	measurement.R = lc_info[p].dG.block(0, 0, 3, 3);
	measurement.t = lc_info[p].dG.block(0, 3, 3, 1);
	measurement.kappa = toRad(1.0);
	measurement.tau = 0.01;
	measurements.push_back(measurement);
}

SESync::SESyncOpts opts;
opts.verbose = false;
opts.num_threads = 1;

SESync::SESyncResult res = SESync::SESync(measurements, opts);

// update pose
for (int p = 0; p < kfrm_storage.size(); p++)
{	
	Eigen::Matrix4d new_G; new_G.setIdentity();

	new_G.block(0, 3, 3, 1) = res.xhat.block(0, (p * 4), 3, 1);
	new_G.block(0, 0, 3, 3) = res.xhat.block(0, (p * 4) + 1, 3, 3);
	kfrm_storage[p].G = new_G;
}

Python binding installation and Inference

I can install the library and run it. However, I'm facing problems when trying to setup the binding for python.

I use the anaconda environment. When I follow the installation instructions in readme, I get ModuleNotFoundError: No module named 'PySESync'.

I also tried manually set the python site lib:
set(Python3_USER_SITE_LIB "/home/vy/anaconda3/envs/habitat/lib/python3.9/site-packages/")

but this doesn't solve my problem.

Am I missing something?

Also, the library doesn't recognize FIX token in g2o file which usually stands for the nodes which are not optimized.

Finally, when running the cpp library on the g2o file, where are the results stored?

The newest/accelerating version compiled in ubuntu 20.04...

When I run "cmake..", it occurs an error in CMakeLists.txt:

CMake Error at CMakeLists.txt:73 (find_package):
By not providing "FindSPQR.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "SPQR", but
CMake did not find one.

Could not find a package configuration file provided by "SPQR" with any of
the following names:

SPQRConfig.cmake
spqr-config.cmake

Add the installation prefix of "SPQR" to CMAKE_PREFIX_PATH or set
"SPQR_DIR" to a directory containing one of the above files. If "SPQR"
provides a separate development package or SDK, be sure it has been
installed.

However, I have already installed libsqpr2, so I don't know what lib I should install to fix this problem.

Synchronizing 2D rotations in 3D space

Hi,

I am still playing around your nice work.

In the following code (MATLAB), I generated multiples of 2pi/4 as the absolute rotation angles around z axis with translation vector of all zero (actually 2D rotations but embedded in 3D matrices). Then, I computed the relative rotations of all pairs which results in 28 edges.

By using your library I expect to get those absolute rotations around the z axis as in this experiment there is no measurement noise. However, if you run the code, you will notice that the results are totally different with what is expected (the rotations are no longer around z axis).

If I redo the same experiment in 2D (xhat.R are 2xn and xhat.t are 2xn), I can exactly reproduce the ground-truth absolute angles.

Could you please tell me what is wrong here?

 % generate 8 angles spaced at multiples of 2pi/8
 step = 0:7;
 absAngD = step * 45;    % absolute angles (ground-truth)
 N = numel(step); 
 
 % compute relative orientations
 k=1;
 for i=1:N-1
     for j=i+1:N
         
         measurements.edges(k,:) = [i j];
         measurements.R{k} = rotz(absAngD(i) - absAngD(j));  % relative angles
         measurements.t{k} = [0;0;0];
         measurements.kappa{k} = 1;
         measurements.tau{k} = 1;
         k = k+1;
         
     end
 end
 
 %% SE3-Sync
 
 num_poses = max(max(measurements.edges));
 d = length(measurements.t{1});
 
 % Set Manopt options (if desired)
 Manopt_opts.tolgradnorm = 1e-2;  
 Manopt_opts.rel_func_tol = 1e-5; 
 Manopt_opts.miniter = 1;  
 Manopt_opts.maxiter = 300;  
 Manopt_opts.maxinner = 500; 
 
 % Set SE-Sync options (if desired)
 SE_Sync_opts.r0 = 5;  
 SE_Sync_opts.rmax = 10;  
 SE_Sync_opts.eig_comp_rel_tol = 1e-4;  
 SE_Sync_opts.min_eig_lower_bound = -1e-3;
 SE_Sync_opts.Cholesky = false;  
 
 use_chordal_initialization = true;  
 
 % Run SE-Sync
 
 % Pass explict settings for SE-Sync and/or Manopt, and use chordal
 % initialization
 fprintf('Computing chordal initialization...\n');
 R = chordal_initialization(measurements);
 Y0 = vertcat(R, zeros(SE_Sync_opts.r0 - d, num_poses*d));
 [SDPval, Yopt, xhat, Fxhat, SE_Sync_info, problem_data] = SE_Sync(measurements, Manopt_opts, 
 SE_Sync_opts, Y0);
 
 %% extract the final absolute rotation axes and angles
 for i=1:N
    curR = xhat.R(:,3*(i-1)+1:3*(i-1)+3);
    axangles(i,1:4) = rotm2axang(curR);
 end

Compiling on macOS

Hello,

I'm not sure if macOS is a target platform, but I wanted to post a single line tweak, if it seems ok!

The following tweak seems to be required to build:
Adding #include <random> to SESync_utils.h

The following bash commands replicate the build on 10.13.4:

brew install llvm@4
cd C++
mkdir build
cd build
env CC=$(which clang) CXX=$(which clang++) cmake ..
make

Thank you!

Where is the ground truth poses for the data folder?

Hi,

Thanks for your excellent work. I could not find the ground truthe poses for the data folder. Could you please give a link to one?

Have a nice day,
Deshun

dataset without weights

Hi,

Would it be possible to sync SE relative parameters for which there is no weighting coefficients for edges? Or what should we set for the weights in case we do not have these information?

Actually, I tried a bit to understand how these weights (measurement.kappa and measurement.tau) are computed (eq. 11 in the long report) and why there are separate weights for rotation and translation but unfortunately I could not figure it out. Could you please tell me if these weights are computed or they are obtained during acquisition?

Thanks

Question regarding SE-Sync vs. Shonan Rotation Averaging

Dear David,

Thank you so much for this amazing work. I wasn't sure where to ask this question, so I'll ask it here.

It appears to me that SE-Sync is faster and more efficient than SRA given the following reasons:

SE-Sync uses the exact hessian and uses fewer parameters in higher dimensions (pd instead of pp per each rotation matrix).
Checking the rank of the matrix (for rank-deficiency test) in SE-Sync is faster than finding the min eigenvalue of matrix C in SRA.

I was wondering if you could provide some clarification on this. I'd appreciate it very much.

Cross compile for other platforms

hi，Can I cross-compile this library on other platforms, such as NVIDIA Jetson Xavier, how to do it?

Compatibility with Cmake 2.8

Hi David,
When compiling the C++ implementation of SE-Sync I saw you require Cmake 3.1 as minimum version. However, for those still working in Ubuntu 14.04 the officially default (newest) version is Cmake 2.8.
I compiled SE-Sync with no problems after changing the minimum version to 2.8 and then changing the (newer) Cmake instruction set(CMAKE_CXX_STANDARD 11) to the older set(CMAKE_CXX_FLAGS "-std=c+11" ${CMAKE_CXX_FLAGS}).
So my question is: Are there more reasons why you would require Cmake 3.1? And, if not, should I make a pull request with these changes to support compatibility with usual Ubuntu 14.04 platforms?

Best,
Jesus

How to optimize a least square problem, where there is only one node with multiple prior factors?

Could you provide an example like curve fitting?@david-m-rosen

sometimes sesync may output incorrect results

Thank you very much for your excellent work. SeSync is efficient and accurate for pose graph optimization in most cases, but it sometimes outputs incorrect results for my input pose graph data, I am not sure is there something wrong with the input data. the adapted matlab code and c++ interface are attached. Thank you.
matlab__.zip
main.zip

the corresponding optimization results are as follows:

Loaded 1124 measurements between 259 poses from file

set opts
Constructing SE-Sync problem instance ... CHOLMOD warning: matrix not positive definite. file: d:\libs\slam\suitesparse\suitesparse\cholmod\supernodal\t_cholmod_super_numeric.c line: 911
elapsed computation time: 0.011 seconds

========= SE-Sync ==========

ALGORITHM SETTINGS:

SE-Sync settings:
SE-Sync problem formulation: Simplified
Initial level of Riemannian staircase: 5
Maximum level of Riemannian staircase: 6
Number of Lanczos vectors to use in minimum eigenvalue computation: 20
Maximum number of iterations for eigenvalue computation: 10000
Tolerance for accepting an eigenvalue as numerically nonnegative in optimality verification: 1e-05
Using Cholesky decomposition to compute orthogonal projections
Initialization method: chordal
Running SE-Sync with 4 threads

Riemannian trust-region settings:
Stopping tolerance for norm of Riemannian gradient: 0.01
Stopping tolerance for norm of preconditioned Riemannian gradient: 0.0001
Stopping tolerance for relative function decrease: 1e-07
Stopping tolerance for the norm of an accepted update step: 0.001
Maximum number of trust-region iterations: 1000
Maximum number of truncated conjugate gradient iterations per outer iteration: 10000
STPCG fractional gradient tolerance (kappa): 0.1
STPCG target q-superlinear convergence rate (1 + theta): 1.5
Preconditioning the truncated conjugate gradient method using regularized Cholesky preconditioner with maximum condition number 1e+06

INITIALIZATION:
Computing chordal initialization ... elapsed computation time: 0.168 seconds
SE-Sync initialization finished; elapsed time: 0.169 seconds

Initial objective value: -26060.9 | isnormal(not nan/inf): 1

====== RIEMANNIAN STAIRCASE (level r = 5) ======

Truncated-Newton trust-region optimization:

Iter: 0, time: 0.000e+00, f: -2.606e+04, |g|: 1.329e+04, |M^{-1}g|: -nan(ind), Delta: 1.000e+00, inner iters: 10000, |h|: -nan(ind), |h|_M: -nan(ind), df: -nan(ind), rho: -nan(ind). Step REJECTED!

Optimization finished!
Algorithm exceeded maximum allowed computation time: (6.495e+01 > 2.000e+01 seconds)
Final objective value: -2.606e+04
Norm of Riemannian gradient: 1.329e+04
Norm of preconditioned Riemannian gradient: -nan(ind)
Total elapsed computation time: 6.495e+01 seconds

Found first-order critical point with value F(Y) = -26060.9! Elapsed computation time: 64.952 seconds

Checking second order optimality ...
Saddle point detected! Minimum eigenvalue: -0.171269. Elapsed computation time: 0.045 seconds (44 matrix-vector multiplications).
Computing escape direction ...

===== END RIEMANNIAN STAIRCASE =====

WARNING: Algorithm exhausted the allotted computation time before finding global optimum!

Rounding solution ... elapsed computation time: 0.01 seconds

SDP RESULTS:
Value of dual SDP solution F(Y): -26060.9
Norm of Riemannian gradient grad F(Y): 13291
Value of primal SDP solution tr(Lambda): -26060.9
Minimum eigenvalue of certificate matrix S - Lambda: -0.171269
SDP duality gap: -1.45519e-11

SE-SYNCHRONIZATION RESULTS:
Value of rounded pose estimates F(x): -26060.9
Suboptimality bound F(x) - tr(Lambda) of recovered pose estimate: -1.0994e-08

Total elapsed computation time: 65.241 seconds

===== END SE-SYNC =====

Saving final poses to file: poses.txt

Documentation on how to start

Hi,
I would like to start off by saying thank you for providing such a great algorithm for non-linear optimization. I would like to start using Se-Sync as a SLAM backend, but am unsure on how to start.

I went to check your main.cpp, the first thing that you do is read measurements from a file ? If understand correctly, that file contains the pose graph? But in real application where would this information come from ?

Is something else suppose to be run before applying the Se-Sync ? Does it mean, that I need to use something else to get the poses before applying your algorithm ? Do I need to use for e.g g2o to create a pose graph before applying Se-Sync ?

I am new in this field so sorry if all of this is obvious.
Thank you for your time

can se-sync work with factor graph generated with GSAM ?

Hello,
Thank you for your great paper !
Few more questions about how to start.

do you have any advice on how or which library is usually used to construct pose graph ?

Is Se-Sync able to optimize ongoing pose-graph or does it need the whole graph ? I am asking because i wonder if this can be used online while constructing the pose-graph (SLAM front-end).

can se-sync work with factor graph generated with GSAM ?

Thank you

liblapack.so.3: undefined references

Problem (ubuntu 18.04):

[100%] Linking CXX executable ../bin/SE-Sync
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zgeru'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sgemaxnrm'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cgemoveT'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zcplxdivide'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_csqtrans'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sgescal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dtrscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zGetNB'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dgezero'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cgeru'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sger'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dgemaxnrm'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dGetNB'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dgescal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_ztrscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zgeset'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sgezero'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cgemaxnrm'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zgezero'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_strscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cgeset'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dgemoveT'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_ccplxdivide'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dgeset'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zcplxinvert'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zsqtrans'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dsqtrans'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_ssqtrans'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_ccplxinvert'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zgemoveT'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sgeset'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_ctrscal'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sGetNB'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_dger'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cGetNB'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_xerbla'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_sgemoveT'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_zgemaxnrm'
//usr/lib/x86_64-linux-gnu/liblapack.so.3: undefined reference to `ATL_cgezero'
collect2: error: ld returned 1 exit status

Sequence of recording vertices and edges in g2o file

Hi, @david-m-rosen, I've been using the C++ SE-Sync tool to optimize a series of poses in SE(3) recently, and discovered that the sequence that I record vertices and edges on the g2o file (I mean in terms of id) affects the optimization result. However, it seems that there is no requirement about the sequence of id here, https://github.com/RainerKuemmerle/g2o/wiki/File-Format. Could you please clarify the required recording sequence of vertices and edges in terms of their id when using SE-Sync tool? Thank you!

add prior constraint

@david-m-rosen Thanks for your great work. is it possible to add prior constraint (in this case, the optimization is formulated as a MAP-maximum a posterior problem)?

Best,
Weisong,

SESync results matrix sometimes flips

Thank you for your extraordinary work, I was attempting to use SESync for a realtime SLAM, however I am facing an issue, while running SESync periodically, the results.xhat matrix inverts values abruptly.
The SESync Result is as follows:

[ INFO] [1689674506.598137576]: SESync x: -0.709801 y: -0.002066
[ INFO] [1689674506.649726984]: SESync x: -0.709801 y: -0.002066
[ INFO] [1689674506.698200718]: SESync x: -0.734062 y: -0.000417
[ INFO] [1689674506.748467310]: SESync x: -0.734062 y: -0.000417
[ INFO] [1689674506.798415183]: SESync x: 0.001456 y: 0.759772
[ INFO] [1689674506.848358097]: SESync x: 0.001456 y: 0.759772
[ INFO] [1689674506.898409615]: SESync x: 0.001456 y: 0.759772
[ INFO] [1689674506.948645089]: SESync x: -0.783955 y: 0.002026
[ INFO] [1689674506.998515216]: SESync x: -0.783955 y: 0.002026
[ INFO] [1689674507.048347022]: SESync x: -0.783955 y: 0.002026

The x and y values get interchanged for a certain set of measurements and abruptly return to normal. The code I am using to access these values is

results = SESync::SESync(measurements, opts);  
int latest_result = results.xhat.cols()/3;      

double x ,y;
x = results.xhat.col(latest_result-1)[0];
y = results.xhat.col(latest_result-1)[1];

If one more value is added and SESync is rerun the output returns to normal. It appears to invert almost randomly. I have printed two consecutive result matrices into a google sheet here

Thanks in advance!

Question regarding prior constraints

@david-m-rosen, thank you for your work, it has really helped me improve the overall result of the map creation algorithm.
I have a question regarding prior constraints that have already been mentioned here: add prior constraints.
I have a number of edges of type g2o::EdgeSE3PriorXYZ. They are used to store the GPS data and contain XYZ coordinates and a small 3x3 information matrix. Since I know that the SE-Sync algorithm can't work with such data, I instead complete them by copying their data into SESync::RelativePoseMeasurement variables and setting the remaining data (about the orientation) to 0. However, when I try to use such edges in the SE-Sync algorithm, it it completely destroys the entire map. Here's the visual example to better understand:

That one was when I was adding such edges. And this one is without them:

As you can see, adding those edges make the map worse. But I can't NOT add them as they are needed later in other applications. So I wanted to ask your advice if there was something I programmed incorrectly? Here's the necessary code:

SESync::measurements_t read_graph_edges()
{
SESync::measurements_t measurements;
SESync::RelativePoseMeasurement measurement;

for(auto& edge : graph_slam->graph->edges())
{
  if(edge->vertices().size() == 2)
  {
    measurement.i = edge->vertices()[0]->id();
    measurement.j = edge->vertices()[1]->id();
    //std::cout << measurement.i << " " << measurement.j << std::endl;
    
    g2o::EdgeSE3* ed = dynamic_cast<g2o::EdgeSE3*>(edge);
    Eigen::Isometry3d iso = ed->measurement();
    Eigen::MatrixXd info = ed->information();
    measurement.t = iso.translation();
    measurement.R = iso.rotation();
    //std::cout << measurement.t << std::endl;
    //std::cout << measurement.R << std::endl;

    Eigen::Matrix3d TranInfo;
    TranInfo(0, 0) = info(0, 0); TranInfo(0, 1) = info(0, 1); TranInfo(0, 2) = info(0, 2);
    TranInfo(1, 0) = info(1, 0); TranInfo(1, 1) = info(1, 1); TranInfo(1, 2) = info(1, 2);
    TranInfo(2, 0) = info(2, 0); TranInfo(2, 1) = info(2, 1); TranInfo(2, 2) = info(2, 2);
    measurement.tau = 3 / TranInfo.inverse().trace();
    //std::cout << measurement.tau << " ";
    Eigen::Matrix3d RotInfo;
    RotInfo(0, 0) = info(3, 3); RotInfo(0, 1) = info(3, 4); RotInfo(0, 2) = info(3, 5);
    RotInfo(1, 0) = info(4, 3); RotInfo(1, 1) = info(4, 4); RotInfo(1, 2) = info(4, 5);
    RotInfo(2, 0) = info(5, 3); RotInfo(2, 1) = info(5, 4); RotInfo(2, 2) = info(5, 5);
    measurement.kappa = 3 / (2 * RotInfo.inverse().trace());
    //std::cout << measurement.kappa << std::endl;
    //std::cout << "\n";

    measurements.push_back(measurement);
  }
  else if(edge->vertices().size() == 1)
  {
    measurement.i = edge->vertices()[0]->id();
    measurement.j = 0;
    //std::cout << measurement.i << " " << measurement.j << std::endl;

    g2o::EdgeSE3PriorXYZ* ed = dynamic_cast<g2o::EdgeSE3PriorXYZ*>(edge);
    Eigen::Vector3d vec = ed->measurement();
    Eigen::MatrixXd info = ed->information();
    measurement.t = vec;
    measurement.R = Eigen::Matrix3d::Identity();
    //std::cout << measurement.t << std::endl;
    //std::cout << measurement.R << std::endl;

    measurement.tau = 3 / info.inverse().trace();
    //std::cout << measurement.tau << " ";
    measurement.kappa = 0;
    //std::cout << measurement.kappa << std::endl;
    //std::cout << "\n";

    measurements.push_back(measurement);
  }
}

return measurements;

}

The rest of the code is a copy of SE-Sync's main.cpp file. The else if(edge->vertices().size() == 1) part is where I work with these edges. Thanks for your help.

Unable to enter visual mode

Error compile with Pangolin-0.8

evaluate_objective.m doubt

Sir,

In the computation of trQYtY in the file evaluate_objective.m, should it be equal to trQYtY = trace(YQ * Y) instead of trQYtY = trace(YQ * Yt) ?

G2o parsing code incorrectly names a variable as covariance instead of info matrix.

G2o file format uses the information matrix (e.g., for SLAM 2D https://github.com/RainerKuemmerle/g2o/wiki/File-Format-SLAM-2D).

In the utils here, the info matrix is incorrectly named TranCov:

SE-Sync/C++/SE-Sync/src/SESync_utils.cpp

Line 69 in 546f99b

Eigen::Matrix<Scalar, 2, 2> TranCov;

which, when creating for example tau, makes it read as tau = d / tr(cov^-1), when actually we want tau = d / tr(cov).

sqrtkappa is unused

SE-Sync/C++/SE-Sync/src/SESync_utils.cpp

Line 285 in 7f7f5ad

double sqrttau, sqrtkappa;

can se sync solve BA or PnP？

The current open source code seems to only solve the problem of pose graph optimization. Can this code solve BA (bundle adjustment) and PnP problems, or can theory proposed in the paper solve BA (bundle adjustment) and PnP problems theoretically .

Which version of Pangolin should I use

Hi @david-m-rosen ,
Thanks for sharing your code.
I would like to know which version of Pangolin should I use if I want to compile the SE-SyncViz.
Thanks.