orthanc-stone: Framework/Toolbox/LinearAlgebra.cpp annotate

annotate Framework/Toolbox/LinearAlgebra.cpp @ 165:8d50e6be565d wasm

LinearAlgebra toolbox

author	Sebastien Jodogne <s.jodogne@gmail.com>
date	Thu, 15 Feb 2018 13:51:29 +0100
parents	432b1f812d14
children	4f661e2f7b6c

rev	line source
158 a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	1 /**
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	2 * Stone of Orthanc
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	4 * Department, University Hospital of Liege, Belgium
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	5 * Copyright (C) 2017-2018 Osimis S.A., Belgium
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	6 *
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	7 * This program is free software: you can redistribute it and/or
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	8 * modify it under the terms of the GNU Affero General Public License
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	9 * as published by the Free Software Foundation, either version 3 of
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	10 * the License, or (at your option) any later version.
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	11 *
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	12 * This program is distributed in the hope that it will be useful, but
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	13 * WITHOUT ANY WARRANTY; without even the implied warranty of
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	15 * Affero General Public License for more details.
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	16 *
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	17 * You should have received a copy of the GNU Affero General Public License
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	18 * along with this program. If not, see <http://www.gnu.org/licenses/>.
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	19 **/
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	20
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	21
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	22 #include "LinearAlgebra.h"
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	23
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	24 #include <Core/Logging.h>
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	25 #include <Core/OrthancException.h>
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	26 #include <Core/Toolbox.h>
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	27
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	28 #include <stdio.h>
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	29 #include <boost/lexical_cast.hpp>
164 432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	30 #include <boost/numeric/ublas/lu.hpp>
158 a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	31
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	32 namespace OrthancStone
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	33 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	34 namespace LinearAlgebra
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	35 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	36 void Print(const Vector& v)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	37 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	38 for (size_t i = 0; i < v.size(); i++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	39 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	40 printf("%g\n", v[i]);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	41 //printf("%8.2f\n", v[i]);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	42 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	43 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	44 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	45
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	46
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	47 void Print(const Matrix& m)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	48 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	49 for (size_t i = 0; i < m.size1(); i++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	50 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	51 for (size_t j = 0; j < m.size2(); j++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	52 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	53 printf("%g ", m(i,j));
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	54 //printf("%8.2f ", m(i,j));
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	55 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	56 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	57 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	58 printf("\n");
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	59 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	60
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	61
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	62 bool ParseVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	63 const std::string& value)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	64 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	65 std::vector<std::string> items;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	66 Orthanc::Toolbox::TokenizeString(items, value, '\\');
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	67
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	68 target.resize(items.size());
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	69
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	70 for (size_t i = 0; i < items.size(); i++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	71 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	72 try
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	73 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	74 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i]));
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	75 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	76 catch (boost::bad_lexical_cast&)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	77 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	78 target.clear();
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	79 return false;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	80 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	81 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	82
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	83 return true;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	84 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	85
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	86
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	87 bool ParseVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	88 const Orthanc::DicomMap& dataset,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	89 const Orthanc::DicomTag& tag)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	90 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	91 std::string value;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	92 return (dataset.CopyToString(value, tag, false) &&
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	93 ParseVector(target, value));
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	94 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	95
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	96
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	97 void AssignVector(Vector& v,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	98 double v1,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	99 double v2)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	100 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	101 v.resize(2);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	102 v[0] = v1;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	103 v[1] = v2;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	104 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	105
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	106
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	107 void AssignVector(Vector& v,
165 8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	108 double v1)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	109 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	110 v.resize(1);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	111 v[0] = v1;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	112 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	113
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	114
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	115 void AssignVector(Vector& v,
158 a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	116 double v1,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	117 double v2,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	118 double v3)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	119 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	120 v.resize(3);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	121 v[0] = v1;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	122 v[1] = v2;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	123 v[2] = v3;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	124 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	125
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	126
161 197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	127 void AssignVector(Vector& v,
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	128 double v1,
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	129 double v2,
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	130 double v3,
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	131 double v4)
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	132 {
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	133 v.resize(4);
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	134 v[0] = v1;
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	135 v[1] = v2;
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	136 v[2] = v3;
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	137 v[3] = v4;
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	138 }
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	139
197a5ddaf68c FiniteProjectiveCamera Sebastien Jodogne <s.jodogne@gmail.com> parents: 160 diff changeset	140
165 8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	141 Vector CreateVector(double v1)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	142 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	143 Vector v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	144 AssignVector(v, v1);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	145 return v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	146 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	147
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	148
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	149 Vector CreateVector(double v1,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	150 double v2)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	151 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	152 Vector v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	153 AssignVector(v, v1, v2);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	154 return v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	155 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	156
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	157
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	158 Vector CreateVector(double v1,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	159 double v2,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	160 double v3)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	161 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	162 Vector v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	163 AssignVector(v, v1, v2, v3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	164 return v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	165 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	166
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	167
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	168 Vector CreateVector(double v1,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	169 double v2,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	170 double v3,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	171 double v4)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	172 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	173 Vector v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	174 AssignVector(v, v1, v2, v3, v4);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	175 return v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	176 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	177
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	178
158 a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	179 bool IsNear(double x,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	180 double y)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	181 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	182 // As most input is read as single-precision numbers, we take the
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	183 // epsilon machine for float32 into consideration to compare numbers
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	184 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon());
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	185 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	186
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	187
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	188 void NormalizeVector(Vector& u)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	189 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	190 double norm = boost::numeric::ublas::norm_2(u);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	191 if (!IsCloseToZero(norm))
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	192 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	193 u = u / norm;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	194 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	195 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	196
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	197
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	198 void CrossProduct(Vector& result,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	199 const Vector& u,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	200 const Vector& v)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	201 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	202 if (u.size() != 3 \|\|
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	203 v.size() != 3)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	204 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	205 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	206 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	207
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	208 result.resize(3);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	209
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	210 result[0] = u[1] * v[2] - u[2] * v[1];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	211 result[1] = u[2] * v[0] - u[0] * v[2];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	212 result[2] = u[0] * v[1] - u[1] * v[0];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	213 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	214
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	215
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	216 void FillMatrix(Matrix& target,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	217 size_t rows,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	218 size_t columns,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	219 const double values[])
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	220 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	221 target.resize(rows, columns);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	222
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	223 size_t index = 0;
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	224
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	225 for (size_t y = 0; y < rows; y++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	226 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	227 for (size_t x = 0; x < columns; x++, index++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	228 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	229 target(y, x) = values[index];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	230 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	231 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	232 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	233
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	234
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	235 void FillVector(Vector& target,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	236 size_t size,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	237 const double values[])
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	238 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	239 target.resize(size);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	240
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	241 for (size_t i = 0; i < size; i++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	242 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	243 target[i] = values[i];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	244 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	245 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	246
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	247
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	248 void Convert(Matrix& target,
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	249 const Vector& source)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	250 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	251 const size_t n = source.size();
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	252
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	253 target.resize(n, 1);
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	254
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	255 for (size_t i = 0; i < n; i++)
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	256 {
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	257 target(i, 0) = source[i];
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	258 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	259 }
159 0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	260
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	261
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	262 double ComputeDeterminant(const Matrix& a)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	263 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	264 if (a.size1() != a.size2())
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	265 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	266 LOG(ERROR) << "Determinant only exists for square matrices";
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	267 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	268 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	269
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	270 // https://en.wikipedia.org/wiki/Rule_of_Sarrus
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	271 if (a.size1() == 1)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	272 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	273 return a(0,0);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	274 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	275 else if (a.size1() == 2)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	276 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	277 return a(0,0) * a(1,1) - a(0,1) * a(1,0);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	278 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	279 else if (a.size1() == 3)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	280 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	281 return (a(0,0) * a(1,1) * a(2,2) +
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	282 a(0,1) * a(1,2) * a(2,0) +
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	283 a(0,2) * a(1,0) * a(2,1) -
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	284 a(2,0) * a(1,1) * a(0,2) -
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	285 a(2,1) * a(1,2) * a(0,0) -
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	286 a(2,2) * a(1,0) * a(0,1));
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	287 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	288 else
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	289 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	290 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	291 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	292 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	293
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	294
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	295 bool IsOrthogonalMatrix(const Matrix& q,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	296 double threshold)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	297 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	298 // https://en.wikipedia.org/wiki/Orthogonal_matrix
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	299
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	300 using namespace boost::numeric::ublas;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	301
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	302 const Matrix check = prod(trans(q), q) - identity_matrix<double>(3);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	303
160 e9dae7e7bffc fix Sebastien Jodogne <s.jodogne@gmail.com> parents: 159 diff changeset	304 type_traits<double>::real_type norm = norm_inf(check);
159 0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	305
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	306 return (norm <= threshold);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	307 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	308
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	309
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	310 bool IsOrthogonalMatrix(const Matrix& q)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	311 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	312 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon());
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	313 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	314
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	315
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	316 bool IsRotationMatrix(const Matrix& r,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	317 double threshold)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	318 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	319 return (IsOrthogonalMatrix(r, threshold) &&
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	320 IsNear(ComputeDeterminant(r), 1.0, threshold));
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	321 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	322
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	323
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	324 bool IsRotationMatrix(const Matrix& r)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	325 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	326 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon());
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	327 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	328
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	329
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	330 void InvertUpperTriangularMatrix(Matrix& output,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	331 const Matrix& k)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	332 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	333 if (k.size1() != k.size2())
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	334 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	335 LOG(ERROR) << "Determinant only exists for square matrices";
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	336 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	337 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	338
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	339 output.resize(k.size1(), k.size2());
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	340
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	341 for (size_t i = 1; i < k.size1(); i++)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	342 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	343 for (size_t j = 0; j < i; j++)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	344 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	345 if (!IsCloseToZero(k(i, j)))
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	346 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	347 LOG(ERROR) << "Not an upper triangular matrix";
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	348 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	349 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	350
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	351 output(i, j) = 0; // The output is also upper triangular
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	352 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	353 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	354
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	355 if (k.size1() == 3)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	356 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	357 // https://math.stackexchange.com/a/1004181
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	358 double a = k(0, 0);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	359 double b = k(0, 1);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	360 double c = k(0, 2);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	361 double d = k(1, 1);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	362 double e = k(1, 2);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	363 double f = k(2, 2);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	364
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	365 if (IsCloseToZero(a) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	366 IsCloseToZero(d) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	367 IsCloseToZero(f))
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	368 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	369 LOG(ERROR) << "Singular upper triangular matrix";
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	370 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	371 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	372 else
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	373 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	374 output(0, 0) = 1.0 / a;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	375 output(0, 1) = -b / (a * d);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	376 output(0, 2) = (b * e - c * d) / (a * f * d);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	377 output(1, 1) = 1.0 / d;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	378 output(1, 2) = -e / (f * d);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	379 output(2, 2) = 1.0 / f;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	380 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	381 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	382 else
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	383 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	384 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	385 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	386 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	387
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	388
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	389 static void GetGivensComponent(double& c,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	390 double& s,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	391 const Matrix& a,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	392 size_t i,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	393 size_t j)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	394 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	395 assert(i < 3 && j < 3);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	396
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	397 double x = a(i, i);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	398 double y = a(i, j);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	399 double n = sqrt(x * x + y * y);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	400
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	401 if (IsCloseToZero(n))
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	402 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	403 c = 1;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	404 s = 0;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	405 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	406 else
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	407 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	408 c = x / n;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	409 s = -y / n;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	410 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	411 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	412
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	413
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	414 /**
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	415 * This function computes the RQ decomposition of a 3x3 matrix,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	416 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	417 * "Multiple View Geometry in Computer Vision" (2nd edition). The
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	418 * output matrix "Q" is a rotation matrix, and "R" is upper
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	419 * triangular.
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	420 **/
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	421 void RQDecomposition3x3(Matrix& r,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	422 Matrix& q,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	423 const Matrix& a)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	424 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	425 using namespace boost::numeric::ublas;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	426
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	427 if (a.size1() != 3 \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	428 a.size2() != 3)
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	429 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	430 LOG(ERROR) << "Only applicable to a 3x3 matrix";
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	431 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	432 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	433
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	434 r.resize(3, 3);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	435 q.resize(3, 3);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	436
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	437 r = a;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	438 q = identity_matrix<double>(3);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	439
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	440 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	441 // Set A(2,1) to zero
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	442 double c, s;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	443 GetGivensComponent(c, s, r, 2, 1);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	444
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	445 double v[9] = { 1, 0, 0,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	446 0, c, -s,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	447 0, s, c };
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	448
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	449 Matrix g;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	450 FillMatrix(g, 3, 3, v);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	451
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	452 r = prod(r, g);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	453 q = prod(trans(g), q);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	454 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	455
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	456
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	457 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	458 // Set A(2,0) to zero
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	459 double c, s;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	460 GetGivensComponent(c, s, r, 2, 0);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	461
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	462 double v[9] = { c, 0, -s,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	463 0, 1, 0,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	464 s, 0, c };
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	465
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	466 Matrix g;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	467 FillMatrix(g, 3, 3, v);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	468
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	469 r = prod(r, g);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	470 q = prod(trans(g), q);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	471 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	472
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	473
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	474 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	475 // Set A(1,0) to zero
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	476 double c, s;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	477 GetGivensComponent(c, s, r, 1, 0);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	478
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	479 double v[9] = { c, -s, 0,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	480 s, c, 0,
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	481 0, 0, 1 };
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	482
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	483 Matrix g;
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	484 FillMatrix(g, 3, 3, v);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	485
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	486 r = prod(r, g);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	487 q = prod(trans(g), q);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	488 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	489
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	490 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	491 !IsRotationMatrix(q) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	492 !IsCloseToZero(r(1, 0)) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	493 !IsCloseToZero(r(2, 0)) \|\|
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	494 !IsCloseToZero(r(2, 1)))
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	495 {
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	496 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	497 }
0a73d76333db populating LinearAlgebra Sebastien Jodogne <s.jodogne@gmail.com> parents: 158 diff changeset	498 }
163 8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	499
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	500
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	501 void InvertMatrix(Matrix& target,
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	502 const Matrix& source)
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	503 {
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	504 if (source.size1() != source.size2())
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	505 {
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	506 LOG(ERROR) << "Inverse only exists for square matrices";
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	507 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	508 }
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	509
164 432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	510 if (source.size1() < 4)
163 8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	511 {
164 432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	512 // For matrices with size below 4, use direct computations
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	513 // instead of LU decomposition
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	514
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	515 if (source.size1() == 0)
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	516 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	517 // By convention, the inverse of the empty matrix, is itself the empty matrix
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	518 target.resize(0, 0);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	519 return;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	520 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	521
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	522 double determinant = ComputeDeterminant(source);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	523
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	524 if (IsCloseToZero(determinant))
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	525 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	526 LOG(ERROR) << "Cannot invert singular matrix";
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	527 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	528 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	529
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	530 double denominator = 1.0 / determinant;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	531
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	532 target.resize(source.size1(), source.size2());
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	533
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	534 if (source.size1() == 1)
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	535 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	536 target(0, 0) = denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	537 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	538 else if (source.size1() == 2)
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	539 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	540 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	541 target(0, 0) = source(1, 1) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	542 target(0, 1) = -source(0, 1) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	543 target(1, 0) = -source(1, 0) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	544 target(1, 1) = source(0, 0) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	545 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	546 else if (source.size1() == 3)
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	547 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	548 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	549 const double a = source(0, 0);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	550 const double b = source(0, 1);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	551 const double c = source(0, 2);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	552 const double d = source(1, 0);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	553 const double e = source(1, 1);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	554 const double f = source(1, 2);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	555 const double g = source(2, 0);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	556 const double h = source(2, 1);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	557 const double i = source(2, 2);
163 8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	558
164 432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	559 target(0, 0) = (e * i - f * h) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	560 target(0, 1) = -(b * i - c * h) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	561 target(0, 2) = (b * f - c * e) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	562 target(1, 0) = -(d * i - f * g) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	563 target(1, 1) = (a * i - c * g) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	564 target(1, 2) = -(a * f - c * d) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	565 target(2, 0) = (d * h - e * g) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	566 target(2, 1) = -(a * h - b * g) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	567 target(2, 2) = (a * e - b * d) * denominator;
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	568 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	569 else
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	570 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	571 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	572 }
163 8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	573 }
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	574 else
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	575 {
164 432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	576 // General case, using LU decomposition
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	577
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	578 Matrix a = source; // Copy the source matrix, as "lu_factorize()" modifies it
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	579
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	580 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1());
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	581
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	582 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0)
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	583 {
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	584 LOG(ERROR) << "Cannot invert singular matrix";
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	585 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	586 }
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	587
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	588 target = boost::numeric::ublas::identity_matrix<double>(source.size1());
432b1f812d14 inversion of general matrices Sebastien Jodogne <s.jodogne@gmail.com> parents: 163 diff changeset	589 lu_substitute(a, permutation, target);
163 8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	590 }
8c5b24892ed2 LinearAlgebra::InvertMatrix Sebastien Jodogne <s.jodogne@gmail.com> parents: 161 diff changeset	591 }
165 8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	592
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	593
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	594 void CreateSkewSymmetric(Matrix& s,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	595 const Vector& v)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	596 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	597 assert(v.size() == 3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	598
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	599 s.resize(3, 3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	600 s(0,0) = 0;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	601 s(0,1) = -v[2];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	602 s(0,2) = v[1];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	603 s(1,0) = v[2];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	604 s(1,1) = 0;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	605 s(1,2) = -v[0];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	606 s(2,0) = -v[1];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	607 s(2,1) = v[0];
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	608 s(2,2) = 0;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	609 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	610
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	611
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	612 void AlignVectorsWithRotation(Matrix& r,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	613 const Vector& a,
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	614 const Vector& b)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	615 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	616 // This is Rodrigues' rotation formula:
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	617 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	618
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	619 // Check also result A4.6 from "Multiple View Geometry in Computer
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	620 // Vision - 2nd edition" (p. 584)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	621
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	622 if (a.size() != 3 \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	623 b.size() != 3)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	624 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	625 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	626 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	627
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	628 double aNorm = boost::numeric::ublas::norm_2(a);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	629 double bNorm = boost::numeric::ublas::norm_2(b);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	630
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	631 if (LinearAlgebra::IsCloseToZero(aNorm) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	632 LinearAlgebra::IsCloseToZero(bNorm))
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	633 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	634 LOG(ERROR) << "Vector with zero norm";
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	635 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	636 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	637
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	638 Vector aUnit, bUnit;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	639 aUnit = a / aNorm;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	640 bUnit = b / bNorm;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	641
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	642 Vector v;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	643 LinearAlgebra::CrossProduct(v, aUnit, bUnit);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	644
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	645 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	646
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	647 if (LinearAlgebra::IsCloseToZero(1 + cosine))
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	648 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	649 // "a == -b": TODO
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	650 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	651 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	652
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	653 Matrix k;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	654 CreateSkewSymmetric(k, v);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	655
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	656 #if 0
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	657 double sine = boost::numeric::ublas::norm_2(v);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	658
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	659 r = (boost::numeric::ublas::identity_matrix<double>(3) +
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	660 sine * k +
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	661 (1 - cosine) * boost::numeric::ublas::prod(k, k));
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	662 #else
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	663 r = (boost::numeric::ublas::identity_matrix<double>(3) +
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	664 k +
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	665 boost::numeric::ublas::prod(k, k) / (1 + cosine));
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	666 #endif
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	667 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	668
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	669
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	670 Matrix InvertScaleTranslationMatrix(const Matrix& t)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	671 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	672 if (t.size1() != 4 \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	673 t.size2() != 4 \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	674 !LinearAlgebra::IsCloseToZero(t(0,1)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	675 !LinearAlgebra::IsCloseToZero(t(0,2)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	676 !LinearAlgebra::IsCloseToZero(t(1,0)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	677 !LinearAlgebra::IsCloseToZero(t(1,2)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	678 !LinearAlgebra::IsCloseToZero(t(2,0)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	679 !LinearAlgebra::IsCloseToZero(t(2,1)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	680 !LinearAlgebra::IsCloseToZero(t(3,0)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	681 !LinearAlgebra::IsCloseToZero(t(3,1)) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	682 !LinearAlgebra::IsCloseToZero(t(3,2)))
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	683 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	684 LOG(ERROR) << "This matrix is more than a zoom/translate transform";
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	685 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	686 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	687
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	688 const double sx = t(0,0);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	689 const double sy = t(1,1);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	690 const double sz = t(2,2);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	691 const double w = t(3,3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	692
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	693 if (LinearAlgebra::IsCloseToZero(sx) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	694 LinearAlgebra::IsCloseToZero(sy) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	695 LinearAlgebra::IsCloseToZero(sz) \|\|
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	696 LinearAlgebra::IsCloseToZero(w))
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	697 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	698 LOG(ERROR) << "Singular transform";
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	699 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	700 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	701
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	702 const double tx = t(0,3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	703 const double ty = t(1,3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	704 const double tz = t(2,3);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	705
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	706 Matrix m = IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	707
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	708 m(0,0) = 1.0 / sx;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	709 m(1,1) = 1.0 / sy;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	710 m(2,2) = 1.0 / sz;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	711 m(3,3) = 1.0 / w;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	712
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	713 m(0,3) = -tx / (sx * w);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	714 m(1,3) = -ty / (sy * w);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	715 m(2,3) = -tz / (sz * w);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	716
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	717 return m;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	718 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	719
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	720
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	721 bool IsShearMatrix(const Matrix& shear)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	722 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	723 return (shear.size1() == 4 &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	724 shear.size2() == 4 &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	725 LinearAlgebra::IsNear(1.0, shear(0,0)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	726 LinearAlgebra::IsNear(0.0, shear(0,1)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	727 LinearAlgebra::IsNear(0.0, shear(0,3)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	728 LinearAlgebra::IsNear(0.0, shear(1,0)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	729 LinearAlgebra::IsNear(1.0, shear(1,1)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	730 LinearAlgebra::IsNear(0.0, shear(1,3)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	731 LinearAlgebra::IsNear(0.0, shear(2,0)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	732 LinearAlgebra::IsNear(0.0, shear(2,1)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	733 LinearAlgebra::IsNear(1.0, shear(2,2)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	734 LinearAlgebra::IsNear(0.0, shear(2,3)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	735 LinearAlgebra::IsNear(0.0, shear(3,0)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	736 LinearAlgebra::IsNear(0.0, shear(3,1)) &&
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	737 LinearAlgebra::IsNear(1.0, shear(3,3)));
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	738 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	739
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	740
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	741 Matrix InvertShearMatrix(const Matrix& shear)
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	742 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	743 if (!IsShearMatrix(shear))
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	744 {
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	745 LOG(ERROR) << "Not a valid shear matrix";
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	746 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	747 }
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	748
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	749 Matrix m = IdentityMatrix(4);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	750 m(0,2) = -shear(0,2);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	751 m(1,2) = -shear(1,2);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	752 m(3,2) = -shear(3,2);
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	753
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	754 return m;
8d50e6be565d LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: 164 diff changeset	755 }
158 a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	756 }
a053ca7fa5c6 LinearAlgebra toolbox Sebastien Jodogne <s.jodogne@gmail.com> parents: diff changeset	757 }

Mercurial > hg > orthanc-stone

annotate Framework/Toolbox/LinearAlgebra.cpp @ 165:8d50e6be565d wasm