0
$\begingroup$

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?

|improve this question
$\endgroup$
  • 1
    $\begingroup$ 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 $\endgroup$ – 50k4 Nov 28 '18 at 10:08
  • $\begingroup$ 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. $\endgroup$ – Vinmean Dec 2 '18 at 9:09
1
$\begingroup$

If your problem is small and uncertainty information is not available, you can just set it with identity matrices. Covariance or information matrix will let you set which relative pose to trust more, but it is meaningless if such information is not available.

|improve this answer
$\endgroup$
  • $\begingroup$ I tried using Identity matrix. But there was no change to the optimized trajectory. Not sure why it behaves like this. I have edited the question to provide the .g2o file which I used for graph optimization. Please have a look. $\endgroup$ – Vinmean Dec 2 '18 at 10:04
  • $\begingroup$ I have checked the file. Obviously, edge information between the start and the end is missing. It would not work without this. Find the relative transformation between the start and end node and add the following. EDGE_SE3:QUAT 19 0 ~~~~~ $\endgroup$ – C.O Park Dec 5 '18 at 1:52
  • $\begingroup$ Oh yes, thank you for pointing out. I knew the relative pose between A and B'. I used it and it worked. I now able to distribute the drift across all nodes. $\endgroup$ – Vinmean Dec 6 '18 at 3:10
  • $\begingroup$ That's good news! $\endgroup$ – C.O Park Dec 8 '18 at 4:25

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.