Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

closed form constrained solvepnp #1826

Draft
wants to merge 3 commits into
base: main
Choose a base branch
from
Draft

closed form constrained solvepnp #1826

wants to merge 3 commits into from

Conversation

mcm001
Copy link
Contributor

@mcm001 mcm001 commented Mar 20, 2025 •
edited
Loading

To test this:

./gradlew photon-lib:test -i --tests=*meme  --debug-jvm

Will produce some prints in stdout like:

PhotonPoseEstimatorTest > meme() STANDARD_OUT
    {"cameraCal":[599.375,599.1666666666669,479.5,359.5],"field2points":{"data":[3.6576,3.6576,3.6576,3.6576,4.002415602917594,4.145396397082404,4.145396397082404,4.002415602917594,6.070092,5.904991999999999,5.904991999999999,6.070092,4.145396397082404,4.002415602917594,4.002415602917594,4.145396397082404,8.313547,8.313547,8.230996999999999,8.230996999999999,8.313547,8.313547,8.230996999999999,8.230996999999999,12.227305999999999,12.227305999999999,12.227305999999999,12.227305999999999,4.10845,3.94335,3.94335,4.10845,3.347593,3.2650429999999995,3.2650429999999995,3.347593,-0.0038099999999999996,-0.0038099999999999996,-0.0038099999999999996,-0.0038099999999999996,4.786757,4.704207,4.704207,4.786757,1.997456,1.832356,1.832356,1.997456,6.220206,6.055105999999999,6.055105999999999,6.220206,4.10845,3.94335,3.94335,4.10845,0.22555199999999997,0.22555199999999997,0.390652,0.390652,0.22555199999999997,0.22555199999999997,0.390652,0.390652,1.2191999999999998,1.2191999999999998,1.3843,1.3843,0.22555199999999997,0.22555199999999997,0.390652,0.390652,1.7964256029175947,1.7964256029175947,1.9394063970824056,1.9394063970824056,1.7964256029175947,1.7964256029175947,1.9394063970824056,1.9394063970824056,0.22555199999999997,0.22555199999999997,0.390652,0.390652,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0],"numRows":4,"numCols":28,"type":"DDRM","2DData":[[3.6576,3.6576,3.6576,3.6576,4.002415602917594,4.145396397082404,4.145396397082404,4.002415602917594,6.070092,5.904991999999999,5.904991999999999,6.070092,4.145396397082404,4.002415602917594,4.002415602917594,4.145396397082404,8.313547,8.313547,8.230996999999999,8.230996999999999,8.313547,8.313547,8.230996999999999,8.230996999999999,12.227305999999999,12.227305999999999,12.227305999999999,12.227305999999999],[4.10845,3.94335,3.94335,4.10845,3.347593,3.2650429999999995,3.2650429999999995,3.347593,-0.0038099999999999996,-0.0038099999999999996,-0.0038099999999999996,-0.0038099999999999996,4.786757,4.704207,4.704207,4.786757,1.997456,1.832356,1.832356,1.997456,6.220206,6.055105999999999,6.055105999999999,6.220206,4.10845,3.94335,3.94335,4.10845],[0.22555199999999997,0.22555199999999997,0.390652,0.390652,0.22555199999999997,0.22555199999999997,0.390652,0.390652,1.2191999999999998,1.2191999999999998,1.3843,1.3843,0.22555199999999997,0.22555199999999997,0.390652,0.390652,1.7964256029175947,1.7964256029175947,1.9394063970824056,1.9394063970824056,1.7964256029175947,1.7964256029175947,1.9394063970824056,1.9394063970824056,0.22555199999999997,0.22555199999999997,0.390652,0.390652],[1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]],"numElements":112},"point_observations":{"data":[401.3559265136719,434.6202697753906,434.3583984375,400.89996337890625,546.8855590820312,558.4163818359375,558.81201171875,547.2379150390625,903.0679931640625,916.6886596679688,918.1712646484375,904.4595336914062,294.8089599609375,301.8063659667969,300.87725830078125,293.8831481933594,618.2979736328125,631.5966186523438,633.6064453125,620.1320190429688,278.15863037109375,291.457275390625,288.9725036621094,275.49810791015625,459.28125,467.887939453125,467.8704833984375,459.2508544921875,352.1061096191406,352.1061096191406,318.7993469238281,318.7993469238281,346.3612365722656,344.3131103515625,315.6606140136719,316.48504638671875,214.31024169921875,211.6664581298828,191.83242797851562,195.1048583984375,344.3131103515625,346.3612365722656,316.48504638671875,315.6606140136719,191.5602264404297,191.5602264404297,178.44384765625,178.44384765625,191.5602264404297,191.5602264404297,178.44384765625,178.44384765625,310.9059143066406,310.9059143066406,302.263427734375,302.263427734375],"numRows":2,"numCols":28,"type":"DDRM","2DData":[[401.3559265136719,434.6202697753906,434.3583984375,400.89996337890625,546.8855590820312,558.4163818359375,558.81201171875,547.2379150390625,903.0679931640625,916.6886596679688,918.1712646484375,904.4595336914062,294.8089599609375,301.8063659667969,300.87725830078125,293.8831481933594,618.2979736328125,631.5966186523438,633.6064453125,620.1320190429688,278.15863037109375,291.457275390625,288.9725036621094,275.49810791015625,459.28125,467.887939453125,467.8704833984375,459.2508544921875],[352.1061096191406,352.1061096191406,318.7993469238281,318.7993469238281,346.3612365722656,344.3131103515625,315.6606140136719,316.48504638671875,214.31024169921875,211.6664581298828,191.83242797851562,195.1048583984375,344.3131103515625,346.3612365722656,316.48504638671875,315.6606140136719,191.5602264404297,191.5602264404297,178.44384765625,178.44384765625,191.5602264404297,191.5602264404297,178.44384765625,178.44384765625,310.9059143066406,310.9059143066406,302.263427734375,302.263427734375]],"numElements":56}}
    DebugData [cameraCal=[599.375, 599.1666666666669, 479.5, 359.5], field2points=Type = DDRM , rows = 4 , cols = 28
     3.6576E+00  3.6576E+00  3.6576E+00  3.6576E+00  4.0024E+00  4.1454E+00  4.1454E+00  4.0024E+00  6.0701E+00  5.9050E+00  5.9050E+00  6.0701E+00  4.1454E+00  4.0024E+00  4.0024E+00  4.1454E+00  8.3135E+00  8.3135E+00  8.2310E+00  8.2310E+00  8.3135E+00  8.3135E+00  8.2310E+00  8.2310E+00  1.2227E+01  1.2227E+01  1.2227E+01  1.2227E+01 
     4.1085E+00  3.9434E+00  3.9434E+00  4.1085E+00  3.3476E+00  3.2650E+00  3.2650E+00  3.3476E+00 -3.8100E-03 -3.8100E-03 -3.8100E-03 -3.8100E-03  4.7868E+00  4.7042E+00  4.7042E+00  4.7868E+00  1.9975E+00  1.8324E+00  1.8324E+00  1.9975E+00  6.2202E+00  6.0551E+00  6.0551E+00  6.2202E+00  4.1085E+00  3.9434E+00  3.9434E+00  4.1085E+00 
     2.2555E-01  2.2555E-01  3.9065E-01  3.9065E-01  2.2555E-01  2.2555E-01  3.9065E-01  3.9065E-01  1.2192E+00  1.2192E+00  1.3843E+00  1.3843E+00  2.2555E-01  2.2555E-01  3.9065E-01  3.9065E-01  1.7964E+00  1.7964E+00  1.9394E+00  1.9394E+00  1.7964E+00  1.7964E+00  1.9394E+00  1.9394E+00  2.2555E-01  2.2555E-01  3.9065E-01  3.9065E-01 
     1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00  1.0000E+00 
    , point_observations=Type = DDRM , rows = 2 , cols = 28
     4.0136E+02  4.3462E+02  4.3436E+02  4.0090E+02  5.4689E+02  5.5842E+02  5.5881E+02  5.4724E+02  9.0307E+02  9.1669E+02  9.1817E+02  9.0446E+02  2.9481E+02  3.0181E+02  3.0088E+02  2.9388E+02  6.1830E+02  6.3160E+02  6.3361E+02  6.2013E+02  2.7816E+02  2.9146E+02  2.8897E+02  2.7550E+02  4.5928E+02  4.6789E+02  4.6787E+02  4.5925E+02 
     3.5211E+02  3.5211E+02  3.1880E+02  3.1880E+02  3.4636E+02  3.4431E+02  3.1566E+02  3.1649E+02  2.1431E+02  2.1167E+02  1.9183E+02  1.9510E+02  3.4431E+02  3.4636E+02  3.1649E+02  3.1566E+02  1.9156E+02  1.9156E+02  1.7844E+02  1.7844E+02  1.9156E+02  1.9156E+02  1.7844E+02  1.7844E+02  3.1091E+02  3.1091E+02  3.0226E+02  3.0226E+02 
    ]
    Ground truth field2robot:
    {"data":[1.0,0.0,0.0,1.0,0.0,1.0,0.0,4.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0],"numRows":4,"numCols":4,"type":"DDRM","2DData":[[1.0,0.0,0.0,1.0],[0.0,1.0,0.0,4.0],[0.0,0.0,1.0,0.0],[0.0,0.0,0.0,1.0]],"numElements":16}
    Ground truth robot2camera:
    {"data":[0.9945218953682733,0.0,0.10452846326765347,-0.30479999999999996,0.0,1.0,0.0,-0.2794,-0.10452846326765347,0.0,0.9945218953682733,0.5,0.0,0.0,0.0,1.0],"numRows":4,"numCols":4,"type":"DDRM","2DData":[[0.9945218953682733,0.0,0.10452846326765347,-0.30479999999999996],[0.0,1.0,0.0,-0.2794],[-0.10452846326765347,0.0,0.9945218953682733,0.5],[0.0,0.0,0.0,1.0]],"numElements":16}

Description

Meta

Merge checklist:

  • Pull Request title is short, imperative summary of proposed changes
  • The description documents the what and why
  • If this PR changes behavior or adds a feature, user documentation is updated
  • If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly
  • If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1
  • If this PR addresses a bug, a regression test for it is added

@mcm001 mcm001 requested a review from a team as a code owner March 20, 2025 02:44
@mcm001 mcm001 marked this pull request as draft March 20, 2025 02:44
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
1 participant
@mcm001