Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
4 contributors

Users who have contributed to this file

@kwagyeman @iabdalkader @peitschie @falkoschindler
12162 lines (10540 sloc) 363 KB
/* This file is part of the OpenMV project.
* Copyright (c) 2013-2017 Ibrahim Abdelkader <iabdalkader@openmv.io> & Kwabena W. Agyeman <kwagyeman@openmv.io>
* This work is licensed under the MIT license, see the file LICENSE for details.
*/
#include <float.h>
#include <stdarg.h>
#include "imlib.h"
#ifdef IMLIB_ENABLE_APRILTAGS
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
All rights reserved.
This software was developed in the APRIL Robotics Lab under the
direction of Edwin Olson, ebolson@umich.edu. This software may be
available under alternative licensing terms; contact the address above.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of the Regents of The University of Michigan.
*/
#define printf(format, ...)
#define fprintf(format, ...)
#define free(ptr) ({ umm_free(ptr); })
#define malloc(size) ({ void *_r = umm_malloc(size); if(!_r) umm_alloc_fail(); _r; })
#define realloc(ptr, size) ({ void *_r = umm_realloc((ptr), (size)); if(!_r) umm_alloc_fail(); _r; })
#define calloc(num, item_size) ({ void *_r = umm_calloc((num), (item_size)); if(!_r) umm_alloc_fail(); _r; })
#define assert(expression)
#define double float
#undef DBL_MIN
#define DBL_MIN FLT_MIN
#undef DBL_MAX
#define DBL_MAX FLT_MAX
#define sqrt(x) fast_sqrtf(x)
#define sqrtf(x) fast_sqrtf(x)
#define floor(x) fast_floorf(x)
#define floorf(x) fast_floorf(x)
#define ceil(x) fast_ceilf(x)
#define ceilf(x) fast_ceilf(x)
#define round(x) fast_roundf(x)
#define roundf(x) fast_roundf(x)
#define atan(x) fast_atanf(x)
#define atanf(x) fast_atanf(x)
#define atan2(y, x) fast_atan2f((y), (x))
#define atan2f(y, x) fast_atan2f((y), (x))
#define exp(x) fast_expf(x)
#define expf(x) fast_expf(x)
#define cbrt(x) fast_cbrtf(x)
#define cbrtf(x) fast_cbrtf(x)
#define fabs(x) fast_fabsf(x)
#define fabsf(x) fast_fabsf(x)
#define log(x) fast_log(x)
#define logf(x) fast_log(x)
#undef log2
#define log2(x) fast_log2(x)
#undef log2f
#define log2f(x) fast_log2(x)
#define sin(x) arm_sin_f32(x)
#define cos(x) arm_cos_f32(x)
#define fmin(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; })
#define fminf(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; })
#define fmax(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; })
#define fmaxf(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; })
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "zarray.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a structure which acts as a resize-able array ala Java's ArrayList.
*/
typedef struct zarray zarray_t;
struct zarray
{
size_t el_sz; // size of each element
int size; // how many elements?
int alloc; // we've allocated storage for how many elements?
char *data;
};
/**
* Creates and returns a variable array structure capable of holding elements of
* the specified size. It is the caller's responsibility to call zarray_destroy()
* on the returned array when it is no longer needed.
*/
static inline zarray_t *zarray_create(size_t el_sz)
{
assert(el_sz > 0);
zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t));
za->el_sz = el_sz;
return za;
}
/**
* Creates and returns a variable array structure capable of holding elements of
* the specified size. It is the caller's responsibility to call zarray_destroy()
* on the returned array when it is no longer needed.
*/
static inline zarray_t *zarray_create_fail_ok(size_t el_sz)
{
assert(el_sz > 0);
zarray_t *za = (zarray_t*) umm_calloc(1, sizeof(zarray_t));
if (za) za->el_sz = el_sz;
return za;
}
/**
* Frees all resources associated with the variable array structure which was
* created by zarray_create(). After calling, 'za' will no longer be valid for storage.
*/
static inline void zarray_destroy(zarray_t *za)
{
if (za == NULL)
return;
if (za->data != NULL)
free(za->data);
memset(za, 0, sizeof(zarray_t));
free(za);
}
/** Allocate a new zarray that contains a copy of the data in the argument. **/
static inline zarray_t *zarray_copy(const zarray_t *za)
{
assert(za != NULL);
zarray_t *zb = (zarray_t*) calloc(1, sizeof(zarray_t));
zb->el_sz = za->el_sz;
zb->size = za->size;
zb->alloc = za->alloc;
zb->data = (char*) malloc(zb->alloc * zb->el_sz);
memcpy(zb->data, za->data, za->size * za->el_sz);
return zb;
}
static int iceillog2(int v)
{
v--;
v |= v >> 1;
v |= v >> 2;
v |= v >> 4;
v |= v >> 8;
v |= v >> 16;
v++;
return v;
}
/**
* Allocate a new zarray that contains a subset of the original
* elements. NOTE: end index is EXCLUSIVE, that is one past the last
* element you want.
*/
static inline zarray_t *zarray_copy_subset(const zarray_t *za,
int start_idx,
int end_idx_exclusive)
{
zarray_t *out = (zarray_t*) calloc(1, sizeof(zarray_t));
out->el_sz = za->el_sz;
out->size = end_idx_exclusive - start_idx;
out->alloc = iceillog2(out->size); // round up pow 2
out->data = (char*) malloc(out->alloc * out->el_sz);
memcpy(out->data, za->data +(start_idx*out->el_sz), out->size*out->el_sz);
return out;
}
/**
* Retrieves the number of elements currently being contained by the passed
* array, which may be different from its capacity. The index of the last element
* in the array will be one less than the returned value.
*/
static inline int zarray_size(const zarray_t *za)
{
assert(za != NULL);
return za->size;
}
/**
* Returns 1 if zarray_size(za) == 0,
* returns 0 otherwise.
*/
/*
JUST CALL zarray_size
int zarray_isempty(const zarray_t *za)
{
assert(za != NULL);
if (za->size <= 0)
return 1;
else
return 0;
}
*/
/**
* Allocates enough internal storage in the supplied variable array structure to
* guarantee that the supplied number of elements (capacity) can be safely stored.
*/
static inline void zarray_ensure_capacity(zarray_t *za, int capacity)
{
assert(za != NULL);
if (capacity <= za->alloc)
return;
while (za->alloc < capacity) {
za->alloc += 8; // use less memory // *= 2;
if (za->alloc < 8)
za->alloc = 8;
}
za->data = (char*) realloc(za->data, za->alloc * za->el_sz);
}
/**
* Adds a new element to the end of the supplied array, and sets its value
* (by copying) from the data pointed to by the supplied pointer 'p'.
* Automatically ensures that enough storage space is available for the new element.
*/
static inline void zarray_add(zarray_t *za, const void *p)
{
assert(za != NULL);
assert(p != NULL);
zarray_ensure_capacity(za, za->size + 1);
memcpy(&za->data[za->size*za->el_sz], p, za->el_sz);
za->size++;
}
/**
* Adds a new element to the end of the supplied array, and sets its value
* (by copying) from the data pointed to by the supplied pointer 'p'.
* Automatically ensures that enough storage space is available for the new element.
*/
static inline void zarray_add_fail_ok(zarray_t *za, const void *p)
{
assert(za != NULL);
assert(p != NULL);
if ((za->size + 1) > za->alloc)
{
char *old_data = za->data;
int old_alloc = za->alloc;
while (za->alloc < (za->size + 1)) {
za->alloc += 8; // use less memory // *= 2;
if (za->alloc < 8)
za->alloc = 8;
}
za->data = (char*) umm_realloc(za->data, za->alloc * za->el_sz);
if (!za->data) {
za->data = old_data;
za->alloc = old_alloc;
return;
}
}
memcpy(&za->data[za->size*za->el_sz], p, za->el_sz);
za->size++;
}
/**
* Retrieves the element from the supplied array located at the zero-based
* index of 'idx' and copies its value into the variable pointed to by the pointer
* 'p'.
*/
static inline void zarray_get(const zarray_t *za, int idx, void *p)
{
assert(za != NULL);
assert(p != NULL);
assert(idx >= 0);
assert(idx < za->size);
memcpy(p, &za->data[idx*za->el_sz], za->el_sz);
}
/**
* Similar to zarray_get(), but returns a "live" pointer to the internal
* storage, avoiding a memcpy. This pointer is not valid across
* operations which might move memory around (i.e. zarray_remove_value(),
* zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()).
* 'p' should be a pointer to the pointer which will be set to the internal address.
*/
inline static void zarray_get_volatile(const zarray_t *za, int idx, void *p)
{
assert(za != NULL);
assert(p != NULL);
assert(idx >= 0);
assert(idx < za->size);
*((void**) p) = &za->data[idx*za->el_sz];
}
inline static void zarray_truncate(zarray_t *za, int sz)
{
assert(za != NULL);
assert(sz <= za->size);
za->size = sz;
}
/**
* Copies the memory array used internally by zarray to store its owned
* elements to the address pointed by 'buffer'. It is the caller's responsibility
* to allocate zarray_size()*el_sz bytes for the copy to be stored and
* to free the memory when no longer needed. The memory allocated at 'buffer'
* and the internal zarray storage must not overlap. 'buffer_bytes' should be
* the size of the 'buffer' memory space, in bytes, and must be at least
* zarray_size()*el_sz.
*
* Returns the number of bytes copied into 'buffer'.
*/
static inline size_t zarray_copy_data(const zarray_t *za, void *buffer, size_t buffer_bytes)
{
assert(za != NULL);
assert(buffer != NULL);
assert(buffer_bytes >= za->el_sz * za->size);
memcpy(buffer, za->data, za->el_sz * za->size);
return za->el_sz * za->size;
}
/**
* Removes the entry at index 'idx'.
* If shuffle is true, the last element in the array will be placed in
* the newly-open space; if false, the zarray is compacted.
*/
static inline void zarray_remove_index(zarray_t *za, int idx, int shuffle)
{
assert(za != NULL);
assert(idx >= 0);
assert(idx < za->size);
if (shuffle) {
if (idx < za->size-1)
memcpy(&za->data[idx*za->el_sz], &za->data[(za->size-1)*za->el_sz], za->el_sz);
za->size--;
return;
} else {
// size = 10, idx = 7. Should copy 2 entries (at idx=8 and idx=9).
// size = 10, idx = 9. Should copy 0 entries.
int ncopy = za->size - idx - 1;
if (ncopy > 0)
memmove(&za->data[idx*za->el_sz], &za->data[(idx+1)*za->el_sz], ncopy*za->el_sz);
za->size--;
return;
}
}
/**
* Remove the entry whose value is equal to the value pointed to by 'p'.
* If shuffle is true, the last element in the array will be placed in
* the newly-open space; if false, the zarray is compacted. At most
* one element will be removed.
*
* Note that objects will be compared using memcmp over the full size
* of the value. If the value is a struct that contains padding,
* differences in the padding bytes can cause comparisons to
* fail. Thus, it remains best practice to bzero all structs so that
* the padding is set to zero.
*
* Returns the number of elements removed (0 or 1).
*/
// remove the entry whose value is equal to the value pointed to by p.
// if shuffle is true, the last element in the array will be placed in
// the newly-open space; if false, the zarray is compacted.
static inline int zarray_remove_value(zarray_t *za, const void *p, int shuffle)
{
assert(za != NULL);
assert(p != NULL);
for (int idx = 0; idx < za->size; idx++) {
if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) {
zarray_remove_index(za, idx, shuffle);
return 1;
}
}
return 0;
}
/**
* Creates a new entry and inserts it into the array so that it will have the
* index 'idx' (i.e. before the item which currently has that index). The value
* of the new entry is set to (copied from) the data pointed to by 'p'. 'idx'
* can be one larger than the current max index to place the new item at the end
* of the array, or zero to add it to an empty array.
*/
static inline void zarray_insert(zarray_t *za, int idx, const void *p)
{
assert(za != NULL);
assert(p != NULL);
assert(idx >= 0);
assert(idx <= za->size);
zarray_ensure_capacity(za, za->size + 1);
// size = 10, idx = 7. Should copy three entries (idx=7, idx=8, idx=9)
int ncopy = za->size - idx;
memmove(&za->data[(idx+1)*za->el_sz], &za->data[idx*za->el_sz], ncopy*za->el_sz);
memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
za->size++;
}
/**
* Sets the value of the current element at index 'idx' by copying its value from
* the data pointed to by 'p'. The previous value of the changed element will be
* copied into the data pointed to by 'outp' if it is not null.
*/
static inline void zarray_set(zarray_t *za, int idx, const void *p, void *outp)
{
assert(za != NULL);
assert(p != NULL);
assert(idx >= 0);
assert(idx < za->size);
if (outp != NULL)
memcpy(outp, &za->data[idx*za->el_sz], za->el_sz);
memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
}
/**
* Calls the supplied function for every element in the array in index order.
* The map function will be passed a pointer to each element in turn and must
* have the following format:
*
* void map_function(element_type *element)
*/
static inline void zarray_map(zarray_t *za, void (*f)(void*))
{
assert(za != NULL);
assert(f != NULL);
for (int idx = 0; idx < za->size; idx++)
f(&za->data[idx*za->el_sz]);
}
/**
* Calls the supplied function for every element in the array in index order.
* HOWEVER values are passed to the function, not pointers to values. In the
* case where the zarray stores object pointers, zarray_vmap allows you to
* pass in the object's destroy function (or free) directly. Can only be used
* with zarray's which contain pointer data. The map function should have the
* following format:
*
* void map_function(element_type *element)
*/
void zarray_vmap(zarray_t *za, void (*f)());
/**
* Removes all elements from the array and sets its size to zero. Pointers to
* any data elements obtained i.e. by zarray_get_volatile() will no longer be
* valid.
*/
static inline void zarray_clear(zarray_t *za)
{
assert(za != NULL);
za->size = 0;
}
/**
* Determines whether any element in the array has a value which matches the
* data pointed to by 'p'.
*
* Returns 1 if a match was found anywhere in the array, else 0.
*/
static inline int zarray_contains(const zarray_t *za, const void *p)
{
assert(za != NULL);
assert(p != NULL);
for (int idx = 0; idx < za->size; idx++) {
if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) {
return 1;
}
}
return 0;
}
/**
* Uses qsort() to sort the elements contained by the array in ascending order.
* Uses the supplied comparison function to determine the appropriate order.
*
* The comparison function will be passed a pointer to two elements to be compared
* and should return a measure of the difference between them (see strcmp()).
* I.e. it should return a negative number if the first element is 'less than'
* the second, zero if they are equivalent, and a positive number if the first
* element is 'greater than' the second. The function should have the following format:
*
* int comparison_function(const element_type *first, const element_type *second)
*
* zstrcmp() can be used as the comparison function for string elements, which
* will call strcmp() internally.
*/
static inline void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*))
{
assert(za != NULL);
assert(compar != NULL);
if (za->size == 0)
return;
qsort(za->data, za->size, za->el_sz, compar);
}
/**
* A comparison function for comparing strings which can be used by zarray_sort()
* to sort arrays with char* elements.
*/
int zstrcmp(const void * a_pp, const void * b_pp);
/**
* Find the index of an element, or return -1 if not found. Remember that p is
* a pointer to the element.
**/
// returns -1 if not in array. Remember p is a pointer to the item.
static inline int zarray_index_of(const zarray_t *za, const void *p)
{
assert(za != NULL);
assert(p != NULL);
for (int i = 0; i < za->size; i++) {
if (!memcmp(p, &za->data[i*za->el_sz], za->el_sz))
return i;
}
return -1;
}
/**
* Add all elements from 'source' into 'dest'. el_size must be the same
* for both lists
**/
static inline void zarray_add_all(zarray_t * dest, const zarray_t * source)
{
assert(dest->el_sz == source->el_sz);
// Don't allocate on stack because el_sz could be larger than ~8 MB
// stack size
char *tmp = (char*)calloc(1, dest->el_sz);
for (int i = 0; i < zarray_size(source); i++) {
zarray_get(source, i, tmp);
zarray_add(dest, tmp);
}
free(tmp);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "zarray.c"
////////////////////////////////////////////////////////////////////////////////////////////////////
int zstrcmp(const void * a_pp, const void * b_pp)
{
assert(a_pp != NULL);
assert(b_pp != NULL);
char * a = *(void**)a_pp;
char * b = *(void**)b_pp;
return strcmp(a,b);
}
void zarray_vmap(zarray_t *za, void (*f)())
{
assert(za != NULL);
assert(f != NULL);
assert(za->el_sz == sizeof(void*));
for (int idx = 0; idx < za->size; idx++) {
void *pp = &za->data[idx*za->el_sz];
void *p = *(void**) pp;
f(p);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "math_util.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
#ifndef M_TWOPI
# define M_TWOPI 6.2831853071795862319959 /* 2*pi */
#endif
#ifndef M_PI
# define M_PI 3.141592653589793238462643383279502884196
#endif
#define to_radians(x) ( (x) * (M_PI / 180.0 ))
#define to_degrees(x) ( (x) * (180.0 / M_PI ))
#define max(A, B) (A < B ? B : A)
#define min(A, B) (A < B ? A : B)
/* DEPRECATE, threshold meaningless without context.
static inline int dequals(double a, double b)
{
double thresh = 1e-9;
return (fabs(a-b) < thresh);
}
*/
static inline int dequals_mag(double a, double b, double thresh)
{
return (fabs(a-b) < thresh);
}
static inline int isq(int v)
{
return v*v;
}
static inline float fsq(float v)
{
return v*v;
}
static inline double sq(double v)
{
return v*v;
}
static inline double sgn(double v)
{
return (v>=0) ? 1 : -1;
}
// random number between [0, 1)
static inline float randf()
{
return ((float) rand()) / (RAND_MAX + 1.0);
}
static inline float signed_randf()
{
return randf()*2 - 1;
}
// return a random integer between [0, bound)
static inline int irand(int bound)
{
int v = (int) (randf()*bound);
if (v == bound)
return (bound-1);
//assert(v >= 0);
//assert(v < bound);
return v;
}
/** Map vin to [0, 2*PI) **/
static inline double mod2pi_positive(double vin)
{
return vin - M_TWOPI * floor(vin / M_TWOPI);
}
/** Map vin to [-PI, PI) **/
static inline double mod2pi(double vin)
{
return mod2pi_positive(vin + M_PI) - M_PI;
}
/** Return vin such that it is within PI degrees of ref **/
static inline double mod2pi_ref(double ref, double vin)
{
return ref + mod2pi(vin - ref);
}
/** Map vin to [0, 360) **/
static inline double mod360_positive(double vin)
{
return vin - 360 * floor(vin / 360);
}
/** Map vin to [-180, 180) **/
static inline double mod360(double vin)
{
return mod360_positive(vin + 180) - 180;
}
static inline int theta_to_int(double theta, int max)
{
theta = mod2pi_ref(M_PI, theta);
int v = (int) (theta / M_TWOPI * max);
if (v == max)
v = 0;
assert (v >= 0 && v < max);
return v;
}
static inline int imin(int a, int b)
{
return (a < b) ? a : b;
}
static inline int imax(int a, int b)
{
return (a > b) ? a : b;
}
static inline int64_t imin64(int64_t a, int64_t b)
{
return (a < b) ? a : b;
}
static inline int64_t imax64(int64_t a, int64_t b)
{
return (a > b) ? a : b;
}
static inline int iclamp(int v, int minv, int maxv)
{
return imax(minv, imin(v, maxv));
}
static inline double dclamp(double a, double min, double max)
{
if (a < min)
return min;
if (a > max)
return max;
return a;
}
static inline int fltcmp (float f1, float f2)
{
float epsilon = f1-f2;
if (epsilon < 0.0)
return -1;
else if (epsilon > 0.0)
return 1;
else
return 0;
}
static inline int dblcmp (double d1, double d2)
{
double epsilon = d1-d2;
if (epsilon < 0.0)
return -1;
else if (epsilon > 0.0)
return 1;
else
return 0;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "svd22.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
void svd22(const double A[4], double U[4], double S[2], double V[4]);
// for the matrix [a b; b d]
void svd_sym_singular_values(double A00, double A01, double A11,
double *Lmin, double *Lmax);
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "svd22.c"
////////////////////////////////////////////////////////////////////////////////////////////////////
/** SVD 2x2.
Computes singular values and vectors without squaring the input
matrix. With double precision math, results are accurate to about
1E-16.
U = [ cos(theta) -sin(theta) ]
[ sin(theta) cos(theta) ]
S = [ e 0 ]
[ 0 f ]
V = [ cos(phi) -sin(phi) ]
[ sin(phi) cos(phi) ]
Our strategy is basically to analytically multiply everything out
and then rearrange so that we can solve for theta, phi, e, and
f. (Derivation by ebolson@umich.edu 5/2016)
V' = [ CP SP ]
[ -SP CP ]
USV' = [ CT -ST ][ e*CP e*SP ]
[ ST CT ][ -f*SP f*CP ]
= [e*CT*CP + f*ST*SP e*CT*SP - f*ST*CP ]
[e*ST*CP - f*SP*CT e*SP*ST + f*CP*CT ]
A00+A11 = e*CT*CP + f*ST*SP + e*SP*ST + f*CP*CT
= e*(CP*CT + SP*ST) + f*(SP*ST + CP*CT)
= (e+f)(CP*CT + SP*ST)
B0 = (e+f)*cos(P-T)
A00-A11 = e*CT*CP + f*ST*SP - e*SP*ST - f*CP*CT
= e*(CP*CT - SP*ST) - f*(-ST*SP + CP*CT)
= (e-f)(CP*CT - SP*ST)
B1 = (e-f)*cos(P+T)
A01+A10 = e*CT*SP - f*ST*CP + e*ST*CP - f*SP*CT
= e(CT*SP + ST*CP) - f*(ST*CP + SP*CT)
= (e-f)*(CT*SP + ST*CP)
B2 = (e-f)*sin(P+T)
A01-A10 = e*CT*SP - f*ST*CP - e*ST*CP + f*SP*CT
= e*(CT*SP - ST*CP) + f(SP*CT - ST*CP)
= (e+f)*(CT*SP - ST*CP)
B3 = (e+f)*sin(P-T)
B0 = (e+f)*cos(P-T)
B1 = (e-f)*cos(P+T)
B2 = (e-f)*sin(P+T)
B3 = (e+f)*sin(P-T)
B3/B0 = tan(P-T)
B2/B1 = tan(P+T)
**/
void svd22(const double A[4], double U[4], double S[2], double V[4])
{
double A00 = A[0];
double A01 = A[1];
double A10 = A[2];
double A11 = A[3];
double B0 = A00 + A11;
double B1 = A00 - A11;
double B2 = A01 + A10;
double B3 = A01 - A10;
double PminusT = atan2(B3, B0);
double PplusT = atan2(B2, B1);
double P = (PminusT + PplusT) / 2;
double T = (-PminusT + PplusT) / 2;
double CP = cos(P), SP = sin(P);
double CT = cos(T), ST = sin(T);
U[0] = CT;
U[1] = -ST;
U[2] = ST;
U[3] = CT;
V[0] = CP;
V[1] = -SP;
V[2] = SP;
V[3] = CP;
// C0 = e+f. There are two ways to compute C0; we pick the one
// that is better conditioned.
double CPmT = cos(P-T), SPmT = sin(P-T);
double C0 = 0;
if (fabs(CPmT) > fabs(SPmT))
C0 = B0 / CPmT;
else
C0 = B3 / SPmT;
// C1 = e-f. There are two ways to compute C1; we pick the one
// that is better conditioned.
double CPpT = cos(P+T), SPpT = sin(P+T);
double C1 = 0;
if (fabs(CPpT) > fabs(SPpT))
C1 = B1 / CPpT;
else
C1 = B2 / SPpT;
// e and f are the singular values
double e = (C0 + C1) / 2;
double f = (C0 - C1) / 2;
if (e < 0) {
e = -e;
U[0] = -U[0];
U[2] = -U[2];
}
if (f < 0) {
f = -f;
U[1] = -U[1];
U[3] = -U[3];
}
// sort singular values.
if (e > f) {
// already in big-to-small order.
S[0] = e;
S[1] = f;
} else {
// Curiously, this code never seems to get invoked. Why is it
// that S[0] always ends up the dominant vector? However,
// this code has been tested (flipping the logic forces us to
// sort the singular values in ascending order).
//
// P = [ 0 1 ; 1 0 ]
// USV' = (UP)(PSP)(PV')
// = (UP)(PSP)(VP)'
// = (UP)(PSP)(P'V')'
S[0] = f;
S[1] = e;
// exchange columns of U and V
double tmp[2];
tmp[0] = U[0];
tmp[1] = U[2];
U[0] = U[1];
U[2] = U[3];
U[1] = tmp[0];
U[3] = tmp[1];
tmp[0] = V[0];
tmp[1] = V[2];
V[0] = V[1];
V[2] = V[3];
V[1] = tmp[0];
V[3] = tmp[1];
}
/*
double SM[4] = { S[0], 0, 0, S[1] };
doubles_print_mat(U, 2, 2, "%20.10g");
doubles_print_mat(SM, 2, 2, "%20.10g");
doubles_print_mat(V, 2, 2, "%20.10g");
printf("A:\n");
doubles_print_mat(A, 2, 2, "%20.10g");
double SVt[4];
doubles_mat_ABt(SM, 2, 2, V, 2, 2, SVt, 2, 2);
double USVt[4];
doubles_mat_AB(U, 2, 2, SVt, 2, 2, USVt, 2, 2);
printf("USVt\n");
doubles_print_mat(USVt, 2, 2, "%20.10g");
double diff[4];
for (int i = 0; i < 4; i++)
diff[i] = A[i] - USVt[i];
printf("diff\n");
doubles_print_mat(diff, 2, 2, "%20.10g");
*/
}
// for the matrix [a b; b d]
void svd_sym_singular_values(double A00, double A01, double A11,
double *Lmin, double *Lmax)
{
double A10 = A01;
double B0 = A00 + A11;
double B1 = A00 - A11;
double B2 = A01 + A10;
double B3 = A01 - A10;
double PminusT = atan2(B3, B0);
double PplusT = atan2(B2, B1);
double P = (PminusT + PplusT) / 2;
double T = (-PminusT + PplusT) / 2;
// C0 = e+f. There are two ways to compute C0; we pick the one
// that is better conditioned.
double CPmT = cos(P-T), SPmT = sin(P-T);
double C0 = 0;
if (fabs(CPmT) > fabs(SPmT))
C0 = B0 / CPmT;
else
C0 = B3 / SPmT;
// C1 = e-f. There are two ways to compute C1; we pick the one
// that is better conditioned.
double CPpT = cos(P+T), SPpT = sin(P+T);
double C1 = 0;
if (fabs(CPpT) > fabs(SPpT))
C1 = B1 / CPpT;
else
C1 = B2 / SPpT;
// e and f are the singular values
double e = (C0 + C1) / 2;
double f = (C0 - C1) / 2;
*Lmin = fmin(e, f);
*Lmax = fmax(e, f);
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "matd.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a matrix structure for holding double-precision values with
* data in row-major order (i.e. index = row*ncols + col).
*
* nrows and ncols are 1-based counts with the exception that a scalar (non-matrix)
* is represented with nrows=0 and/or ncols=0.
*/
typedef struct
{
unsigned int nrows, ncols;
double data[];
// double *data;
} matd_t;
#define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage };
/**
* Defines a small value which can be used in place of zero for approximating
* calculations which are singular at zero values (i.e. inverting a matrix with
* a zero or near-zero determinant).
*/
#define MATD_EPS 1e-8
/**
* A macro to reference a specific matd_t data element given it's zero-based
* row and column indexes. Suitable for both retrieval and assignment.
*/
#define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))]
/**
* Creates a double matrix with the given number of rows and columns (or a scalar
* in the case where rows=0 and/or cols=0). All data elements will be initialized
* to zero. It is the caller's responsibility to call matd_destroy() on the
* returned matrix.
*/
matd_t *matd_create(int rows, int cols);
/**
* Creates a double matrix with the given number of rows and columns (or a scalar
* in the case where rows=0 and/or cols=0). All data elements will be initialized
* using the supplied array of data, which must contain at least rows*cols elements,
* arranged in row-major order (i.e. index = row*ncols + col). It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*/
matd_t *matd_create_data(int rows, int cols, const double *data);
/**
* Creates a double matrix with the given number of rows and columns (or a scalar
* in the case where rows=0 and/or cols=0). All data elements will be initialized
* using the supplied array of float data, which must contain at least rows*cols elements,
* arranged in row-major order (i.e. index = row*ncols + col). It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*/
matd_t *matd_create_dataf(int rows, int cols, const float *data);
/**
* Creates a square identity matrix with the given number of rows (and
* therefore columns), or a scalar with value 1 in the case where dim=0.
* It is the caller's responsibility to call matd_destroy() on the
* returned matrix.
*/
matd_t *matd_identity(int dim);
/**
* Creates a scalar with the supplied value 'v'. It is the caller's responsibility
* to call matd_destroy() on the returned matrix.
*
* NOTE: Scalars are different than 1x1 matrices (implementation note:
* they are encoded as 0x0 matrices). For example: for matrices A*B, A
* and B must both have specific dimensions. However, if A is a
* scalar, there are no restrictions on the size of B.
*/
matd_t *matd_create_scalar(double v);
/**
* Retrieves the cell value for matrix 'm' at the given zero-based row and column index.
* Performs more thorough validation checking than MATD_EL().
*/
double matd_get(const matd_t *m, int row, int col);
/**
* Assigns the given value to the matrix cell at the given zero-based row and
* column index. Performs more thorough validation checking than MATD_EL().
*/
void matd_put(matd_t *m, int row, int col, double value);
/**
* Retrieves the scalar value of the given element ('m' must be a scalar).
* Performs more thorough validation checking than MATD_EL().
*/
double matd_get_scalar(const matd_t *m);
/**
* Assigns the given value to the supplied scalar element ('m' must be a scalar).
* Performs more thorough validation checking than MATD_EL().
*/
void matd_put_scalar(matd_t *m, double value);
/**
* Creates an exact copy of the supplied matrix 'm'. It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*/
matd_t *matd_copy(const matd_t *m);
/**
* Creates a copy of a subset of the supplied matrix 'a'. The subset will include
* rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1',
* inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0)
* will return only the first cell). Cannot be used on scalars or to extend
* beyond the number of rows/columns of 'a'. It is the caller's responsibility to
* call matd_destroy() on the returned matrix.
*/
matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1);
/**
* Prints the supplied matrix 'm' to standard output by applying the supplied
* printf format specifier 'fmt' for each individual element. Each row will
* be printed on a separate newline.
*/
void matd_print(const matd_t *m, const char *fmt);
/**
* Prints the transpose of the supplied matrix 'm' to standard output by applying
* the supplied printf format specifier 'fmt' for each individual element. Each
* row will be printed on a separate newline.
*/
void matd_print_transpose(const matd_t *m, const char *fmt);
/**
* Adds the two supplied matrices together, cell-by-cell, and returns the results
* as a new matrix of the same dimensions. The supplied matrices must have
* identical dimensions. It is the caller's responsibility to call matd_destroy()
* on the returned matrix.
*/
matd_t *matd_add(const matd_t *a, const matd_t *b);
/**
* Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the
* contents of 'a' with the results. The supplied matrices must have
* identical dimensions.
*/
void matd_add_inplace(matd_t *a, const matd_t *b);
/**
* Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results
* as a new matrix of the same dimensions. The supplied matrices must have
* identical dimensions. It is the caller's responsibility to call matd_destroy()
* on the returned matrix.
*/
matd_t *matd_subtract(const matd_t *a, const matd_t *b);
/**
* Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the
* contents of 'a' with the results. The supplied matrices must have
* identical dimensions.
*/
void matd_subtract_inplace(matd_t *a, const matd_t *b);
/**
* Scales all cell values of matrix 'a' by the given scale factor 's' and
* returns the result as a new matrix of the same dimensions. It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*/
matd_t *matd_scale(const matd_t *a, double s);
/**
* Scales all cell values of matrix 'a' by the given scale factor 's' and
* overwrites the contents of 'a' with the results.
*/
void matd_scale_inplace(matd_t *a, double s);
/**
* Multiplies the two supplied matrices together (matrix product), and returns the
* results as a new matrix. The supplied matrices must have dimensions such that
* columns(a) = rows(b). The returned matrix will have a row count of rows(a)
* and a column count of columns(b). It is the caller's responsibility to call
* matd_destroy() on the returned matrix.
*/
matd_t *matd_multiply(const matd_t *a, const matd_t *b);
/**
* Creates a matrix which is the transpose of the supplied matrix 'a'. It is the
* caller's responsibility to call matd_destroy() on the returned matrix.
*/
matd_t *matd_transpose(const matd_t *a);
/**
* Calculates the determinant of the supplied matrix 'a'.
*/
double matd_det(const matd_t *a);
/**
* Attempts to compute an inverse of the supplied matrix 'a' and return it as
* a new matrix. This is strictly only possible if the determinant of 'a' is
* non-zero (matd_det(a) != 0).
*
* If the determinant is zero, NULL is returned. It is otherwise the
* caller's responsibility to cope with the results caused by poorly
* conditioned matrices. (E.g.., if such a situation is likely to arise, compute
* the pseudo-inverse from the SVD.)
**/
matd_t *matd_inverse(const matd_t *a);
static inline void matd_set_data(matd_t *m, const double *data)
{
memcpy(m->data, data, m->nrows * m->ncols * sizeof(double));
}
/**
* Determines whether the supplied matrix 'a' is a scalar (positive return) or
* not (zero return, indicating a matrix of dimensions at least 1x1).
*/
static inline int matd_is_scalar(const matd_t *a)
{
assert(a != NULL);
return a->ncols == 0 || a->nrows == 0;
}
/**
* Determines whether the supplied matrix 'a' is a row or column vector
* (positive return) or not (zero return, indicating either 'a' is a scalar or a
* matrix with at least one dimension > 1).
*/
static inline int matd_is_vector(const matd_t *a)
{
assert(a != NULL);
return a->ncols == 1 || a->nrows == 1;
}
/**
* Determines whether the supplied matrix 'a' is a row or column vector
* with a dimension of 'len' (positive return) or not (zero return).
*/
static inline int matd_is_vector_len(const matd_t *a, int len)
{
assert(a != NULL);
return (a->ncols == 1 && a->nrows == len) || (a->ncols == len && a->nrows == 1);
}
/**
* Calculates the magnitude of the supplied matrix 'a'.
*/
double matd_vec_mag(const matd_t *a);
/**
* Calculates the magnitude of the distance between the points represented by
* matrices 'a' and 'b'. Both 'a' and 'b' must be vectors and have the same
* dimension (although one may be a row vector and one may be a column vector).
*/
double matd_vec_dist(const matd_t *a, const matd_t *b);
/**
* Same as matd_vec_dist, but only uses the first 'n' terms to compute distance
*/
double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n);
/**
* Calculates the dot product of two vectors. Both 'a' and 'b' must be vectors
* and have the same dimension (although one may be a row vector and one may be
* a column vector).
*/
double matd_vec_dot_product(const matd_t *a, const matd_t *b);
/**
* Calculates the normalization of the supplied vector 'a' (i.e. a unit vector
* of the same dimension and orientation as 'a' with a magnitude of 1) and returns
* it as a new vector. 'a' must be a vector of any dimension and must have a
* non-zero magnitude. It is the caller's responsibility to call matd_destroy()
* on the returned matrix.
*/
matd_t *matd_vec_normalize(const matd_t *a);
/**
* Calculates the cross product of supplied matrices 'a' and 'b' (i.e. a x b)
* and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension
* 3, but can be either row or column vectors. It is the caller's responsibility
* to call matd_destroy() on the returned matrix.
*/
matd_t *matd_crossproduct(const matd_t *a, const matd_t *b);
double matd_err_inf(const matd_t *a, const matd_t *b);
/**
* Creates a new matrix by applying a series of matrix operations, as expressed
* in 'expr', to the supplied list of matrices. Each matrix to be operated upon
* must be represented in the expression by a separate matrix placeholder, 'M',
* and there must be one matrix supplied as an argument for each matrix
* placeholder in the expression. All rules and caveats of the corresponding
* matrix operations apply to the operated-on matrices. It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*
* Available operators (in order of increasing precedence):
* M+M add two matrices together
* M-M subtract one matrix from another
* M*M multiply to matrices together (matrix product)
* MM multiply to matrices together (matrix product)
* -M negate a matrix
* M^-1 take the inverse of a matrix
* M' take the transpose of a matrix
*
* Expressions can be combined together and grouped by enclosing them in
* parenthesis, i.e.:
* -M(M+M+M)-(M*M)^-1
*
* Scalar values can be generated on-the-fly, i.e.:
* M*2.2 scales M by 2.2
* -2+M adds -2 to all elements of M
*
* All whitespace in the expression is ignored.
*/
matd_t *matd_op(const char *expr, ...);
/**
* Frees the memory associated with matrix 'm', being the result of an earlier
* call to a matd_*() function, after which 'm' will no longer be usable.
*/
void matd_destroy(matd_t *m);
typedef struct
{
matd_t *U;
matd_t *S;
matd_t *V;
} matd_svd_t;
/** Compute a complete SVD of a matrix. The SVD exists for all
* matrices. For a matrix MxN, we will have:
*
* A = U*S*V'
*
* where A is MxN, U is MxM (and is an orthonormal basis), S is MxN
* (and is diagonal up to machine precision), and V is NxN (and is an
* orthonormal basis).
*
* The caller is responsible for destroying U, S, and V.
**/
matd_svd_t matd_svd(matd_t *A);
#define MATD_SVD_NO_WARNINGS 1
matd_svd_t matd_svd_flags(matd_t *A, int flags);
////////////////////////////////
// PLU Decomposition
// All square matrices (even singular ones) have a partially-pivoted
// LU decomposition such that A = PLU, where P is a permutation
// matrix, L is a lower triangular matrix, and U is an upper
// triangular matrix.
//
typedef struct
{
// was the input matrix singular? When a zero pivot is found, this
// flag is set to indicate that this has happened.
int singular;
unsigned int *piv; // permutation indices
int pivsign; // either +1 or -1
// The matd_plu_t object returned "owns" the enclosed LU matrix. It
// is not expected that the returned object is itself useful to
// users: it contains the L and U information all smushed
// together.
matd_t *lu; // combined L and U matrices, permuted so they can be triangular.
} matd_plu_t;
matd_plu_t *matd_plu(const matd_t *a);
void matd_plu_destroy(matd_plu_t *mlu);
double matd_plu_det(const matd_plu_t *lu);
matd_t *matd_plu_p(const matd_plu_t *lu);
matd_t *matd_plu_l(const matd_plu_t *lu);
matd_t *matd_plu_u(const matd_plu_t *lu);
matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b);
// uses LU decomposition internally.
matd_t *matd_solve(matd_t *A, matd_t *b);
////////////////////////////////
// Cholesky Factorization
/**
* Creates a double matrix with the Cholesky lower triangular matrix
* of A. A must be symmetric, positive definite. It is the caller's
* responsibility to call matd_destroy() on the returned matrix.
*/
//matd_t *matd_cholesky(const matd_t *A);
typedef struct
{
int is_spd;
matd_t *u;
} matd_chol_t;
matd_chol_t *matd_chol(matd_t *A);
matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b);
void matd_chol_destroy(matd_chol_t *chol);
// only sensible on PSD matrices
matd_t *matd_chol_inverse(matd_t *a);
void matd_ltransposetriangle_solve(matd_t *u, const double *b, double *x);
void matd_ltriangle_solve(matd_t *u, const double *b, double *x);
void matd_utriangle_solve(matd_t *u, const double *b, double *x);
double matd_max(matd_t *m);
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "matd.c"
////////////////////////////////////////////////////////////////////////////////////////////////////
// a matd_t with rows=0 cols=0 is a SCALAR.
// to ease creating mati, matf, etc. in the future.
#define TYPE double
matd_t *matd_create(int rows, int cols)
{
assert(rows >= 0);
assert(cols >= 0);
if (rows == 0 || cols == 0)
return matd_create_scalar(0);
matd_t *m = calloc(1, sizeof(matd_t) + (rows*cols*sizeof(double)));
m->nrows = rows;
m->ncols = cols;
return m;
}
matd_t *matd_create_scalar(TYPE v)
{
matd_t *m = calloc(1, sizeof(matd_t) + sizeof(double));
m->nrows = 0;
m->ncols = 0;
m->data[0] = v;
return m;
}
matd_t *matd_create_data(int rows, int cols, const TYPE *data)
{
if (rows == 0 || cols == 0)
return matd_create_scalar(data[0]);
matd_t *m = matd_create(rows, cols);
for (int i = 0; i < rows * cols; i++)
m->data[i] = data[i];
return m;
}
matd_t *matd_create_dataf(int rows, int cols, const float *data)
{
if (rows == 0 || cols == 0)
return matd_create_scalar(data[0]);
matd_t *m = matd_create(rows, cols);
for (int i = 0; i < rows * cols; i++)
m->data[i] = (double)data[i];
return m;
}
matd_t *matd_identity(int dim)
{
if (dim == 0)
return matd_create_scalar(1);
matd_t *m = matd_create(dim, dim);
for (int i = 0; i < dim; i++)
MATD_EL(m, i, i) = 1;
return m;
}
// row and col are zero-based
TYPE matd_get(const matd_t *m, int row, int col)
{
assert(m != NULL);
assert(!matd_is_scalar(m));
assert(row >= 0);
assert(row < m->nrows);
assert(col >= 0);
assert(col < m->ncols);
return MATD_EL(m, row, col);
}
// row and col are zero-based
void matd_put(matd_t *m, int row, int col, TYPE value)
{
assert(m != NULL);
if (matd_is_scalar(m)) {
matd_put_scalar(m, value);
return;
}
assert(row >= 0);
assert(row < m->nrows);
assert(col >= 0);
assert(col < m->ncols);
MATD_EL(m, row, col) = value;
}
TYPE matd_get_scalar(const matd_t *m)
{
assert(m != NULL);
assert(matd_is_scalar(m));
return (m->data[0]);
}
void matd_put_scalar(matd_t *m, TYPE value)
{
assert(m != NULL);
assert(matd_is_scalar(m));
m->data[0] = value;
}
matd_t *matd_copy(const matd_t *m)
{
assert(m != NULL);
matd_t *x = matd_create(m->nrows, m->ncols);
if (matd_is_scalar(m))
x->data[0] = m->data[0];
else
memcpy(x->data, m->data, sizeof(TYPE)*m->ncols*m->nrows);
return x;
}
matd_t *matd_select(const matd_t * a, int r0, int r1, int c0, int c1)
{
assert(a != NULL);
assert(r0 >= 0 && r0 < a->nrows);
assert(c0 >= 0 && c0 < a->ncols);
int nrows = r1 - r0 + 1;
int ncols = c1 - c0 + 1;
matd_t * r = matd_create(nrows, ncols);
for (int row = r0; row <= r1; row++)
for (int col = c0; col <= c1; col++)
MATD_EL(r,row-r0,col-c0) = MATD_EL(a,row,col);
return r;
}
void matd_print(const matd_t *m, const char *fmt)
{
assert(m != NULL);
assert(fmt != NULL);
if (matd_is_scalar(m)) {
printf(fmt, MATD_EL(m, 0, 0));
printf("\n");
} else {
for (int i = 0; i < m->nrows; i++) {
for (int j = 0; j < m->ncols; j++) {
printf(fmt, MATD_EL(m, i, j));
}
printf("\n");
}
}
}
void matd_print_transpose(const matd_t *m, const char *fmt)
{
assert(m != NULL);
assert(fmt != NULL);
if (matd_is_scalar(m)) {
printf(fmt, MATD_EL(m, 0, 0));
printf("\n");
} else {
for (int j = 0; j < m->ncols; j++) {
for (int i = 0; i < m->nrows; i++) {
printf(fmt, MATD_EL(m, i, j));
}
printf("\n");
}
}
}
void matd_destroy(matd_t *m)
{
if (!m)
return;
assert(m != NULL);
free(m);
}
matd_t *matd_multiply(const matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
if (matd_is_scalar(a))
return matd_scale(b, a->data[0]);
if (matd_is_scalar(b))
return matd_scale(a, b->data[0]);
assert(a->ncols == b->nrows);
matd_t *m = matd_create(a->nrows, b->ncols);
for (int i = 0; i < m->nrows; i++) {
for (int j = 0; j < m->ncols; j++) {
TYPE acc = 0;
for (int k = 0; k < a->ncols; k++) {
acc += MATD_EL(a, i, k) * MATD_EL(b, k, j);
}
MATD_EL(m, i, j) = acc;
}
}
return m;
}
matd_t *matd_scale(const matd_t *a, double s)
{
assert(a != NULL);
if (matd_is_scalar(a))
return matd_create_scalar(a->data[0] * s);
matd_t *m = matd_create(a->nrows, a->ncols);
for (int i = 0; i < m->nrows; i++) {
for (int j = 0; j < m->ncols; j++) {
MATD_EL(m, i, j) = s * MATD_EL(a, i, j);
}
}
return m;
}
void matd_scale_inplace(matd_t *a, double s)
{
assert(a != NULL);
if (matd_is_scalar(a)) {
a->data[0] *= s;
return;
}
for (int i = 0; i < a->nrows; i++) {
for (int j = 0; j < a->ncols; j++) {
MATD_EL(a, i, j) *= s;
}
}
}
matd_t *matd_add(const matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(a->nrows == b->nrows);
assert(a->ncols == b->ncols);
if (matd_is_scalar(a))
return matd_create_scalar(a->data[0] + b->data[0]);
matd_t *m = matd_create(a->nrows, a->ncols);
for (int i = 0; i < m->nrows; i++) {
for (int j = 0; j < m->ncols; j++) {
MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j);
}
}
return m;
}
void matd_add_inplace(matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(a->nrows == b->nrows);
assert(a->ncols == b->ncols);
if (matd_is_scalar(a)) {
a->data[0] += b->data[0];
return;
}
for (int i = 0; i < a->nrows; i++) {
for (int j = 0; j < a->ncols; j++) {
MATD_EL(a, i, j) += MATD_EL(b, i, j);
}
}
}
matd_t *matd_subtract(const matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(a->nrows == b->nrows);
assert(a->ncols == b->ncols);
if (matd_is_scalar(a))
return matd_create_scalar(a->data[0] - b->data[0]);
matd_t *m = matd_create(a->nrows, a->ncols);
for (int i = 0; i < m->nrows; i++) {
for (int j = 0; j < m->ncols; j++) {
MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j);
}
}
return m;
}
void matd_subtract_inplace(matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(a->nrows == b->nrows);
assert(a->ncols == b->ncols);
if (matd_is_scalar(a)) {
a->data[0] -= b->data[0];
return;
}
for (int i = 0; i < a->nrows; i++) {
for (int j = 0; j < a->ncols; j++) {
MATD_EL(a, i, j) -= MATD_EL(b, i, j);
}
}
}
matd_t *matd_transpose(const matd_t *a)
{
assert(a != NULL);
if (matd_is_scalar(a))
return matd_create_scalar(a->data[0]);
matd_t *m = matd_create(a->ncols, a->nrows);
for (int i = 0; i < a->nrows; i++) {
for (int j = 0; j < a->ncols; j++) {
MATD_EL(m, j, i) = MATD_EL(a, i, j);
}
}
return m;
}
static
double matd_det_general(const matd_t *a)
{
// Use LU decompositon to calculate the determinant
matd_plu_t *mlu = matd_plu(a);
matd_t *L = matd_plu_l(mlu);
matd_t *U = matd_plu_u(mlu);
// The determinants of the L and U matrices are the products of
// their respective diagonal elements
double detL = 1; double detU = 1;
for (int i = 0; i < a->nrows; i++) {
detL *= matd_get(L, i, i);
detU *= matd_get(U, i, i);
}
// The determinant of a can be calculated as
// epsilon*det(L)*det(U),
// where epsilon is just the sign of the corresponding permutation
// (which is +1 for an even number of permutations and is −1
// for an uneven number of permutations).
double det = mlu->pivsign * detL * detU;
// Cleanup
matd_plu_destroy(mlu);
matd_destroy(L);
matd_destroy(U);
return det;
}
double matd_det(const matd_t *a)
{
assert(a != NULL);
assert(a->nrows == a->ncols);
switch(a->nrows) {
case 0:
// scalar: invalid
assert(a->nrows > 0);
break;
case 1:
// 1x1 matrix
return a->data[0];
case 2:
// 2x2 matrix
return a->data[0] * a->data[3] - a->data[1] * a->data[2];
case 3:
// 3x3 matrix
return a->data[0]*a->data[4]*a->data[8]
- a->data[0]*a->data[5]*a->data[7]
+ a->data[1]*a->data[5]*a->data[6]
- a->data[1]*a->data[3]*a->data[8]
+ a->data[2]*a->data[3]*a->data[7]
- a->data[2]*a->data[4]*a->data[6];
case 4: {
// 4x4 matrix
double m00 = MATD_EL(a,0,0), m01 = MATD_EL(a,0,1), m02 = MATD_EL(a,0,2), m03 = MATD_EL(a,0,3);
double m10 = MATD_EL(a,1,0), m11 = MATD_EL(a,1,1), m12 = MATD_EL(a,1,2), m13 = MATD_EL(a,1,3);
double m20 = MATD_EL(a,2,0), m21 = MATD_EL(a,2,1), m22 = MATD_EL(a,2,2), m23 = MATD_EL(a,2,3);
double m30 = MATD_EL(a,3,0), m31 = MATD_EL(a,3,1), m32 = MATD_EL(a,3,2), m33 = MATD_EL(a,3,3);
return m00 * m11 * m22 * m33 - m00 * m11 * m23 * m32 -
m00 * m21 * m12 * m33 + m00 * m21 * m13 * m32 + m00 * m31 * m12 * m23 -
m00 * m31 * m13 * m22 - m10 * m01 * m22 * m33 +
m10 * m01 * m23 * m32 + m10 * m21 * m02 * m33 -
m10 * m21 * m03 * m32 - m10 * m31 * m02 * m23 +
m10 * m31 * m03 * m22 + m20 * m01 * m12 * m33 -
m20 * m01 * m13 * m32 - m20 * m11 * m02 * m33 +
m20 * m11 * m03 * m32 + m20 * m31 * m02 * m13 -
m20 * m31 * m03 * m12 - m30 * m01 * m12 * m23 +
m30 * m01 * m13 * m22 + m30 * m11 * m02 * m23 -
m30 * m11 * m03 * m22 - m30 * m21 * m02 * m13 +
m30 * m21 * m03 * m12;
}
default:
return matd_det_general(a);
}
assert(0);
return 0;
}
// returns NULL if the matrix is (exactly) singular. Caller is
// otherwise responsible for knowing how to cope with badly
// conditioned matrices.
matd_t *matd_inverse(const matd_t *x)
{
matd_t *m = NULL;
assert(x != NULL);
assert(x->nrows == x->ncols);
if (matd_is_scalar(x)) {
if (x->data[0] == 0)
return NULL;
return matd_create_scalar(1.0 / x->data[0]);
}
switch(x->nrows) {
case 1: {
double det = x->data[0];
if (det == 0)
return NULL;
double invdet = 1.0 / det;
m = matd_create(x->nrows, x->nrows);
MATD_EL(m, 0, 0) = 1.0 * invdet;
return m;
}
case 2: {
double det = x->data[0] * x->data[3] - x->data[1] * x->data[2];
if (det == 0)
return NULL;
double invdet = 1.0 / det;
m = matd_create(x->nrows, x->nrows);
MATD_EL(m, 0, 0) = MATD_EL(x, 1, 1) * invdet;
MATD_EL(m, 0, 1) = - MATD_EL(x, 0, 1) * invdet;
MATD_EL(m, 1, 0) = - MATD_EL(x, 1, 0) * invdet;
MATD_EL(m, 1, 1) = MATD_EL(x, 0, 0) * invdet;
return m;
}
default: {
matd_plu_t *plu = matd_plu(x);
matd_t *inv = NULL;
if (!plu->singular) {
matd_t *ident = matd_identity(x->nrows);
inv = matd_plu_solve(plu, ident);
matd_destroy(ident);
}
matd_plu_destroy(plu);
return inv;
}
}
return NULL; // unreachable
}
// TODO Optimization: Some operations we could perform in-place,
// saving some memory allocation work. E.g., ADD, SUBTRACT. Just need
// to make sure that we don't do an in-place modification on a matrix
// that was an input argument!
// handle right-associative operators, greedily consuming them. These
// include transpose and inverse. This is called by the main recursion
// method.
static inline matd_t *matd_op_gobble_right(const char *expr, int *pos, matd_t *acc, matd_t **garb, int *garbpos)
{
while (expr[*pos] != 0) {
switch (expr[*pos]) {
case '\'': {
assert(acc != NULL); // either a syntax error or a math op failed, producing null
matd_t *res = matd_transpose(acc);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
(*pos)++;
break;
}
// handle inverse ^-1. No other exponents are allowed.
case '^': {
assert(acc != NULL);
assert(expr[*pos+1] == '-');
assert(expr[*pos+2] == '1');
matd_t *res = matd_inverse(acc);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
(*pos)+=3;
break;
}
default:
return acc;
}
}
return acc;
}
// @garb, garbpos A list of every matrix allocated during evaluation... used to assist cleanup.
// @oneterm: we should return at the end of this term (i.e., stop at a PLUS, MINUS, LPAREN).
static matd_t *matd_op_recurse(const char *expr, int *pos, matd_t *acc, matd_t **args, int *argpos,
matd_t **garb, int *garbpos, int oneterm)
{
while (expr[*pos] != 0) {
switch (expr[*pos]) {
case '(': {
if (oneterm && acc != NULL)
return acc;
(*pos)++;
matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 0);
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
if (acc == NULL) {
acc = rhs;
} else {
matd_t *res = matd_multiply(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
}
break;
}
case ')': {
if (oneterm)
return acc;
(*pos)++;
return acc;
}
case '*': {
(*pos)++;
matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
if (acc == NULL) {
acc = rhs;
} else {
matd_t *res = matd_multiply(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
}
break;
}
case 'F': {
matd_t *rhs = args[*argpos];
garb[*garbpos] = rhs;
(*garbpos)++;
(*pos)++;
(*argpos)++;
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
if (acc == NULL) {
acc = rhs;
} else {
matd_t *res = matd_multiply(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
}
break;
}
case 'M': {
matd_t *rhs = args[*argpos];
(*pos)++;
(*argpos)++;
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
if (acc == NULL) {
acc = rhs;
} else {
matd_t *res = matd_multiply(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
}
break;
}
/*
case 'D': {
int rows = expr[*pos+1]-'0';
int cols = expr[*pos+2]-'0';
matd_t *rhs = matd_create(rows, cols);
break;
}
*/
// a constant (SCALAR) defined inline. Treat just like M, creating a matd_t on the fly.
// case '0':
// case '1':
// case '2':
// case '3':
// case '4':
// case '5':
// case '6':
// case '7':
// case '8':
// case '9':
// case '.': {
// const char *start = &expr[*pos];
// char *end;
// double s = strtod(start, &end);
// (*pos) += (end - start);
// matd_t *rhs = matd_create_scalar(s);
// garb[*garbpos] = rhs;
// (*garbpos)++;
// rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
// if (acc == NULL) {
// acc = rhs;
// } else {
// matd_t *res = matd_multiply(acc, rhs);
// garb[*garbpos] = res;
// (*garbpos)++;
// acc = res;
// }
// break;
// }
case '+': {
if (oneterm && acc != NULL)
return acc;
// don't support unary plus
assert(acc != NULL);
(*pos)++;
matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
matd_t *res = matd_add(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
break;
}
case '-': {
if (oneterm && acc != NULL)
return acc;
if (acc == NULL) {
// unary minus
(*pos)++;
matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
matd_t *res = matd_scale(rhs, -1);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
} else {
// subtract
(*pos)++;
matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
matd_t *res = matd_subtract(acc, rhs);
garb[*garbpos] = res;
(*garbpos)++;
acc = res;
}
break;
}
case ' ': {
// nothing to do. spaces are meaningless.
(*pos)++;
break;
}
default: {
fprintf(stderr, "matd_op(): Unknown character: '%c'\n", expr[*pos]);
assert(expr[*pos] != expr[*pos]);
}
}
}
return acc;
}
// always returns a new matrix.
matd_t *matd_op(const char *expr, ...)
{
int nargs = 0;
int exprlen = 0;
assert(expr != NULL);
for (const char *p = expr; *p != 0; p++) {
if (*p == 'M' || *p == 'F')
nargs++;
exprlen++;
}
assert(nargs > 0);
if (!exprlen) // expr = ""
return NULL;
va_list ap;
va_start(ap, expr);
matd_t *args[nargs];
for (int i = 0; i < nargs; i++) {
args[i] = va_arg(ap, matd_t*);
// XXX: sanity check argument; emit warning/error if args[i]
// doesn't look like a matd_t*.
}
va_end(ap);
int pos = 0;
int argpos = 0;
int garbpos = 0;
matd_t *garb[2*exprlen]; // can't create more than 2 new result per character
// one result, and possibly one argument to free
matd_t *res = matd_op_recurse(expr, &pos, NULL, args, &argpos, garb, &garbpos, 0);
// 'res' may need to be freed as part of garbage collection (i.e. expr = "F")
matd_t *res_copy = (res ? matd_copy(res) : NULL);
for (int i = 0; i < garbpos; i++) {
matd_destroy(garb[i]);
}
return res_copy;
}
double matd_vec_mag(const matd_t *a)
{
assert(a != NULL);
assert(matd_is_vector(a));
double mag = 0.0;
int len = a->nrows*a->ncols;
for (int i = 0; i < len; i++)
mag += sq(a->data[i]);
return sqrt(mag);
}
double matd_vec_dist(const matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(matd_is_vector(a) && matd_is_vector(b));
assert(a->nrows*a->ncols == b->nrows*b->ncols);
int lena = a->nrows*a->ncols;
return matd_vec_dist_n(a, b, lena);
}
double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n)
{
assert(a != NULL);
assert(b != NULL);
assert(matd_is_vector(a) && matd_is_vector(b));
int lena = a->nrows*a->ncols;
int lenb = b->nrows*b->ncols;
assert(n <= lena && n <= lenb);
double mag = 0.0;
for (int i = 0; i < n; i++)
mag += sq(a->data[i] - b->data[i]);
return sqrt(mag);
}
// find the index of the off-diagonal element with the largest mag
static inline int max_idx(const matd_t *A, int row, int maxcol)
{
int maxi = 0;
double maxv = -1;
for (int i = 0; i < maxcol; i++) {
if (i == row)
continue;
double v = fabs(MATD_EL(A, row, i));
if (v > maxv) {
maxi = i;
maxv = v;
}
}
return maxi;
}
double matd_vec_dot_product(const matd_t *a, const matd_t *b)
{
assert(a != NULL);
assert(b != NULL);
assert(matd_is_vector(a) && matd_is_vector(b));
int adim = a->ncols*a->nrows;
int bdim = b->ncols*b->nrows;
assert(adim == bdim);
double acc = 0;
for (int i = 0; i < adim; i++) {
acc += a->data[i] * b->data[i];
}
return acc;
}
matd_t *matd_vec_normalize(const matd_t *a)
{
assert(a != NULL);
assert(matd_is_vector(a));
double mag = matd_vec_mag(a);
assert(mag > 0);
matd_t *b = matd_create(a->nrows, a->ncols);
int len = a->nrows*a->ncols;
for(int i = 0; i < len; i++)
b->data[i] = a->data[i] / mag;
return b;
}
matd_t *matd_crossproduct(const matd_t *a, const matd_t *b)
{ // only defined for vecs (col or row) of length 3
assert(a != NULL);
assert(b != NULL);
assert(matd_is_vector_len(a, 3) && matd_is_vector_len(b, 3));
matd_t * r = matd_create(a->nrows, a->ncols);
r->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1];
r->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2];
r->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0];
return r;
}
TYPE matd_err_inf(const matd_t *a, const matd_t *b)
{
assert(a->nrows == b->nrows);
assert(a->ncols == b->ncols);
TYPE maxf = 0;
for (int i = 0; i < a->nrows; i++) {
for (int j = 0; j < a->ncols; j++) {
TYPE av = MATD_EL(a, i, j);
TYPE bv = MATD_EL(b, i, j);
TYPE err = fabs(av - bv);
maxf = fmax(maxf, err);
}
}
return maxf;
}
// Computes an SVD for square or tall matrices. This code doesn't work
// for wide matrices, because the bidiagonalization results in one
// non-zero element too far to the right for us to rotate away.
//
// Caller is responsible for destroying U, S, and V.
static matd_svd_t matd_svd_tall(matd_t *A, int flags)
{
matd_t *B = matd_copy(A);
// Apply householder reflections on each side to reduce A to
// bidiagonal form. Specifically:
//
// A = LS*B*RS'
//
// Where B is bidiagonal, and LS/RS are unitary.
//
// Why are we doing this? Some sort of transformation is necessary
// to reduce the matrix's nz elements to a square region. QR could
// work too. We need nzs confined to a square region so that the
// subsequent iterative process, which is based on rotations, can
// work. (To zero out a term at (i,j), our rotations will also
// affect (j,i).
//
// We prefer bidiagonalization over QR because it gets us "closer"
// to the SVD, which should mean fewer iterations.
// LS: cumulative left-handed transformations
matd_t *LS = matd_identity(A->nrows);
// RS: cumulative right-handed transformations.
matd_t *RS = matd_identity(A->ncols);
for (int hhidx = 0; hhidx < A->nrows; hhidx++) {
if (hhidx < A->ncols) {
// We construct the normal of the reflection plane: let u
// be the vector to reflect, x =[ M 0 0 0 ] the target
// location for u (u') after reflection (with M = ||u||).
//
// The normal vector is then n = (u - x), but since we
// could equally have the target location be x = [-M 0 0 0
// ], we could use n = (u + x).
//
// We then normalize n. To ensure a reasonable magnitude,
// we select the sign of M so as to maximize the magnitude
// of the first element of (x +/- M). (Otherwise, we could
// end up with a divide-by-zero if u[0] and M cancel.)
//
// The householder reflection matrix is then H=(I - nn'), and
// u' = Hu.
//
//
int vlen = A->nrows - hhidx;
double v[vlen];
double mag2 = 0;
for (int i = 0; i < vlen; i++) {
v[i] = MATD_EL(B, hhidx+i, hhidx);
mag2 += v[i]*v[i];
}
double oldv0 = v[0];
if (oldv0 < 0)
v[0] -= sqrt(mag2);
else
v[0] += sqrt(mag2);
mag2 += -oldv0*oldv0 + v[0]*v[0];
// normalize v
double mag = sqrt(mag2);
// this case arises with matrices of all zeros, for example.
if (mag == 0)
continue;
for (int i = 0; i < vlen; i++)
v[i] /= mag;
// Q = I - 2vv'
//matd_t *Q = matd_identity(A->nrows);
//for (int i = 0; i < vlen; i++)
// for (int j = 0; j < vlen; j++)
// MATD_EL(Q, i+hhidx, j+hhidx) -= 2*v[i]*v[j];
// LS = matd_op("F*M", LS, Q);
// Implementation: take each row of LS, compute dot product with n,
// subtract n (scaled by dot product) from it.
for (int i = 0; i < LS->nrows; i++) {
double dot = 0;
for (int j = 0; j < vlen; j++)
dot += MATD_EL(LS, i, hhidx+j) * v[j];
for (int j = 0; j < vlen; j++)
MATD_EL(LS, i, hhidx+j) -= 2*dot*v[j];
}
// B = matd_op("M*F", Q, B); // should be Q', but Q is symmetric.
for (int i = 0; i < B->ncols; i++) {
double dot = 0;
for (int j = 0; j < vlen; j++)
dot += MATD_EL(B, hhidx+j, i) * v[j];
for (int j = 0; j < vlen; j++)
MATD_EL(B, hhidx+j, i) -= 2*dot*v[j];
}
}
if (hhidx+2 < A->ncols) {
int vlen = A->ncols - hhidx - 1;
double v[vlen];
double mag2 = 0;
for (int i = 0; i < vlen; i++) {
v[i] = MATD_EL(B, hhidx, hhidx+i+1);
mag2 += v[i]*v[i];
}
double oldv0 = v[0];
if (oldv0 < 0)
v[0] -= sqrt(mag2);
else
v[0] += sqrt(mag2);
mag2 += -oldv0*oldv0 + v[0]*v[0];
// compute magnitude of ([1 0 0..]+v)
double mag = sqrt(mag2);
// this case can occur when the vectors are already perpendicular
if (mag == 0)
continue;
for (int i = 0; i < vlen; i++)
v[i] /= mag;
// TODO: optimize these multiplications
// matd_t *Q = matd_identity(A->ncols);
// for (int i = 0; i < vlen; i++)
// for (int j = 0; j < vlen; j++)
// MATD_EL(Q, i+1+hhidx, j+1+hhidx) -= 2*v[i]*v[j];
// RS = matd_op("F*M", RS, Q);
for (int i = 0; i < RS->nrows; i++) {
double dot = 0;
for (int j = 0; j < vlen; j++)
dot += MATD_EL(RS, i, hhidx+1+j) * v[j];
for (int j = 0; j < vlen; j++)
MATD_EL(RS, i, hhidx+1+j) -= 2*dot*v[j];
}
// B = matd_op("F*M", B, Q); // should be Q', but Q is symmetric.
for (int i = 0; i < B->nrows; i++) {
double dot = 0;
for (int j = 0; j < vlen; j++)
dot += MATD_EL(B, i, hhidx+1+j) * v[j];
for (int j = 0; j < vlen; j++)
MATD_EL(B, i, hhidx+1+j) -= 2*dot*v[j];
}
}
}
// maxiters used to be smaller to prevent us from looping forever,
// but this doesn't seem to happen any more with our more stable
// svd22 implementation.
int maxiters = 1UL << 5; // 1UL << 30;
assert(maxiters > 0); // reassure clang
int iter;
double maxv; // maximum non-zero value being reduced this iteration
double tol = 1E-5; // 1E-10;
// which method will we use to find the largest off-diagonal
// element of B?
const int find_max_method = 1; //(B->ncols < 6) ? 2 : 1;
// for each of the first B->ncols rows, which index has the
// maximum absolute value? (used by method 1)
int maxrowidx[B->ncols];
int lastmaxi, lastmaxj;
if (find_max_method == 1) {
for (int i = 2; i < B->ncols; i++)
maxrowidx[i] = max_idx(B, i, B->ncols);
// note that we started the array at 2. That's because by setting
// these values below, we'll recompute first two entries on the
// first iteration!
lastmaxi = 0, lastmaxj = 1;
}
for (iter = 0; iter < maxiters; iter++) {
// No diagonalization required for 0x0 and 1x1 matrices.
if (B->ncols < 2)
break;
// find the largest off-diagonal element of B, and put its
// coordinates in maxi, maxj.
int maxi, maxj;
if (find_max_method == 1) {
// method 1 is the "smarter" method which does at least
// 4*ncols work. More work might be needed (up to
// ncols*ncols), depending on data. Thus, this might be a
// bit slower than the default method for very small
// matrices.
maxi = -1;
maxv = -1;
// every iteration, we must deal with the fact that rows
// and columns lastmaxi and lastmaxj have been
// modified. Update maxrowidx accordingly.
// now, EVERY row also had columns lastmaxi and lastmaxj modified.
for (int rowi = 0; rowi < B->ncols; rowi++) {
// the magnitude of the largest off-diagonal element
// in this row.
double thismaxv;
// row 'lastmaxi' and 'lastmaxj' have been completely
// changed. compute from scratch.
if (rowi == lastmaxi || rowi == lastmaxj) {
maxrowidx[rowi] = max_idx(B, rowi, B->ncols);
thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
goto endrowi;
}
// our maximum entry was just modified. We don't know
// if it went up or down, and so we don't know if it
// is still the maximum. We have to update from
// scratch.
if (maxrowidx[rowi] == lastmaxi || maxrowidx[rowi] == lastmaxj) {
maxrowidx[rowi] = max_idx(B, rowi, B->ncols);
thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
goto endrowi;
}
// This row is unchanged, except for columns
// 'lastmaxi' and 'lastmaxj', and those columns were
// not previously the largest entry... just check to
// see if they are now the maximum entry in their
// row. (Remembering to consider off-diagonal entries
// only!)
thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
// check column lastmaxi. Is it now the maximum?
if (lastmaxi != rowi) {
double v = fabs(MATD_EL(B, rowi, lastmaxi));
if (v > thismaxv) {
thismaxv = v;
maxrowidx[rowi] = lastmaxi;
}
}
// check column lastmaxj
if (lastmaxj != rowi) {
double v = fabs(MATD_EL(B, rowi, lastmaxj));
if (v > thismaxv) {
thismaxv = v;
maxrowidx[rowi] = lastmaxj;
}
}
// does this row have the largest value we've seen so far?
endrowi:
if (thismaxv > maxv) {
maxv = thismaxv;
maxi = rowi;
}
}
assert(maxi >= 0);
maxj = maxrowidx[maxi];
// save these for the next iteration.
lastmaxi = maxi;
lastmaxj = maxj;
if (maxv < tol)
break;
} else if (find_max_method == 2) {
// brute-force (reference) version.
maxv = -1;
// only search top "square" portion
for (int i = 0; i < B->ncols; i++) {
for (int j = 0; j < B->ncols; j++) {
if (i == j)
continue;
double v = fabs(MATD_EL(B, i, j));
if (v > maxv) {
maxi = i;
maxj = j;
maxv = v;
}
}
}
// termination condition.
if (maxv < tol)
break;
} else {
assert(0);
}
// printf(">>> %5d %3d, %3d %15g\n", maxi, maxj, iter, maxv);
// Now, solve the 2x2 SVD problem for the matrix
// [ A0 A1 ]
// [ A2 A3 ]
double A0 = MATD_EL(B, maxi, maxi);
double A1 = MATD_EL(B, maxi, maxj);
double A2 = MATD_EL(B, maxj, maxi);
double A3 = MATD_EL(B, maxj, maxj);
if (1) {
double AQ[4];
AQ[0] = A0;
AQ[1] = A1;
AQ[2] = A2;
AQ[3] = A3;
double U[4], S[2], V[4];
svd22(AQ, U, S, V);
/* Reference (slow) implementation...
// LS = LS * ROT(theta) = LS * QL
matd_t *QL = matd_identity(A->nrows);
MATD_EL(QL, maxi, maxi) = U[0];
MATD_EL(QL, maxi, maxj) = U[1];
MATD_EL(QL, maxj, maxi) = U[2];
MATD_EL(QL, maxj, maxj) = U[3];
matd_t *QR = matd_identity(A->ncols);
MATD_EL(QR, maxi, maxi) = V[0];
MATD_EL(QR, maxi, maxj) = V[1];
MATD_EL(QR, maxj, maxi) = V[2];
MATD_EL(QR, maxj, maxj) = V[3];
LS = matd_op("F*M", LS, QL);
RS = matd_op("F*M", RS, QR); // remember we'll transpose RS.
B = matd_op("M'*F*M", QL, B, QR);
matd_destroy(QL);
matd_destroy(QR);
*/
// LS = matd_op("F*M", LS, QL);
for (int i = 0; i < LS->nrows; i++) {
double vi = MATD_EL(LS, i, maxi);
double vj = MATD_EL(LS, i, maxj);
MATD_EL(LS, i, maxi) = U[0]*vi + U[2]*vj;
MATD_EL(LS, i, maxj) = U[1]*vi + U[3]*vj;
}
// RS = matd_op("F*M", RS, QR); // remember we'll transpose RS.
for (int i = 0; i < RS->nrows; i++) {
double vi = MATD_EL(RS, i, maxi);
double vj = MATD_EL(RS, i, maxj);
MATD_EL(RS, i, maxi) = V[0]*vi + V[2]*vj;
MATD_EL(RS, i, maxj) = V[1]*vi + V[3]*vj;
}
// B = matd_op("M'*F*M", QL, B, QR);
// The QL matrix mixes rows of B.
for (int i = 0; i < B->ncols; i++) {
double vi = MATD_EL(B, maxi, i);
double vj = MATD_EL(B, maxj, i);
MATD_EL(B, maxi, i) = U[0]*vi + U[2]*vj;
MATD_EL(B, maxj, i) = U[1]*vi + U[3]*vj;
}
// The QR matrix mixes columns of B.
for (int i = 0; i < B->nrows; i++) {
double vi = MATD_EL(B, i, maxi);
double vj = MATD_EL(B, i, maxj);
MATD_EL(B, i, maxi) = V[0]*vi + V[2]*vj;
MATD_EL(B, i, maxj) = V[1]*vi + V[3]*vj;
}
}
}
if (!(flags & MATD_SVD_NO_WARNINGS) && iter == maxiters) {
printf("WARNING: maximum iters (maximum = %d, matrix %d x %d, max=%.15f)\n",
iter, A->nrows, A->ncols, maxv);
// matd_print(A, "%15f");
}
// them all positive by flipping the corresponding columns of
// U/LS.
int idxs[A->ncols];
double vals[A->ncols];
for (int i = 0; i < A->ncols; i++) {
idxs[i] = i;
vals[i] = MATD_EL(B, i, i);
}
// A bubble sort. Seriously.
int changed;
do {
changed = 0;
for (int i = 0; i + 1 < A->ncols; i++) {
if (fabs(vals[i+1]) > fabs(vals[i])) {
int tmpi = idxs[i];
idxs[i] = idxs[i+1];
idxs[i+1] = tmpi;
double tmpv = vals[i];
vals[i] = vals[i+1];
vals[i+1] = tmpv;
changed = 1;
}
}
} while (changed);
matd_t *LP = matd_identity(A->nrows);
matd_t *RP = matd_identity(A->ncols);
for (int i = 0; i < A->ncols; i++) {
MATD_EL(LP, idxs[i], idxs[i]) = 0; // undo the identity above
MATD_EL(RP, idxs[i], idxs[i]) = 0;
MATD_EL(LP, idxs[i], i) = vals[i] < 0 ? -1 : 1;
MATD_EL(RP, idxs[i], i) = 1; //vals[i] < 0 ? -1 : 1;
}
// we've factored:
// LP*(something)*RP'
// solve for (something)
B = matd_op("M'*F*M", LP, B, RP);
// update LS and RS, remembering that RS will be transposed.
LS = matd_op("F*M", LS, LP);
RS = matd_op("F*M", RS, RP);
matd_destroy(LP);
matd_destroy(RP);
matd_svd_t res;
memset(&res, 0, sizeof(res));
// make B exactly diagonal
for (int i = 0; i < B->nrows; i++) {
for (int j = 0; j < B->ncols; j++) {
if (i != j)
MATD_EL(B, i, j) = 0;
}
}
res.U = LS;
res.S = B;
res.V = RS;
return res;
}
matd_svd_t matd_svd(matd_t *A)
{
return matd_svd_flags(A, 0);
}
matd_svd_t matd_svd_flags(matd_t *A, int flags)
{
matd_svd_t res;
if (A->ncols <= A->nrows) {
res = matd_svd_tall(A, flags);
} else {
matd_t *At = matd_transpose(A);
// A =U S V'
// A'=V S' U'
matd_svd_t tmp = matd_svd_tall(At, flags);
memset(&res, 0, sizeof(res));
res.U = tmp.V; //matd_transpose(tmp.V);
res.S = matd_transpose(tmp.S);
res.V = tmp.U; //matd_transpose(tmp.U);
matd_destroy(tmp.S);
matd_destroy(At);
}
/*
matd_t *check = matd_op("M*M*M'-M", res.U, res.S, res.V, A);
double maxerr = 0;
for (int i = 0; i < check->nrows; i++)
for (int j = 0; j < check->ncols; j++)
maxerr = fmax(maxerr, fabs(MATD_EL(check, i, j)));
matd_destroy(check);
if (maxerr > 1e-7) {
printf("bad maxerr: %15f\n", maxerr);
}
if (maxerr > 1e-5) {
printf("bad maxerr: %15f\n", maxerr);
matd_print(A, "%15f");
assert(0);
}
*/
return res;
}
matd_plu_t *matd_plu(const matd_t *a)
{
unsigned int *piv = calloc(a->nrows, sizeof(unsigned int));
int pivsign = 1;
matd_t *lu = matd_copy(a);
// only for square matrices.
assert(a->nrows == a->ncols);
matd_plu_t *mlu = calloc(1, sizeof(matd_plu_t));
for (int i = 0; i < a->nrows; i++)
piv[i] = i;
for (int j = 0; j < a->ncols; j++) {
for (int i = 0; i < a->nrows; i++) {
int kmax = i < j ? i : j; // min(i,j)
// compute dot product of row i with column j (up through element kmax)
double acc = 0;
for (int k = 0; k < kmax; k++)
acc += MATD_EL(lu, i, k) * MATD_EL(lu, k, j);
MATD_EL(lu, i, j) -= acc;
}
// find pivot and exchange if necessary.
int p = j;
if (1) {
for (int i = j+1; i < lu->nrows; i++) {
if (fabs(MATD_EL(lu,i,j)) > fabs(MATD_EL(lu, p, j))) {
p = i;
}
}
}
// swap rows p and j?
if (p != j) {
TYPE tmp[lu->ncols];
memcpy(tmp, &MATD_EL(lu, p, 0), sizeof(TYPE) * lu->ncols);
memcpy(&MATD_EL(lu, p, 0), &MATD_EL(lu, j, 0), sizeof(TYPE) * lu->ncols);
memcpy(&MATD_EL(lu, j, 0), tmp, sizeof(TYPE) * lu->ncols);
int k = piv[p];
piv[p] = piv[j];
piv[j] = k;
pivsign = -pivsign;
}
double LUjj = MATD_EL(lu, j, j);
// If our pivot is very small (which means the matrix is
// singular or nearly singular), replace with a new pivot of the
// right sign.
if (fabs(LUjj) < MATD_EPS) {
/*
if (LUjj < 0)
LUjj = -MATD_EPS;
else
LUjj = MATD_EPS;
MATD_EL(lu, j, j) = LUjj;
*/
mlu->singular = 1;
}
if (j < lu->ncols && j < lu->nrows && LUjj != 0) {
LUjj = 1.0 / LUjj;
for (int i = j+1; i < lu->nrows; i++)
MATD_EL(lu, i, j) *= LUjj;
}
}
mlu->lu = lu;
mlu->piv = piv;
mlu->pivsign = pivsign;
return mlu;
}
void matd_plu_destroy(matd_plu_t *mlu)
{
matd_destroy(mlu->lu);
free(mlu->piv);
memset(mlu, 0, sizeof(matd_plu_t));
free(mlu);
}
double matd_plu_det(const matd_plu_t *mlu)
{
matd_t *lu = mlu->lu;
double det = mlu->pivsign;
if (lu->nrows == lu->ncols) {
for (int i = 0; i < lu->ncols; i++)
det *= MATD_EL(lu, i, i);
}
return det;
}
matd_t *matd_plu_p(const matd_plu_t *mlu)
{
matd_t *lu = mlu->lu;
matd_t *P = matd_create(lu->nrows, lu->nrows);
for (int i = 0; i < lu->nrows; i++) {
MATD_EL(P, mlu->piv[i], i) = 1;
}
return P;
}
matd_t *matd_plu_l(const matd_plu_t *mlu)
{
matd_t *lu = mlu->lu;
matd_t *L = matd_create(lu->nrows, lu->ncols);
for (int i = 0; i < lu->nrows; i++) {
MATD_EL(L, i, i) = 1;
for (int j = 0; j < i; j++) {
MATD_EL(L, i, j) = MATD_EL(lu, i, j);
}
}
return L;
}
matd_t *matd_plu_u(const matd_plu_t *mlu)
{
matd_t *lu = mlu->lu;
matd_t *U = matd_create(lu->ncols, lu->ncols);
for (int i = 0; i < lu->ncols; i++) {
for (int j = 0; j < lu->ncols; j++) {
if (i <= j)
MATD_EL(U, i, j) = MATD_EL(lu, i, j);
}
}
return U;
}
// PLU = A
// Ax = B
// PLUx = B
// LUx = P'B
matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b)
{
matd_t *x = matd_copy(b);
// permute right hand side
for (int i = 0; i < mlu->lu->nrows; i++)
memcpy(&MATD_EL(x, i, 0), &MATD_EL(b, mlu->piv[i], 0), sizeof(TYPE) * b->ncols);
// solve Ly = b
for (int k = 0; k < mlu->lu->nrows; k++) {
for (int i = k+1; i < mlu->lu->nrows; i++) {
double LUik = -MATD_EL(mlu->lu, i, k);
for (int t = 0; t < b->ncols; t++)
MATD_EL(x, i, t) += MATD_EL(x, k, t) * LUik;
}
}
// solve Ux = y
for (int k = mlu->lu->ncols-1; k >= 0; k--) {
double LUkk = 1.0 / MATD_EL(mlu->lu, k, k);
for (int t = 0; t < b->ncols; t++)
MATD_EL(x, k, t) *= LUkk;
for (int i = 0; i < k; i++) {
double LUik = -MATD_EL(mlu->lu, i, k);
for (int t = 0; t < b->ncols; t++)
MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
}
}
return x;
}
matd_t *matd_solve(matd_t *A, matd_t *b)
{
matd_plu_t *mlu = matd_plu(A);
matd_t *x = matd_plu_solve(mlu, b);
matd_plu_destroy(mlu);
return x;
}
#if 0
static int randi()
{
int v = random()&31;
v -= 15;
return v;
}
static double randf()
{
double v = 1.0 *random() / RAND_MAX;
return 2*v - 1;
}
int main(int argc, char *argv[])
{
if (1) {
int maxdim = 16;
matd_t *A = matd_create(maxdim, maxdim);
for (int iter = 0; 1; iter++) {
srand(iter);
if (iter % 1000 == 0)
printf("%d\n", iter);
int m = 1 + (random()%(maxdim-1));
int n = 1 + (random()%(maxdim-1));
for (int i = 0; i < m*n; i++)
A->data[i] = randi();
A->nrows = m;
A->ncols = n;
// printf("%d %d ", m, n);
matd_svd_t svd = matd_svd(A);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
}
/* matd_t *A = matd_create_data(2, 5, (double[]) { 1, 5, 2, 6,
3, 3, 0, 7,
1, 1, 0, -2,
4, 0, 9, 9, 2, 6, 1, 3, 2, 5, 5, 4, -1, 2, 5, 9, 8, 2 });
matd_svd(A);
*/
return 0;
}
struct svd22 s;
srand(0);
matd_t *A = matd_create(2, 2);
MATD_EL(A,0,0) = 4;
MATD_EL(A,0,1) = 7;
MATD_EL(A,1,0) = 2;
MATD_EL(A,1,1) = 6;
matd_t *U = matd_create(2, 2);
matd_t *V = matd_create(2, 2);
matd_t *S = matd_create(2, 2);
for (int iter = 0; 1; iter++) {
if (iter % 100000 == 0)
printf("%d\n", iter);
MATD_EL(A,0,0) = randf();
MATD_EL(A,0,1) = randf();
MATD_EL(A,1,0) = randf();
MATD_EL(A,1,1) = randf();
matd_svd22_impl(A->data, &s);
memcpy(U->data, s.U, 4*sizeof(double));
memcpy(V->data, s.V, 4*sizeof(double));
MATD_EL(S,0,0) = s.S[0];
MATD_EL(S,1,1) = s.S[1];
assert(s.S[0] >= s.S[1]);
assert(s.S[0] >= 0);
assert(s.S[1] >= 0);
if (s.S[0] == 0) {
// printf("*"); fflush(NULL);
// printf("%15f %15f %15f %15f\n", MATD_EL(A,0,0), MATD_EL(A,0,1), MATD_EL(A,1,0), MATD_EL(A,1,1));
}
if (s.S[1] == 0) {
// printf("#"); fflush(NULL);
}
matd_t *USV = matd_op("M*M*M'", U, S, V);
double maxerr = 0;
for (int i = 0; i < 4; i++)
maxerr = fmax(maxerr, fabs(USV->data[i] - A->data[i]));
if (0) {
printf("------------------------------------\n");
printf("A:\n");
matd_print(A, "%15f");
printf("\nUSV':\n");
matd_print(USV, "%15f");
printf("maxerr: %.15f\n", maxerr);
printf("\n\n");
}
matd_destroy(USV);
assert(maxerr < 0.00001);
}
}
#endif
// XXX NGV Cholesky
/*static double *matd_cholesky_raw(double *A, int n)
{
double *L = (double*)calloc(n * n, sizeof(double));
for (int i = 0; i < n; i++) {
for (int j = 0; j < (i+1); j++) {
double s = 0;
for (int k = 0; k < j; k++)
s += L[i * n + k] * L[j * n + k];
L[i * n + j] = (i == j) ?
sqrt(A[i * n + i] - s) :
(1.0 / L[j * n + j] * (A[i * n + j] - s));
}
}
return L;
}
matd_t *matd_cholesky(const matd_t *A)
{
assert(A->nrows == A->ncols);
double *L_data = matd_cholesky_raw(A->data, A->nrows);
matd_t *L = matd_create_data(A->nrows, A->ncols, L_data);
free(L_data);
return L;
}*/
// NOTE: The below implementation of Cholesky is different from the one
// used in NGV.
matd_chol_t *matd_chol(matd_t *A)
{
assert(A->nrows == A->ncols);
int N = A->nrows;
// make upper right
matd_t *U = matd_copy(A);
// don't actually need to clear lower-left... we won't touch it.
/* for (int i = 0; i < U->nrows; i++) {
for (int j = 0; j < i; j++) {
// assert(MATD_EL(U, i, j) == MATD_EL(U, j, i));
MATD_EL(U, i, j) = 0;
}
}
*/
int is_spd = 1; // (A->nrows == A->ncols);
for (int i = 0; i < N; i++) {
double d = MATD_EL(U, i, i);
is_spd &= (d > 0);
if (d < MATD_EPS)
d = MATD_EPS;
d = 1.0 / sqrt(d);
for (int j = i; j < N; j++)
MATD_EL(U, i, j) *= d;
for (int j = i+1; j < N; j++) {
double s = MATD_EL(U, i, j);
if (s == 0)
continue;
for (int k = j; k < N; k++) {
MATD_EL(U, j, k) -= MATD_EL(U, i, k)*s;
}
}
}
matd_chol_t *chol = calloc(1, sizeof(matd_chol_t));
chol->is_spd = is_spd;
chol->u = U;
return chol;
}
void matd_chol_destroy(matd_chol_t *chol)
{
matd_destroy(chol->u);
free(chol);
}
// Solve: (U')x = b, U is upper triangular
void matd_ltransposetriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
{
int n = u->ncols;
memcpy(x, b, n*sizeof(TYPE));
for (int i = 0; i < n; i++) {
x[i] /= MATD_EL(u, i, i);
for (int j = i+1; j < u->ncols; j++) {
x[j] -= x[i] * MATD_EL(u, i, j);
}
}
}
// Solve: Lx = b, L is lower triangular
void matd_ltriangle_solve(matd_t *L, const TYPE *b, TYPE *x)
{
int n = L->ncols;
for (int i = 0; i < n; i++) {
double acc = b[i];
for (int j = 0; j < i; j++) {
acc -= MATD_EL(L, i, j)*x[j];
}
x[i] = acc / MATD_EL(L, i, i);
}
}
// solve Ux = b, U is upper triangular
void matd_utriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
{
for (int i = u->ncols-1; i >= 0; i--) {
double bi = b[i];
double diag = MATD_EL(u, i, i);
for (int j = i+1; j < u->ncols; j++)
bi -= MATD_EL(u, i, j)*x[j];
x[i] = bi / diag;
}
}
matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b)
{
matd_t *u = chol->u;
matd_t *x = matd_copy(b);
// LUx = b
// solve Ly = b ==> (U')y = b
for (int i = 0; i < u->nrows; i++) {
for (int j = 0; j < i; j++) {
// b[i] -= L[i,j]*x[j]... replicated across columns of b
// ==> i.e., ==>
// b[i,k] -= L[i,j]*x[j,k]
for (int k = 0; k < b->ncols; k++) {
MATD_EL(x, i, k) -= MATD_EL(u, j, i)*MATD_EL(x, j, k);
}
}
// x[i] = b[i] / L[i,i]
for (int k = 0; k < b->ncols; k++) {
MATD_EL(x, i, k) /= MATD_EL(u, i, i);
}
}
// solve Ux = y
for (int k = u->ncols-1; k >= 0; k--) {
double LUkk = 1.0 / MATD_EL(u, k, k);
for (int t = 0; t < b->ncols; t++)
MATD_EL(x, k, t) *= LUkk;
for (int i = 0; i < k; i++) {
double LUik = -MATD_EL(u, i, k);
for (int t = 0; t < b->ncols; t++)
MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
}
}
return x;
}
/*void matd_chol_solve(matd_chol_t *chol, const TYPE *b, TYPE *x)
{
matd_t *u = chol->u;
TYPE y[u->ncols];
matd_ltransposetriangle_solve(u, b, y);
matd_utriangle_solve(u, y, x);
}
*/
// only sensible on PSD matrices. had expected it to be faster than
// inverse via LU... for now, doesn't seem to be.
matd_t *matd_chol_inverse(matd_t *a)
{
assert(a->nrows == a->ncols);
matd_chol_t *chol = matd_chol(a);
matd_t *eye = matd_identity(a->nrows);
matd_t *inv = matd_chol_solve(chol, eye);
matd_destroy(eye);
matd_chol_destroy(chol);
return inv;
}
double matd_max(matd_t *m)
{
double d = -DBL_MAX;
for(int x=0; x<m->nrows; x++) {
for(int y=0; y<m->ncols; y++) {
if(MATD_EL(m, x, y) > d)
d = MATD_EL(m, x, y);
}
}
return d;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "homography.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
/** Given a 3x3 homography matrix and the focal lengths of the
* camera, compute the pose of the tag. The focal lengths should
* be given in pixels. For example, if the camera's focal length
* is twice the width of the sensor, and the sensor is 600 pixels
* across, the focal length in pixels is 2*600. Note that the
* focal lengths in the fx and fy direction will be approximately
* equal for most lenses, and is not a function of aspect ratio.
*
* Theory: The homography matrix is the product of the camera
* projection matrix and the tag's pose matrix (the matrix that
* projects points from the tag's local coordinate system to the
* camera's coordinate frame).
*
* [ h00 h01 h02 h03] = [ fx 0 cx 0 ] [ R00 R01 R02 TX ]
* [ h10 h11 h12 h13] = [ 0 fy cy 0 ] [ R10 R11 R12 TY ]
* [ h20 h21 h22 h23] = [ 0 0 s 0 ] [ R20 R21 R22 TZ ]
* [ 0 0 0 1 ]
*
* fx is the focal length in the x direction of the camera
* (typically measured in pixels), fy is the focal length. cx and
* cy give the focal center (usually the middle of the image), and
* s is either +1 or -1, depending on the conventions you use. (We
* use 1.)
* When observing a tag, the points we project in world space all
* have z=0, so we can form a 3x3 matrix by eliminating the 3rd
* column of the pose matrix.
*
* [ h00 h01 h02 ] = [ fx 0 cx 0 ] [ R00 R01 TX ]
* [ h10 h11 h12 ] = [ 0 fy cy 0 ] [ R10 R11 TY ]
* [ h20 h21 h22 ] = [ 0 0 s 0 ] [ R20 R21 TZ ]
* [ 0 0 1 ]
*
* (note that these h's are different from the ones above.)
*
* We can multiply the right-hand side to yield a set of equations
* relating the values of h to the values of the pose matrix.
*
* There are two wrinkles. The first is that the homography matrix
* is known only up to scale. We recover the unknown scale by
* constraining the magnitude of the first two columns of the pose
* matrix to be 1. We use the geometric average scale. The sign of
* the scale factor is recovered by constraining the observed tag
* to be in front of the camera. Once scaled, we recover the first
* two colmuns of the rotation matrix. The third column is the
* cross product of these.
*
* The second wrinkle is that the computed rotation matrix might
* not be exactly orthogonal, so we perform a polar decomposition
* to find a good pure rotation approximation.
*
* Tagsize is the size of the tag in your desired units. I.e., if
* your tag measures 0.25m along the side, your tag size is
* 0.25. (The homography is computed in terms of *half* the tag
* size, i.e., that a tag is 2 units wide as it spans from -1 to
* +1, but this code makes the appropriate adjustment.)
*
* A note on signs:
*
* The code below incorporates no additional negative signs, but
* respects the sign of any parameters that you pass in. Flipping
* the signs allows you to modify the projection to suit a wide
* variety of conditions.
*
* In the "pure geometry" projection matrix, the image appears
* upside down; i.e., the x and y coordinates on the left hand
* side are the opposite of those on the right of the camera
* projection matrix. This would happen for all parameters
* positive: recall that points in front of the camera have
* negative Z values, which will cause the sign of all points to
* flip.
*
* However, most cameras flip things so that the image appears
* "right side up" as though you were looking through the lens
* directly. This means that the projected points should have the
* same sign as the points on the right of the camera projection
* matrix. To achieve this, flip fx and fy.
*
* One further complication: cameras typically put y=0 at the top
* of the image, instead of the bottom. Thus you generally want to
* flip y yet again (so it's now positive again).
*
* General advice: you probably want fx negative, fy positive, cx
* and cy positive, and s=1.
**/
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
// Specifically, float [] { a, b, c, d } where x = [a b], y = [c d].
#define HOMOGRAPHY_COMPUTE_FLAG_INVERSE 1
#define HOMOGRAPHY_COMPUTE_FLAG_SVD 0
matd_t *homography_compute(zarray_t *correspondences, int flags);
//void homography_project(const matd_t *H, double x, double y, double *ox, double *oy);
static inline void homography_project(const matd_t *H, double x, double y, double *ox, double *oy)
{
double xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2);
double yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2);
double zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2);
*ox = xx / zz;
*oy = yy / zz;
}
// assuming that the projection matrix is:
// [ fx 0 cx 0 ]
// [ 0 fy cy 0 ]
// [ 0 0 1 0 ]
//
// And that the homography is equal to the projection matrix times the model matrix,
// recover the model matrix (which is returned). Note that the third column of the model
// matrix is missing in the expresison below, reflecting the fact that the homography assumes
// all points are at z=0 (i.e., planar) and that the element of z is thus omitted.
// (3x1 instead of 4x1).
//
// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
// [ 0 0 1 ]
//
// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
// fx*R01 + cx*R21 = H01
// fx*TX + cx*TZ = H02
// fy*R10 + cy*R20 = H10
// fy*R11 + cy*R21 = H11
// fy*TY + cy*TZ = H12
// R20 = H20
// R21 = H21
// TZ = H22
matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy);
// Similar to above
// Recover the model view matrix assuming that the projection matrix is:
//
// [ F 0 A 0 ] (see glFrustrum)
// [ 0 G B 0 ]
// [ 0 0 C D ]
// [ 0 0 -1 0 ]
matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D);
////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "homography.c"
////////////////////////////////////////////////////////////////////////////////////////////////////
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
matd_t *homography_compute(zarray_t *correspondences, int flags)
{
// compute centroids of both sets of points (yields a better
// conditioned information matrix)
double x_cx = 0, x_cy = 0;
double y_cx = 0, y_cy = 0;
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
x_cx += c[0];
x_cy += c[1];
y_cx += c[2];
y_cy += c[3];
}
int sz = zarray_size(correspondences);
x_cx /= sz;
x_cy /= sz;
y_cx /= sz;
y_cy /= sz;
// NB We don't normalize scale; it seems implausible that it could
// possibly make any difference given the dynamic range of IEEE
// doubles.
matd_t *A = matd_create(9,9);
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
// (below world is "x", and image is "y")
double worldx = c[0] - x_cx;
double worldy = c[1] - x_cy;
double imagex = c[2] - y_cx;
double imagey = c[3] - y_cy;
double a03 = -worldx;
double a04 = -worldy;
double a05 = -1;
double a06 = worldx*imagey;
double a07 = worldy*imagey;
double a08 = imagey;
MATD_EL(A, 3, 3) += a03*a03;
MATD_EL(A, 3, 4) += a03*a04;
MATD_EL(A, 3, 5) += a03*a05;
MATD_EL(A, 3, 6) += a03*a06;
MATD_EL(A, 3, 7) += a03*a07;
MATD_EL(A, 3, 8) += a03*a08;
MATD_EL(A, 4, 4) += a04*a04;
MATD_EL(A, 4, 5) += a04*a05;
MATD_EL(A, 4, 6) += a04*a06;
MATD_EL(A, 4, 7) += a04*a07;
MATD_EL(A, 4, 8) += a04*a08;
MATD_EL(A, 5, 5) += a05*a05;
MATD_EL(A, 5, 6) += a05*a06;
MATD_EL(A, 5, 7) += a05*a07;
MATD_EL(A, 5, 8) += a05*a08;
MATD_EL(A, 6, 6) += a06*a06;
MATD_EL(A, 6, 7) += a06*a07;
MATD_EL(A, 6, 8) += a06*a08;
MATD_EL(A, 7, 7) += a07*a07;
MATD_EL(A, 7, 8) += a07*a08;
MATD_EL(A, 8, 8) += a08*a08;
double a10 = worldx;
double a11 = worldy;
double a12 = 1;
double a16 = -worldx*imagex;
double a17 = -worldy*imagex;
double a18 = -imagex;
MATD_EL(A, 0, 0) += a10*a10;
MATD_EL(A, 0, 1) += a10*a11;
MATD_EL(A, 0, 2) += a10*a12;
MATD_EL(A, 0, 6) += a10*a16;
MATD_EL(A, 0, 7) += a10*a17;
MATD_EL(A, 0, 8) += a10*a18;
MATD_EL(A, 1, 1) += a11*a11;
MATD_EL(A, 1, 2) += a11*a12;
MATD_EL(A, 1, 6) += a11*a16;
MATD_EL(A, 1, 7) += a11*a17;
MATD_EL(A, 1, 8) += a11*a18;
MATD_EL(A, 2, 2) += a12*a12;
MATD_EL(A, 2, 6) += a12*a16;
MATD_EL(A, 2, 7) += a12*a17;
MATD_EL(A, 2, 8) += a12*a18;
MATD_EL(A, 6, 6) += a16*a16;
MATD_EL(A, 6, 7) += a16*a17;
MATD_EL(A, 6, 8) += a16*a18;
MATD_EL(A, 7, 7) += a17*a17;
MATD_EL(A, 7, 8) += a17*a18;
MATD_EL(A, 8, 8) += a18*a18;
double a20 = -worldx*imagey;
double a21 = -worldy*imagey;
double a22 = -imagey;
double a23 = worldx*imagex;
double a24 = worldy*imagex;
double a25 = imagex;
MATD_EL(A, 0, 0) += a20*a20;
MATD_EL(A, 0, 1) += a20*a21;
MATD_EL(A, 0, 2) += a20*a22;
MATD_EL(A, 0, 3) += a20*a23;
MATD_EL(A, 0, 4) += a20*a24;
MATD_EL(A, 0, 5) += a20*a25;
MATD_EL(A, 1, 1) += a21*a21;
MATD_EL(A, 1, 2) += a21*a22;
MATD_EL(A, 1, 3) += a21*a23;
MATD_EL(A, 1, 4) += a21*a24;
MATD_EL(A, 1, 5) += a21*a25;
MATD_EL(A, 2, 2) += a22*a22;
MATD_EL(A, 2, 3) += a22*a23;
MATD_EL(A, 2, 4) += a22*a24;
MATD_EL(A, 2, 5) += a22*a25;
MATD_EL(A, 3, 3) += a23*a23;
MATD_EL(A, 3, 4) += a23*a24;
MATD_EL(A, 3, 5) += a23*a25;
MATD_EL(A, 4, 4) += a24*a24;
MATD_EL(A, 4, 5) += a24*a25;
MATD_EL(A, 5, 5) += a25*a25;
}
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
MATD_EL(A, j, i) = MATD_EL(A, i, j);
matd_t *H = matd_create(3,3);
if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
// compute singular vector by (carefully) inverting the rank-deficient matrix.
if (1) {
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(Ainv);
} else {
matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
matd_t *Ainv = NULL;
if (0) {
matd_plu_t *lu = matd_plu(A);
Ainv = matd_plu_solve(lu, b);
matd_plu_destroy(lu);
} else {
matd_chol_t *chol = matd_chol(A);
Ainv = matd_chol_solve(chol, b);
matd_chol_destroy(chol);
}
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(b);
matd_destroy(Ainv);
}
} else {
// compute singular vector using SVD. A bit slower, but more accurate.
matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
}
matd_t *Tx = matd_identity(3);
MATD_EL(Tx,0,2) = -x_cx;
MATD_EL(Tx,1,2) = -x_cy;
matd_t *Ty = matd_identity(3);
MATD_EL(Ty,0,2) = y_cx;
MATD_EL(Ty,1,2) = y_cy;
matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
matd_destroy(A);
matd_destroy(Tx);
matd_destroy(Ty);
matd_destroy(H);
return H2;
}
// assuming that the projection matrix is:
// [ fx 0 cx 0 ]
// [ 0 fy cy 0 ]
// [ 0 0 1 0 ]
//
// And that the homography is equal to the projection matrix times the
// model matrix, recover the model matrix (which is returned). Note
// that the third column of the model matrix is missing in the
// expresison below, reflecting the fact that the homography assumes
// all points are at z=0 (i.e., planar) and that the element of z is
// thus omitted. (3x1 instead of 4x1).
//
// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
// [ 0 0 1 ]
//
// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
// fx*R01 + cx*R21 = H01
// fx*TX + cx*TZ = H02
// fy*R10 + cy*R20 = H10
// fy*R11 + cy*R21 = H11
// fy*TY + cy*TZ = H12
// R20 = H20
// R21 = H21
// TZ = H22
matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy)
{
// Note that every variable that we compute is proportional to the scale factor of H.
double R20 = MATD_EL(H, 2, 0);
double R21 = MATD_EL(H, 2, 1);
double TZ = MATD_EL(H, 2, 2);
double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx;
double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx;
double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx;
double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy;
double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy;
double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy;
// compute the scale by requiring that the rotation columns are unit length
// (Use geometric average of the two length vectors we have)
double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
double s = 1.0 / sqrtf(length1 * length2);
// get sign of S by requiring the tag to be in front the camera;
// we assume camera looks in the -Z direction.
if (TZ > 0)
s *= -1;
R20 *= s;
R21 *= s;
TZ *= s;
R00 *= s;
R01 *= s;
TX *= s;
R10 *= s;
R11 *= s;
TY *= s;
// now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
double R02 = R10*R21 - R20*R11;
double R12 = R20*R01 - R00*R21;
double R22 = R00*R11 - R10*R01;
// Improve rotation matrix by applying polar decomposition.
if (1) {
// do polar decomposition. This makes the rotation matrix
// "proper", but probably increases the reprojection error. An
// iterative alignment step would be superior.
matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02,
R10, R11, R12,
R20, R21, R22 });
matd_svd_t svd = matd_svd(R);
matd_destroy(R);
R = matd_op("M*M'", svd.U, svd.V);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
R00 = MATD_EL(R, 0, 0);
R01 = MATD_EL(R, 0, 1);
R02 = MATD_EL(R, 0, 2);
R10 = MATD_EL(R, 1, 0);
R11 = MATD_EL(R, 1, 1);
R12 = MATD_EL(R, 1, 2);
R20 = MATD_EL(R, 2, 0);
R21 = MATD_EL(R, 2, 1);
R22 = MATD_EL(R, 2, 2);
matd_destroy(R);
}
return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX,
R10, R11, R12, TY,
R20, R21, R22, TZ,
0, 0, 0, 1 });
}
// Similar to above
// Recover the model view matrix assuming that the projection mat