Fix dh parameter labels and add method for calculating covariance in tangent space

[Levi-Armstrong]

This addresses two things and exposes one issue and not sure how to fix.

• The parameter labels in stored in the optimize function were not correct because Eigen is column major so the lables need to be stored the same way.
• The compute covariance includes all parameters even if a parameter block is constant or sub parameters are constant. This adds another function but was thinking about updating the existing to do this. @marip8 Let me know what you think about updating the existing functions to do this instead of having a separate function?
• Also noticed something odd where on some tests the parameter block was in an incorrect order. I have added a check and now the unit tests are failing because this is being detected. Some how the target dh parameter block and camera dh parameter block is being swapped. @marip8 Do you know why this would be happening.

[Levi-Armstrong]

It turns out that some of the code assumes the vector of parameter blocks returned from ceres would be in the same order as when they were added. This is not the case because ceres is storing it in a map and when requested it iterates over the map and create a vector using the key so the order may not be the same.

[Levi-Armstrong]

It also now prints out the parameters as shown below and it can be seen in this case ceres has the Target DH parameters listed first.

Target DH Parameters:
    - Constant
Camera DH Parameters:
    - j1_d
    - j2_d
    - j3_d
    - j4_d
    - j5_d
    - j6_d
    - j1_theta
    - j2_theta
    - j3_theta
    - j4_theta
    - j5_theta
    - j6_theta
    - j1_r
    - j2_r
    - j3_r
    - j4_r
    - j5_r
    - j6_r
    - j1_alpha
    - j2_alpha
    - j3_alpha
    - j4_alpha
    - j5_alpha
    - j6_alpha
Camera Mount to Camera Position Parameters:
    - camera_mount_to_camera_x
    - camera_mount_to_camera_y
    - camera_mount_to_camera_z
Camera Mount to Camera Orientation Parameters:
    - camera_mount_to_camera_rx
    - camera_mount_to_camera_ry
    - camera_mount_to_camera_rz
Target Mount to Target Position Parameters:
    - target_mount_to_target_x
    - target_mount_to_target_y
    - target_mount_to_target_z
Target Mount to Target Orientation Parameters:
    - target_mount_to_target_rx
    - target_mount_to_target_ry
    - target_mount_to_target_rz
Camera Chain Base to Target Chain Base Position Parameters:
    - camera_base_to_target_x
    - camera_base_to_target_y
    - camera_base_to_target_z
Camera Chain Base to Target Chain Base Orientation Parameters:
    - camera_base_to_target_rx
    - camera_base_to_target_ry
    - camera_base_to_target_rz

[Levi-Armstrong]

@marip8 Do you think all the function should be updated to return this as the covariance or keep it as a separate?

[Levi-Armstrong]

@marip8 It should be ready for another look.

[Levi-Armstrong]

@marip8 you good with me merging this?

[Levi-Armstrong]

I haven't had a chance to fully look through this yet; I'll look at it in more detail tonight. One thought though: does it ever make sense not to compute the covariance in the tangent space? It seems like covariance between static variables is not valid

Yea, that was my thought is it is not needed. If you wanted the full covariance you would just pass it an empty masks. If you onboard I can just remove the new function and update the documentation on the other functions. Let me know if you agree and I will make the change.

[marip8]

If you onboard I can just remove the new function and update the documentation on the other functions. Let me know if you agree and I will make the change.

I think that makes sense; I'm on board with doing that. Like you mentioned, if you want the full covariance you can just pass empty masks (or we could make that the default value for that argument)

[Levi-Armstrong]

I have updated computeCovariance to take a mask with the default being empty.

[Levi-Armstrong]

It seems like this PR is making 5 sets of unrelated changes:

• Removing the Xenial build
• Fix for YAML include
• Moving print headers to rct_common
• Adding a helper to the DH chain to get a relative transform
• Updating the covariance calculation

I'm okay with making these changes, but I would really prefer if we merged them as separate PRs in the order above. It makes it hard to look back and understand where changes were introduced if too many unrelated ones are made in the same PR

I wound not agree these are unrelated changes. These were all necessary to get something that was broken tested and fixed. I will clean up the commit history but I am not going to separate them into different PRs. In the end the only thing that is important is that the commit history reflect these changes.

[Levi-Armstrong]

I will follow up with a different PR to add a clang format file, CI Job and format the entire repo.

[marip8]

I will clean up the commit history but I am not going to separate them into different PRs. In the end the only thing that is important is that the commit history reflect these changes.

That's fine; the main thing is being able to see the individual changes

[Levi-Armstrong]

I will clean up the commit history but I am not going to separate them into different PRs. In the end the only thing that is important is that the commit history reflect these changes.

That's fine; the main thing is being able to see the individual changes

Sorry, I reread my comment. Please don't take it the wrong way. I understand your point but in this case I needed all of these changes with the exception of the moving the print utils in one branch for everything to work on a machine. The CI change was also need because some change I made broke the xenial build and I did not want to spend time fixing it for something that was EOL.

[Levi-Armstrong]

I think everything is address except the formatting issues, which I will fix in a followup PR using clang format.

[Levi-Armstrong]

@marip8 I created a PR add the linters which I will rebase and run after this is merged.

[marip8]

Can you remove the empty extrinsic data header file? It still looks to be added in this PR. Otherwise I think it should be ready to merge

[Levi-Armstrong]

@marip8 I removed that file.

[Levi-Armstrong]

@marip8 Thanks for reviewing this!