外参矩阵转四元数,左右手坐标系转化1

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);
}

---------------------------------------------------------

Rodrigues变换是为了实现opencv计算的外姿将旋转向量转化为旋转矩阵

			cv::Mat Rv, T, R;

			// 根据模板的真实点坐标设置objectPoints, imagePoints的值，调用更初级的一个函数FindExtrinsic2或类似的名字
			bool bSolved = cv::solvePnP(objectPoints, imagePoints, m_paraIntrinsic.cameraMatrix, m_paraIntrinsic.distCoeffs, Rv, T, false, CV_ITERATIVE);
			if(bSolved)
			{
				cv::Rodrigues(Rv, R);

				Mat Rt = R.t();
				
				QuaternionFromMatrix(Rt, 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 = - Rt * T;

				m_fExtrinsic[4] = (float)Tt.at<double>(0);
				m_fExtrinsic[5] = - (float)Tt.at<double>(1);
				m_fExtrinsic[6] = (float)Tt.at<double>(2);

				m_fExtrinsic[7] = (float)m_paraIntrinsic.fov;

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