Trinary factor python implementation.

[Aryaman Patel]

I am new to writing custom factors in gtsam python
I am trying to implement a trinary factor that can jointly minimize the error between the relative transformation between the poses (Like the between factor) and also the estimate the registration to align the two frames.

My error function is as follows:

frame2(1_T_2) = inv(frame1_T_frame2) * (frame1_T1.between(frame1_T2))

error =  inv(frame1_T_frame2) * (frame1_T1.between(frame1_T2)) - frame2(1_T_2)

I referred the math.pdf doc in the gtsam library to understand how the pushforwards are calculated and came up with my own implementation of a trinary factor:

def evalError_Trinary(measurement: Pose3, this: CustomFactor,
                values: Values,
                H: List[np.ndarray]) -> np.ndarray :
    """
    Evaluate the error and the Jacobians of the error function
    for the Trinary factor.

    Args:
        measurement (gtsam.Pose3): Measurement.
        this (gtsam.CustomFactor): This factor.
        values (gtsam.Values): Values.
        H (List[np.ndarray]): List of Jacobians.

    Returns:
        np.ndarray: Error.
    """

    # Get the values of the poses
    key0 = this.keys()[0]
    key1 = this.keys()[1]
    key2 = this.keys()[2]

    frame1_T_frame2, pose1, pose2 = values.atPose3(key0), values.atPose3(key1), values.atPose3(key2)

    relative_pose = frame1_T_frame2.between(pose1.between(pose2)) # -> to bring in frame2 frame.
    error = measurement.localCoordinates(relative_pose)
    print(error)

    if H is not None:

        res1 = pose1.between(pose2)
        res2 = res1.between(frame1_T_frame2)
        H[0] = -res2.AdjointMap()
        H[1] = -res1.inverse().AdjointMap()
        H[2] = np.eye(6)

    return error

The above implementation is trying to optimized for the transformation frame_1_T_frame2
along with the poses pose1, pose2 which are both in frame1.
I am making sure to give reasonable estimate for the initial estimate of the registration and also making sure I constrain it accordingly (atleast 3 poses). Also I am making sure that all the poses are connected to same initial estimate of the registration.

My question:

Is there something wrong in the way I am calculating pushforwards?
How can I unit test this in python?

[Dellaert, Frank]

No time to check code in detail, but:

1. Seem like a lot of inverses, composes, betweens. Please look at definition of between and see whether you can simplify error function
2. Explicitly using Pose3::inverse, compose, etc will allow you to get their Jacobians and then you can use the chain rule
3. Mantra: *all* derivatives are wrong until unit-tested against numerical derivatives 😊. Look at test* in GTSAM for examples. I typically use macro in factorTesting.h for factors: very easy.

 

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 on the web visit https://groups.google.com/d/msgid/gtsam-users/7d1daffa-432e-45e8-842c-c493fb69ff91n%40googlegroups.com.