ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

492
thirdparty/opengv/src/math/Sturm.cpp vendored Normal file
View File

@@ -0,0 +1,492 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/Sturm.hpp>
#include <iostream>
opengv::math::Bracket::Bracket( double lowerBound, double upperBound ) :
_lowerBound(lowerBound),
_upperBound(upperBound),
_lowerBoundChangesComputed(false),
_upperBoundChangesComputed(false),
_lowerBoundChanges(0),
_upperBoundChanges(0)
{}
opengv::math::Bracket::Bracket(
double lowerBound, double upperBound, size_t changes, bool setUpperBoundChanges ) :
_lowerBound(lowerBound),
_upperBound(upperBound),
_lowerBoundChangesComputed(false),
_upperBoundChangesComputed(false),
_lowerBoundChanges(0),
_upperBoundChanges(0)
{
if( setUpperBoundChanges )
{
_upperBoundChanges = changes;
_upperBoundChangesComputed = true;
}
else
{
_lowerBoundChanges = changes;
_lowerBoundChangesComputed = true;
}
}
opengv::math::Bracket::~Bracket()
{}
bool
opengv::math::Bracket::dividable( double eps ) const
{
if( numberRoots() == 1 && (_upperBound - _lowerBound ) < eps )
return false;
if( numberRoots() == 0 )
return false;
double center = (_upperBound + _lowerBound) / 2.0;
if( center == _upperBound || center == _lowerBound)
return false;
return true;
}
void
opengv::math::Bracket::divide( std::list<Ptr> & brackets ) const
{
double center = (_upperBound + _lowerBound) / 2.0;
Ptr lowerBracket(new Bracket(_lowerBound,center,_lowerBoundChanges,false));
Ptr upperBracket(new Bracket(center,_upperBound,_upperBoundChanges,true));
brackets.push_back(lowerBracket);
brackets.push_back(upperBracket);
}
double
opengv::math::Bracket::lowerBound() const
{
return _lowerBound;
}
double
opengv::math::Bracket::upperBound() const
{
return _upperBound;
}
bool
opengv::math::Bracket::lowerBoundChangesComputed() const
{
return _lowerBoundChangesComputed;
}
bool
opengv::math::Bracket::upperBoundChangesComputed() const
{
return _upperBoundChangesComputed;
}
void
opengv::math::Bracket::setLowerBoundChanges( size_t changes )
{
_lowerBoundChanges = changes;
_lowerBoundChangesComputed = true;
}
void
opengv::math::Bracket::setUpperBoundChanges( size_t changes )
{
_upperBoundChanges = changes;
_upperBoundChangesComputed = true;
}
size_t
opengv::math::Bracket::numberRoots() const
{
if( !_lowerBoundChangesComputed || !_upperBoundChangesComputed )
{
std::cout << "Error: cannot evaluate number of roots" <<std::endl;
return 0;
}
return _lowerBoundChanges - _upperBoundChanges;
}
opengv::math::Sturm::Sturm( const Eigen::MatrixXd & p ) :
_C(Eigen::MatrixXd(p.cols(),p.cols()))
{
_dimension = (size_t) _C.cols();
_C = Eigen::MatrixXd(_dimension,_dimension);
_C.setZero();
_C.row(0) = p;
for( size_t i = 1; i < _dimension; i++ )
_C(1,i) = _C(0,i-1) * (_dimension-i);
for( size_t i = 2; i < _dimension; i++ )
{
Eigen::MatrixXd p1 = _C.block(i-2,i-2,1,_dimension-(i-2));
Eigen::MatrixXd p2 = _C.block(i-1,i-1,1,_dimension-(i-1));
Eigen::MatrixXd r;
computeNegatedRemainder(p1,p2,r);
_C.block(i,i,1,_dimension-i) = r.block(0,2,1,_dimension-i);
}
}
opengv::math::Sturm::Sturm( const std::vector<double> & p ) :
_C(Eigen::MatrixXd(p.size(),p.size()))
{
_dimension = (size_t) _C.cols();
_C = Eigen::MatrixXd(_dimension,_dimension);
_C.setZero();
for( size_t i = 0; i < _dimension; i++ )
_C(0,i) = p[i];
for( size_t i = 1; i < _dimension; i++ )
_C(1,i) = _C(0,i-1) * (_dimension-i);
for( size_t i = 2; i < _dimension; i++ )
{
Eigen::MatrixXd p1 = _C.block(i-2,i-2,1,_dimension-(i-2));
Eigen::MatrixXd p2 = _C.block(i-1,i-1,1,_dimension-(i-1));
Eigen::MatrixXd r;
computeNegatedRemainder(p1,p2,r);
_C.block(i,i,1,_dimension-i) = r.block(0,2,1,_dimension-i);
}
}
opengv::math::Sturm::~Sturm() {};
void
opengv::math::Sturm::findRoots2( std::vector<double> & roots, double eps_x, double eps_val )
{
double absoluteBound = computeLagrangianBound();
std::list<Bracket::Ptr> stack;
stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound)));
stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) );
stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) );
roots.reserve(stack.back()->numberRoots());
//some variables for pollishing
Eigen::MatrixXd monomials(_dimension,1);
monomials(_dimension-1,0) = 1.0;
while( !stack.empty() )
{
Bracket::Ptr bracket = stack.front();
stack.pop_front();
if( bracket->dividable(eps_x) )
{
bool divide = true;
if( bracket->numberRoots() == 1 )
{
//new part, we try immediately to do the pollishing here
bool converged = false;
double root = 0.5 * (bracket->lowerBound() + bracket->upperBound());
for(size_t i = 2; i <= _dimension; i++)
monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*root;
Eigen::MatrixXd matValue = _C.row(0) * monomials;
double value = matValue(0,0);
while( !converged )
{
Eigen::MatrixXd matDerivative = _C.row(1) * monomials;
double derivative = matDerivative(0,0);
double newRoot = root - (value/derivative);
if( newRoot < bracket->lowerBound() || newRoot > bracket->upperBound() )
break;
for(size_t i = 2; i <= _dimension; i++)
monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*newRoot;
matValue = _C.row(0) * monomials;
double newValue = matValue(0,0);
if( fabs(newValue) > fabs(value) )
break;
//do update
value = newValue;
root = newRoot;
//check if converged
if( fabs(value) < eps_val )
converged = true;
}
if( converged )
{
divide = false;
roots.push_back(root);
}
}
if(divide)
{
bracket->divide(stack);
std::list<Bracket::Ptr>::iterator it = stack.end();
it--;
size_t changes = evaluateChain2((*it)->lowerBound());
(*it)->setLowerBoundChanges(changes);
it--;
(*it)->setUpperBoundChanges(changes);
}
}
else
{
if( bracket->numberRoots() > 0 )
roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound()));
}
}
}
std::vector<double>
opengv::math::Sturm::findRoots()
{
//bracket the roots
std::vector<double> roots;
bracketRoots(roots);
//pollish
Eigen::MatrixXd monomials(_dimension,1);
monomials(_dimension-1,0) = 1.0;
for(size_t r = 0; r < roots.size(); r++ )
{
//Now do Gauss iterations here
//evaluate all monomials at the left bound
for(size_t k = 0; k < 5; k++ )
{
for(size_t i = 2; i <= _dimension; i++)
monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*roots[r];
Eigen::MatrixXd value = _C.row(0) * monomials;
Eigen::MatrixXd derivative = _C.row(1) * monomials;
//correction
roots[r] = roots[r] - (value(0,0)/derivative(0,0));
}
}
return roots;
}
void
opengv::math::Sturm::bracketRoots( std::vector<double> & roots, double eps )
{
double absoluteBound = computeLagrangianBound();
std::list<Bracket::Ptr> stack;
stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound)));
stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) );
stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) );
double localEps = eps;
if( eps < 0.0 )
localEps = absoluteBound / (10.0 * stack.back()->numberRoots());
roots.reserve(stack.back()->numberRoots());
while( !stack.empty() )
{
Bracket::Ptr bracket = stack.front();
stack.pop_front();
if( bracket->dividable( localEps) )
{
bracket->divide(stack);
std::list<Bracket::Ptr>::iterator it = stack.end();
it--;
size_t changes = evaluateChain2((*it)->lowerBound());
(*it)->setLowerBoundChanges(changes);
it--;
(*it)->setUpperBoundChanges(changes);
}
else
{
if( bracket->numberRoots() > 0 )
roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound()));
}
}
}
size_t
opengv::math::Sturm::evaluateChain( double bound )
{
Eigen::MatrixXd monomials(_dimension,1);
monomials(_dimension-1,0) = 1.0;
//evaluate all monomials at the bound
for(size_t i = 2; i <= _dimension; i++)
monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*bound;
Eigen::MatrixXd signs(_dimension,1);
for( size_t i = 0; i < _dimension; i++ )
signs.block(i,0,1,1) = _C.block(i,i,1,_dimension-i) * monomials.block(i,0,_dimension-i,1);
bool positive = false;
if( signs(0,0) > 0.0 )
positive = true;
int signChanges = 0;
for( size_t i = 1; i < _dimension; i++ )
{
if( positive )
{
if( signs(i,0) < 0.0 )
{
signChanges++;
positive = false;
}
}
else
{
if( signs(i,0) > 0.0 )
{
signChanges++;
positive = true;
}
}
}
return signChanges;
}
size_t
opengv::math::Sturm::evaluateChain2( double bound )
{
std::vector<double> monomials;
monomials.resize(_dimension);
monomials[_dimension-1] = 1.0;
//evaluate all monomials at the bound
for(size_t i = 2; i <= _dimension; i++)
monomials[_dimension-i] = monomials[_dimension-i+1]*bound;
std::vector<double> signs;
signs.reserve(_dimension);
for( size_t i = 0; i < _dimension; i++ )
{
signs.push_back(0.0);
for( size_t j = i; j < _dimension; j++ )
signs.back() += _C(i,j) * monomials[j];
}
bool positive = false;
if( signs[0] > 0.0 )
positive = true;
int signChanges = 0;
for( size_t i = 1; i < _dimension; i++ )
{
if( positive )
{
if( signs[i] < 0.0 )
{
signChanges++;
positive = false;
}
}
else
{
if( signs[i] > 0.0 )
{
signChanges++;
positive = true;
}
}
}
return signChanges;
}
void
opengv::math::Sturm::computeNegatedRemainder(
const Eigen::MatrixXd & p1,
const Eigen::MatrixXd & p2,
Eigen::MatrixXd & r )
{
//we have to create 3 subtraction polynomials
Eigen::MatrixXd poly_1(1,p1.cols());
poly_1.block(0,0,1,p2.cols()) = (p1(0,0)/p2(0,0)) * p2;
poly_1(0,p1.cols()-1) = 0.0;
Eigen::MatrixXd poly_2(1,p1.cols());
poly_2.block(0,1,1,p2.cols()) = (p1(0,1)/p2(0,0)) * p2;
poly_2(0,0) = 0.0;
Eigen::MatrixXd poly_3(1,p1.cols());
poly_3.block(0,1,1,p2.cols()) = (-p2(0,1)*p1(0,0)/pow(p2(0,0),2)) * p2;
poly_3(0,0) = 0.0;
//compute remainder
r = -p1 + poly_1 + poly_2 + poly_3;
}
double
opengv::math::Sturm::computeLagrangianBound()
{
std::vector<double> coefficients;
coefficients.reserve(_dimension-1);
for(size_t i=0; i < _dimension-1; i++)
coefficients.push_back(pow(fabs(_C(0,i+1)/_C(0,0)),(1.0/(i+1))));
size_t j = 0;
double max1 = -1.0;
for( size_t i = 0; i < coefficients.size(); i++ )
{
if( coefficients[i] > max1 )
{
j = i;
max1 = coefficients[i];
}
}
coefficients[j] = -1.0;
double max2 = -1.0;
for( size_t i=0; i < coefficients.size(); i++ )
{
if( coefficients[i] > max2 )
max2 = coefficients[i];
}
double bound = max1 + max2;
return bound;
}

98
thirdparty/opengv/src/math/arun.cpp vendored Normal file
View File

@@ -0,0 +1,98 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/arun.hpp>
opengv::rotation_t
opengv::math::arun( const Eigen::MatrixXd & Hcross )
{
//decompose matrix H to obtain rotation
Eigen::JacobiSVD< Eigen::MatrixXd > SVDcross(
Hcross,
Eigen::ComputeFullU | Eigen::ComputeFullV );
Eigen::Matrix3d V = SVDcross.matrixV();
Eigen::Matrix3d U = SVDcross.matrixU();
rotation_t R = V * U.transpose();
//modify the result in case the rotation has determinant=-1
if( R.determinant() < 0 )
{
Eigen::Matrix3d V_prime;
V_prime.col(0) = V.col(0);
V_prime.col(1) = V.col(1);
V_prime.col(2) = -V.col(2);
R = V_prime * U.transpose();
}
return R;
}
opengv::transformation_t
opengv::math::arun_complete(
const points_t & p1,
const points_t & p2 )
{
assert(p1.size() == p2.size());
//derive the centroid of the two point-clouds
point_t pointsCenter1 = Eigen::Vector3d::Zero();
point_t pointsCenter2 = Eigen::Vector3d::Zero();
for( size_t i = 0; i < p1.size(); i++ )
{
pointsCenter1 += p1[i];
pointsCenter2 += p2[i];
}
pointsCenter1 = pointsCenter1 / p1.size();
pointsCenter2 = pointsCenter2 / p2.size();
//compute the matrix H = sum(f'*f^{T})
Eigen::MatrixXd Hcross(3,3);
Hcross = Eigen::Matrix3d::Zero();
for( size_t i = 0; i < p1.size(); i++ )
{
Eigen::Vector3d f = p1[i] - pointsCenter1;
Eigen::Vector3d fprime = p2[i] - pointsCenter2;
Hcross += fprime * f.transpose();
}
//decompose this matrix (SVD) to obtain rotation
rotation_t rotation = arun(Hcross);
translation_t translation = pointsCenter1 - rotation*pointsCenter2;
transformation_t solution;
solution.block<3,3>(0,0) = rotation;
solution.col(3) = translation;
return solution;
}

88
thirdparty/opengv/src/math/cayley.cpp vendored Normal file
View File

@@ -0,0 +1,88 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/cayley.hpp>
opengv::rotation_t
opengv::math::cayley2rot( const cayley_t & cayley)
{
rotation_t R;
double scale = 1+pow(cayley[0],2)+pow(cayley[1],2)+pow(cayley[2],2);
R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2);
R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]);
R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]);
R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]);
R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2);
R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]);
R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]);
R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]);
R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2);
R = (1/scale) * R;
return R;
}
opengv::rotation_t
opengv::math::cayley2rot_reduced( const cayley_t & cayley)
{
rotation_t R;
R(0,0) = 1+pow(cayley[0],2)-pow(cayley[1],2)-pow(cayley[2],2);
R(0,1) = 2*(cayley[0]*cayley[1]-cayley[2]);
R(0,2) = 2*(cayley[0]*cayley[2]+cayley[1]);
R(1,0) = 2*(cayley[0]*cayley[1]+cayley[2]);
R(1,1) = 1-pow(cayley[0],2)+pow(cayley[1],2)-pow(cayley[2],2);
R(1,2) = 2*(cayley[1]*cayley[2]-cayley[0]);
R(2,0) = 2*(cayley[0]*cayley[2]-cayley[1]);
R(2,1) = 2*(cayley[1]*cayley[2]+cayley[0]);
R(2,2) = 1-pow(cayley[0],2)-pow(cayley[1],2)+pow(cayley[2],2);
return R;
}
opengv::cayley_t
opengv::math::rot2cayley( const rotation_t & R )
{
Eigen::Matrix3d C1;
Eigen::Matrix3d C2;
Eigen::Matrix3d C;
C1 = R-Eigen::Matrix3d::Identity();
C2 = R+Eigen::Matrix3d::Identity();
C = C1 * C2.inverse();
cayley_t cayley;
cayley[0] = -C(1,2);
cayley[1] = C(0,2);
cayley[2] = -C(0,1);
return cayley;
}

154
thirdparty/opengv/src/math/gauss_jordan.cpp vendored Normal file
View File

@@ -0,0 +1,154 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/gauss_jordan.hpp>
#include <math.h>
void
opengv::math::gauss_jordan(
std::vector<std::vector<double>*> & matrix, int exitCondition )
{
//create some constants for zero-checking etc.
double precision = 1.0e-10;
int rows = matrix.size();
int cols = matrix[0]->size();
//the working row
int frontRow;
//first step down
for( frontRow = 0; frontRow < rows; frontRow++ )
{
// first iterate through the rows and find the row that has the biggest
// leading coefficient
double maxValue = -1.0;
int row = -1;
for( int tempRow = frontRow; tempRow < rows; tempRow++ )
{
double value = fabs((*(matrix[tempRow]))[frontRow]);
if( value > maxValue )
{
row = tempRow;
maxValue = value;
}
}
//rowIter is now the row that should go in the place of frontRow->swap
std::swap( matrix[row], matrix[frontRow] );
//ok, now use frontRow!
//first divide all coefficients by the leading coefficient
int col = frontRow;
double leadingCoefficient_inv = 1.0 / (*(matrix[frontRow]))[col];
(*(matrix[frontRow]))[col] = 1.0;
++col;
while( col < cols )
{
(*(matrix[frontRow]))[col] *= leadingCoefficient_inv;
++col;
}
//create a vector of bools indicating the cols that need to be manipulated
col = frontRow;
std::vector<int> nonzeroIdx;
nonzeroIdx.reserve( cols - frontRow );
while( col < cols )
{
if( fabs(((*(matrix[frontRow]))[col])) > precision )
nonzeroIdx.push_back(col);
col++;
}
//iterate through all remaining rows, and subtract correct multiple of
//first row (if leading coefficient is non-zero!)
row = frontRow;
++row;
while( row < rows )
{
col = frontRow;
double leadingCoefficient = (*(matrix[row]))[col];
if( fabs(leadingCoefficient) > precision )
{
for( int col = 0; col < (int) nonzeroIdx.size(); col++ )
(*(matrix[row]))[nonzeroIdx[col]] -=
leadingCoefficient * ((*(matrix[frontRow]))[nonzeroIdx[col]]);
}
++row;
}
}
//set index to the last non-zero row
--frontRow;
//Now step up
while( frontRow > exitCondition )
{
//create a vector of bools indicating the cols that need to be manipulated
int col = frontRow;
std::vector<int> nonzeroIdx;
nonzeroIdx.reserve( cols - frontRow );
while( col < cols )
{
if( fabs(((*(matrix[frontRow]))[col])) > precision )
nonzeroIdx.push_back(col);
col++;
}
//get the working row
int row = frontRow;
do
{
//decrement working row
--row;
//working column
//now get the leading coefficient
double leadingCoefficient = (*(matrix[row]))[frontRow];
//Now iterator until the end, and subtract each time the multiplied
//front-row
if( fabs(leadingCoefficient) > precision )
{
for( int col = 0; col < (int) nonzeroIdx.size(); col++ )
(*(matrix[row]))[nonzeroIdx[col]] -=
leadingCoefficient * (*(matrix[frontRow]))[nonzeroIdx[col]];
}
}
while( row > exitCondition );
--frontRow;
}
}

107
thirdparty/opengv/src/math/quaternion.cpp vendored Normal file
View File

@@ -0,0 +1,107 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/quaternion.hpp>
opengv::rotation_t
opengv::math::quaternion2rot( const quaternion_t & q)
{
rotation_t R;
R(0,0) = pow(q[0],2)+pow(q[1],2)-pow(q[2],2)-pow(q[3],2);
R(0,1) = 2.0*(q[1]*q[2]-q[0]*q[3]);
R(0,2) = 2.0*(q[1]*q[3]+q[0]*q[2]);
R(1,0) = 2.0*(q[1]*q[2]+q[0]*q[3]);
R(1,1) = pow(q[0],2)-pow(q[1],2)+pow(q[2],2)-pow(q[3],2);
R(1,2) = 2.0*(q[2]*q[3]-q[0]*q[1]);
R(2,0) = 2.0*(q[1]*q[3]-q[0]*q[2]);
R(2,1) = 2.0*(q[2]*q[3]+q[0]*q[1]);
R(2,2) = pow(q[0],2)-pow(q[1],2)-pow(q[2],2)+pow(q[3],2);
return R;
}
opengv::quaternion_t
opengv::math::rot2quaternion( const rotation_t & R )
{
quaternion_t q;
q[0] = ( R(0,0) + R(1,1) + R(2,2) + 1.0) / 4.0;
q[1] = ( R(0,0) - R(1,1) - R(2,2) + 1.0) / 4.0;
q[2] = (-R(0,0) + R(1,1) - R(2,2) + 1.0) / 4.0;
q[3] = (-R(0,0) - R(1,1) + R(2,2) + 1.0) / 4.0;
if(q[0] < 0.0) q[0] = 0.0;
if(q[1] < 0.0) q[1] = 0.0;
if(q[2] < 0.0) q[2] = 0.0;
if(q[3] < 0.0) q[3] = 0.0;
q[0] = sqrt(q[0]);
q[1] = sqrt(q[1]);
q[2] = sqrt(q[2]);
q[3] = sqrt(q[3]);
if(q[0] >= q[1] && q[0] >= q[2] && q[0] >= q[3])
{
q[0] *= +1.0;
q[1] *= ((R(2,1)-R(1,2))/fabs(R(2,1)-R(1,2)));
q[2] *= ((R(0,2)-R(2,0))/fabs(R(0,2)-R(2,0)));
q[3] *= ((R(1,0)-R(0,1))/fabs(R(1,0)-R(0,1)));
}
else if(q[1] >= q[0] && q[1] >= q[2] && q[1] >= q[3])
{
q[0] *= ((R(2,1)-R(1,2))/fabs(R(2,1)-R(1,2)));
q[1] *= +1.0;
q[2] *= ((R(1,0)+R(0,1))/fabs(R(1,0)+R(0,1)));
q[3] *= ((R(0,2)+R(2,0))/fabs(R(0,2)+R(2,0)));
}
else if(q[2] >= q[0] && q[2] >= q[1] && q[2] >= q[3])
{
q[0] *= ((R(0,2)-R(2,0))/fabs(R(0,2)-R(2,0)));
q[1] *= ((R(1,0)+R(0,1))/fabs(R(1,0)+R(0,1)));
q[2] *= +1.0;
q[3] *= ((R(2,1)+R(1,2))/fabs(R(2,1)+R(1,2)));
}
else if(q[3] >= q[0] && q[3] >= q[1] && q[3] >= q[2])
{
q[0] *= ((R(1,0)-R(0,1))/fabs(R(1,0)-R(0,1)));
q[1] *= ((R(2,0)+R(0,2))/fabs(R(2,0)+R(0,2)));
q[2] *= ((R(2,1)+R(1,2))/fabs(R(2,1)+R(1,2)));
q[3] *= +1.0;
}
else
{};/*std::cout << "quaternion error" << std::endl;*/
double scale = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
q[0] /= scale;
q[1] /= scale;
q[2] /= scale;
q[3] /= scale;
return q;
}

187
thirdparty/opengv/src/math/roots.cpp vendored Normal file
View File

@@ -0,0 +1,187 @@
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
#include <opengv/math/roots.hpp>
#include <complex>
#include <math.h>
std::vector<double>
opengv::math::o3_roots( const std::vector<double> & p )
{
const double & a = p[0];
const double & b = p[1];
const double & c = p[2];
const double & d = p[3];
double theta_0 = b*b - 3.0*a*c;
double theta_1 = 2.0*b*b*b - 9.0*a*b*c + 27.0*a*a*d;
double term = theta_1 * theta_1 - 4.0 * theta_0 * theta_0 * theta_0;
std::complex<double> u1( 1.0, 0.0);
std::complex<double> u2(-0.5, 0.5*sqrt(3.0));
std::complex<double> u3(-0.5,-0.5*sqrt(3.0));
std::complex<double> C;
if( term >= 0.0 )
{
double C3 = 0.5 * (theta_1 + sqrt(term));
if( C3 < 0.0 )
C = std::complex<double>(-pow(-C3,(1.0/3.0)),0.0);
else
C = std::complex<double>( pow( C3,(1.0/3.0)),0.0);
}
else
{
std::complex<double> C3( 0.5*theta_1, 0.5*sqrt(-term) );
//take the third root of this complex number
double r3 = sqrt(pow(C3.real(),2.0)+pow(C3.imag(),2.0));
double a3 = atan(C3.imag() / C3.real());
if( C3.real() < 0 )
a3 += M_PI;
double r = pow(r3,(1.0/3.0));
C = std::complex<double>(r*cos(a3/3.0),r*sin(a3/3.0));
}
std::complex<double> r1 = -(1.0/(3.0*a)) * (b + u1*C + theta_0 / (u1*C) );
std::complex<double> r2 = -(1.0/(3.0*a)) * (b + u2*C + theta_0 / (u2*C) );
std::complex<double> r3 = -(1.0/(3.0*a)) * (b + u3*C + theta_0 / (u3*C) );
std::vector<double> roots;
roots.push_back(r1.real());
roots.push_back(r2.real());
roots.push_back(r3.real());
return roots;
}
std::vector<double>
opengv::math::o4_roots( const Eigen::MatrixXd & p )
{
double A = p(0,0);
double B = p(1,0);
double C = p(2,0);
double D = p(3,0);
double E = p(4,0);
double A_pw2 = A*A;
double B_pw2 = B*B;
double A_pw3 = A_pw2*A;
double B_pw3 = B_pw2*B;
double A_pw4 = A_pw3*A;
double B_pw4 = B_pw3*B;
double alpha = -3*B_pw2/(8*A_pw2)+C/A;
double beta = B_pw3/(8*A_pw3)-B*C/(2*A_pw2)+D/A;
double gamma = -3*B_pw4/(256*A_pw4)+B_pw2*C/(16*A_pw3)-B*D/(4*A_pw2)+E/A;
double alpha_pw2 = alpha*alpha;
double alpha_pw3 = alpha_pw2*alpha;
std::complex<double> P (-alpha_pw2/12-gamma,0);
std::complex<double> Q (-alpha_pw3/108+alpha*gamma/3-pow(beta,2)/8,0);
std::complex<double> R = -Q/2.0+sqrt(pow(Q,2.0)/4.0+pow(P,3.0)/27.0);
std::complex<double> U = pow(R,(1.0/3.0));
std::complex<double> y;
if (U.real() == 0)
y = -5.0*alpha/6.0-pow(Q,(1.0/3.0));
else
y = -5.0*alpha/6.0-P/(3.0*U)+U;
std::complex<double> w = sqrt(alpha+2.0*y);
std::vector<double> realRoots;
std::complex<double> temp;
temp = -B/(4.0*A) + 0.5*(w+sqrt(-(3.0*alpha+2.0*y+2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(w-sqrt(-(3.0*alpha+2.0*y+2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(-w+sqrt(-(3.0*alpha+2.0*y-2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(-w-sqrt(-(3.0*alpha+2.0*y-2.0*beta/w)));
realRoots.push_back(temp.real());
return realRoots;
}
std::vector<double>
opengv::math::o4_roots( const std::vector<double> & p )
{
double A = p[0];
double B = p[1];
double C = p[2];
double D = p[3];
double E = p[4];
double A_pw2 = A*A;
double B_pw2 = B*B;
double A_pw3 = A_pw2*A;
double B_pw3 = B_pw2*B;
double A_pw4 = A_pw3*A;
double B_pw4 = B_pw3*B;
double alpha = -3*B_pw2/(8*A_pw2)+C/A;
double beta = B_pw3/(8*A_pw3)-B*C/(2*A_pw2)+D/A;
double gamma = -3*B_pw4/(256*A_pw4)+B_pw2*C/(16*A_pw3)-B*D/(4*A_pw2)+E/A;
double alpha_pw2 = alpha*alpha;
double alpha_pw3 = alpha_pw2*alpha;
std::complex<double> P (-alpha_pw2/12-gamma,0);
std::complex<double> Q (-alpha_pw3/108+alpha*gamma/3-pow(beta,2)/8,0);
std::complex<double> R = -Q/2.0+sqrt(pow(Q,2.0)/4.0+pow(P,3.0)/27.0);
std::complex<double> U = pow(R,(1.0/3.0));
std::complex<double> y;
if (U.real() == 0)
y = -5.0*alpha/6.0-pow(Q,(1.0/3.0));
else
y = -5.0*alpha/6.0-P/(3.0*U)+U;
std::complex<double> w = sqrt(alpha+2.0*y);
std::vector<double> realRoots;
std::complex<double> temp;
temp = -B/(4.0*A) + 0.5*(w+sqrt(-(3.0*alpha+2.0*y+2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(w-sqrt(-(3.0*alpha+2.0*y+2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(-w+sqrt(-(3.0*alpha+2.0*y-2.0*beta/w)));
realRoots.push_back(temp.real());
temp = -B/(4.0*A) + 0.5*(-w-sqrt(-(3.0*alpha+2.0*y-2.0*beta/w)));
realRoots.push_back(temp.real());
return realRoots;
}