# smooth robot 3D trajectory using graph optimization

I want to solve a trajectory smoothing problem using graph optimization. I have asked questions on how to solve this problem in a previous post and got directed towards using graph optimization.

I have, say 5 poses between points A and B. Hence there are 5 graph nodes. I know the relative poses between each graph node as well. I got a new pose for the 5th node point B'. I want to elastically move the entire trajectory with points A and B' as fixed. Please see pic here. I am stuck on how to formulate the information matrix needed for each node pair. I have not taken a robotics course and hence finding it difficult to comprehend the documents about graph-slam. Please help me with the questions below

I am planning to use ceres c++ library for pose graph optimization (especially the pose_graph_3d executable). I was trying to formulate my problem into a pose graph.

1. I have set up the nodes in the following format VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
2. I know the relative poses between the nodes. I want to setup the relative constraint between the nodes in the format

EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66

1. I am able to setup the format upto the relative position and relative quaternions for a node pair. But how to calculate the information matrix? I do not have landmarks and is not a SLAM problem.

2. I understand that the information matrix is the inverse of covariance matrix. I want to provide an equal amount of uncertainty to each node relative poses and if am correct, the flexibility of my trajectory will depend on how big is this uncertainty. But what are the variables the covariance matrix describes? Is it the relative position x y z and relative orientation euler angles rx ry rz ?

3. My understanding is that I have to setup the information matrix myself empirically. If that is so, what value can I set it to?

Edit1: I set up all the information matrices to Identity I. I then took the vertices and constraints and formulated a graph-slam problem in .g2o format. I perturbed the last node alone from B to B'. I ran the graph optimization on it, but the optimized trajectory resembled AB. Please find the unperturbed and perturbed trajectory files in the link. The same thing happens even if I set the information matrix as a multiple of I matrix (to increase the uncertainty in 6D parameters) . I used pose_graph_3d executable of the ceres library. I expected the drift to be distributed across all the nodes. Why doesn't this happen?

• I am not sure why this is a graph-slam problem. I read your first post and the answers... finding the correct position of your waypoints might be solved with a slam method, but trajectroy planning is usually not a slam problem. As the answers for your first question suggested, you can use splines to define a smooth trajectory between the waypoints. Furthermore, fitting a spline for 5 point is quite fast, so you can re-fit every time your waypoint coordinates change
– 50k4
Nov 28 '18 at 10:08
• May be 'smoothing' the trajectory was not the right word. I wanted to distribute the drift evenly across all the nodes. If you saw my picture, the trajectory AB and AB' look almost same but drifted due to the changed position of B'. Do you think thats possible with graph-slam? If am right, the uncertainty of each edge should be able to take us to B' instead of B. Dec 2 '18 at 9:09