Information matrix order in TORO file

[fokrey]

It appears that the information matrix order in the code example provided in FGraph_M3500.py is not following the correct format. The current implementation seems to serialize the information matrix in an incorrect order.

According to the standard serialization in the TORO and g20 formats, the information matrix should be serialized as the upper triangular part in row-major order. However, in the code, the order seems to be as follows: I11 I12 I22 I33 I13 I23. Adding information matrix in th correct order:

    # Serializing the information matrix and measurement
    self.toro_lines += f"EDGE_SE2 {origin} {target} {meas[0]:.6f} {meas[1]:.6f} {meas[2]:.6f} "
    self.toro_lines += f"{info[0, 0]:.6f} {info[0, 1]:.6f} {info[1, 1]:.6f} {info[2, 2]:.6f} {info[0, 2]:.6f} {info[1, 2]:.6f}\n"