DATA-516 Fix GetPosition Orientation Response (ORBSLAM) #60
Conversation
…iam/slam into 10062022-FixPoseOrientation
ghost
changed the title
ghost
changed the title
ghost
marked this pull request as ready for review
| // 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 ticket?
There was a problem hiding this comment.
added it but its on two lines due to line limitations
|
that was my fault, I did not properly include the ORBSLAM3 submodule changes before and worked with Jeremy to get them added back |
|
In that case it's a LGTM after adding a link to the ticket as mentioned above. |
There was a problem hiding this comment.
Looks good! A few comments.
Using the new variable in the GetPositionResponse, the raw axis angles are returned to be processed on the RDK side. The axis angles are stored in a google protobuf struct: ov while the orientation values in the pose returned are set to 0.
Just want to clarify, the quaternion coefficients are being returned, not the axis angle values.
Also, please make sure there are no submodule changes in the commit.
| @@ -69,6 +70,29 @@ ::grpc::Status SLAMServiceImpl::GetPosition(ServerContext *context, | |||
| myPose->set_x(actualPose[4]); | |||
| myPose->set_y(actualPose[5]); | |||
| myPose->set_z(actualPose[6]); | |||
|
|
|||
| // TODO DATA-531: Remove extraction and conversion of quaternion from the | |||
| // extra field in the response once the Rust SDK is available and the | |||
There was a problem hiding this comment.
Nit: this depends on the rust spatial math library (https://viam.atlassian.net/browse/RSDK-611), not the rust sdk.
| // 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 | ||
| myPose->set_o_x(0.0); |
There was a problem hiding this comment.
Please remove the computation of o_x, o_y, o_z and theta from above.
| << "x: " << actualPose[4] << " y: " << actualPose[5] | ||
| << " z: " << actualPose[6] << " o_theta: " << actualPose[3] | ||
| << " o_x: " << actualPose[0] << " o_y: " << actualPose[1] | ||
| << " o_z: " << actualPose[2]; |
There was a problem hiding this comment.
We shouldn't log these as o_x, o_y, o_z, and o_theta, since these are the quaternion coefficients, not orientation vector coefficients.
| q->mutable_fields()->operator[]("otheta").set_number_value(actualPose[3]); | ||
| q->mutable_fields()->operator[]("ox").set_number_value(actualPose[0]); | ||
| q->mutable_fields()->operator[]("oy").set_number_value(actualPose[1]); | ||
| q->mutable_fields()->operator[]("oz").set_number_value(actualPose[2]); |
There was a problem hiding this comment.
I suggest calling these q_0, q_1, q_2, and q_3 to make it clear they are quaternion coefficients.
There was a problem hiding this comment.
I'll use the rdk representation real, imag, jmag, kmag
Using the new variable in the GetPositionResponse, the raw axis angles are returned to be processed on the RDK side. The axis angles are stored in a google protobuf struct: ov while the orientation values in the pose returned are set to 0.
Ticket: DATA-516
Associated PR: #1459
Note: This will be removed once spatial math is available in the C++ code via the Rust SDK (see ticket DATA-531).