Skip to content

Instantly share code, notes, and snippets.

@ReaperUnreal
Created October 26, 2012 20:49
Show Gist options
  • Save ReaperUnreal/3961408 to your computer and use it in GitHub Desktop.
Save ReaperUnreal/3961408 to your computer and use it in GitHub Desktop.
QR Decomposition of 3x3 Matrix Using Householder Reflections
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
void printMatrix3x3(float *mat)
{
printf("%0.3f\t%0.3f\t%0.3f\n", mat[0], mat[1], mat[2]);
printf("%0.3f\t%0.3f\t%0.3f\n", mat[3], mat[4], mat[5]);
printf("%0.3f\t%0.3f\t%0.3f\n", mat[6], mat[7], mat[8]);
printf("\n");
}
void makeEricsMatrix(float *mat, float a, float b, float c, float d, float x, float y)
{
mat[0] = a; mat[1] = b; mat[2] = x;
mat[3] = c; mat[4] = d; mat[5] = y;
mat[6] = 0; mat[7] = 0; mat[8] = 1;
}
float sign(float x)
{
if(x > 0.0f)
return 1.0f;
if(x < 0.0f)
return -1.0f;
return 0.0f;
}
float mag3(float *x)
{
return sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
}
float mag2(float *x)
{
return sqrtf(x[0] * x[0] + x[1] * x[1]);
}
void getQ1(float *mat, float *q1)
{
float x[3];
x[0] = mat[0]; x[1] = mat[3]; x[2] = mat[6];
//printf("x = [%0.3f, %0.3f, %0.3f]\n", x[0], x[1], x[2]);
float magx = mag3(x);
//printf("magx = %0.3f\n", magx);
float alpha = magx * -1.0f * sign(mat[0]); //alpha = ||x|| * sign(xk)
//printf("alpha = %0.3f\n", alpha);
float u[3];
u[0] = x[0] + alpha; u[1] = x[1]; u[2] = x[2]; //u = x + alpha * e1
//printf("u = [%0.3f, %0.3f, %0.3f]\n", u[0], u[1], u[2]);
float magu = mag3(u);
//printf("magu = %0.3f\n", magu);
float v[3];
v[0] = u[0] / magu; v[1] = u[1] / magu; v[2] = u[2] / magu; //v = u / ||u||
//printf("v = [%0.3f, %0.3f, %0.3f]\n", v[0], v[1], v[2]);
//q = I - 2vvT
q1[0] = 1 - 2 * v[0] * v[0]; q1[1] = -2 * v[0] * v[1]; q1[2] = -2 * v[0] * v[2];
q1[3] = -2 * v[0] * v[1]; q1[4] = 1 - 2 * v[1] * v[1]; q1[5] = -2 * v[1] * v[2];
q1[6] = -2 * v[0] * v[2]; q1[7] = -2 * v[1] * v[2]; q1[8] = 1 - 2 * v[2] * v[2];
}
void getQ2(float *mat, float *q2)
{
float x[2];
x[0] = mat[4]; x[1] = mat[7];
float magx = mag2(x);
float alpha = magx * -1.0f * sign(mat[4]); //alpha = ||x|| * sign(xk)
float u[2];
u[0] = x[0] + alpha; u[1] = x[1]; //u = x + alpha * e1
float magu = mag2(u);
float v[2];
v[0] = u[0] / magu; v[1] = u[1] / magu; //v = u / ||u||
//q = I - 2vvT
q2[0] = 1; q2[1] = 0; q2[2] = 0;
q2[3] = 0; q2[4] = 1 - 2 * v[0] * v[0]; q2[5] = -2 * v[0] * v[1];
q2[6] = 0; q2[7] = -2 * v[0] * v[1]; q2[8] = 1 - 2 * v[1] * v[1];
}
void matMult(float *res, float *a, float *b)
{
res[0] = a[0] * b[0] + a[1] * b[3] + a[2] * b[6]; res[1] = a[0] * b[1] + a[1] * b[4] + a[2] * b[7]; res[2] = a[0] * b[2] + a[1] * b[5] + a[2] * b[8];
res[3] = a[3] * b[0] + a[4] * b[3] + a[5] * b[6]; res[4] = a[3] * b[1] + a[4] * b[4] + a[5] * b[7]; res[5] = a[3] * b[2] + a[4] * b[5] + a[5] * b[8];
res[6] = a[6] * b[0] + a[7] * b[3] + a[8] * b[6]; res[7] = a[6] * b[1] + a[7] * b[4] + a[8] * b[7]; res[8] = a[6] * b[2] + a[7] * b[5] + a[8] * b[8];
}
void transpose(float *res, float *a)
{
res[0] = a[0]; res[1] = a[3]; res[2] = a[6];
res[3] = a[1]; res[4] = a[4]; res[5] = a[7];
res[6] = a[2]; res[7] = a[5]; res[8] = a[8];
}
void qr(float *mat, float *q, float *r)
{
float q1[9] = {0}, q2[9] = {0};
float res1[9] = {0};
//printMatrix3x3(mat);
getQ1(mat, q1);
//printMatrix3x3(q1);
matMult(res1, q1, mat);
//printMatrix3x3(res1);
getQ2(res1, q2);
//printMatrix3x3(q2);
matMult(r, q2, res1);
//printMatrix3x3(r);
float q1t[9] = {0}, q2t[9] = {0};
transpose(q1t, q1);
transpose(q2t, q2);
matMult(q, q1t, q2t);
//printMatrix3x3(q);
}
int main()
{
float mat[9] = {12, -51, 4,
6, 167, -68,
-4, 24, -41};
float q[9] = {0}, r[9] = {0};
printf("Matrix:\n");
printMatrix3x3(mat);
qr(mat, q, r);
printf("Q:\n");
printMatrix3x3(q);
printf("R:\n");
printMatrix3x3(r);
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment