cogrob / distributed-mapper Goto Github PK

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

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)?

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?

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

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

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!

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.

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

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.

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