Issues about Robust Distributed Initialization

[chengm0-0]

Hello, your Kimera-multi is really an excellent job! I have a small question. I couldn't find the Robot Distributed Initialization section in the code. If I want to only initialize the initial pose of the robot, can I directly use the testMultirobotFrameAlign.cpp code? Looking forward to your reply!

[yunzc]

Hi @chengm0-0 Thanks for your interest in our code. The distributed initialization is handled by DPGO here.

The Multirobot Frame Align here does something similar, but in a centralized fashion.

Hope that helps!

[yuluntian]

@chengm0-0 To follow up on Yun's reply, the exact location where robust initialization happens is here, in particular, the implementation is contained in the computeRobustNeighborTransformTwoStage function.

[chengm0-0]

Thank you very much for your reply!!! I want to know if this and https://github.com/MIT-SPARK/Kimera-RPGO/blob/295c9b9a1b567973ec50d204d4722df85cde867f/include/KimeraRPGO/outlier/Pcm.h#L1007 are using the same technical route?

[yunzc]

Yes technically both do the same thing: performing robust pose averaging to obtain the initial global alignment. The main difference is that DPGO by default uses a two-stage (rotation -> translation) process and RPGO does full pose pose averaging.

[chengm0-0]

Thank you for your answers! I will test using code https://github.com/MIT-SPARK/Kimera-RPGO/blob/master/tests/testMultirobotFrameAlign.cpp first. Thanks agagin :)

[chengm0-0]

Hello, I'm sorry to bother you again. I used testMultirobotFrameAlign.cpp to align the initial coordinate systems of two robots. The initial frame of Robot 1 is (0,0,0), and the ground truth of Robot 2's initial frame is (10,10,0). However, since its true value is unknown at the beginning, we also chose it as (0,0,0) for processing. The final factor graph with 3 rounds of mutual localization was constructed as follows:

Factor 0: PriorFactor on a0
  prior mean:  (0, 0, 0)
isotropic dim=3 sigma=0.001

Factor 1: BetweenFactor(a0,b0)
  measured:  (0, 0, 0)
isotropic dim=3 sigma=100

Factor 2: BetweenFactor(a0,a1)
  measured:  (0, 0, 0)
isotropic dim=3 sigma=0.001

Factor 3: BetweenFactor(b0,b1)
  measured:  (-6, -11, 0.0635762)
isotropic dim=3 sigma=0.001

Factor 4: BetweenFactor(a1,b1)
  measured:  (4.14229, -1.05693, -0.0731904)
isotropic dim=3 sigma=0.1

Factor 5: BetweenFactor(a0,a2)
  measured:  (0, 0, 0)
isotropic dim=3 sigma=0.001

Factor 6: BetweenFactor(b0,b2)
  measured:  (-5.93705, -10.996, 0.0635963)
isotropic dim=3 sigma=0.001

Factor 7: BetweenFactor(a2,b2)
  measured:  (4.23866, -1.06357, -0.148816)
isotropic dim=3 sigma=0.1

Factor 8: BetweenFactor(a0,a3)
  measured:  (0, 0, 0)
isotropic dim=3 sigma=0.001

Factor 9: BetweenFactor(b0,b3)
  measured:  (-5.87414, -10.992, 0.0636567)
isotropic dim=3 sigma=0.001

Factor 10: BetweenFactor(a3,b3)
  measured:  (4.26937, -1.05025, -0.105281)
isotropic dim=3 sigma=0.1

The results are:

Value a0: (gtsam::Pose2)
(0, 0, 0)

Value a1: (gtsam::Pose2)
(0, 0, 0)

Value a2: (gtsam::Pose2)
(0, 0, 0)

Value a3: (gtsam::Pose2)
(0, 0, 0)

Value b0: (gtsam::Pose2)
(0, 0, 0)

Value b1: (gtsam::Pose2)
(-6, -11, 0.0635762)

Value b2: (gtsam::Pose2)
(-5.93705, -10.996, 0.0635963)

Value b3: (gtsam::Pose2)
(-5.87414, -10.992, 0.0636567)

It seems that it cannot restore b0 to its ground truth pose. May I ask if there is a problem with my handling method? Hoping for your reply. Thanks!

[yunzc]

Kimera-RPGO assumes an odometry backbone (a0->a1, a1->a2, a2->a3 for example), this is necessary in order to set up the alignment (since the poses used for robust pose averaging requires chaining measurements together). Your setup does not have such a structure, which is probably why it is not working.

[chengm0-0]

Thanks! I will try it soon.