cogrob / distributed-mapper Goto Github PK

This library is an implementation of the algorithm described in Distributed Trajectory Estimation with Privacy and Communication Constraints: a Two-Stage Distributed Gauss-Seidel Approach.

Home Page: https://cognitiverobotics.github.io/distributed-mapper/

CMake 6.04% C++ 89.84% MATLAB 2.99% Dockerfile 1.13%

distributed-mapper's Introduction

Distributed-Mapper

This library is an implementation of the algorithm described in Distributed Trajectory Estimation with Privacy and Communication Constraints: a Two-Stage Distributed Gauss-Seidel Approach. The core library is developed in C++ language.

Distributed-Mapper is developed by Siddharth Choudhary, Luca Carlone, Carlos Nieto and John Rogers as part of the collaboration between Georgia Tech, MIT and Army Research Lab.

Prerequisites

CMake (Ubuntu: sudo apt-get install cmake), compilation configuration tool.
Boost (Ubuntu: sudo apt-get install libboost-all-dev), portable C++ source libraries.
GTSAM develop branch, a C++ library that implement smoothing and mapping (SAM) framework in robotics and vision. Here we use factor graph implementations and inference/optimization tools provided by GTSAM. This repository has been tested on the latest develop branch of GTSAM 4.0 (Commit ID: d304358deeaa4625cf24a8e0d94145bb3435d5bc).
To install a particular commit of GTSAM follow the following instructions:

$ git clone https://github.com/borglab/gtsam.git && cd gtsam
$ git checkout d304358deeaa4625cf24a8e0d94145bb3435d5bc -b dist-mapper
$ mkdir build && cd build
$ cmake ..
$ make -j$(nproc)
$ sudo make install

Compilation & Installation

In the distributed_mapper_core/cpp folder excute:

$ mkdir build
$ cd build
$ cmake ..
$ make -j3
$ make check  # optional, run unit tests
$ make install

Run Experiments On Simulated Block World data

In the distributed_mapper_core/cpp/build folder, run:

$ make testDistributedMapper.run

To plot per-robot graph errors, convergence and other statistics: change the variable nrRobots and filename in matlab/plotTrace.m and run it. filename corresponds to the tracefile name mentioned in the unit test. See plotTrace.m for an example.

Run Distributed Mapper on a dataset

In the distributed_mapper_core/cpp/build/ folder, run:

$ ./runDistributedMapper --nrRobots <num_robots> --dataDir <data_directory>

For example:

$ ./runDistributedMapper --nrRobots 2 --dataDir ../../data/example_2robots/

$ ./runDistributedMapper --nrRobots 4 --dataDir ../../data/example_4robots/
$ ./runDistributedMapper --nrRobots 9 --dataDir ../../data/example_9robots/
$ ./runDistributedMapper --nrRobots 16 --dataDir ../../data/example_16robots/
$ ./runDistributedMapper --nrRobots 25 --dataDir ../../data/example_25robots/
$ ./runDistributedMapper --nrRobots 36 --dataDir ../../data/example_36robots/
$ ./runDistributedMapper --nrRobots 49 --dataDir ../../data/example_49robots/

Data Format

Each robot's graph is written in g2o format and is indexed from 0. For example, for a 4 robot scenario, the directory will contain 0.g2o, 1.g2o, 2.g2o and 3.g2o. An example dataset for 4 robots is given in data/example_4robots. Each robot is specified using a character prefix symbol like 'a', 'b', 'c', 'd' for 4 robot case.

Vertices

All the vertices corresponding to the first robot will be prefixed using 'a' using gtsam.Symbol like gtsam.Symbol('a', 1), gtsam.Symbol('a',2) etc. Similarly the second robot will be prefixed using 'b' like gtsam.Symbol('b', 1), gtsam.Symbol('b',2) etc. For example, the vertices for the first robot (in 0.g2o) in 4 robot scenario is written as,

VERTEX_SE3:QUAT 6989586621679009792 0.324676 0.212487 0.042821 0.00270783 0.0121983 0.00760222 0.999893
VERTEX_SE3:QUAT 6989586621679009793 0.0716917 2.00724 -0.0729262 -0.00363348 0.00166876 0.00765756 0.999963
VERTEX_SE3:QUAT 6989586621679009796 1.99449 0.184786 -0.0642561 0.0125092 0.0130271 0.00220908 0.999834
VERTEX_SE3:QUAT 6989586621679009797 1.93989 1.89999 0.143294 0.00127135 0.0167209 0.0057457 0.999843
VERTEX_SE3:QUAT 6989586621679009808 0.487317 0.0604815 2.01166 0.00685618 0.00528219 -0.00553837 0.999947
VERTEX_SE3:QUAT 6989586621679009809 -0.351155 1.94853 2.14991 -0.000449498 -0.00170132 -0.00501099 0.999986
VERTEX_SE3:QUAT 6989586621679009812 1.95844 0.179349 2.08246 -0.0015633 -0.00662232 0.00133146 0.999976
VERTEX_SE3:QUAT 6989586621679009813 2.01223 2.04334 1.72038 -0.0031675 0.00127498 0.00478204 0.999983
VERTEX_SE3:QUAT 6989586621679009824 0.375458 0.121848 3.97795 -0.00206729 0.00230435 0.00676051 0.999972
VERTEX_SE3:QUAT 6989586621679009825 0.0907182 1.8793 3.8406 0.00151304 0.000763284 0.00242012 0.999996
VERTEX_SE3:QUAT 6989586621679009828 1.78298 -0.141432 3.93967 -0.00459483 -0.0122962 -0.00755554 0.999885
VERTEX_SE3:QUAT 6989586621679009829 1.80735 2.22278 3.68278 0.00296836 -0.000200073 -0.0104751 0.999941
VERTEX_SE3:QUAT 6989586621679009840 0.157452 -0.255627 5.88308 0.00907326 0.0300276 0.0308352 0.999032
VERTEX_SE3:QUAT 6989586621679009841 0.021695 1.93194 5.81483 0.00423999 0.00464847 -0.00542489 0.999965
VERTEX_SE3:QUAT 6989586621679009844 1.8204 -0.431461 6.01878 0.00230292 -0.00198441 0.00720772 0.999969
VERTEX_SE3:QUAT 6989586621679009845 1.92566 2.23857 6.19073 0.0152246 0.0101054 -0.00845407 0.999797

Intra-Robot Edges

All the non-communication edges corresponding to the first robot is written in g2o format in 0.g2o and likewise for the other robots in 1.g2o, 2.g2o, 3.g2o respectively. For example, the intra-robot edges for the first robot (in 0.g2o) in 4 robot scenario is written as,

EDGE_SE3:QUAT 6989586621679009792 6989586621679009793 -0.390787 2.11308 0.0155589 0.00042254 -0.00328797 -0.010647 0.999938 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009792 6989586621679009796 2.30676 0.546708 0.0334878 -0.00546279 -0.00389958 0.00693602 0.999953 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009793 6989586621679009797 2.05794 0.233082 -0.181741 0.00845604 -0.0123624 -0.00407352 0.99988 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009796 6989586621679009797 -0.110238 1.98252 -0.0695641 0.0111376 -0.00568388 0.00216275 0.999919 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009792 6989586621679009808 0.0306595 -0.250865 1.76544 0.0014922 0.00916009 0.00622605 0.999938 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009808 6989586621679009809 -0.322983 2.44921 0.147856 -0.00149178 -0.0108401 -0.0127671 0.999859 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009793 6989586621679009809 -0.342576 -0.0919789 2.2182 -0.00111701 -0.0100567 -0.0180511 0.999786 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009808 6989586621679009812 2.09196 -0.0435013 0.159225 0.00238566 -0.00721739 -0.0031607 0.999966 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009796 6989586621679009812 0.104281 -0.198205 1.94937 -0.0269272 -0.009021 -0.0134929 0.999506 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009809 6989586621679009813 1.83031 -0.0480742 0.120586 -0.00384726 0.000411292 0.00880903 0.999954 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009812 6989586621679009813 -0.284145 2.13387 0.136647 0.00681912 -0.000686726 -0.013227 0.999889 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009797 6989586621679009813 0.161651 0.115788 2.15257 0.00367834 -0.0131576 -0.00764767 0.999877 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009808 6989586621679009824 0.0574064 -0.0929209 2.07686 -0.00229244 0.0244892 0.0109378 0.999638 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009824 6989586621679009825 -0.169446 1.88482 -0.0151826 0.0141175 -0.000934893 -0.00330174 0.999894 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009809 6989586621679009825 -0.19564 -0.128205 2.1041 0.0177383 0.0125375 -0.00137839 0.999763 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009824 6989586621679009828 2.28126 0.0993667 0.0165661 0.0171145 0.00232563 0.00442653 0.999841 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009812 6989586621679009828 -0.309201 0.0866565 2.02059 0.00138903 0.0162411 -0.0135298 0.999776 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009825 6989586621679009829 1.73039 -0.35086 -0.0727618 -0.00615263 0.00433336 -0.00495048 0.999959 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009828 6989586621679009829 -0.0416471 1.69897 0.361948 -0.0130913 0.0039124 -0.00542152 0.999892 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009813 6989586621679009829 0.181719 -0.230844 2.25151 0.00432985 0.010696 -0.00106693 0.999933 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009824 6989586621679009840 0.0508859 -0.0305667 1.88852 0.00665235 -0.00531186 0.0022339 0.999961 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009840 6989586621679009841 -0.0479563 2.1842 -0.181376 0.0040515 0.00743545 0.00587749 0.999947 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009825 6989586621679009841 -0.168767 0.388688 1.95062 -0.00167067 0.0121836 0.00128056 0.999924 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009840 6989586621679009844 1.90164 -0.19386 -0.0948216 -0.0140169 0.0117721 -0.00722882 0.999806 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009828 6989586621679009844 -0.164132 0.0202259 2.2168 -0.00906032 0.00422716 -0.00577513 0.999933 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009841 6989586621679009845 2.12955 0.209913 0.264619 -0.00740909 0.0158267 -0.0160603 0.999718 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009844 6989586621679009845 -0.121132 2.05434 0.0717287 -0.00141595 0.000218583 -0.00451449 0.999989 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009829 6989586621679009845 -0.00151322 0.142061 1.90749 0.0233327 0.00108301 -0.00310063 0.999722 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

Inter-Robot Communication Edges

Communication edges between the two robots are written in the g2o files corresponding to both the robots. For example, the communication edges between the first and second robot (in 0.g2o and 1.g2o) in 4 robot scenario is written as,

EDGE_SE3:QUAT 6989586621679009793 7061644215716937730 0.160437 1.90624 -0.00931847 0.00910267 0.0069412 0.000641041 0.999934 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009797 7061644215716937734 -0.00807974 2.13944 0.0337605 -0.0165965 -0.0143173 0.00265868 0.999756 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009809 7061644215716937746 -0.111132 1.72969 0.0728423 -0.00874126 -0.000574261 -0.0014352 0.999961 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009813 7061644215716937750 -0.227448 2.1286 -0.00255201 0.0131718 0.00877742 0.001412 0.999874 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009825 7061644215716937762 0.111804 2.11273 -0.0676793 0.0143307 0.0173408 -0.00356963 0.999741 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009829 7061644215716937766 -0.0236693 1.74212 0.0115127 0.00212283 0.00521578 0.00242397 0.999981 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009841 7061644215716937778 0.0934036 2.00091 0.0652181 0.00253681 -0.00700303 0.00387134 0.999965 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6989586621679009845 7061644215716937782 0.145824 2.18015 -0.265128 0.000747624 0.00244069 -0.00381022 0.999989 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

Questions & Bug reporting

Please use Github issue tracker to report bugs. For other questions please contact Siddharth Choudhary.

Acknowledgements

This work was partially funded by the ARL MAST CTA Project 1436607 “Autonomous Multifunctional Mobile Microsystems”.

Citing

If you use this work, please cite any of the following publications:

@inproceedings{Choudhary16icra,
  author    = {Siddharth Choudhary and
               Luca Carlone and
	       Carlos Nieto and
	       John Rogers and
               Henrik I. Christensen and
               Frank Dellaert},
  title     = {Distributed Trajectory Estimation with Privacy and Communication Constraints:
a Two-Stage Distributed Gauss-Seidel Approach},
  booktitle = {IEEE International Conference on Robotics and Automation 2016},
  year      = {2016}
}

@article{Choudhary17arXiv,
  author    = {Siddharth Choudhary and
               Luca Carlone and
               Carlos Nieto{-}Granda and
               John G. Rogers III and
               Henrik I. Christensen and
               Frank Dellaert},
  title     = {Distributed Mapping with Privacy and Communication Constraints: Lightweight
               Algorithms and Object-based Models},
  journal   = {CoRR},
  volume    = {abs/1702.03435},
  year      = {2017},
  url       = {http://arxiv.org/abs/1702.03435},
}

License

Distributed-Mapper is released under the BSD license, reproduced in the file LICENSE in this directory.

distributed-mapper's People

Contributors

Stargazers

Watchers

Forkers

llcc343 caomw repo-fork rzel pangzilu boroson lajoiepy hzm8341 akashsharma02 oriondream sysuxyt metavai muchensun leoli9901 yeweihuang hjx-zju danmcgann maximilityl

distributed-mapper's Issues

Representing Landmarks in g2o

Hi @itzsid,

I am trying to figure out how to optimize the landmarks while running distributed mapper. Upon reviewing your paper, (https://itzsid.github.io/publications/Cieslewski18icra.pdf), there does not seem to be any mention of how the landmark positions and the map were updated through DOpt. It seems that, by Prof. Cyrill's lecture (https://www.youtube.com/watch?v=mZBdPgBtrCM), you can actually add landmarks as separate nodes/edges in your pose graph, but there isn't documentation in g2o detailing that adding landmarks is valid. Moreover, you only seem to consider inter-robot and intra-robot communication edges in distributed-mapper, upon looking at this repository's README.

I'm curious to know how map updates were done in your paper, and if distributed-mapper is the right place to be looking. Is there another package available that handles updating landmarks in Occupancy Grid maps?

Thanks for your response!
Quest2GM

Question about ChordalFactor Jacobians

Hi,thank you for the great work! @itzsid
I have some problem deriding the jacobian of ChordalFactor in line. My jacobian wrt Ri is a bit different from the code,and my answer pattern is RiSRij.

  // fill in Jacobian wrt pose of key_i
  Matrix Mi = Matrix::Zero(12,6);
  Mi.block(0,0,3,3) = Ri_Rij*S1*Rijt;
  Mi.block(3,0,3,3) = Ri_Rij*S2*Rijt;
  Mi.block(6,0,3,3) = Ri_Rij*S3*Rijt;

The follows are my draft deriding the jacobian of pi and pj, the other jacobians match with the code.Could you please help me check out?

error: static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)

When I finished make and input command：
$ make check # optional, run unit tests
The program failed with some errors
@itzsid
In file included from /usr/local/include/Eigen/Core:366:0,
from /usr/local/include/Eigen/Dense:1,
from /usr/local/include/gtsam/base/OptionalJacobian.h:22,
from /usr/local/include/gtsam/base/Matrix.h:27,
from /usr/local/include/gtsam/base/Manifold.h:22,
from /usr/local/include/gtsam/base/Lie.h:25,
from /home/nature/kitti/test_1/distributed-mapper/distributed_mapper_core/cpp/src/BetweenChordalFactor.h:21,
from /home/nature/kitti/test_1/distributed-mapper/distributed_mapper_core/cpp/src/MultiRobotUtils.h:3,
from /home/nature/kitti/test_1/distributed-mapper/distributed_mapper_core/cpp/src/DistributedMapper.h:3,
from /home/nature/kitti/test_1/distributed-mapper/distributed_mapper_core/cpp/tests/testBetweenChordalFactor.cpp:7:
/usr/local/include/Eigen/src/Core/AssignEvaluator.h: In instantiation of ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Matrix<double, -1, 1>; Src = Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op, Eigen::Matrix<double, 6, 6, 0, 6, 6> >; Func = Eigen::internal::assign_op<double, double>]’:
/usr/local/include/Eigen/src/Core/PlainObjectBase.h:732:41: required from ‘Derived& Eigen::PlainObjectBase::_set_noalias(const Eigen::DenseBase&) [with OtherDerived = Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op, Eigen::Matrix<double, 6, 6, 0, 6, 6> >; Derived = Eigen::Matrix<double, -1, 1>]’
/usr/local/include/Eigen/src/Core/PlainObjectBase.h:537:19: required from ‘Eigen::PlainObjectBase::PlainObjectBase(const Eigen::DenseBase&) [with OtherDerived = Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op, Eigen::Matrix<double, 6, 6, 0, 6, 6> >; Derived = Eigen::Matrix<double, -1, 1>]’
/usr/local/include/Eigen/src/Core/Matrix.h:377:29: required from ‘Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(const Eigen::EigenBase&) [with OtherDerived = Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op, Eigen::Matrix<double, 6, 6, 0, 6, 6> >; _Scalar = double; int _Rows = -1; int _Cols = 1; int _Options = 0; int _MaxRows = -1; int _MaxCols = 1]’
/home/nature/kitti/test_1/distributed-mapper/distributed_mapper_core/cpp/tests/testBetweenChordalFactor.cpp:111:102: required from here
/usr/local/include/Eigen/src/Core/AssignEvaluator.h:833:3: error: static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)
^
tests/CMakeFiles/testBetweenChordalFactor.dir/build.make:62: recipe for target 'tests/CMakeFiles/testBetweenChordalFactor.dir/testBetweenChordalFactor.cpp.o' failed
make[3]: *** [tests/CMakeFiles/testBetweenChordalFactor.dir/testBetweenChordalFactor.cpp.o] Error 1
CMakeFiles/Makefile2:365: recipe for target 'tests/CMakeFiles/testBetweenChordalFactor.dir/all' failed
make[2]: *** [tests/CMakeFiles/testBetweenChordalFactor.dir/all] Error 2
CMakeFiles/Makefile2:284: recipe for target 'CMakeFiles/check.dir/rule' failed
make[1]: *** [CMakeFiles/check.dir/rule] Error 2
Makefile:253: recipe for target 'check' failed
make: *** [check] Error 2

I am very confused, Does anyone have the same problem as me

Multiple tests failing

Hi Sid,

I tried compiling the codebase with the latest gtsam after making some changes.
However, when I run the tests using the command specified:

make testDistributedMapper.run

I get the following errors:

/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:171: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:189: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:208: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:227: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:247: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:267: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
/home/akashsharma/Documents/projects/distributed-mapper/distributed_mapper_core/cpp/tests/testDistributedMapper.cpp:287: Failure: "Exception: Requested variable 'Z9999999' is not in this VectorValues."
There were 7 failures
make[3]: *** [tests/CMakeFiles/testDistributedMapper.run.dir/build.make:77: tests/CMakeFiles/testDistributedMapper.run] Error 7
make[2]: *** [CMakeFiles/Makefile2:459: tests/CMakeFiles/testDistributedMapper.run.dir/all] Error 2
make[1]: *** [CMakeFiles/Makefile2:466: tests/CMakeFiles/testDistributedMapper.run.dir/rule] Error 2
make: *** [Makefile:351: testDistributedMapper.run] Error 2

If the tests are resolved, I can submit a PR with my edits to support the latest GTSAM version as well.

Cannot Find GTSAM Checkout Point

Hello, when I was trying to install the Prerequisites GTSAM, there was an error when running this command:
git checkout b7c695fa71efd43b40972eec154df265617fc07d -b dist-mapper
The error is:
fatal: reference is not a tree: b7c695fa71efd43b40972eec154df265617fc07d
How to fix this problem, thanks.

confusing about test error

when I run
./runDistributedMapper --nrRobots 4 --dataDir ../../data/example_4robots/
I can get results like this:
Initial Error: 20.5862 Centralized Two Stage Error: 1.9521 Centralized Two Stage + GN Error: 1.94262 Distributed Error: 1.95323
1、I don't quite understand the meaning of these errors, does it represent the error between the final estimate and the ground truth, or is there another meaning? How can I get the ground truth?
2、If it doesn't mean the error between the final estimate and groundtruth, how should I test the error between the optimized pose and the ground truth.
3、I observed that the file only outputs the result of the whole graph, is this the result of fusing multiple robot graphs? How should I test the error from the ground truth?
Hope to get your help! Thank you very much!
@itzsid

Question about G2O file

Hi @itzsid,

I have a quick question about the g2o file example in the README for intra-robot edges. The question has to do with how these three entries align:

VERTEX_SE3:QUAT 6989586621679009792 0.324676 0.212487 0.042821 0.00270783 0.0121983 0.00760222 0.999893
VERTEX_SE3:QUAT 6989586621679009793 0.0716917 2.00724 -0.0729262 -0.00363348 0.00166876 0.00765756 0.999963
EDGE_SE3:QUAT 6989586621679009792 6989586621679009793 -0.390787 2.11308 0.0155589 0.00042254 -0.00328797 -0.010647 0.999938 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

The x values for example for these two vertices are 0.324676 and 0.0716917. But the corresponding dx value in the edge that connects them is -0.390787. Should it not be -0.25298? When we are inputting our edges, do we need to deliberately add noise the dx value (which would explain the difference)?

Thanks for your reply,
Quest2GM

Confusing about the algorithms

Hello, paper "Distributed Trajectory Estimation with Privacy and Communication Constraints: a Two-Stage Distributed Gauss-Seidel Approach" describe the two stage distrbuted pose graph optimization is based on Distributed Gauss Seidel(or JOR/SOR), the core iteration of the DGS(SOR/JOR) for solving the linear equation is equation (18) and (19) in paper page 8.
However, what's I found in the repo is, at line

  newLinearizedPoses_ = distGFG.optimize();

, a GaussianFactorGraph is adopted to solve the local linear equation with variable elimination. Then the local poses are updated by
y_(k+1) = (1-gamma)y_k+gamma y_(k+1)
with code

          linearizedPoses_.at(key) = (1-gamma_)*linearizedPoses_.at(key) + gamma_*newLinearizedPoses_.at(key);

at line

My question is, why these codes equal to function (18)-(19)?

questions about test result

Hello, I have some questions about the testing datasets.
First, for dataset in /data/example_2robots, why are the serial numbers of vertexs in both 0.g2o and 1.g2o not continuous? And how does the optimized results sort the vertexs of two robots?
Second, I have tried to make a two robot dataset as follows:
for 0.g2o:
VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 2 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 3 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 4 0 0 0 0 0 0 1
EDGE_SE3:QUAT 1 2 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 2 3 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 3 4 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 1 3 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 3 6 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 3 5 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
for 1.g2o:
VERTEX_SE3:QUAT 5 0 0 0 -0.7071067811865475 0.0 0.0 0.7071067811865475
VERTEX_SE3:QUAT 6 0 0 0 -0.7071067811865475 0.0 0.0 0.7071067811865475
VERTEX_SE3:QUAT 7 0 0 0 -0.7071067811865475 0.0 0.0 0.7071067811865475
VERTEX_SE3:QUAT 8 0 0 0 -0.7071067811865475 0.0 0.0 0.7071067811865475
EDGE_SE3:QUAT 5 6 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 6 7 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 7 8 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 3 6 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1
EDGE_SE3:QUAT 3 5 0 0 0 0 0 0 1 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

The output from terminal is
"Graph is disconnected:
Copying initial to optimized"

Looking forward to your reply! Thank you very much!

https://cognitiverobotics.github.io/distributed-mapper/ should redirect to here.

https://cognitiverobotics.github.io/distributed-mapper/ should 301 redirect to here.

Recommend Projects

React

A declarative, efficient, and flexible JavaScript library for building user interfaces.
Vue.js

🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
Typescript

TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
TensorFlow

An Open Source Machine Learning Framework for Everyone
Django

The Web framework for perfectionists with deadlines.
Laravel

A PHP framework for web artisans
D3

Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

javascript

JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
web

Some thing interesting about web. New door for the world.
server

A server is a program made to process requests and deliver data to clients.
Machine learning

Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Visualization

Some thing interesting about visualization, use data art
Game

Some thing interesting about game, make everyone happy.

Recommend Org

Facebook

We are working to build community through open source technology. NB: members must have two-factor auth.
Microsoft

Open source projects and samples from Microsoft.
Google

Google ❤️ Open Source for everyone.
Alibaba

Alibaba Open Source for everyone
D3

Data-Driven Documents codes.
Tencent

China tencent open source team.