private void ReportNewAsaRelPose(Odometry relPose) { var anchor = missioncontroller.GetAnchorControllerById(relPose.header.frame_id); if (anchor == null) { Debug.Log("Anchor for pose was null"); return; } TransformHelper.transform.SetParent(anchor.transform, false); var position = relPose.pose.pose.position; var orientation = relPose.pose.pose.orientation; (var pos, var or) = PoseExtensions.Ros2Unity(position, orientation); TransformHelper.transform.localPosition = pos; TransformHelper.transform.localRotation = or; if (anchor != null) { //Calculate the relative position to the current anchor //pos = anchor.transform.InverseTransformPoint(TransformHelper.transform.position); //or = (Quaternion.Inverse(anchor.transform.rotation) * TransformHelper.transform.rotation); robotState.Pose.SetFromPosition(pos); robotState.Pose.SetFromRotation(or); } else { //we do not have a current anchor yet, so assume that the anchor is still the one at origin robotState.Pose.SetFromRosSharp(position, orientation); } //Animate the robot's base odometry if (robotModelActive) { RobotBase.transform.SetParent(anchor.transform, false); RobotBase.transform.localPosition = pos; RobotBase.transform.localRotation = or; } //if(targetPose != null) //{ // Debug.Log($"{robotState.Pose.X - targetPose.X} | {robotState.Pose.Z - targetPose.Z}"); //} Robot.Delegates.UpdateRobotState(robotState); }
private void UpdateStereoPerspective() { Camera camera = this.Camera; // Apply camera settings to the frustum. this._frustum.NearClip = camera.nearClipPlane; this._frustum.FarClip = camera.farClipPlane; this._frustum.CameraOffset = this._cameraOffset; this._frustum.Ipd = Mathf.Lerp( 0, camera.stereoSeparation, this._stereoWeight); this._frustum.HeadPose = PoseExtensions.Lerp( this._frustum.DefaultHeadPose, this._headTarget.Pose, this._stereoWeight); // Update the camera's view matrices for the // center, left, and right eyes. camera.transform.SetLocalPose(this.GetLocalPose(ZEye.Center)); camera.SetStereoViewMatrix( Camera.StereoscopicEye.Left, this._frustum.GetViewMatrix(ZEye.Left, this.WorldScale) * this._monoWorldToCameraMatrix); camera.SetStereoViewMatrix( Camera.StereoscopicEye.Right, this._frustum.GetViewMatrix(ZEye.Right, this.WorldScale) * this._monoWorldToCameraMatrix); // Update the camera's projection matrices for the // center, left, and right eyes. camera.projectionMatrix = this._frustum.GetProjectionMatrix(ZEye.Center); camera.SetStereoProjectionMatrix( Camera.StereoscopicEye.Left, this._frustum.GetProjectionMatrix(ZEye.Left)); camera.SetStereoProjectionMatrix( Camera.StereoscopicEye.Right, this._frustum.GetProjectionMatrix(ZEye.Right)); }
void Update() { var pose = PoseExtensions.Slerp(transform.Pose(), other.Pose(), trackingSpeed); transform.SetPositionAndRotation(pose); }