2D state example

[Titibo26]

Hi there,

Would it be possible to get an example of graph optimization with 2D state and 2D measurment and so on...
I'm trying to models my problem where each nodes is a Robot's Pos (x,y) and each nodes has an odometry constraint (dx, dy) with respective covariance matrix, and a loop closure adds an observation (tx, ty) between two nodes.
I get confused when trying to apply the given exemple in more than 1 dim. I can't manage to properly derive my own state class and observation class for 2D problems.

Thanks,

TD

[HWouters]

Instead of deriving your state from State<double> derive it from State<pos>

[HWouters]

You can also check the unit tests for more examples

[Titibo26]

Thank you for your fast answer.
I need to say i didn't understand the shade on your 1st message. About unit tests, it tough me a lot on the weighted example but nothing about 2d poses. But i gave my try :

I created a new state class based on the example's existing one :
`
internal class state : State
{
public State sX;
public State sY;
public State sZ;

    public state(State sx, State sy, State sz)
    {
        sX = new State(sx);
        sY = new State(sy);
        sZ = new State(sz);
    }
}

`

also made observations class :
`
class icpMeasure : Observation
{
private readonly double m_Measurement;
private readonly State m_State;

    public icpMeasure(double measurement, State state, double sd = 0.2)
        : base(new[] { sd }, state)
    {
        m_State = state;
        m_Measurement = measurement;
    }

    protected override double[] OnComputeError()
    {
        var error = m_Measurement - m_State;

        return new[] { error };
    }
}

class odometryMeasure : Observation
{
    private readonly double m_Measurement;
    private readonly State m_State;

    public odometryMeasure(double measurement, State state, double sd = 0.5)
        : base(new[] { sd }, state)
    {
        m_State = state;
        m_Measurement = measurement;
    }

    protected override double[] OnComputeError()
    {
        var error = m_Measurement - m_State;

        return new[] { error };
    }
}

`

and then runned the optimization wich worked :
`
//Initial x,y,z pos
var statex = new State(0);
var statey = new State(0);
var statez = new State(0);

        //initial pos containing each axes
        var nstat = new state(statex, statey, statez);
        
        //Odometry
        var measurement11 = new icpMeasure(-110, nstat.sX, 0.5);
        var measurement21 = new icpMeasure(95, nstat.sY,0.5);
        var measurement31 = new icpMeasure(30, nstat.sZ, 0.5);

        //Icp (best measures)
        var measurement12 = new icpMeasure(-90, nstat.sX,0.2);
        var measurement22 = new icpMeasure(85, nstat.sY, 0.2);
        var measurement32 = new icpMeasure(10, nstat.sZ,0.2);

        var graph = new Graph();
        
        // odométry
        graph.AddObservation(measurement11);
        graph.AddObservation(measurement21);
        graph.AddObservation(measurement31);
        graph.AddObservation(measurement12);
        graph.AddObservation(measurement22);
        graph.AddObservation(measurement32);

        graph.Optimize();

        // Should be closer to icpMeasures
        Console.WriteLine(nstat.sX.Estimate);
        Console.WriteLine(nstat.sY.Estimate);
        Console.WriteLine(nstat.sZ.Estimate);`

But i'm feeling like not doing it the good way. in :

` internal class State : State
{
public State(double initialValue = 0.0)
: base(initialValue, 3)
{ }

    protected override double OnUpdate(double[] delta)
    {
        return Estimate + delta[0];
    }
}`

I don't get what the dimension argument of base(initialValue, 3) is meant for.
Am i still on the right way ? Thank you,

[HWouters]

Sorry, my first comment was not rendered correctly due to special characters... I have updated the comment and should make more sense now.

[HWouters]

You are on the right track, you have to fix the base of your State class.
The dimension parameter specifies the dimension of your state. It is reflected in the length of your array in the OnUpdate parameter delta.
Also you should make your intial value 3d

https://github.com/fugro/g2o/blob/e732db40c3e369bd31793a89561dc38cfabfad3c/Fugro/Test/PrecisionMatrixTest.cs#L186

[Titibo26]

Hi,

PrecisionMatrixTest helped me a lot. But i still not sure how to use "Point2DObservation".
I'm trying to make a loop optimization with a first node which is fixed, some nodes constrained one after the other by odometry measurments, and a last nodes constrained by his odometry measure and by an ICP from a loop closure.
For each odometry measure i add this kind of observation :
Point2DObservation currentpts = new Point2DObservation(globalCoordinate, new[] { 1/5, 0.0, 0.0, 1/5 }, currentPoint);
For the last node i add this observation :
Point2DObservation icpObservation = new Point2DObservation(globalCoordinateSeenWithICP, new[] { 1/2, 0.0, 0.0, 1/2 }, lastPoint);

When doing this, optimisation doesn't change anything on estimations.
I'm quite surprised that it is working with global coordinate, in theory, each constraint is a relative constraint but it doesn't seems to be the case in PrecisionMatrixTest.cs...

[EvertSchippers]

Odometry measurements would require 2 states (points). And the measurement would be the measured travelled distance between those points (or vector, if you know direction - making it a 2D error). And in the OnComputeError() you compute an error based on the difference between the estimates of those two states and the measured distance.

So, you need to make another Observation class, pretty similar to Point2DObservation, but maybe returning a 1D array. And in the constructor, provide 2 states (e.g. fromPoint and toPoint) - DO NOT FORGET to pass both states to the base constructor! Else the framework does not know about the dependency of that observation to both states.