SFM landmark points not updating durin motion.

[Peter Milani]

I'm trying to investigate the utility of locating a landmark from mulitple viewpoints. However my estimate for the landmark doesn't change despite multiple ProjectionFactorCal3_S2 indicating something else.

My problem is a 2D one with a robot that has a line camera (W pixels x1), but I've had to work in  3d space. I've adjusted my camera and calibration for the line camera, and the projection values I get seem correct. I've adjusted from the python examples so numpy==1.25 and matplotlib==3.8

I have put an estimated location off the correct location, and project camera measurements from the correct location. The graph looks correct with a GenericProjectionFactor every 3 poses and the landmark. however my landmark's position never updates from the incorrect initial location. I am using iSam as the optimiser.

What am I doing wrong? Full code below:

```

import gtsam
import numpy as np
from gtsam.symbol_shorthand import T, X
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt

def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values,
                        key: int):
    """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2."""

    # Print the current estimates computed using iSAM2.
    #print("*"*50 + f"\nInference after State {key+1}:\n")
    #print(current_estimate)

    # Compute the marginals for all states in the graph.
    marginals = gtsam.Marginals(graph, current_estimate)

    # Plot the newly updated iSAM2 inference.
    fig = plt.figure(0)
    axes = fig.gca()
    plt.cla()

    print(f'Current Estimate {current_estimate}')
    i = 1
    while current_estimate.exists(X(i)):
        c3 = current_estimate.atPose3(X(i))
        c2=gtsam.Pose2(c3.x(), c3.y(), c3.rotation().yaw())

        gtsam_plot.plot_pose2(0, c2, 0.5, marginals.marginalCovariance(X(i))[3:5, 3:5])
        i += 1

    if current_estimate.exists(T(0)):
        p3 = current_estimate.atPoint3(T(0))
        print(f"updating T0: {p3}")
        p2 = gtsam.Point2(p3[0], p3[1])
        gtsam_plot.plot_point2(0, p2, 0.5, marginals.marginalCovariance(T(0))[:2, :2])

    plt.axis('equal')
    axes.set_xlim(-25, 25)
    axes.set_ylim(-25, 25)
    plt.pause(1)

class LandmarkTracker():

    def __init__(self, fov, ray_no, initialtgtguess: gtsam.Point2):
        self.initial_tgt_guess=gtsam.Point3(initialtgtguess[0], initialtgtguess[1], 0.)
        self.cal = gtsam.Cal3_S2(fov, ray_no, 1)
        self.count = 0
        self.pending_meas=[]
        self.graph = gtsam.NonlinearFactorGraph()

        self.prior_xy_sigma = 0.3
        self.prior_theta_sigma=5* np.pi/180
        self.odom_xy_sigma=0.3
        self.odom_theta_sigma=5*np.pi/180.

        self.PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
            0.001,
            0.001,
            self.prior_theta_sigma,
            self.prior_xy_sigma,
            self.prior_xy_sigma,
            0.001
            ]))
        self.TGT_PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
            1.3,
            1.3,
            0.001
            ]))
        self.ODOM_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
            0.001,
            0.001,
            self.odom_theta_sigma,
            self.odom_xy_sigma,
            self.odom_xy_sigma,
            0.001,
            ]))

        self.MEAS_NOISE = gtsam.noiseModel.Isotropic.Sigma(2, 1.) #one pixel in u and v
        self.m_count=0
        params = gtsam.ISAM2Params()
        params.setRelinearizeThreshold(0.1)
        params.relinearizeSkip = 1
        self.isam = gtsam.ISAM2(params)
        self.initial_estimate=gtsam.Values()


    def add_odom(self, pose2d: gtsam.Pose2):
        current_estimate =[]# self.initial_estimate
        if self.count < 1:
            self.graph.push_back(gtsam.PriorFactorPose3(
                X(self.count),
                gtsam.Pose3(pose2d),
                self.PRIOR_NOISE))
            self.initial_estimate.insert(X(self.count), gtsam.Pose3(pose2d))
            self.isam.update(self.graph, self.initial_estimate )
            self.initial_estimate.clear()
           
            self.priorpos=pose2d
            self.count +=1
        else:
            btfactor =  pose2d * self.priorpos.inverse()
           
            self.graph.push_back(gtsam.BetweenFactorPose3(
                X(self.count-1),
                X(self.count),
                gtsam.Pose3(btfactor),
                self.ODOM_NOISE
                ))

            self.priorpos=pose2d
            self.initial_estimate.insert(X(self.count), gtsam.Pose3(pose2d))
            self.count +=1
            self.isam.update(self.graph, self.initial_estimate )
            self.isam.update()
            current_estimate = self.isam.calculateEstimate()
            self.initial_estimate.clear()

        return current_estimate, self.count


    def add_measurement(self, idx, tgtclass, pose2d: gtsam.Pose2):
        curr_est, i = self.add_odom(pose2d)
        print(f"MCount {self.m_count}")
        if self.m_count ==0:
            self.initial_estimate.insert(T(0),self.initial_tgt_guess)
            self.graph.push_back(gtsam.PriorFactorPoint3(
                T(0),
                self.initial_tgt_guess,
                self.TGT_PRIOR_NOISE))


       
        measurement= gtsam.Point2(idx, 0.5)
        factor = gtsam.GenericProjectionFactorCal3_S2(
            measurement,
            self.MEAS_NOISE,
            X(self.count-1),
            T(0),
            self.cal
            )
        self.graph.push_back(factor)

        #print(f"Graph {self.graph}, mest {self.m_estimate}")
        self.isam.update(self.graph, self.initial_estimate )
        self.isam.update()
        current_estimate = self.isam.calculateEstimate()
        self.initial_estimate.clear()
        self.m_count +=1
        return current_estimate, self.count

def test():
    TGT = gtsam.Point2(20., 0.)
    TGT_NOISE = gtsam.Point2(3.5, 1.5)
    tt = LandmarkTracker(10, 20, TGT+TGT_NOISE)

    pose =gtsam.Pose2(0., 0., 0.)
    COUNT=10

    K=gtsam.Cal3_S2(10., 20, 1)

    for b in range(COUNT):
        print(f'b {b} pose {pose}')
        if b % 3 == 0:

            posein3 = gtsam.Pose3(pose)
            #Rotate camera pose so it is looking down x direction
            pose3 = gtsam.Pose3(gtsam.Rot3.Ry(np.pi/2)*posein3.rotation(), posein3.translation())
           
            camera = gtsam.PinholeCameraCal3_S2(pose3,  K)            
            measurement = camera.project(gtsam.Point3(TGT[0], TGT[1], 0.))
            print(f'Pose3 {pose3}, measurement {measurement}')
            curest, i = tt.add_measurement(measurement[0], 2, pose)
            report_on_progress(tt.graph, curest, X(i))

        else:
            curest, i =tt.add_odom( pose)
            if i > 1:
                report_on_progress(tt.graph, curest, X(i))
        pose *= gtsam.Pose2(1., 0., 0.)

    print(f"Graph {tt.graph}")
    plt.ioff()
    plt.show()
if __name__ == "__main__":
    test()

```

[Peter Milani]

I realise the above is probably a bit complicated to answer. I've simplified my scenario to just two camera measurement points, each linked with a between factor, a prior on the first measurement point and a prior on the landmark. The prior on the landmark is slightly incorrect, everything else is exact. I expect to see the landmark location updated to roughly the correct location (maybe with some adjustment of the camera measurement locations), but it seems the GenericProjectionFactorCal3_S2 are not having any effect.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import T, X

def testsimple():
    #Noise Values.
    prior_xy_sigma = 1.0
    prior_theta_sigma=5* np.pi/180
    odom_xy_sigma=1.0
    odom_theta_sigma=5*np.pi/180.
    PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
        0.001,
        0.001,
        prior_theta_sigma,
        prior_xy_sigma,
        prior_xy_sigma,
        0.001
        ]))
    TGT_PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
        10.3,
        10.3,
        10.1
        ]))
    ODOM_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([
        0.001,
        0.001,
        odom_theta_sigma,
        odom_xy_sigma,
        odom_xy_sigma,
        0.001,
        ]))
   
    #Camera Measurement Noise

    MEAS_NOISE = gtsam.noiseModel.Isotropic.Sigma(2, 1.) #one pixel in u and v

    #Landmark Point
    landmark3 = gtsam.Point3(10., 0., 0.)

    #Two Measurement Poses
    poseOne = gtsam.Pose2(0., 0., 0.)
    poseOne3 = gtsam.Pose3(poseOne)

    poseTwo = gtsam.Pose2(1., 0., 0.)
    poseTwo3 = gtsam.Pose3(poseTwo)

    #Camera Calibration - 20 x 1 line camera with fov of 10 degrees
    K = gtsam.Cal3_S2(10., 20, 1)

    #Rotate camera pose so it is looking down x direction

    #Measurement from Pose one
    cameraPoseOne3 = gtsam.Pose3(gtsam.Rot3.Ry(np.pi/2)*poseOne3.rotation(), poseOne3.translation())
    cameraOne = gtsam.PinholeCameraCal3_S2(cameraPoseOne3,  K)            
    measurementOne = cameraOne.project(landmark3)

    #Measurement From Pose two
    cameraPoseTwo3 = gtsam.Pose3(gtsam.Rot3.Ry(np.pi/2)*poseTwo3.rotation(), poseTwo3.translation())
    cameraTwo = gtsam.PinholeCameraCal3_S2(cameraPoseTwo3, K)
    measurementTwo = cameraTwo.project(landmark3)

    #initialise values and graph
    initestimate = gtsam.Values()
    graph = gtsam.NonlinearFactorGraph()

    #Add Factors.

    #Add Priors:
    #add landmark prior
    landmark_estimate_error = gtsam.Point3(1., 1., 0.3)
    initestimate.insert(T(0), landmark3)
    graph.push_back(gtsam.PriorFactorPoint3(
                T(0),
                landmark3 + landmark_estimate_error,
                TGT_PRIOR_NOISE))

    #add first pose prior
    graph.push_back(gtsam.PriorFactorPose3(
                X(0),
                poseOne3,
                PRIOR_NOISE))
    initestimate.insert(X(0), poseOne3)

    #Add between factor for poses.
    btfactor =  poseTwo3 * poseOne3.inverse()
           
    graph.push_back(gtsam.BetweenFactorPose3(
                X(0),
                X(1),
                btfactor,
                ODOM_NOISE
                ))

    initestimate.insert(X(1), poseTwo3)

    #Add camera measurements
    factor1 = gtsam.GenericProjectionFactorCal3_S2(
            measurementOne,
            MEAS_NOISE,
            X(0),
            T(0),
            K
            )
    graph.push_back(factor1)

    factor2 = gtsam.GenericProjectionFactorCal3_S2(
            measurementTwo,
            MEAS_NOISE,
            X(1),
            T(0),
            K
            )
    graph.push_back(factor2)
 
    #Optimise:

    params = gtsam.ISAM2Params()
    params.setRelinearizeThreshold(0.1)
    params.relinearizeSkip = 1

    isam = gtsam.ISAM2(params)

    print(f"Init estimate \n {initestimate}")
    isam.update(graph, initestimate )
    current_estimate = isam.calculateEstimate()
    print(f"Current estimate \n {current_estimate}")

if __name__ == "__main__":

    testsimple()

The std output of values from the two print statements at the end is:

init estimate
 Values with 3 values:
Value t0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
0;
0
]

Value x0: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0

Value x1: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 1 0 0


Current estimate
 Values with 3 values:
Value t0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
11;
1;
0.3
]

Value x0: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0

Value x1: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 1 0 0

As you can see neither X1 or X0 are changed, and T0 is only updated to conform to the prior on the landmark. I am expecting to see a general adjustment across all variables.

cheers

Peter

[Peter Milani]

I was able to get the system to optimise. Mainly through updating the values of T0 with the incorrect landmark location using the Point3 that was used for the prior. This cause the optimiser to actually do some optimisation. I think that the optimiser with the correct value for T0 was exiting early at step 0 as it must have been hitting some termination condition.

There are also a couple of errors in the above code, the poses X should be camera poses and be initialised with the pose used for the back projection measurement, but it was the change above that allowed the optimiser to actually optimise. 

[Dellaert, Frank]

Glad you found out!

PS I had good luck with chatgpt o1 checking code. It knows quite a bit about GTSAM.

FD

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/1f79442e-f182-4485-a350-0322fd3fe2e2n%40googlegroups.com.

[Peter Milani]

Yeah I may have to use something, the optimised result doesn’t make sense and doesn’t appear to be consistent with the measurements or what should be the ground truth.

I expect there’s an assumption i have wrong about the frames. Im assuming a camera pose with z in the direction of the camera pointing and x along image columns. But my optimised poses don’t have the zaxis pointing at the target.

[Peter Milani]

thanks Frank, 

I did give ChatGPT a go, I found it good for general suggestions but the detail around arguments and values is not accurate (for example camera calibration initialisation it was getting confused between field of view and focal length and whether that was in degrees or radians).

It did suggest a solution to my initial problem though.

And a few other suggestions such as use the PinholeCamera.LookAt to get correct rotation matricies for landmarks, though mine were correct as far as I can tell.

Can you confirm the camera frame coordinates are the same as OpenCV? ie as per the picture attached? ChatGPT suggests this, but when I use it to define the Camera pose orientations, the optimisation  aligns all my poses along the z axis (instead of the intiial y-axis) When I make the camera frame align with x down and y across (with z axis toward landmark) I get a optimisation that is at least orientated along the y axis, but this is not in accordance with the coordinate frame of opencv.

cheers

Peter

[Dellaert, Frank]

The camera frame convention is correct.

 

I had a quick look at your code, but hard to understand the orientations. I always advise people to use 3x3 camera matrices and label them as wRc, so the columns of wRc are the X,Y,Z axes of the camera.

 

I also saw that the cameras and the landmark were colinear? That is a singular configuration for any camera so expect trouble.

 

Apart from that: add some unit tests to check your understanding of the projections.

 

Best

FD

 

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/d0a0d2e3-cbb7-4ad8-8827-9079eb3d3238n%40googlegroups.com.

[Peter Milani]

Hi Frank,

I did get it to optimise correctly by reversing the order of multiplication for the between factors. It appears my intuition regarding between factors was failing me.

My intuition was to use between factors in the global frame, but the between factor from X0 to X1 is in the frame of X0, not the global frame. I didn't take into account the changed axes of X0.

So when checking the measurement for the BetweenFactor, what seemed correct as a global offset was actually an incorrect BetweenFactor measurement. The axes for rotation and translation needed to conform to the axes for X0 which has camera axes convention. 

To get the correct between factor I used:

btfactor = cameraPoseOne3.inverse() * cameraPoseTwo3

instead of

btfactor =  cameraPoseTwo3 * cameraPoseOne3.inverse()

I've actually run into this problem before  when working with a global odometry position but using between factors but didn't realise this was the issue I ended up just using priorFactors. But TLDR this is the problem I encountered. 

thanks for the help!

[Matías Mattamala]

Probably very late to contribute to the discussion, but just for reference if it helps in the future: The post I wrote some time ago explains why the between factor is defined in the local frame. See the section "Bringing everything together" here, there are some figures to illustrate it too.

Cheers