Initial values Setting in examples.

gitouni commented 1 year ago

Dear @dongjing3309, thanks for sharing your examples about gtsam. I have some questions about initial value setting in this cpp. The graph has been built through BetweenFactor<Pose2>.

graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));

And initial values are set like this:

Values initials;
initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));

The first question is why I need to set initial values, as they can be derived from BetweenFactor given to the FactorGraph before.

The second question is how can I know the ground-truth values of BetweenFactor (or Odometry Factor) when building the FactorGraph. I tried to replace values of BetweenFactor with values totally consistent with initial values and got worse results.

  graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5.1-0.2, 0.3-(-0.3), -0.1-0.2), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(9.9-5.1, -0.1-0.3, -M_PI_2-0.2+0.1), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(10.2-9.9, -5.0+0.1, -M_PI_2+0.1-(-M_PI_2 - 0.2)), odomModel));
  graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5.1-10.2,-5.1+5.0, M_PI_2-0.1-(-M_PI+0.1)), odomModel));

  // loop closure measurement noise model
  noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));

  // Add the loop closure constraint
  graph.add(BetweenFactor<Pose2>(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));

  // print factor graph
  graph.print("\nFactor Graph:\n"); 

  // initial varible values for the optimization
  // add random noise from ground truth values
  Values initials;
  initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
  initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
  initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
  initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
  initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));

Values with 5 values:
Value x1: (gtsam::Pose2)
(-8.60007678887e-17, 7.59842037274e-16, -1.99209857654e-16)

Value x2: (gtsam::Pose2)
(4.9, 0.6, -0.3)

Value x3: (gtsam::Pose2)
(8.3175658955, -3.30944733126, -2.09969925188)

Value x4: (gtsam::Pose2)
(3.67786474175, -3.41810905958, -2.28250304384)

Value x5: (gtsam::Pose2)
(6.36035687763, -1.76061339263, 1.87372938534)

I will be very grateful for your reply.

TheSeanParker commented 4 months ago

I see you the chinese student, so you my zhihu https://zhuanlan.zhihu.com/p/695313843 maybe help you