How can i use ROS_INFO to show "graph" and "initialEstimate"?
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.