外參矩陣轉四元數,左右手座標系轉化1
阿新 • • 發佈:2019-01-26
1, 外參矩陣轉四元數
double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, flags | CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5); Rodrigues(rvecs[0], r); cout << rvecs[0] << endl; CcalinitEctrinsicMat(m_fExtrinsic, r, tvecs[0]); //cv::solvePnP(object_points, corners, cameraMatrix, distCoeffs, rvecs, tvecs); 注意rvecs, tvecs是Mat還是vector<Mat>
2 //計算四元數
int sign(float x) { return x >= 0 ? 1 : -1; } float myMax(float x, float y) { return x >y ? x : y; } void QuaternionFromMatrix(const Mat& R, float quat[]) { // Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm /*quat[0] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] + R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2; quat[1] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] - R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2; quat[2] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] + R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2; quat[3] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] - R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2; quat[1] *= sign(R.ptr<float>(2)[1] - R.ptr<float>(1)[2]); quat[2] *= sign(R.ptr<float>(0)[2] - R.ptr<float>(2)[0]); quat[3] *= sign(R.ptr<float>(1)[0] - R.ptr<float>(0)[1]);*/ quat[0] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) + R.at<float>(1, 1) + R.at<float>(2, 2))) / 2; quat[1] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) - R.at<float>(1, 1) - R.at<float>(2, 2))) / 2; quat[2] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) + R.at<float>(1, 1) - R.at<float>(2, 2))) / 2; quat[3] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) - R.at<float>(1, 1) + R.at<float>(2, 2))) / 2; quat[1] *= sign(R.at<float>(2, 1) - R.at<float>(1, 2)); quat[2] *= sign(R.at<float>(0, 2) - R.at<float>(2, 0)); quat[3] *= sign(R.at<float>(1, 0) - R.at<float>(0, 1)); }
3, //左右手座標系轉化,計算四元數
void CcalinitEctrinsicMat(float* m_fExtrinsic, Mat rr, Mat tt) { rr.convertTo(rr, CV_32F); tt.convertTo(tt, CV_32F); Mat R = Mat(3, 3, CV_32F), T = Mat(3, 1, CV_32F); T = tt; R = rr; ////mrightEX = mrightEX.inv(); //for (size_t i = 0; i < 3; i++) //{ // for (size_t j = 0; j < 3; j++) // { // R.at<float>(i, j) = mrightEX.at<float>(i, j); // //T.at<float>(i, 0) = mrightEX.at<float>(i, 3); // } // T.at<float>(i, 0) = mrightEX.at<float>(i, 3); //} ////左右手座標系轉化 Mat R_CPP, R_inv, T_CPP; R_CPP = R; R_inv = R_CPP.inv(); T_CPP = T; QuaternionFromMatrix(R_inv, m_fExtrinsic); // We need to invert rotations on X and Z axis m_fExtrinsic[1] = -m_fExtrinsic[1]; m_fExtrinsic[3] = -m_fExtrinsic[3]; Mat Tt = -R_inv * T_CPP; m_fExtrinsic[4] = (float)Tt.at<float>(0); m_fExtrinsic[5] = -(float)Tt.at<float>(1); m_fExtrinsic[6] = (float)Tt.at<float>(2); }
4,Google Cardboard裡面的寫法
// Copyright 2014 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
using UnityEngine;
/// @cond
/// Encapsulates a rotation and a translation. This is a convenience class that allows
/// construction and value access either by Matrix4x4 or Quaternion + Vector3 types.
public class Pose3D {
/// Right-handed to left-handed matrix converter (and vice versa).
protected static readonly Matrix4x4 flipZ = Matrix4x4.Scale(new Vector3(1, 1, -1));
/// The translation component of the pose.
public Vector3 Position { get; protected set; }
/// The rotation component of the pose.
public Quaternion Orientation { get; protected set; }
/// The pose as a matrix in Unity gameobject convention (left-handed).
public Matrix4x4 Matrix { get; protected set; }
/// The pose as a matrix in right-handed coordinates.
public Matrix4x4 RightHandedMatrix {
get {
return flipZ * Matrix * flipZ;
}
}
/// Default constructor.
/// Initializes position to the origin and orientation to the identity rotation.
public Pose3D() {
Position = Vector3.zero;
Orientation = Quaternion.identity;
Matrix = Matrix4x4.identity;
}
/// Constructor that takes a Vector3 and a Quaternion.
public Pose3D(Vector3 position, Quaternion orientation) {
Set(position, orientation);
}
/// Constructor that takes a Matrix4x4.
public Pose3D(Matrix4x4 matrix) {
Set(matrix);
}
protected void Set(Vector3 position, Quaternion orientation) {
Position = position;
Orientation = orientation;
Matrix = Matrix4x4.TRS(position, orientation, Vector3.one);
}
protected void Set(Matrix4x4 matrix) {
Matrix = matrix;
Position = matrix.GetColumn(3);
Orientation = Quaternion.LookRotation(matrix.GetColumn(2), matrix.GetColumn(1));
}
}
/// @endcond
/// @cond
/// Mutable version of Pose3D.
public class MutablePose3D : Pose3D {
/// Sets the position and orientation from a Vector3 + Quaternion.
public new void Set(Vector3 position, Quaternion orientation) {
base.Set(position, orientation);
}
/// Sets the position and orientation from a Matrix4x4.
public new void Set(Matrix4x4 matrix) {
base.Set(matrix);
}
/// Sets the position and orientation from a right-handed Matrix4x4.
public void SetRightHanded(Matrix4x4 matrix) {
Set(flipZ * matrix * flipZ);
}
}
/// @endcond