I allocate this post to providing different code snippets, demonstrating different methods of rotation (parameterized by quaternions) averaging. These are highly needed operations for people working with camera poses yet somehow are rarely covered.
1. Quaternion Averaging
Since quaternions are not regular vectors, but rather representations of orientation, an average quaternion cannot just be obtained by taking a weighted mean. This function implements the work done by F. Landis Merkley to calculate the average quaternion. The algorithm is explained by F. Landis Markley.
For this particular implementation, I would also like to reference Mandar Harshe.
While being basic and straightforward, this algorithm is compared with rotqrmean from VoiceBox and found to produce quite similar results, yet it is more elegant, much simpler to implement and follow. (Though, there might be difference in signs)
For this particular implementation, I would also like to reference Mandar Harshe.
While being basic and straightforward, this algorithm is compared with rotqrmean from VoiceBox and found to produce quite similar results, yet it is more elegant, much simpler to implement and follow. (Though, there might be difference in signs)
Vector4d avg_quaternion_markley(MatrixXd Q){
Matrix4d A = Matrix4d::Zero();
int M = Q.rows();
for(int i=0; i<M; i++){
Vector4d q = Q.row(i);
if (q[0]<0)
q = -q;
A = q*q.adjoint() + A;
}
A=(1.0/M)*A;
SelfAdjointEigenSolver<MatrixXd> eig(A);
// cout<<"A"<<endl<<A<<endl;
// cout<<"vecs"<<endl<<eig.eigenvectors()<<endl;
// cout<<"vals"<<endl<<eig.eigenvalues()<<endl;
Vector4d qavg=eig.eigenvectors().col(3);
return qavg;
}
2. Weighted Quaternion Averaging
In some applications, we have weights available as confidences to our poses. In such cases, might be a good alternative to use:
Vector4d wavg_quaternion_markley(MatrixXd Q, VectorXd weights){
Matrix4d A = Matrix4d::Zero();
int M = Q.rows();
double weightSum = 0;
for(int i=0; i<M; i++){
Vector4d q = Q.row(i);
if (q[0]<0)
q = -q;
A = weights[i]*q*q.adjoint() + A;
weightSum += weights[i];
}
A=(1.0/weightSum)*A;
SelfAdjointEigenSolver<MatrixXd> eig(A);
// cout<<"A"<<endl<<A<<endl;
// cout<<"vecs"<<endl<<eig.eigenvectors()<<endl;
// cout<<"vals"<<endl<<eig.eigenvalues()<<endl;
Vector4d qavg=eig.eigenvectors().col(3);
return qavg;
}
3. Lp-Geometric Median on the Manifold of Quaternions
It is trivial to compute a median neither on the $SO(3)$ nor on $H_1$, the Hamiltonians. Thus we benefit from a Riemannian adapted version of the Weiszfeld algorithm that is tailored for iteratively computing the geometric median. The following publication provides elaborate discussions of implementing such a manifold-median:
Generalized Weiszfeld Algorithms for L1 Optimization Khurrum Aftab, Richard Hartley and Jochen Trumpf
https://core.ac.uk/download/pdf/156625039.pdf
In particular, I focus on Section 4.2 L_q Geodesic Median in \(SO(3)\). The algorithm operates directly on the quaternion parameterization, starts from an initial guess, set to the mean, and converges . Such sequence of iterates will converge to the L_q mean of the rotations, provided all the rotations and the initial extimate (mean) lie within a ball of radius \(pi/2\).
\(p\): The norm power. The algorithm ultimately computes the \(L_p\) mean. If \(p=1\), this resorts to standard median, while choices like 0.3, 0.5 are possible.
maxAngularUpdate: The iterates are stopped when the update of the angular component of the resulting L_q-mean is smaller than this value.
maxIterations: Maximum number of iterations. If this value is small, the algorithm may terminate without convergence.
maxIterations: Maximum number of iterations. If this value is small, the algorithm may terminate without convergence.
Below is the source code of this algorithm:
static inline Vector4d get_q(Quaterniond q) {
return Vector4d(q.w(), q.x(), q.y(), q.z());
}
static inline void t_qfix(Quaterniond &q) {
if (q.w() < 0) {
q.w() = -q.w();
q.x() = -q.x();
q.y() = -q.y();
q.z() = -q.z();
}
}
// Assumes that the quaternions are unit and the antipodal part is flipped consistently.
// Depends on the *avg_quaternion_markley* function defined above to initialize.
// Quaternions are assumed to be stored in columns!
Vector4d median_quaternions_weiszfeld(MatrixXd Q, double p = 1, double maxAngularUpdate = 0.0001, int maxIterations = 1000) {
Matrix4d A = Matrix4d::Zero();
int M = Q.cols();
Vector4d st = avg_quaternion_markley(Q.transpose());
Quaterniond qMedian(st[0], st[1], st[2], st[3]);
const double epsAngle = 0.0000001;
maxAngularUpdate = std::max(maxAngularUpdate, epsAngle);
double theta = 10* maxAngularUpdate;
int i = 0;
while (theta > maxAngularUpdate && i <= maxIterations) {
Vector3d delta(0,0,0);
double weightSum = 0;
for (int j = 0; j < M; j++) {
Vector4d q = Q.col(j);
Quaterniond qj = Quaterniond(q[0], q[1], q[2], q[3])*(qMedian.conjugate());
double theta = 2 * acos(qj.w());
if (theta > epsAngle) {
Vector3d axisAngle = qj.vec() / sin(theta / 2);
axisAngle *= theta;
double weight = 1.0 / pow(theta, 2 - p);
delta += weight * axisAngle;
weightSum += weight;
}
}
if (weightSum > epsAngle) {
delta /= weightSum;
theta = delta.norm();
if (theta > epsAngle) {
double stby2 = sin(theta*0.5);
delta /= theta;
Quaterniond q(cos(theta*0.5), stby2*delta(0), stby2*delta(1), stby2*delta(2));
qMedian = q * qMedian;
t_qfix(qMedian);
}
}
else
theta = 0;
i++;
}
return get_q(qMedian);
}
4. Weighted Lp-Geometric Median on the Manifold of Quaternions
Similar to the case of simple mean, one can also speak of the weighted median. The weights shift the Fermat’s problem to Weber’s problem and requires a special care. Therefore, I am currently not sure if the algorithm below is convergent or well behaving. However, it is a simple approach to incorporate ways into the computation of geometric median:
static inline Vector4d get_q(Quaterniond q) {
return Vector4d(q.w(), q.x(), q.y(), q.z());
}
static inline void t_qfix(Quaterniond &q) {
if (q.w() < 0) {
q.w() = -q.w();
q.x() = -q.x();
q.y() = -q.y();
q.z() = -q.z();
}
}
// Assumes that the quaternions are unit and the antipodal part is flipped consistently.
// Depends on the *avg_quaternion_markley* function defined above to initialize.
// Quaternions are assumed to be stored in columns!
Vector4d median_quaternions_weiszfeld(MatrixXd Q, std::vector<double> weights, double p = 1, double maxAngularUpdate = 0.0001, int maxIterations = 1000) {
Matrix4d A = Matrix4d::Zero();
int M = Q.cols();
Vector4d st = wavg_quaternion_markley(Q.transpose(), weights);
Quaterniond qMedian(st[0], st[1], st[2], st[3]);
const double epsAngle = 0.0000001;
maxAngularUpdate = std::max(maxAngularUpdate, epsAngle);
double theta = 10* maxAngularUpdate;
int i = 0;
while (theta > maxAngularUpdate && i <= maxIterations) {
Vector3d delta(0,0,0);
double weightSum = 0;
for (int j = 0; j < M; j++) {
Vector4d q = Q.col(j);
Quaterniond qj = Quaterniond(q[0], q[1], q[2], q[3])*(qMedian.conjugate());
double theta = 2 * acos(qj.w());
if (theta > epsAngle) {
Vector3d axisAngle = qj.vec() / sin(theta / 2);
axisAngle *= theta;
double weight = weights[j] / pow(theta, 2 - p);
delta += weight * axisAngle;
weightSum += weight;
}
}
if (weightSum > epsAngle) {
delta /= weightSum;
theta = delta.norm();
if (theta > epsAngle) {
double stby2 = sin(theta*0.5);
delta /= theta;
Quaterniond q(cos(theta*0.5), stby2*delta(0), stby2*delta(1), stby2*delta(2));
qMedian = q * qMedian;
t_qfix(qMedian);
}
}
else
theta = 0;
i++;
}
return get_q(qMedian);
}
