Skip to content

Instantly share code, notes, and snippets.

/LICENSE.txt

Created November 25, 2014 08:38
Show Gist options
  • Star 1 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save anonymous/ce42371a4f6370acb519 to your computer and use it in GitHub Desktop.
Save anonymous/ce42371a4f6370acb519 to your computer and use it in GitHub Desktop.
Download ZIP
3x3 Least Squares and QEF solvers based on Singular Value Decomposition (SVD) using Jacobi Rotations
Raw
LICENSE.txt
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.
In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
For more information, please refer to <http://unlicense.org/>
Raw
qef.cpp
/*
* This is free and unencumbered software released into the public domain.
*
* Anyone is free to copy, modify, publish, use, compile, sell, or
* distribute this software, either in source code form or as a compiled
* binary, for any purpose, commercial or non-commercial, and by any
* means.
*
* In jurisdictions that recognize copyright laws, the author or authors
* of this software dedicate any and all copyright interest in the
* software to the public domain. We make this dedication for the benefit
* of the public at large and to the detriment of our heirs and
* successors. We intend this dedication to be an overt act of
* relinquishment in perpetuity of all present and future rights to this
* software under copyright law.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* For more information, please refer to <http://unlicense.org/>
*/
#include "qef.h"
#include <stdexcept>
namespace svd
{
QefData::QefData()
{
this->clear();
}
QefData::QefData(const double ata_00, const double ata_01,
const double ata_02, const double ata_11, const double ata_12,
const double ata_22, const double atb_x, const double atb_y,
const double atb_z, const double btb, const double massPoint_x,
const double massPoint_y, const double massPoint_z,
const int numPoints)
{
this->set(ata_00, ata_01, ata_02, ata_11, ata_12, ata_22, atb_x, atb_y,
atb_z, btb, massPoint_x, massPoint_y, massPoint_z, numPoints);
}
QefData::QefData(const QefData &rhs)
{
this->set(rhs);
}
void QefData::add(const QefData &rhs)
{
this->ata_00 += rhs.ata_00;
this->ata_01 += rhs.ata_01;
this->ata_02 += rhs.ata_02;
this->ata_11 += rhs.ata_11;
this->ata_12 += rhs.ata_12;
this->ata_22 += rhs.ata_22;
this->atb_x += rhs.atb_x;
this->atb_y += rhs.atb_y;
this->atb_z += rhs.atb_z;
this->btb += rhs.btb;
this->massPoint_x += rhs.massPoint_x;
this->massPoint_y += rhs.massPoint_y;
this->massPoint_z += rhs.massPoint_z;
this->numPoints += rhs.numPoints;
}
void QefData::clear()
{
this->set(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
}
void QefData::set(const double ata_00, const double ata_01,
const double ata_02, const double ata_11, const double ata_12,
const double ata_22, const double atb_x, const double atb_y,
const double atb_z, const double btb, const double massPoint_x,
const double massPoint_y, const double massPoint_z,
const int numPoints)
{
this->ata_00 = ata_00;
this->ata_01 = ata_01;
this->ata_02 = ata_02;
this->ata_11 = ata_11;
this->ata_12 = ata_12;
this->ata_22 = ata_22;
this->atb_x = atb_x;
this->atb_y = atb_y;
this->atb_z = atb_z;
this->btb = btb;
this->massPoint_x = massPoint_x;
this->massPoint_y = massPoint_y;
this->massPoint_z = massPoint_z;
this->numPoints = numPoints;
}
void QefData::set(const QefData &rhs)
{
this->set(rhs.ata_00, rhs.ata_01, rhs.ata_02, rhs.ata_11, rhs.ata_12,
rhs.ata_22, rhs.atb_x, rhs.atb_y, rhs.atb_z, rhs.btb,
rhs.massPoint_x, rhs.massPoint_y, rhs.massPoint_z,
rhs.numPoints);
}
#ifndef NO_OSTREAM
std::ostream &operator<<(std::ostream &os, const QefData &qef)
{
SMat3 ata;
Vec3 atb, mp;
ata.setSymmetric(qef.ata_00, qef.ata_01, qef.ata_02, qef.ata_11,
qef.ata_12, qef.ata_22);
atb.set(qef.atb_x, qef.atb_y, qef.atb_z);
mp.set(qef.massPoint_x, qef.massPoint_y, qef.massPoint_z);
if (qef.numPoints > 0) {
VecUtils::scale(mp, 1.0 / qef.numPoints);
}
os << "QefData [ " << std::endl
<< " ata =" << std::endl << ata << "," << std::endl
<< " atb = " << atb << "," << std::endl
<< " btb = " << qef.btb << "," << std::endl
<< " massPoint = " << mp << "," << std::endl
<< " numPoints = " << qef.numPoints << "]";
return os;
}
#endif
QefSolver::QefSolver() : data(), ata(), atb(), massPoint(), x(),
hasSolution(false) {}
static void normalize(double &nx, double &ny, double &nz)
{
Vec3 tmpv(nx, ny, nz);
VecUtils::normalize(tmpv);
nx = tmpv.x;
ny = tmpv.y;
nz = tmpv.z;
}
void QefSolver::add(const double px, const double py, const double pz,
double nx, double ny, double nz)
{
this->hasSolution = false;
normalize(nx, ny, nz);
this->data.ata_00 += nx * nx;
this->data.ata_01 += nx * ny;
this->data.ata_02 += nx * nz;
this->data.ata_11 += ny * ny;
this->data.ata_12 += ny * nz;
this->data.ata_22 += nz * nz;
const double dot = nx * px + ny * py + nz * pz;
this->data.atb_x += dot * nx;
this->data.atb_y += dot * ny;
this->data.atb_z += dot * nz;
this->data.btb += dot * dot;
this->data.massPoint_x += px;
this->data.massPoint_y += py;
this->data.massPoint_z += pz;
++this->data.numPoints;
}
void QefSolver::add(const Vec3 &p, const Vec3 &n)
{
this->add(p.x, p.y, p.z, n.x, n.y, n.z);
}
void QefSolver::add(const QefData &rhs)
{
this->hasSolution = false;
this->data.add(rhs);
}
void QefSolver::getData(QefData &out)
{
out.set(this->data);
}
double QefSolver::getError()
{
if (!this->hasSolution) {
throw std::runtime_error("illegal state");
}
return this->getError(this->x);
}
double QefSolver::getError(const Vec3 &pos)
{
if (!this->hasSolution) {
this->setAta();
this->setAtb();
}
Vec3 atax;
MatUtils::vmul_symmetric(atax, this->ata, pos);
return VecUtils::dot(pos, atax) - 2 * VecUtils::dot(pos, this->atb)
+ this->data.btb;
}
void QefSolver::reset()
{
this->hasSolution = false;
this->data.clear();
}
void QefSolver::setAta()
{
this->ata.setSymmetric(this->data.ata_00, this->data.ata_01,
this->data.ata_02, this->data.ata_11, this->data.ata_12,
this->data.ata_22);
}
void QefSolver::setAtb()
{
this->atb.set(this->data.atb_x, this->data.atb_y, this->data.atb_z);
}
double QefSolver::solve(Vec3 &outx, const double svd_tol,
const int svd_sweeps, const double pinv_tol)
{
if (this->data.numPoints == 0) {
throw std::invalid_argument("...");
}
this->massPoint.set(this->data.massPoint_x, this->data.massPoint_y,
this->data.massPoint_z);
VecUtils::scale(this->massPoint, 1.0 / this->data.numPoints);
this->setAta();
this->setAtb();
Vec3 tmpv;
MatUtils::vmul_symmetric(tmpv, this->ata, this->massPoint);
VecUtils::sub(this->atb, this->atb, tmpv);
this->x.clear();
const double result = Svd::solveSymmetric(this->ata, this->atb,
this->x, svd_tol, svd_sweeps, pinv_tol);
VecUtils::addScaled(this->x, 1, this->massPoint);
this->setAtb();
outx.set(x);
this->hasSolution = true;
return result;
}
}
Raw
qef.h
/*
* This is free and unencumbered software released into the public domain.
*
* Anyone is free to copy, modify, publish, use, compile, sell, or
* distribute this software, either in source code form or as a compiled
* binary, for any purpose, commercial or non-commercial, and by any
* means.
*
* In jurisdictions that recognize copyright laws, the author or authors
* of this software dedicate any and all copyright interest in the
* software to the public domain. We make this dedication for the benefit
* of the public at large and to the detriment of our heirs and
* successors. We intend this dedication to be an overt act of
* relinquishment in perpetuity of all present and future rights to this
* software under copyright law.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* For more information, please refer to <http://unlicense.org/>
*/
#ifndef QEF_H
#define QEF_H
#ifndef NO_OSTREAM
#include <iostream>
#endif
#include "svd.h"
namespace svd
{
class QefData
{
public:
double ata_00, ata_01, ata_02, ata_11, ata_12, ata_22;
double atb_x, atb_y, atb_z;
double btb;
double massPoint_x, massPoint_y, massPoint_z;
int numPoints;
QefData();
QefData(const double ata_00, const double ata_01,
const double ata_02, const double ata_11, const double ata_12,
const double ata_22, const double atb_x, const double atb_y,
const double atb_z, const double btb, const double massPoint_x,
const double massPoint_y, const double massPoint_z,
const int numPoints) ;
void add(const QefData &rhs) ;
void clear() ;
void set(const double ata_00, const double ata_01,
const double ata_02, const double ata_11, const double ata_12,
const double ata_22, const double atb_x, const double atb_y,
const double atb_z, const double btb, const double massPoint_x,
const double massPoint_y, const double massPoint_z,
const int numPoints) ;
void set(const QefData &rhs);
private:
QefData(const QefData &rhs);
QefData &operator= (const QefData &rhs);
};
#ifndef NO_OSTREAM
std::ostream &operator<<(std::ostream &os, const QefData &d) ;
#endif
class QefSolver
{
private:
QefData data;
SMat3 ata;
Vec3 atb, massPoint, x;
bool hasSolution;
public:
QefSolver() ;
public:
void add(const double px, const double py, const double pz,
double nx, double ny, double nz) ;
void add(const Vec3 &p, const Vec3 &n);
void add(const QefData &rhs) ;
void getData(QefData &out) ;
double getError();
double getError(const Vec3 &pos);
void reset();
double solve(Vec3 &outx, const double svd_tol,
const int svd_sweeps, const double pinv_tol) ;
private:
QefSolver(const QefSolver &rhs);
QefSolver &operator=(const QefSolver &rhs);
void setAta();
void setAtb() ;
};
};
#endif
Raw
svd.cpp
/*
* This is free and unencumbered software released into the public domain.
*
* Anyone is free to copy, modify, publish, use, compile, sell, or
* distribute this software, either in source code form or as a compiled
* binary, for any purpose, commercial or non-commercial, and by any
* means.
*
* In jurisdictions that recognize copyright laws, the author or authors
* of this software dedicate any and all copyright interest in the
* software to the public domain. We make this dedication for the benefit
* of the public at large and to the detriment of our heirs and
* successors. We intend this dedication to be an overt act of
* relinquishment in perpetuity of all present and future rights to this
* software under copyright law.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* For more information, please refer to <http://unlicense.org/>
*/
#include "svd.h"
#include <math.h>
namespace svd
{
Mat3::Mat3()
{
this->clear();
}
Mat3::Mat3(const double m00, const double m01, const double m02,
const double m10, const double m11, const double m12,
const double m20, const double m21, const double m22)
{
this->set(m00, m01, m02, m10, m11, m12, m20, m21, m22);
}
Mat3::Mat3(const Mat3 &rhs)
{
this->set(rhs);
}
void Mat3::clear()
{
this->set(0, 0, 0, 0, 0, 0, 0, 0, 0);
}
void Mat3::set(const double m00, const double m01, const double m02,
const double m10, const double m11, const double m12,
const double m20, const double m21, const double m22)
{
this->m00 = m00;
this->m01 = m01;
this->m02 = m02;
this->m10 = m10;
this->m11 = m11;
this->m12 = m12;
this->m20 = m20;
this->m21 = m21;
this->m22 = m22;
}
void Mat3::set(const Mat3 &rhs)
{
this->set(rhs.m00, rhs.m01, rhs.m02, rhs.m10, rhs.m11, rhs.m12, rhs.m20,
rhs.m21, rhs.m22);
}
void Mat3::setSymmetric(const SMat3 &rhs)
{
this->setSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
void Mat3::setSymmetric(const double a00, const double a01, const double a02,
const double a11, const double a12, const double a22)
{
this->set(a00, a01, a02, a01, a11, a12, a02, a12, a22);
}
SMat3::SMat3()
{
this->clear();
}
SMat3::SMat3(const double m00, const double m01, const double m02,
const double m11, const double m12, const double m22)
{
this->setSymmetric(m00, m01, m02, m11, m12, m22);
}
SMat3::SMat3(const SMat3 &rhs)
{
this->setSymmetric(rhs);
}
void SMat3::clear()
{
this->setSymmetric(0, 0, 0, 0, 0, 0);
}
void SMat3::setSymmetric(const SMat3 &rhs)
{
this->setSymmetric(rhs.m00, rhs.m01, rhs.m02, rhs.m11, rhs.m12, rhs.m22);
}
void SMat3::setSymmetric(const double a00, const double a01, const double a02,
const double a11, const double a12, const double a22)
{
this->m00 = a00;
this->m01 = a01;
this->m02 = a02;
this->m11 = a11;
this->m12 = a12;
this->m22 = a22;
}
Vec3::Vec3() : x(0), y(0), z(0) { }
Vec3::Vec3(const Vec3 &rhs) : Vec3()
{
this->set(rhs);
}
Vec3::Vec3(const double x, const double y, const double z) : Vec3()
{
this->set(x, y, z);
}
void Vec3::clear()
{
this->set(0, 0, 0);
}
void Vec3::set(const double x, const double y, const double z)
{
this->x = x;
this->y = y;
this->z = z;
}
void Vec3::set(const Vec3 &rhs)
{
this->set(rhs.x, rhs.y, rhs.z);
}
#ifndef NO_OSTREAM
std::ostream &operator<<(std::ostream &os, const Mat3 &m)
{
os << "[[" << m.m00 << ", " << m.m01 << ", " << m.m02 << "]" <<
std::endl;
os << " [" << m.m10 << ", " << m.m11 << ", " << m.m12 << "]" <<
std::endl;
os << " [" << m.m20 << ", " << m.m21 << ", " << m.m22 << "]]";
return os;
}
std::ostream &operator<<(std::ostream &os, const SMat3 &m)
{
os << "[[" << m.m00 << ", " << m.m01 << ", " << m.m02 << "]" <<
std::endl;
os << " [" << m.m01 << ", " << m.m11 << ", " << m.m12 << "]" <<
std::endl;
os << " [" << m.m02 << ", " << m.m12 << ", " << m.m22 << "]]";
return os;
}
std::ostream &operator<<(std::ostream &os, const Vec3 &v)
{
os << "[" << v.x << ", " << v.y << ", " << v.z << "]";
return os;
}
#endif
double MatUtils::fnorm(const Mat3 &a)
{
return sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m10 * a.m10) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m20 * a.m20) + (a.m21 * a.m21) + (a.m22 * a.m22));
}
double MatUtils::fnorm(const SMat3 &a)
{
return sqrt((a.m00 * a.m00) + (a.m01 * a.m01) + (a.m02 * a.m02)
+ (a.m01 * a.m01) + (a.m11 * a.m11) + (a.m12 * a.m12)
+ (a.m02 * a.m02) + (a.m12 * a.m12) + (a.m22 * a.m22));
}
double MatUtils::off(const Mat3 &a)
{
return sqrt((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m10 * a.m10)
+ (a.m12 * a.m12) + (a.m20 * a.m20) + (a.m21 * a.m21));
}
double MatUtils::off(const SMat3 &a)
{
return sqrt(2 * ((a.m01 * a.m01) + (a.m02 * a.m02) + (a.m12 * a.m12)));
}
void MatUtils::mmul(Mat3 &out, const Mat3 &a, const Mat3 &b)
{
out.set(a.m00 * b.m00 + a.m01 * b.m10 + a.m02 * b.m20,
a.m00 * b.m01 + a.m01 * b.m11 + a.m02 * b.m21,
a.m00 * b.m02 + a.m01 * b.m12 + a.m02 * b.m22,
a.m10 * b.m00 + a.m11 * b.m10 + a.m12 * b.m20,
a.m10 * b.m01 + a.m11 * b.m11 + a.m12 * b.m21,
a.m10 * b.m02 + a.m11 * b.m12 + a.m12 * b.m22,
a.m20 * b.m00 + a.m21 * b.m10 + a.m22 * b.m20,
a.m20 * b.m01 + a.m21 * b.m11 + a.m22 * b.m21,
a.m20 * b.m02 + a.m21 * b.m12 + a.m22 * b.m22);
}
void MatUtils::mmul_ata(SMat3 &out, const Mat3 &a)
{
out.setSymmetric(a.m00 * a.m00 + a.m10 * a.m10 + a.m20 * a.m20,
a.m00 * a.m01 + a.m10 * a.m11 + a.m20 * a.m21,
a.m00 * a.m02 + a.m10 * a.m12 + a.m20 * a.m22,
a.m01 * a.m01 + a.m11 * a.m11 + a.m21 * a.m21,
a.m01 * a.m02 + a.m11 * a.m12 + a.m21 * a.m22,
a.m02 * a.m02 + a.m12 * a.m12 + a.m22 * a.m22);
}
void MatUtils::transpose(Mat3 &out, const Mat3 &a)
{
out.set(a.m00, a.m10, a.m20, a.m01, a.m11, a.m21, a.m02, a.m12, a.m22);
}
void MatUtils::vmul(Vec3 &out, const Mat3 &a, const Vec3 &v)
{
out.x = (a.m00 * v.x) + (a.m01 * v.y) + (a.m02 * v.z);
out.y = (a.m10 * v.x) + (a.m11 * v.y) + (a.m12 * v.z);
out.z = (a.m20 * v.x) + (a.m21 * v.y) + (a.m22 * v.z);
}
void MatUtils::vmul_symmetric(Vec3 &out, const SMat3 &a, const Vec3 &v)
{
out.x = (a.m00 * v.x) + (a.m01 * v.y) + (a.m02 * v.z);
out.y = (a.m01 * v.x) + (a.m11 * v.y) + (a.m12 * v.z);
out.z = (a.m02 * v.x) + (a.m12 * v.y) + (a.m22 * v.z);
}
void VecUtils::addScaled(Vec3 &v, const double s, const Vec3 &rhs)
{
v.x += s * rhs.x;
v.y += s * rhs.y;
v.z += s * rhs.z;
}
double VecUtils::dot(const Vec3 &a, const Vec3 &b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
void VecUtils::normalize(Vec3 &v)
{
const double len2 = VecUtils::dot(v, v);
if (fabs(len2) < 1e-12) {
v.clear();
} else {
VecUtils::scale(v, 1 / sqrt(len2));
}
}
void VecUtils::scale(Vec3 &v, const double s)
{
v.x *= s;
v.y *= s;
v.z *= s;
}
void VecUtils::sub(Vec3 &c, const Vec3 &a, const Vec3 &b)
{
const double v0 = a.x - b.x;
const double v1 = a.y - b.y;
const double v2 = a.z - b.z;
c.x = v0;
c.y = v1;
c.z = v2;
}
void Givens::rot01_post(Mat3 &m, const double c, const double s)
{
const double m00 = m.m00, m01 = m.m01, m10 = m.m10, m11 = m.m11, m20 = m.m20,
m21 = m.m21;
m.set(c * m00 - s * m01, s * m00 + c * m01, m.m02, c * m10 - s * m11,
s * m10 + c * m11, m.m12, c * m20 - s * m21, s * m20 + c * m21, m.m22);
}
void Givens::rot02_post(Mat3 &m, const double c, const double s)
{
const double m00 = m.m00, m02 = m.m02, m10 = m.m10, m12 = m.m12,
m20 = m.m20, m22 = m.m22 ;
m.set(c * m00 - s * m02, m.m01, s * m00 + c * m02, c * m10 - s * m12, m.m11,
s * m10 + c * m12, c * m20 - s * m22, m.m21, s * m20 + c * m22);
}
void Givens::rot12_post(Mat3 &m, const double c, const double s)
{
const double m01 = m.m01, m02 = m.m02, m11 = m.m11, m12 = m.m12,
m21 = m.m21, m22 = m.m22;
m.set(m.m00, c * m01 - s * m02, s * m01 + c * m02, m.m10, c * m11 - s * m12,
s * m11 + c * m12, m.m20, c * m21 - s * m22, s * m21 + c * m22);
}
static void calcSymmetricGivensCoefficients(const double a_pp,
const double a_pq, const double a_qq, double &c, double &s)
{
if (a_pq == 0) {
c = 1;
s = 0;
return;
}
const double tau = (a_qq - a_pp) / (2 * a_pq);
const double stt = sqrt(1.0 + tau * tau);
const double tan = 1.0 / ((tau >= 0) ? (tau + stt) : (tau - stt));
c = 1.0 / sqrt(1 + tan * tan);
s = tan * c;
}
void Schur2::rot01(SMat3 &m, double &c, double &s)
{
svd::calcSymmetricGivensCoefficients(m.m00, m.m01, m.m11, c, s);
const double cc = c * c;
const double ss = s * s;
const double mix = 2 * c * s * m.m01;
m.setSymmetric(cc * m.m00 - mix + ss * m.m11, 0, c * m.m02 - s * m.m12,
ss * m.m00 + mix + cc * m.m11, s * m.m02 + c * m.m12, m.m22);
}
void Schur2::rot02(SMat3 &m, double &c, double &s)
{
svd::calcSymmetricGivensCoefficients(m.m00, m.m02, m.m22, c, s);
const double cc = c * c;
const double ss = s * s;
const double mix = 2 * c * s * m.m02;
m.setSymmetric(cc * m.m00 - mix + ss * m.m22, c * m.m01 - s * m.m12, 0,
m.m11, s * m.m01 + c * m.m12, ss * m.m00 + mix + cc * m.m22);
}
void Schur2::rot12(SMat3 &m, double &c, double &s)
{
svd::calcSymmetricGivensCoefficients(m.m11, m.m12, m.m22, c, s);
const double cc = c * c;
const double ss = s * s;
const double mix = 2 * c * s * m.m12;
m.setSymmetric(m.m00, c * m.m01 - s * m.m02, s * m.m01 + c * m.m02,
cc * m.m11 - mix + ss * m.m22, 0, ss * m.m11 + mix + cc * m.m22);
}
static void rotate01(SMat3 &vtav, Mat3 &v)
{
if (vtav.m01 == 0) {
return;
}
double c, s;
Schur2::rot01(vtav, c, s);
Givens::rot01_post(v, c, s);
}
static void rotate02(SMat3 &vtav, Mat3 &v)
{
if (vtav.m02 == 0) {
return;
}
double c, s;
Schur2::rot02(vtav, c, s);
Givens::rot02_post(v, c, s);
}
static void rotate12(SMat3 &vtav, Mat3 &v)
{
if (vtav.m12 == 0) {
return;
}
double c, s;
Schur2::rot12(vtav, c, s);
Givens::rot12_post(v, c, s);
}
void Svd::getSymmetricSvd(const SMat3 &a, SMat3 &vtav, Mat3 &v,
const double tol,
const int max_sweeps)
{
vtav.setSymmetric(a);
v.set(1, 0, 0, 0, 1, 0, 0, 0, 1);
const double delta = tol * MatUtils::fnorm(vtav);
for (int i = 0; i < max_sweeps
&& MatUtils::off(vtav) > delta; ++i) {
rotate01(vtav, v);
rotate02(vtav, v);
rotate12(vtav, v);
}
}
static double calcError(const Mat3 &A, const Vec3 &x,
const Vec3 &b)
{
Vec3 vtmp;
MatUtils::vmul(vtmp, A, x);
VecUtils::sub(vtmp, b, vtmp);
return VecUtils::dot(vtmp, vtmp);
}
static double calcError(const SMat3 &origA, const Vec3 &x,
const Vec3 &b)
{
Mat3 A;
Vec3 vtmp;
A.setSymmetric(origA);
MatUtils::vmul(vtmp, A, x);
VecUtils::sub(vtmp, b, vtmp);
return VecUtils::dot(vtmp, vtmp);
}
static double pinv(const double x, const double tol)
{
return (fabs(x) < tol || fabs(1 / x) < tol) ? 0 : (1 / x);
}
void Svd::pseudoinverse(Mat3 &out, const SMat3 &d, const Mat3 &v,
const double tol)
{
const double d0 = pinv(d.m00, tol), d1 = pinv(d.m11, tol), d2 = pinv(d.m22,
tol);
out.set(v.m00 * d0 * v.m00 + v.m01 * d1 * v.m01 + v.m02 * d2 * v.m02,
v.m00 * d0 * v.m10 + v.m01 * d1 * v.m11 + v.m02 * d2 * v.m12,
v.m00 * d0 * v.m20 + v.m01 * d1 * v.m21 + v.m02 * d2 * v.m22,
v.m10 * d0 * v.m00 + v.m11 * d1 * v.m01 + v.m12 * d2 * v.m02,
v.m10 * d0 * v.m10 + v.m11 * d1 * v.m11 + v.m12 * d2 * v.m12,
v.m10 * d0 * v.m20 + v.m11 * d1 * v.m21 + v.m12 * d2 * v.m22,
v.m20 * d0 * v.m00 + v.m21 * d1 * v.m01 + v.m22 * d2 * v.m02,
v.m20 * d0 * v.m10 + v.m21 * d1 * v.m11 + v.m22 * d2 * v.m12,
v.m20 * d0 * v.m20 + v.m21 * d1 * v.m21 + v.m22 * d2 * v.m22);
}
double Svd::solveSymmetric(const SMat3 &A, const Vec3 &b, Vec3 &x,
const double svd_tol, const int svd_sweeps, const double pinv_tol)
{
Mat3 mtmp, pinv, V;
SMat3 VTAV;
Svd::getSymmetricSvd(A, VTAV, V, svd_tol, svd_sweeps);
Svd::pseudoinverse(pinv, VTAV, V, pinv_tol);
MatUtils::vmul(x, pinv, b);
return svd::calcError(A, x, b);
}
double
LeastSquares::solveLeastSquares(const Mat3 &a, const Vec3 &b, Vec3 &x,
const double svd_tol, const int svd_sweeps, const double pinv_tol)
{
Mat3 at;
SMat3 ata;
Vec3 atb;
MatUtils::transpose(at, a);
MatUtils::mmul_ata(ata, a);
MatUtils::vmul(atb, at, b);
return Svd::solveSymmetric(ata, atb, x, svd_tol, svd_sweeps, pinv_tol);
}
}
Raw
svd.h
/*
* This is free and unencumbered software released into the public domain.
*
* Anyone is free to copy, modify, publish, use, compile, sell, or
* distribute this software, either in source code form or as a compiled
* binary, for any purpose, commercial or non-commercial, and by any
* means.
*
* In jurisdictions that recognize copyright laws, the author or authors
* of this software dedicate any and all copyright interest in the
* software to the public domain. We make this dedication for the benefit
* of the public at large and to the detriment of our heirs and
* successors. We intend this dedication to be an overt act of
* relinquishment in perpetuity of all present and future rights to this
* software under copyright law.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* For more information, please refer to <http://unlicense.org/>
*/
#ifndef SVD_H
#define SVD_H
#ifndef NO_OSTREAM
#include <iostream>
#endif
namespace svd
{
class SMat3
{
public:
double m00, m01, m02, m11, m12, m22;
public:
SMat3();
SMat3(const double m00, const double m01, const double m02,
const double m11, const double m12, const double m22);
void clear() ;
void setSymmetric(const double m00, const double m01, const double m02,
const double m11,
const double m12, const double m22) ;
void setSymmetric(const SMat3 &rhs) ;
private:
SMat3(const SMat3 &rhs);
SMat3 &operator=(const SMat3 &rhs);
};
class Mat3
{
public:
double m00, m01, m02, m10, m11, m12, m20, m21, m22;
public:
Mat3();
Mat3(const double m00, const double m01, const double m02,
const double m10, const double m11, const double m12,
const double m20, const double m21, const double m22);
void clear() ;
void set(const double m00, const double m01, const double m02,
const double m10, const double m11, const double m12,
const double m20, const double m21, const double m22) ;
void set(const Mat3 &rhs) ;
void setSymmetric(const double a00, const double a01, const double a02,
const double a11, const double a12, const double a22) ;
void setSymmetric(const SMat3 &rhs);
private:
Mat3(const Mat3 &rhs);
Mat3 &operator=(const Mat3 &rhs);
};
class Vec3
{
public:
double x, y, z;
public:
Vec3();
Vec3(const double x, const double y, const double z);
void clear();
void set(const double x, const double y, const double z);
void set(const Vec3 &rhs);
private:
Vec3(const Vec3 &rhs);
Vec3 &operator=(const Vec3 &rhs);
};
#ifndef NO_OSTREAM
std::ostream &operator<<(std::ostream &os, const Mat3 &m) ;
std::ostream &operator<<(std::ostream &os, const SMat3 &m) ;
std::ostream &operator<<(std::ostream &os, const Vec3 &v) ;
#endif
class MatUtils
{
public:
static double fnorm(const Mat3 &a) ;
static double fnorm(const SMat3 &a) ;
static double off(const Mat3 &a) ;
static double off(const SMat3 &a) ;
public:
static void mmul(Mat3 &out, const Mat3 &a, const Mat3 &b) ;
static void mmul_ata(SMat3 &out, const Mat3 &a) ;
static void transpose(Mat3 &out, const Mat3 &a);
static void vmul(Vec3 &out, const Mat3 &a, const Vec3 &v) ;
static void vmul_symmetric(Vec3 &out, const SMat3 &a, const Vec3 &v) ;
};
class VecUtils
{
public:
static void addScaled(Vec3 &v, const double s, const Vec3 &rhs) ;
static double dot(const Vec3 &a, const Vec3 &b) ;
static void normalize(Vec3 &v) ;
static void scale(Vec3 &v, const double s) ;
static void sub(Vec3 &c, const Vec3 &a, const Vec3 &b) ;
};
class Givens
{
public:
static void rot01_post(Mat3 &m, const double c, const double s);
static void rot02_post(Mat3 &m, const double c, const double s);
static void rot12_post(Mat3 &m, const double c, const double s);
};
class Schur2
{
public:
static void rot01(SMat3 &out, double &c, double &s) ;
static void rot02(SMat3 &out, double &c, double &s) ;
static void rot12(SMat3 &out, double &c, double &s) ;
};
class Svd
{
public:
static void getSymmetricSvd(const SMat3 &a, SMat3 &vtav, Mat3 &v,
const double tol, const int max_sweeps);
static void pseudoinverse(Mat3 &out, const SMat3 &d, const Mat3 &v,
const double tol);
static double solveSymmetric(const SMat3 &A, const Vec3 &b, Vec3 &x,
const double svd_tol, const int svd_sweeps, const double pinv_tol);
};
class LeastSquares
{
public:
static double solveLeastSquares(const Mat3 &a, const Vec3 &b, Vec3 &x,
const double svd_tol, const int svd_sweeps, const double pinv_tol) ;
};
};
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment