DATA-516 Fix GetPosition Orientation Response (RDK) #1459
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
ghost
mentioned this pull request
ghost
commented
Oct 7, 2022
services/slam/builtin/builtin.go
Outdated
| slamSvc.logger.Warnf("invalid format for quaternion returned, %v, skipping quaternion transform", q) | ||
| return pInFrame, nil | ||
| } | ||
| slamSvc.logger.Infof("valid format for quaternion returned, %v, performing quaternion transform", q) |
There was a problem hiding this comment.
Logging of position has been added, however depending on the frequency at which GetPosition is called by the UI this may be removed in the future.
There was a problem hiding this comment.
missed that there was logging in both rdk and orbslam, probably only want to log in one of them if you know the grpc call is working
There was a problem hiding this comment.
If you leave this in, I would make it a debug instead of info.
There was a problem hiding this comment.
@JohnN193 I'll plan on removing the logging here and keeping it in orbslam
ghost
closed this
ghost
reopened this
ghost
changed the title
ghost
changed the title
ghost
mentioned this pull request
ghost
marked this pull request as ready for review
JohnN193
approved these changes
Oct 10, 2022
kkufieta
approved these changes
Oct 10, 2022
services/slam/builtin/builtin.go
Outdated
Comment on lines
376
to
377
| // TODO DATA-531: Remove extraction and conversion of quaternion from the extra field in the response once the Rust SDK | ||
| // is available and the desired math can be implemented on the orbSLAM side |
There was a problem hiding this comment.
[nit] Can you add a link to the relevant ticket?
services/slam/builtin/builtin.go
Outdated
Comment on lines
393
to
397
| ori := spatialmath.QuatToOV(quat.Number{Real: valTheta.(float64), Imag: valX.(float64), Jmag: valY.(float64), Kmag: valZ.(float64)}) | ||
| newPose := spatialmath.NewPoseFromOrientation(pInFrame.Pose().Point(), ori) | ||
| pInFrame = referenceframe.NewPoseInFrame(pInFrame.FrameName(), newPose) | ||
|
|
||
| } |
There was a problem hiding this comment.
[major] Agreed with John, might make sense to ask e.g. Peter L. for a quick glance at this.
services/slam/builtin/builtin.go
Outdated
| slamSvc.logger.Warnf("invalid format for quaternion returned, %v, skipping quaternion transform", q) | ||
| return pInFrame, nil | ||
| } | ||
| slamSvc.logger.Infof("valid format for quaternion returned, %v, performing quaternion transform", q) |
There was a problem hiding this comment.
If you leave this in, I would make it a debug instead of info.
biotinker
reviewed
Oct 11, 2022
services/slam/builtin/builtin.go
Outdated
| return pInFrame, nil | ||
| } | ||
| slamSvc.logger.Infof("valid format for quaternion returned, %v, performing quaternion transform", q) | ||
| ori := spatialmath.QuatToOV(quat.Number{Real: valTheta.(float64), Imag: valX.(float64), Jmag: valY.(float64), Kmag: valZ.(float64)}) |
There was a problem hiding this comment.
Since we're parsing these values out of a map[string]interface, it might be worth checking if the (float64) conversion works.
Also, instead of importing gonum.org/v1/gonum/num/quat it can be kept within RDK with
spatialmath.QuatToOV(&spatialmath.Quaternion{Real: valTheta.(float64), Imag: valX.(float64), Jmag: valY.(float64), Kmag: valZ.(float64)})
Though the two are equivalent, Quaternion wraps a quat.Number
There was a problem hiding this comment.
Removes need for QuatToOV too! Thanks Peter
biotinker
reviewed
Oct 11, 2022
added 2 commits
October 11, 2022 16:16
|
Closed
This pull request was closed.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
A quaternion is returned to RDK from orbSLAM via a new response filed "extra". This value is then checked to see if it contains an 'quat' element, that element is then parsed into its respective elements: ox, oy, oz, and otheta. These elements are converted into a orientation vector via the QuatToOV function before being added to the new pose returned by the function.
In no value in 'extra' is found with the name 'quat' then this process is skipped and the pose is returned unaltered. This allows for flexibility between libraries and their return types if needed.
Ticket: DATA-516
Associated PR: #60
Note: This will be removed once spatial math is available in the C++ code via the Rust SDK (see ticket: DATA-531).