How to obtain the output poses

[BIT-TYJ]

Thanks for your nice work! I wonder how to obtain the output poses in the examples of MultiRobotExample.cpp and SingleRobotExample.cpp?

[yuluntian]

Hello, for SingleRobotExample.cpp, the output poses are stored in the matrix X at this line. X is a block row matrix containing the agent's optimized poses: X = [R1 t1 ... Rn tn], where Ri and ti are the i-th rotation matrix and translation vector, respectively.

For MultiRobotExample.cpp, after the optimization (eg before this line), you can call PGOAgent::getTrajectoryInGlobalFrame(Matrix &Trajectory) to get each agent's output poses; see definition here. This function will store the optimized poses in the input Trajectory variable, and the format is the same as X the first paragraph.

[BIT-TYJ]

Thank you very much for your reply!

[hhhhhhhh213]

This is an amazing work. I wonder if it is possible to get the final optimized trajectory of the first robot in this way for (unsigned iter = 0; iter < numIters; ++iter) { // Perform operations for each iteration ... } Matrix globalTrajectory; if (agents[0]->getTrajectoryInGlobalFrame(globalTrajectory)) { std::cout << "Trajectory in global frame:\n" << globalTrajectory << std::endl; } else { std::cout << "Failed to get trajectory in global frame." << std::endl; } for (auto agentPtr : agents) { agentPtr->reset(); }

[yuluntian]

@hhhhhhhh213 Thanks for your interest in this work. Yes, the snippet you shared should work, for example, right after this line in the multi robot example.