How can i use ROS_INFO to show "graph" and "initialEstimate"?

asked 2020-02-13 16:48:12 -0500

updated 2020-02-14 05:43:29 -0500

ct2034 gravatar image

the c++ code like this.

NonlinearFactorGraph graph;

graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n");

Values initial;

 initial.insert(1, Pose2(0.5, 0.0, 0.2));
 initial.insert(2, Pose2(2.3, 0.1, -0.2));
 initial.insert(3, Pose2(4.1, 0.1, 0.1));
 initial.print("\nInitial Estimate:\n");

if i use print

the result looks like this

Factor Graph: size: 3

Factor 0: PriorFactor on 1 prior mean: (0, 0, 0) noise model: diagonal sigmas[0.3; 0.3; 0.1];

Factor 1: BetweenFactor(1,2)
measured: (2, 0, 0) noise model: diagonal sigmas[0.2; 0.2; 0.1];

Factor 2: BetweenFactor(2,3)
measured: (2, 0, 0) noise model: diagonal sigmas[0.2; 0.2; 0.1];

Initial Estimate: Values with 3 values: Value 1: (N5gtsam5Pose2E) (0.5, 0, 0.2)

Value 2: (N5gtsam5Pose2E) (2.3, 0.1, -0.2)

Value 3: (N5gtsam5Pose2E) (4.1, 0.1, 0.1)

Final Result: Values with 3 values: Value 1: (N5gtsam5Pose2E) (7.46978322e-16, -5.34409103e-16, -1.78381865e-16)

Value 2: (N5gtsam5Pose2E) (2, -1.09236637e-15, -2.48671183e-16)

Value 3: (N5gtsam5Pose2E) (4, -1.70076058e-15, -2.50943867e-16)

How can i use ROS_INFO show the same things as mentioned above which was printed.

edit retag flag offensive close merge delete