-nig and -ntfv /-ntfl are conflicting

[brunofavs]

There are 3 issues overlapping each other :

1

ntfv adds rotation correctly, but translation not so well :

-ntfv 0 1 Correct

-ntfv 1 0 Wrong

Adding less than intended

-ntfv 1 1 Wrong

The noises are not being added independently

2

nig by itself impacts odom transformation

I reckon -nig adds noise to all estimated transformations, which is including additional_tfs

nig 1 1

nig 1 0 and nig 0 1 show the same behavior, but the noises here are being added independently.

3

ntfv conflicts with nig :

-ntfv 0 1 -nig 0 1 Wrong

-ntfv 0 1 -nig 1 1 Wrong

TLDR

• Adding ntfl translation affects both components

• Adding nig affects all estimated transformations, including the odometry tf.

Plan of action

1. Fix ....

   • Adding ntfl translation affects both components .... without taking nig into consideration

2. Fix ...

   • Adding nig affects all estimated transformations, including the odometry tf. ... without taking ntfvinto consideration

3. Add both back together

[miguelriemoliveira]

Hi @brunofavs ,

from the plan of action I see that you want to give priority to ntfv (it would be helpful to say what it means somewhere in the issue).

Thats option 1, right? Are the 1. 2. and 3. alternative options?

That makes sense to me. If nig and ntfv are used simultaneously, nig will not affect the transforms defined for the ntfv. The script should print a warning saying something like the sentence above.

[brunofavs]

Hey @miguelriemoliveira,

from the plan of action I see that you want to give priority to ntfv (it would be helpful to say what it means somewhere in the issue).

Yeah I should've done that considering its a new flag.

• noisy tf values ---> ntfv
  • noisy tf links ---> ntfl

Thats option 1, right? Are the 1. 2. and 3. alternative options?

They are not alternative options, there are 2 problems rn. The first one is that ntfv is adding rotation noise when it should only be rotation. The second is that 'nigshould'nt add noise to transformations specified byntfv`.

The script should print a warning saying something like the sentence above.

Yes this does makes sense. I will implement a ATOM Warning for this behavior.

[miguelriemoliveira]

The first one is that ntfv is adding rotation noise when it should only be rotation

I am confused with this sentence ...

[brunofavs]

Sorry @miguelriemoliveira for not making much sense on my prior comment. I reckon I was already confused when I wrote it and I may have mistyped what I intended to express.

I have been smashing my head in the wall for the past 3 hours. I couldn't find the root of the problem.

I think I may be onto something. There is bug prior to my work.

Observation 1

4 consecutive runs with

rosrun atom_calibration calibrate -json $ATOM_DATASETS/softbot/long_train_dataset1/dataset_corrected.json -v -csf 'lambda x:int(x)<5' -ipg -rv -si -ctgt -max_nfev 1 -nig 0 1

On the 4th one, I have the expected output : Equal magnitude noise added to each estimated atomic transformation.

On the 3rd one, one tf has the correct noise magnitude, other has it wrong.

On the 2nd, vice-versa

On the 1st, they are both wrong.

What is wrong?

Is the addNoiseToTF wrong?

Is the addNoiseToInitialGuess wrong?

Is the printComparisonToGroundTruth wrong?

Is the compareAtomTransforms wrong?

To verify if it was the printComparisonToGroundTruth , I commented out all the noises except the initial guess and used the compareAtomTransforms function manually inside addNoiseToTF :

def addNoiseToTF(dataset, selected_collection_key, calibration_parent, calibration_child, nig_trans, nig_rot):

    ...

        for collection_key, collection in dataset["collections"].items():

            # Get original transformation
            tf_gt = copy.deepcopy(dataset['collections'][collection_key]['transforms'][transform_key])

            ...

            print(f'{Fore.BLUE}{type(new_quat)}{Style.RESET_ALL}')
            print(f'{Fore.BLUE}{type(list(new_translation))}{Style.RESET_ALL}')

            from atom_core.utilities import compareAtomTransforms
            from pprint import pprint

            et,er = compareAtomTransforms(tf_gt,dataset['collections'][collection_key]['transforms'][transform_key])
            pprint(tf_gt)
            pprint(dataset['collections'][collection_key]['transforms'][transform_key])

            print("\n\nTranslation")
            print(f'{Fore.BLUE}{list(np.around(np.array(translation),4))}{Style.RESET_ALL}')
            print(f'{Fore.RED}{list(np.around(np.array(new_translation),4))}{Style.RESET_ALL}')
            print(f'{Fore.GREEN}Diference\n{round(et,3)}{Style.RESET_ALL}')

            print("\n\nRotation")
            print(f'{Fore.BLUE}{list(np.around(np.array(euler_angles),4))}{Style.RESET_ALL}')
            print(f'{Fore.RED}{list(np.around(np.array(new_angles),4))}{Style.RESET_ALL}')

            print(f'{Fore.GREEN}Diference\n{round(er,3)}{Style.RESET_ALL}')

Which allowed me to see if there were differences between this and the printComparisonToGroundTruth. There were.

For this run, with only -nig 0 1 :

For all collections, I checked the rotation error, computed by compareAtomTransforms to be as intended:

To verify that there wasn't some wrong gap between the addNoiseToInitialGuess and addNoiseToTF, I tried to rule out the possibility that noise was being added to more atomic transformations than the ones to be estimated. This was not the case. Or atleast addNoiseToInitialGuess only calls addNoiseToTF n times for the n tf's to be estimated, also with the correct link names.

Observation 2

Given the prior conclusion, I checked if my manual compareAtomTransforms implementation was consistent. It was not

From left to right the nig values across 5 collections are :

• 0 1
• 1 1
• 1 0

What is wrong?

From what I see, the rotation is correct. The translation either :

• Is being added wrongly
• compareAtomTransformsis computing the translation error wrongly.

I spent already a long time analyzing both the codes for the comparisson and the addNoisetoTF. I'm stuck.

[miguelriemoliveira]

Hi @brunofavs ,

not sure what's wrong. We can discuss tomorrow a bit.

[brunofavs]

Yeah I will leave it until tomorrow. I might be overlooking something.

[brunofavs]

This issue was solved in #929. There was a underlying prior issue with the noise functionality.