diff options
Diffstat (limited to 'extern/libmv/third_party/ssba/Math/v3d_mathutilities.h')
-rw-r--r-- | extern/libmv/third_party/ssba/Math/v3d_mathutilities.h | 59 |
1 files changed, 59 insertions, 0 deletions
diff --git a/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h new file mode 100644 index 00000000000..9e38b92a94b --- /dev/null +++ b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h @@ -0,0 +1,59 @@ +// -*- C++ -*- +/* +Copyright (c) 2008 University of North Carolina at Chapel Hill + +This file is part of SSBA (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef V3D_MATH_UTILITIES_H +#define V3D_MATH_UTILITIES_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_utils.h" + +#include <vector> + +namespace V3D +{ + + template <typename Vec, typename Mat> + inline void + createRotationMatrixRodriguez(Vec const& omega, Mat& R) + { + assert(omega.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + double const theta = norm_L2(omega); + makeIdentityMatrix(R); + if (fabs(theta) > 1e-6) + { + Matrix3x3d J, J2; + makeCrossProductMatrix(omega, J); + multiply_A_B(J, J, J2); + double const c1 = sin(theta)/theta; + double const c2 = (1-cos(theta))/(theta*theta); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + R[i][j] += c1*J[i][j] + c2*J2[i][j]; + } + } // end createRotationMatrixRodriguez() + + template <typename T> inline double sqr(T x) { return x*x; } + +} // namespace V3D + +#endif |