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

109
thirdparty/opengv/include/opengv/Indices.hpp vendored Normal file
View File

@@ -0,0 +1,109 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file Indices.hpp
* \brief Generic functor base for use with the Eigen-nonlinear optimization
* toolbox.
*/
#ifndef OPENGV_INDICES_HPP_
#define OPENGV_INDICES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
using namespace std;
using namespace Eigen;
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* Index class used internally so we can efficiently access either all or
* just a subset of the indices, using the same syntax
*/
struct Indices
{
/** Indexing into vector of indices? */
bool _useIndices;
/** Pointer to a vector of indices (if used) */
const std::vector<int> * _indices;
/** The number of correspondences */
size_t _numberCorrespondences;
/**
* \brief Constructor using index-vector.
* \param[in] indices The index-vector.
*/
Indices( const std::vector<int> & indices) :
_useIndices(true),
_indices(&indices),
_numberCorrespondences(indices.size())
{}
/**
* \brief Constructor without index-vector (uses all correspondences).
* \param[in] numberCorrespondences The number of correspondences.
*/
Indices(size_t numberCorrespondences) :
_useIndices(false),
_numberCorrespondences(numberCorrespondences)
{}
/**
* \brief Get the number of correspondences.
* \return The number of correspondences.
*/
size_t size() const
{
return _numberCorrespondences;
}
/**
* \brief Get an index.
* \param[in] i The index.
* \return The index (either directly or from the index-vector).
*/
int operator[](int i) const
{
if( _useIndices )
return (*_indices)[i];
return i;
}
};
}
#endif /* OPENGV_INDICES_HPP_ */

103
thirdparty/opengv/include/opengv/OptimizationFunctor.hpp vendored Normal file
View File

@@ -0,0 +1,103 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file OptimizationFunctor.hpp
* \brief Generic functor base for use with the Eigen-nonlinear optimization
* toolbox.
*/
#ifndef OPENGV_OPTIMIZATIONFUNCTOR_HPP_
#define OPENGV_OPTIMIZATIONFUNCTOR_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
using namespace std;
using namespace Eigen;
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* Generic functor base for use with the Eigen-nonlinear optimization
* toolbox. Please refer to the Eigen-documentation for further information.
*/
template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
struct OptimizationFunctor
{
/** undocumented */
typedef _Scalar Scalar;
/** undocumented */
enum
{
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
/** undocumented */
typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
/** undocumented */
typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
/** undocumented */
typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
/** undocumented */
const int m_inputs;
/** undocumented */
const int m_values;
/** undocumented */
OptimizationFunctor() :
m_inputs(InputsAtCompileTime),
m_values(ValuesAtCompileTime) {}
/** undocumented */
OptimizationFunctor(int inputs, int values) :
m_inputs(inputs),
m_values(values) {}
/** undocumented */
int inputs() const
{
return m_inputs;
}
/** undocumented */
int values() const
{
return m_values;
}
};
}
#endif /* OPENGV_OPTIMIZATIONFUNCTOR_HPP_ */

181
thirdparty/opengv/include/opengv/absolute_pose/AbsoluteAdapterBase.hpp vendored Normal file
View File

@@ -0,0 +1,181 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file AbsoluteAdapterBase.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the absolute-pose algorithms.
*/
#ifndef OPENGV_ABSOLUTE_POSE_ABSOLUTEADAPTERBASE_HPP_
#define OPENGV_ABSOLUTE_POSE_ABSOLUTEADAPTERBASE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* The AbsoluteAdapterBase is the base-class for the visitors to the central
* and non-central absolute pose algorithms. It provides a unified interface to
* opengv-methods to access bearing-vectors, world points, priors or
* known variables for the absolute pose, and the multi-camera configuration in
* the non-central case. Derived classes may hold the data in any user-specific
* format, and adapt to opengv-types.
*/
class AbsoluteAdapterBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor.
*/
AbsoluteAdapterBase() :
_t(Eigen::Vector3d::Zero()),
_R(Eigen::Matrix3d::Identity()) {};
/**
* \brief Constructor.
* \param[in] R A prior or known value for the rotation from the viewpoint
* to the world frame.
*/
AbsoluteAdapterBase( const opengv::rotation_t & R ) :
_t(Eigen::Vector3d::Zero()),
_R(R) {};
/**
* \brief Constructor.
* \param[in] t A prior or known value for the position of the viewpoint seen
* from the world frame.
* \param[in] R A prior or known value for the rotation from the viewpoint
* to the world frame.
*/
AbsoluteAdapterBase(
const opengv::translation_t & t,
const opengv::rotation_t & R ) :
_t(t),
_R(R) {};
/**
* \brief Destructor.
*/
virtual ~AbsoluteAdapterBase() {};
//Access of correspondences
/**
* \brief Retrieve the bearing vector of a correspondence.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding bearing vector.
*/
virtual opengv::bearingVector_t getBearingVector(size_t index ) const = 0;
/**
* \brief Retrieve the weight of a correspondence. The weight is supposed to
* reflect the quality of a correspondence, and typically is between
* 0 and 1.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding weight.
*/
virtual double getWeight( size_t index ) const = 0;
/**
* \brief Retrieve the position of a camera of a correspondence
* seen from the viewpoint origin.
* \param[in] index The serialized index of the correspondence.
* \return The position of the corresponding camera seen from the viewpoint
* origin.
*/
virtual opengv::translation_t getCamOffset( size_t index ) const = 0;
/**
* \brief Retrieve the rotation from a camera of a correspondence to the
* viewpoint origin.
* \param[in] index The serialized index of the correspondence.
* \return The rotation from the corresponding camera back to the viewpoint
* origin.
*/
virtual opengv::rotation_t getCamRotation( size_t index ) const = 0;
/**
* \brief Retrieve the world point of a correspondence.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding world point.
*/
virtual opengv::point_t getPoint( size_t index ) const = 0;
/**
* \brief Retrieve the number of correspondences.
* \return The number of correspondences.
*/
virtual size_t getNumberCorrespondences() const = 0;
//Access of priors or known values
/**
* \brief Retrieve the prior or known value for the position.
* \return The prior or known value for the position.
*/
opengv::translation_t gett() const { return _t; };
/**
* \brief Set the prior or known value for the position.
* \param[in] t The prior or known value for the position.
*/
void sett(const opengv::translation_t & t) { _t = t; };
/**
* \brief Retrieve the prior or known value for the rotation.
* \return The prior or known value for the rotation.
*/
opengv::rotation_t getR() const { return _R; };
/**
* \brief Set the prior or known value for the rotation.
* \param[in] R The prior or known value for the rotation.
*/
void setR(const opengv::rotation_t & R) { _R = R; };
protected:
/** The prior or known value for the position of the viewpoint seen from the
* world frame. Initialized to zero if not provided.
*/
opengv::translation_t _t;
/** The prior or known value for the rotation from the viewpoint back to the
* world frame. Initialized to identity if not provided.
*/
opengv::rotation_t _R;
};
}
};
#endif /* OPENGV_ABSOLUTE_POSE_ABSOLUTEADAPTERBASE_HPP_ */

229
thirdparty/opengv/include/opengv/absolute_pose/AbsoluteMultiAdapterBase.hpp vendored Normal file
View File

@@ -0,0 +1,229 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file AbsoluteMultiAdapterBase.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the absolute-pose algorithms. Intended for absolute non-central-
* viewpoint problems. Access of correspondences etc. via an additional
* frame-index referring to the camera.
*/
#ifndef OPENGV_ABSOLUTE_POSE_ABSOLUTEMULTIADAPTERBASE_HPP_
#define OPENGV_ABSOLUTE_POSE_ABSOLUTEMULTIADAPTERBASE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* See the documentation of AbsoluteAdapterBase to understand the meaning of
* an AbsoluteAdapter. AbsoluteMultiAdapterBase extends the interface of
* AbsoluteAdapterBase by an additional frame-index for referring to a
* camera. Intended for non-central absolute viewpoint problems, allowing
* camera-wise access of correspondences. Derived classes need to implement
* functionalities for deriving unique serialization of multi-indices.
*/
class AbsoluteMultiAdapterBase : public AbsoluteAdapterBase
{
protected:
using AbsoluteAdapterBase::_t;
using AbsoluteAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor.
*/
AbsoluteMultiAdapterBase() :
AbsoluteAdapterBase() {};
/**
* \brief Constructor.
* \param[in] R A prior or known value for the rotation from the viewpoint
* to the world frame.
*/
AbsoluteMultiAdapterBase( const opengv::rotation_t & R ) :
AbsoluteAdapterBase(R) {};
/**
* \brief Constructor.
* \param[in] t A prior or known value for the position of the viewpoint seen
* from the world frame.
* \param[in] R A prior or known value for the rotation from the viewpoint
* to the world frame.
*/
AbsoluteMultiAdapterBase(
const opengv::translation_t & t,
const opengv::rotation_t & R ) :
AbsoluteAdapterBase(t,R) {};
/**
* \brief Destructor.
*/
virtual ~AbsoluteMultiAdapterBase() {};
//camera-wise access of correspondences
/**
* \brief Retrieve the bearing vector of a correspondence in a certain frame.
* \param[in] frameIndex Index of the frame.
* \param[in] correspondenceIndex Index of the correspondence in this frame.
* \return The corresponding bearing vector.
*/
virtual opengv::bearingVector_t getBearingVector(
size_t frameIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the weight of a correspondence. The weight is supposed to
* reflect the quality of a correspondence, and typically is between
* 0 and 1.
* \param[in] frameIndex Index of the frame.
* \param[in] correspondenceIndex Index of the correspondence in this frame.
* \return The corresponding weight.
*/
virtual double getWeight(
size_t frameIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the position of a camera seen from the viewpoint origin.
* \param[in] frameIndex Index of the frame.
* \return The position of the corresponding camera seen from the viewpoint
* origin.
*/
virtual opengv::translation_t getMultiCamOffset( size_t frameIndex ) const = 0;
/**
* \brief Retrieve the rotation from a camera to the viewpoint frame.
* \param[in] frameIndex Index of the frame.
* \return The rotation from the corresponding camera back to the viewpoint
* origin.
*/
virtual opengv::rotation_t getMultiCamRotation( size_t frameIndex ) const = 0;
/**
* \brief Retrieve the world point of a correspondence.
* \param[in] frameIndex Index of the frame.
* \param[in] correspondenceIndex Index of the correspondence in this frame.
* \return The corresponding world point.
*/
virtual opengv::point_t getPoint(
size_t frameIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the number of correspondences for a camera.
* \param[in] frameIndex Index of the camera.
* \return The number of correspondences in this camera.
*/
virtual size_t getNumberCorrespondences( size_t frameIndex ) const = 0;
/**
* \brief Retrieve the number of cameras.
* \return The number of cameras.
*/
virtual size_t getNumberFrames() const = 0;
//Conversion to and from serialized indices
/**
* \brief Convert an array of (frameIndex,correspondenceIndex)-pairs into an
* array of serialized indices.
* \param[in] multiIndices Array of (frameIndex,correspondenceIndex)-pairs.
* \return Array of single serialized indices referring uniquely to
* (frameIndex,correspondenceIndex)-pairs.
*/
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const = 0;
/**
* \brief Convert a (frameIndex,correspondenceIndex)-pair into a serialized
* index.
* \param[in] frameIndex The index of the camera.
* \param[in] correspondenceIndex The index of the correspondence in the camera.
* \return Array of single serialized indices referring uniquely to
* (frameIndex,correspondenceIndex)-pairs.
*/
virtual int convertMultiIndex(
size_t frameIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Get the frame-index corresponding to a serialized index.
* \param[in] index The serialized index.
* \return The frame index.
*/
virtual int multiFrameIndex( size_t index ) const = 0;
/**
* \brief Get the correspondence-index in a camera for a serialized index.
* \param[in] index The serialized index.
* \return The correspondence-index in the camera.
*/
virtual int multiCorrespondenceIndex( size_t index ) const = 0;
//the classic interface (with serialized indices, used by the opengv-methods)
/** See parent-class (no need to overload) */
virtual bearingVector_t getBearingVector( size_t index ) const
{
return getBearingVector(
multiFrameIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual double getWeight( size_t index ) const
{
return getWeight(
multiFrameIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual translation_t getCamOffset( size_t index ) const
{ return getMultiCamOffset( multiFrameIndex(index) ); }
/** See parent-class (no need to overload) */
virtual rotation_t getCamRotation( size_t index ) const
{ return getMultiCamRotation( multiFrameIndex(index) ); }
/** See parent-class (no need to overload) */
virtual point_t getPoint( size_t index ) const
{
return getPoint(
multiFrameIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual size_t getNumberCorrespondences() const
{
size_t numberCorrespondences = 0;
for(size_t i = 0; i < getNumberFrames(); i++)
numberCorrespondences += getNumberCorrespondences(i);
return numberCorrespondences;
}
};
}
};
#endif /* OPENGV_ABSOLUTE_POSE_ABSOLUTEMULTIADAPTERBASE_HPP_ */

122
thirdparty/opengv/include/opengv/absolute_pose/CentralAbsoluteAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,122 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file CentralAbsoluteAdapter.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the central absolute-pose algorithms. It maps opengv types
* back to opengv types.
*/
#ifndef OPENGV_ABSOLUTE_POSE_CENTRALABSOLUTEADAPTER_HPP_
#define OPENGV_ABSOLUTE_POSE_CENTRALABSOLUTEADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* an AbsoluteAdapter. This child-class is for the central case and holds data
* in form of references to opengv-types.
*/
class CentralAbsoluteAdapter : public AbsoluteAdapterBase
{
protected:
using AbsoluteAdapterBase::_t;
using AbsoluteAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const points_t & points );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const points_t & points,
const rotation_t & R );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const points_t & points,
const translation_t & t,
const rotation_t & R );
/**
* Destructor
*/
virtual ~CentralAbsoluteAdapter();
//Access of correspondences
/** See parent-class */
virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual opengv::translation_t getCamOffset( size_t index ) const;
/** See parent-class Returns identity for this adapter. */
virtual opengv::rotation_t getCamRotation( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** Reference to the bearing-vectors expressed in the camera-frame */
const bearingVectors_t & _bearingVectors;
/** Reference to the points expressed in the world-frame. */
const points_t & _points;
};
}
}
#endif /* OPENGV_ABSOLUTE_POSE_CENTRALABSOLUTEADAPTER_HPP_ */

115
thirdparty/opengv/include/opengv/absolute_pose/MACentralAbsolute.hpp vendored Normal file
View File

@@ -0,0 +1,115 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MACentralAbsolute.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the central absolute-pose algorithms. It maps matlab types
* back to opengv types.
*/
#ifndef OPENGV_ABSOLUTE_POSE_MACENTRALABSOLUTE_HPP_
#define OPENGV_ABSOLUTE_POSE_MACENTRALABSOLUTE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* an AbsoluteAdapter. This child-class is for the central case and holds data
* in form of pointers to matlab data.
*/
class MACentralAbsolute : public AbsoluteAdapterBase
{
protected:
using AbsoluteAdapterBase::_t;
using AbsoluteAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MACentralAbsolute(
const double * points,
const double * bearingVectors,
int numberPoints,
int numberBearingVectors );
/**
* Destructor
*/
virtual ~MACentralAbsolute();
//Access of correspondences
/** See parent-class */
virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual opengv::translation_t getCamOffset( size_t index ) const;
/** See parent-class Returns identity for this adapter. */
virtual opengv::rotation_t getCamRotation( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** A pointer to the point data */
const double * _points;
/** A pointer to the bearing-vectors */
const double * _bearingVectors;
/** The number of points */
int _numberPoints;
/** The number of bearing vectors */
int _numberBearingVectors;
};
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MACENTRALABSOLUTE_HPP_ */

115
thirdparty/opengv/include/opengv/absolute_pose/MANoncentralAbsolute.hpp vendored Normal file
View File

@@ -0,0 +1,115 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MANoncentralAbsolute.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the non-central absolute-pose algorithms. It maps matlab
* types to opengv types.
*/
#ifndef OPENGV_ABSOLUTE_POSE_MANONCENTRALABSOLUTE_HPP_
#define OPENGV_ABSOLUTE_POSE_MANONCENTRALABSOLUTE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* an AbsoluteAdapter. This child-class is for the non-central case and holds
* data in form of pointers to matlab-data.
*/
class MANoncentralAbsolute : public AbsoluteAdapterBase
{
protected:
using AbsoluteAdapterBase::_t;
using AbsoluteAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MANoncentralAbsolute(
const double * points,
const double * bearingVectors,
int numberPoints,
int numberBearingVectors );
/**
* Destructor
*/
virtual ~MANoncentralAbsolute();
//Access of correspondences
/** See parent-class */
virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual opengv::translation_t getCamOffset( size_t index ) const;
/** See parent-class */
virtual opengv::rotation_t getCamRotation( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** A pointer to the point data */
const double * _points;
/** A pointer to the bearing-vectors */
const double * _bearingVectors;
/** The number of points */
int _numberPoints;
/** The number of bearing vectors */
int _numberBearingVectors;
};
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MANONCENTRALABSOLUTE_HPP_ */

150
thirdparty/opengv/include/opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,150 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file NoncentralAbsoluteAdapter.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the non-central absolute-pose algorithms. It maps opengv
* types back to opengv types.
*/
#ifndef OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEADAPTER_HPP_
#define OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* an AbsoluteAdapter. This child-class is for the non-central case and holds
* data in form of references to opengv-types.
*/
class NoncentralAbsoluteAdapter : public AbsoluteAdapterBase
{
protected:
using AbsoluteAdapterBase::_t;
using AbsoluteAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** A type defined for the camera-correspondences, see protected
* class-members
*/
typedef std::vector<int> camCorrespondences_t;
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const camCorrespondences_t & camCorrespondences,
const points_t & points,
const translations_t & camOffsets,
const rotations_t & camRotations );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const camCorrespondences_t & camCorrespondences,
const points_t & points,
const translations_t & camOffsets,
const rotations_t & camRotations,
const rotation_t & R );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralAbsoluteAdapter(
const bearingVectors_t & bearingVectors,
const camCorrespondences_t & camCorrespondences,
const points_t & points,
const translations_t & camOffsets,
const rotations_t & camRotations,
const translation_t & t,
const rotation_t & R );
/**
* \brief Destructor.
*/
virtual ~NoncentralAbsoluteAdapter();
//Access of correspondences
/** See parent-class */
virtual opengv::bearingVector_t getBearingVector( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual opengv::translation_t getCamOffset( size_t index ) const;
/** See parent-class */
virtual opengv::rotation_t getCamRotation( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** Reference to the bearing-vectors expressed in the camera-frames */
const bearingVectors_t & _bearingVectors;
/** Reference to an array of camera-indices for the bearing vectors. Length
* equals to number of bearing-vectors, and elements are indices of cameras
* in the _camOffsets and _camRotations arrays.
*/
const camCorrespondences_t & _camCorrespondences;
/** Reference to the points expressed in the world-frame. */
const points_t & _points;
/** Reference to positions of the different cameras seen from the
* viewpoint.
*/
const translations_t & _camOffsets;
/** Reference to rotations from the different cameras back to the
* viewpoint.
*/
const rotations_t & _camRotations;
};
}
};
#endif /* OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEADAPTER_HPP_ */

147
thirdparty/opengv/include/opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,147 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file NoncentralAbsoluteMultiAdapter.hpp
* \brief Adapter-class for passing bearing-vector-to-point correspondences to
* the non-central absolute-pose algorithms. It maps opengv
* types back to opengv types. Manages multiple match-lists for each camera.
* This allows to draw samples homogeneously over the cameras.
*/
#ifndef OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEMULTIADAPTER_HPP_
#define OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEMULTIADAPTER_HPP_
#include <memory>
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* an AbsoluteAdapter. This child-class is for the non-central case and holds
* data in form of references to opengv-types.
*/
class NoncentralAbsoluteMultiAdapter : public AbsoluteMultiAdapterBase
{
protected:
using AbsoluteMultiAdapterBase::_t;
using AbsoluteMultiAdapterBase::_R;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralAbsoluteMultiAdapter(
std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors,
std::vector<std::shared_ptr<points_t> > points,
const translations_t & camOffsets,
const rotations_t & camRotations );
/**
* \brief Destructor.
*/
virtual ~NoncentralAbsoluteMultiAdapter();
//camera-wise access of correspondences
/** See parent-class */
virtual point_t getPoint(
size_t frameIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector(
size_t frameIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual double getWeight( size_t frameIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual translation_t getMultiCamOffset( size_t frameIndex ) const;
/** See parent-class */
virtual rotation_t getMultiCamRotation( size_t frameIndex ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences( size_t frameIndex ) const;
/** See parent-class */
virtual size_t getNumberFrames() const;
//Conversion to and from serialized indices
/** See parent-class */
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const;
/** See parent-class */
virtual int convertMultiIndex(
size_t frameIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual int multiFrameIndex( size_t index ) const;
/** See parent-class */
virtual int multiCorrespondenceIndex( size_t index ) const;
protected:
/** References to multiple sets of bearing-vectors (the ones from each camera).
*/
std::vector<std::shared_ptr<bearingVectors_t> > _bearingVectors;
/** References to multiple sets of points (the ones from each camera).
*/
std::vector<std::shared_ptr<points_t> > _points;
/** Reference to positions of the different cameras seen from the
* viewpoint.
*/
const translations_t & _camOffsets;
/** Reference to rotations from the different cameras back to the
* viewpoint.
*/
const rotations_t & _camRotations;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiFrameIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiKeypointIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> singleIndexOffsets;
};
}
};
#endif /* OPENGV_ABSOLUTE_POSE_NONCENTRALABSOLUTEADAPTER_HPP_ */

322
thirdparty/opengv/include/opengv/absolute_pose/methods.hpp vendored Normal file
View File

@@ -0,0 +1,322 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file absolute_pose/methods.hpp
* \brief Methods for computing the absolute pose of a calibrated viewpoint.
*
* The collection includes both minimal and non-minimal solutions for
* computing the absolute pose of a calibrated, either central or non-central
* viewpoint.
*/
#ifndef OPENGV_ABSOLUTE_POSE_METHODS_HPP_
#define OPENGV_ABSOLUTE_POSE_METHODS_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/** \brief Compute the pose of a central viewpoint with known rotation using two
* point correspondences.
*
* \param[in] adapter Visitor holding bearing vectors, world points, and
* known rotation.
* \param[in] indices Indices of the two correspondences that are used for
* deriving the translation.
* \return The position of the viewpoint seen from the world frame.
*/
translation_t p2p(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a central viewpoint with known rotation using two
* point correspondences.
*
* \param[in] adapter Visitor holding bearing vectors, world points, and
* known rotation.
* \param[in] index0 Index of the first correspondence used for deriving the
* translation (use default value if only two vectors provided
* by the visitor anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* translation (use default value if only two vectors provided
* by the visitor anyway).
* \return The position of the viewpoint seen from the world frame.
*/
translation_t p2p(
const AbsoluteAdapterBase & adapter,
size_t index0 = 0,
size_t index1 = 1 );
/**
* \brief Compute the pose of a central viewpoint using three point
* correspondences and Kneip's method [1].
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \param[in] indices Indices of the three correspondences that are used for
* deriving the pose.
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world, frame, maximum 4 solutions).
*/
transformations_t p3p_kneip(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a central viewpoint using three point
* correspondences and Kneip's method [1].
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \param[in] index0 Index of the first correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index2 Index of the third correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame, maximum 4 solutions).
*/
transformations_t p3p_kneip(
const AbsoluteAdapterBase & adapter,
size_t index0 = 0,
size_t index1 = 1,
size_t index2 = 2 );
/**
* \brief Compute the pose of a central viewpoint using three point correspondences
* and Gao's method [2].
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \param[in] indices Indices of the three correspondences that are used for
* deriving the pose.
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame, maximum 4 solutions).
*/
transformations_t p3p_gao(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a central viewpoint using three point
* correspondences and Gao's method [2].
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \param[in] index0 Index of the first correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index2 Index of the third correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame, maximum 4 solutions).
*/
transformations_t p3p_gao(
const AbsoluteAdapterBase & adapter,
size_t index0 = 0,
size_t index1 = 1,
size_t index2 = 2 );
/**
* \brief Compute the pose of a non-central viewpoint using three point
* correspondences and Kneip's method [3].
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \param[in] indices Indices of the three correspondences that are used for
* deriving the pose.
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from
* viewpoint to world frame, maximum 8 solutions).
*/
transformations_t gp3p(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a non-central viewpoint using three point
* correspondences and Kneip's method [3].
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \param[in] index0 Index of the first correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \param[in] index2 Index of the third correspondence used for deriving the
* pose (use default value if only three correspondences
* provided anyway).
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from
* viewpoint to world frame, maximum 8 solutions).
*/
transformations_t gp3p(
const AbsoluteAdapterBase & adapter,
size_t index0 = 0,
size_t index1 = 1,
size_t index2 = 2 );
/**
* \brief Compute the pose of a central viewpoint using the EPnP method [4].
* Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t epnp( const AbsoluteAdapterBase & adapter );
/**
* \brief Compute the pose of a central viewpoint using the EPnP method [4].
*
* \param[in] adapter Visitor holding bearing vector to world point correspondences.
* \param[in] indices Indices of the n correspondences that are used for
* deriving the pose.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t epnp(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a non-central viewpoint using the gPnP method [3].
* Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t gpnp( const AbsoluteAdapterBase & adapter );
/**
* \brief Compute the pose of a non-central viewpoint using the gPnP method [3].
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \param[in] indices Indices of the n correspondences that are used for
* deriving the pose.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t gpnp(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the poses of a non-central viewpoint using the uPnP method.
* Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformations_t upnp( const AbsoluteAdapterBase & adapter );
/**
* \brief Compute the poses of a non-central viewpoint using the uPnP method.
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, plus the multi-camera configuration.
* \param[in] indices Indices of the n correspondences that are used for
* deriving the pose.
* \return Poses of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformations_t upnp(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose of a viewpoint using nonlinear optimization. Using
* all available correspondences. Works for central and non-central case.
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, the multi-camera configuration, plus
* the initial values.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t optimize_nonlinear( const AbsoluteAdapterBase & adapter );
/**
* \brief Compute the pose of a viewpoint using nonlinear optimization. Works
* for central and non-central viewpoints.
*
* \param[in] adapter Visitor holding bearing vector to world point
* correspondences, the multi-camera configuration, plus
* the initial values.
* \param[in] indices Indices of the n correspondences that are used for
* deriving the pose.
* \return Pose of viewpoint (position seen from world frame and orientation
* from viewpoint to world frame, transforms points from viewpoint to
* world frame).
*/
transformation_t optimize_nonlinear(
const AbsoluteAdapterBase & adapter,
const std::vector<int> & indices );
}
}
#endif /* OPENGV_ABSOLUTE_POSE_METHODS_HPP_ */

161
thirdparty/opengv/include/opengv/absolute_pose/modules/Epnp.hpp vendored Normal file
View File

@@ -0,0 +1,161 @@
/******************************************************************************
* 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. *
******************************************************************************/
// Note: this code has been downloaded from the homepage of the "Computer
// Vision Laboratory" at EPFL Lausanne, and was originally developped by the
// authors of [4]. I only adapted it to Eigen.
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
class Epnp
{
public:
Epnp(void);
~Epnp();
void set_maximum_number_of_correspondences(const int n);
void reset_correspondences(void);
void add_correspondence(
const double X,
const double Y,
const double Z,
const double x,
const double y,
const double z);
double compute_pose(double R[3][3], double T[3]);
void relative_error(
double & rot_err,
double & transl_err,
const double Rtrue[3][3],
const double ttrue[3],
const double Rest[3][3],
const double test[3]);
void print_pose(const double R[3][3], const double t[3]);
double reprojection_error(const double R[3][3], const double t[3]);
private:
void choose_control_points(void);
void compute_barycentric_coordinates(void);
void fill_M(
Eigen::MatrixXd & M,
const int row,
const double * alphas,
const double u,
const double v);
void compute_ccs(const double * betas, const Eigen::MatrixXd & ut);
void compute_pcs(void);
void solve_for_sign(void);
void find_betas_approx_1(
const Eigen::Matrix<double,6,10> & L_6x10,
const Eigen::Matrix<double,6,1> & Rho,
double * betas);
void find_betas_approx_2(
const Eigen::Matrix<double,6,10> & L_6x10,
const Eigen::Matrix<double,6,1> & Rho,
double * betas);
void find_betas_approx_3(
const Eigen::Matrix<double,6,10> & L_6x10,
const Eigen::Matrix<double,6,1> & Rho,
double * betas);
void qr_solve(
Eigen::Matrix<double,6,4> & A,
Eigen::Matrix<double,6,1> & b,
Eigen::Matrix<double,4,1> & X);
double dot(const double * v1, const double * v2);
double dist2(const double * p1, const double * p2);
void compute_rho(Eigen::Matrix<double,6,1> & Rho);
void compute_L_6x10(
const Eigen::MatrixXd & Ut,
Eigen::Matrix<double,6,10> & L_6x10 );
void gauss_newton(
const Eigen::Matrix<double,6,10> & L_6x10,
const Eigen::Matrix<double,6,1> & Rho,
double current_betas[4]);
void compute_A_and_b_gauss_newton(
const Eigen::Matrix<double,6,10> & L_6x10,
const Eigen::Matrix<double,6,1> & Rho,
double cb[4],
Eigen::Matrix<double,6,4> & A,
Eigen::Matrix<double,6,1> & b);
double compute_R_and_t(
const Eigen::MatrixXd & Ut,
const double * betas,
double R[3][3],
double t[3]);
void estimate_R_and_t(double R[3][3], double t[3]);
void copy_R_and_t(
const double R_dst[3][3],
const double t_dst[3],
double R_src[3][3],
double t_src[3]);
void mat_to_quat(const double R[3][3], double q[4]);
double uc, vc, fu, fv;
double * pws, * us, * alphas, * pcs;
int * signs; //added!
int maximum_number_of_correspondences;
int number_of_correspondences;
double cws[4][3], ccs[4][3];
double cws_determinant;
};
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_EPNP_HPP_ */

193
thirdparty/opengv/include/opengv/absolute_pose/modules/gp3p/modules.hpp vendored Normal file
View File

@@ -0,0 +1,193 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GP3P_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GP3P_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gp3p
{
void init(
Eigen::Matrix<double,48,85> & groebnerMatrix,
const Eigen::Matrix<double,3,3> & f,
const Eigen::Matrix<double,3,3> & v,
const Eigen::Matrix<double,3,3> & p );
void compute( Eigen::Matrix<double,48,85> & groebnerMatrix );
void sPolynomial9( Eigen::Matrix<double,48,85> & groebnerMatrix );
void sPolynomial10( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow9_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow10_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow10_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow11_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow10_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow12_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow12_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow12_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,48,85> & groebnerMatrix );
void sPolynomial15( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow14_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial16( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow15_010000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow10_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow12_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow15_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow15_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial17( Eigen::Matrix<double,48,85> & groebnerMatrix );
void sPolynomial18( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow15_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow17_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial19( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow12_010000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow16_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial20( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow12_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow15_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial21( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow19_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow19_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial22( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow19_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow18_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial23( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow19_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow20_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial24( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow15_100100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow16_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow21_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial25( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow15_100010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow16_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow22_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow15_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial26( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow15_100001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow16_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow23_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial27( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow12_100100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow24_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial28( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow12_100010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow25_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial29( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow12_100001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow10_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow26_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial30( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow28_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow27_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial31( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow29_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial32( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow31_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow30_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow31_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow30_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial33( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow32_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial34( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow32_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow33_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow33_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial35( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow34_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow34_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial36( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow35_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow35_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial37( Eigen::Matrix<double,48,85> & groebnerMatrix );
void sPolynomial38( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow37_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow36_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow37_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial39( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow38_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow38_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial40( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow39_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow39_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial41( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow40_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow40_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial42( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow32_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow33_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow34_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow33_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow34_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow35_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow37_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow38_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow37_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow38_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow39_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow41_100000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow41_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial43( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow33_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow37_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow42_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial44( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow31_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow30_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial45( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow36_000100_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow36_010000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow44_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial46( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow36_000010_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow45_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void sPolynomial47( Eigen::Matrix<double,48,85> & groebnerMatrix );
void groebnerRow36_000001_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow43_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
void groebnerRow46_000000_f( Eigen::Matrix<double,48,85> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GP3P_MODULES_HPP_ */

67
thirdparty/opengv/include/opengv/absolute_pose/modules/gpnp1/modules.hpp vendored Normal file
View File

@@ -0,0 +1,67 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp1
{
void init(
Eigen::Matrix<double,5,3> & groebnerMatrix,
const Eigen::Matrix<double,12,1> & a,
Eigen::Matrix<double,12,1> & n,
Eigen::Vector3d & c0,
Eigen::Vector3d & c1,
Eigen::Vector3d & c2,
Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,5,3> & groebnerMatrix );
void sPolynomial3( Eigen::Matrix<double,5,3> & groebnerMatrix );
void sPolynomial4( Eigen::Matrix<double,5,3> & groebnerMatrix );
void groebnerRow3_0_f( Eigen::Matrix<double,5,3> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP1_MODULES_HPP_ */

75
thirdparty/opengv/include/opengv/absolute_pose/modules/gpnp2/modules.hpp vendored Normal file
View File

@@ -0,0 +1,75 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp2
{
void init(
Eigen::Matrix<double,10,6> & groebnerMatrix,
const Eigen::Matrix<double,12,1> & a,
Eigen::Matrix<double,12,1> & n,
Eigen::Matrix<double,12,1> & m,
Eigen::Vector3d & c0,
Eigen::Vector3d & c1,
Eigen::Vector3d & c2,
Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,10,6> & groebnerMatrix );
void sPolynomial5( Eigen::Matrix<double,10,6> & groebnerMatrix );
void sPolynomial6( Eigen::Matrix<double,10,6> & groebnerMatrix );
void groebnerRow5_00_f( Eigen::Matrix<double,10,6> & groebnerMatrix, int targetRow );
void sPolynomial7( Eigen::Matrix<double,10,6> & groebnerMatrix );
void groebnerRow6_00_f( Eigen::Matrix<double,10,6> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,10,6> & groebnerMatrix );
void groebnerRow7_10_f( Eigen::Matrix<double,10,6> & groebnerMatrix, int targetRow );
void groebnerRow7_00_f( Eigen::Matrix<double,10,6> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,10,6> & groebnerMatrix );
void groebnerRow8_00_f( Eigen::Matrix<double,10,6> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP2_MODULES_HPP_ */

100
thirdparty/opengv/include/opengv/absolute_pose/modules/gpnp3/modules.hpp vendored Normal file
View File

@@ -0,0 +1,100 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP3_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP3_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp3
{
void init(
Eigen::Matrix<double,15,18> & groebnerMatrix,
const Eigen::Matrix<double,12,1> & a,
Eigen::Matrix<double,12,1> & n,
Eigen::Matrix<double,12,1> & m,
Eigen::Matrix<double,12,1> & k,
Eigen::Vector3d & c0,
Eigen::Vector3d & c1,
Eigen::Vector3d & c2,
Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,15,18> & groebnerMatrix );
void sPolynomial4( Eigen::Matrix<double,15,18> & groebnerMatrix );
void sPolynomial5( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow4_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial6( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow5_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial7( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow5_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow6_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow6_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow4_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow7_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow3_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow5_010_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow3_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow8_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial10( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow4_010_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow9_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow9_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow10_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow10_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow11_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow11_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow11_010_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow12_010_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow12_100_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void groebnerRow12_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,15,18> & groebnerMatrix );
void groebnerRow13_000_f( Eigen::Matrix<double,15,18> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP3_MODULES_HPP_ */

145
thirdparty/opengv/include/opengv/absolute_pose/modules/gpnp4/modules.hpp vendored Normal file
View File

@@ -0,0 +1,145 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp4
{
void init(
Eigen::Matrix<double,25,37> & groebnerMatrix,
const Eigen::Matrix<double,12,1> & a,
Eigen::Matrix<double,12,1> & n,
Eigen::Matrix<double,12,1> & m,
Eigen::Matrix<double,12,1> & k,
Eigen::Matrix<double,12,1> & l,
Eigen::Vector3d & c0,
Eigen::Vector3d & c1,
Eigen::Vector3d & c2,
Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial5( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial6( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial7( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow5_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow6_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow7_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial10( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow9_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow4_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow10_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow11_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow12_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow13_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial15( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial16( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow13_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial17( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow12_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial18( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial19( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial20( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial21( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial22( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow19_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial23( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial24( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow23_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_ */

250
thirdparty/opengv/include/opengv/absolute_pose/modules/gpnp5/modules.hpp vendored Normal file
View File

@@ -0,0 +1,250 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP5_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP5_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp5
{
void init(
Eigen::Matrix<double,44,80> & groebnerMatrix,
const Eigen::Matrix<double,12,1> & a,
Eigen::Matrix<double,12,1> & n,
Eigen::Matrix<double,12,1> & m,
Eigen::Matrix<double,12,1> & k,
Eigen::Matrix<double,12,1> & l,
Eigen::Matrix<double,12,1> & p,
Eigen::Vector3d & c0,
Eigen::Vector3d & c1,
Eigen::Vector3d & c2,
Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,44,80> & groebnerMatrix );
void sPolynomial6( Eigen::Matrix<double,44,80> & groebnerMatrix );
void sPolynomial7( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow6_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow7_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow8_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial10( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow9_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow10_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow6_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow7_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow8_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow9_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow10_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow6_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow7_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow8_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow9_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow10_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow10_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow9_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow5_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow11_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow5_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow5_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow8_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow12_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow7_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow13_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial15( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow14_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial16( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow6_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow15_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial17( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow7_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow5_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow16_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial18( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow6_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow17_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial19( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow15_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow16_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow17_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow18_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow18_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial20( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow14_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow19_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial21( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow8_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow9_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow10_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow20_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial22( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow13_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow21_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial23( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow6_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow7_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow22_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial24( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow12_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow23_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial25( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow5_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow24_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow24_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial26( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow11_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow25_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow25_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial27( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow10_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow26_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow26_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial28( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow27_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow27_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial29( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow9_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow28_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow28_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial30( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow18_00001_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow24_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow29_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow29_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow29_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial31( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow14_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow8_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow18_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow25_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow30_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow30_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow30_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial32( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow14_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow6_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow7_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow18_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow26_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow31_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow31_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow31_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial33( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow32_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial34( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow32_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow33_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow33_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial35( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow34_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow34_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial36( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow35_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow35_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial37( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow36_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow36_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial38( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow32_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial39( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow35_00001_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow36_00001_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_00001_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_00001_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial40( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow36_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_00010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial41( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow32_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_10010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_10010_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_10100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_10100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_10100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow36_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_00100_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial42( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow39_02000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_02000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_02000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow38_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_11000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow37_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_01000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void sPolynomial43( Eigen::Matrix<double,44,80> & groebnerMatrix );
void groebnerRow38_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow39_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow40_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow41_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow42_20000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow42_10000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
void groebnerRow42_00000_f( Eigen::Matrix<double,44,80> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP5_MODULES_HPP_ */

97
thirdparty/opengv/include/opengv/absolute_pose/modules/main.hpp vendored Normal file
View File

@@ -0,0 +1,97 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_
#include <stdlib.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
void p3p_kneip_main(
const bearingVectors_t & f,
const points_t & p,
transformations_t & solutions );
void p3p_gao_main(
const bearingVectors_t & f,
const points_t & p,
transformations_t & solutions );
void gp3p_main(
const Eigen::Matrix3d & f,
const Eigen::Matrix3d & v,
const Eigen::Matrix3d & p,
transformations_t & solutions );
void gpnp_main(
const Eigen::Matrix<double,12,1> & a,
const Eigen::Matrix<double,12,12> & V,
const points_t & c,
transformation_t & transformation );
double gpnp_evaluate(
const Eigen::Matrix<double,12,1> & solution,
const points_t & c,
translation_t & t,
rotation_t & R );
void gpnp_optimize(
const Eigen::Matrix<double,12,1> & a,
const Eigen::Matrix<double,12,12> & V,
const points_t & c,
std::vector<double> & factors );
void upnp_fill_s(
const Eigen::Vector4d & quaternion,
Eigen::Matrix<double,10,1> & s );
void upnp_main(
const Eigen::Matrix<double,10,10> & M,
const Eigen::Matrix<double,1,10> & C,
double gamma,
std::vector<
std::pair<double,Eigen::Vector4d>,
Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> >
> & quaternions );
void upnp_main_sym(
const Eigen::Matrix<double,10,10> & M,
const Eigen::Matrix<double,1,10> & C,
double gamma,
std::vector<
std::pair<double,Eigen::Vector4d>,
Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> >
> & quaternions );
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_MAIN_HPP_ */

58
thirdparty/opengv/include/opengv/absolute_pose/modules/upnp2.hpp vendored Normal file
View File

@@ -0,0 +1,58 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_
#include <stdlib.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace upnp
{
void setupAction_gj(
const Eigen::Matrix<double,10,10> & M,
const Eigen::Matrix<double,1,10> & C,
double gamma,
Eigen::Matrix<double,16,16> & Action );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_UPNP2_HPP_ */

58
thirdparty/opengv/include/opengv/absolute_pose/modules/upnp4.hpp vendored Normal file
View File

@@ -0,0 +1,58 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_
#include <stdlib.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace upnp
{
void setupAction_sym_gj(
const Eigen::Matrix<double,10,10> & M,
const Eigen::Matrix<double,1,10> & C,
double gamma,
Eigen::Matrix<double,8,8> & Action );
}
}
}
}
#endif /* OPENGV_ABSOLUTE_POSE_MODULES_UPNP4_HPP_ */

165
thirdparty/opengv/include/opengv/math/Sturm.hpp vendored Normal file
View File

@@ -0,0 +1,165 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file Sturm.hpp
* \brief Class for evaluating Sturm chains on polynomials, and bracketing the
* real roots.
*/
#ifndef OPENGV_STURM_HPP_
#define OPENGV_STURM_HPP_
#include <memory>
#include <stdlib.h>
#include <vector>
#include <list>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
class Bracket
{
public:
typedef std::shared_ptr<Bracket> Ptr;
typedef std::shared_ptr<const Bracket> ConstPtr;
Bracket( double lowerBound, double upperBound );
Bracket( double lowerBound, double upperBound, size_t changes, bool setUpperBoundChanges );
virtual ~Bracket();
bool dividable( double eps ) const;
void divide( std::list<Ptr> & brackets ) const;
double lowerBound() const;
double upperBound() const;
bool lowerBoundChangesComputed() const;
bool upperBoundChangesComputed() const;
void setLowerBoundChanges( size_t changes );
void setUpperBoundChanges( size_t changes );
size_t numberRoots() const;
private:
double _lowerBound;
double _upperBound;
bool _lowerBoundChangesComputed;
bool _upperBoundChangesComputed;
size_t _lowerBoundChanges;
size_t _upperBoundChanges;
};
/**
* Sturm is initialized over polynomials of arbitrary order, and used to compute
* the real roots of the polynomial.
*/
class Sturm
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** A pair of values bracketing a real root */
typedef std::pair<double,double> bracket_t;
/**
* \brief Contructor.
* \param[in] p The polynomial coefficients (poly = p(0,0)*x^n + p(0,1)*x^(n-1) ...).
*/
Sturm( const Eigen::MatrixXd & p );
/**
* \brief Contructor.
* \param[in] p The polynomial coefficients (poly = p[0]*x^n + p[1]*x^(n-1) ...).
*/
Sturm( const std::vector<double> & p );
/**
* \brief Destructor.
*/
virtual ~Sturm();
void findRoots2( std::vector<double> & roots, double eps_x = 0.001, double eps_val = 0.001 );
/**
* \brief Finds the roots of the polynomial.
* \return An array with the real roots of the polynomial.
*/
std::vector<double> findRoots();
/**
* \brief Finds brackets for the real roots of the polynomial.
* \return A list of brackets for the real roots of the polynomial.
*/
void bracketRoots( std::vector<double> & roots, double eps = -1.0 );
/**
* \brief Evaluates the Sturm chain at a single bound.
* \param[in] bound The bound.
* \return The number of sign changes on the bound.
*/
size_t evaluateChain( double bound );
/**
* \brief Evaluates the Sturm chain at a single bound.
* \param[in] bound The bound.
* \return The number of sign changes on the bound.
*/
size_t evaluateChain2( double bound );
/**
* \brief Composes an initial bracket for all the roots of the polynomial.
* \return The maximum of the absolute values of the bracket-values (That's
* what the Lagrangian bound is able to find).
*/
double computeLagrangianBound();
private:
/**
* \brief Internal function used for composing the Sturm chain
* \param[in] p1 First polynomial.
* \param[in] p2 Second polynomial.
* \param[out] r The negated remainder of the polynomial division p1/p2.
*/
void computeNegatedRemainder(
const Eigen::MatrixXd & p1,
const Eigen::MatrixXd & p2,
Eigen::MatrixXd & r );
/** A matrix containing the coefficients of the Sturm-chain of the polynomial */
Eigen::MatrixXd _C;
/** The dimension _C, which corresponds to (polynomial order+1) */
size_t _dimension;
};
}
}
#endif /* OPENGV_STURM_HPP_ */

83
thirdparty/opengv/include/opengv/math/arun.hpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file arun.hpp
* \brief Arun's method for computing the rotation between two point sets.
*/
#ifndef OPENGV_ARUN_HPP_
#define OPENGV_ARUN_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
/**
* \brief Arun's method for computing the rotation between two point sets.
* Core function [13].
*
* \param[in] Hcross The summation over the exterior products between the
* normalized points.
* \return The rotation matrix that aligns the points.
*/
rotation_t arun( const Eigen::MatrixXd & Hcross );
/**
* \brief Arun's method for complete point cloud alignment [13]. The method
* actually does the same than threept_arun, but has a different
* interface.
*
* \param[in] p1 The points expressed in the first frame.
* \param[in] p2 The points expressed in the second frame.
* \return The Transformation from frame 2 to frame 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
* frame 1, and \f$ \mathbf{R} \f$ being the rotation from
* frame 2 to frame 1).
*/
transformation_t arun_complete( const points_t & p1, const points_t & p2 );
}
}
#endif /* OPENGV_ARUN_HPP_ */

84
thirdparty/opengv/include/opengv/math/cayley.hpp vendored Normal file
View File

@@ -0,0 +1,84 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file cayley.hpp
* \brief Functions for back-and-forth transformation between rotation matrices
* and Cayley-parameters.
*/
#ifndef OPENGV_CAYLEY_HPP_
#define OPENGV_CAYLEY_HPP_
#include <stdlib.h>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
/**
* \brief Compute a rotation matrix from Cayley-parameters, following [14].
*
* \param[in] cayley The Cayley-parameters of a rotation.
* \return The 3x3 rotation matrix.
*/
rotation_t cayley2rot( const cayley_t & cayley);
/**
* \brief Compute a fake rotation matrix from Cayley-parameters, following [14].
* The rotation matrix is missing the scaling parameter of the
* Cayley-transform. This short form is useful for the Jacobian-based
* iterative rotation optimization of the eigensolver [11].
*
* \param[in] cayley The Cayley-parameters of the rotation.
* \return The false 3x3 rotation matrix.
*/
rotation_t cayley2rot_reduced( const cayley_t & cayley);
/**
* \brief Compute the Cayley-parameters of a rotation matrix, following [14].
*
* \param[in] R The 3x3 rotation matrix.
* \return The Cayley-parameters.
*/
cayley_t rot2cayley( const rotation_t & R );
}
}
#endif /* OPENGV_CAYLEY_HPP_ */

67
thirdparty/opengv/include/opengv/math/gauss_jordan.hpp vendored Normal file
View File

@@ -0,0 +1,67 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file gauss_jordan.hpp
* \brief Sparse, fast Gauss Jordan elimination.
*/
#ifndef OPENGV_GAUSS_JORDAN_HPP_
#define OPENGV_GAUSS_JORDAN_HPP_
#include <stdlib.h>
#include <vector>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
/**
* \brief Sparse, fast Gauss Jordan elimination on matrices with a left square
* invertible block.
*
* \param[in] matrix The matrix.
* \param[in] exitCondition The last row we process when stepping up.
*/
void gauss_jordan(
std::vector<std::vector<double>*> & matrix,
int exitCondition = 0 );
}
}
#endif /* OPENGV_GAUSS_JORDAN_HPP_ */

74
thirdparty/opengv/include/opengv/math/quaternion.hpp vendored Normal file
View File

@@ -0,0 +1,74 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file quaternion.hpp
* \brief Functions for back-and-forth transformation between rotation matrices
* and quaternion-parameters.
*/
#ifndef OPENGV_QUATERNION_HPP_
#define OPENGV_QUATERNION_HPP_
#include <stdlib.h>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
/**
* \brief Compute a rotation matrix from quaternion-parameters. Assumes that the
* quaternion has unit norm.
*
* \param[in] quaternion The quaternion-parameters of a rotation.
* \return The 3x3 rotation matrix.
*/
rotation_t quaternion2rot( const quaternion_t & quaternion);
/**
* \brief Compute the quaternion-parameters of a rotation matrix.
*
* \param[in] R The 3x3 rotation matrix.
* \return The quaternion-parameters.
*/
quaternion_t rot2quaternion( const rotation_t & R );
}
}
#endif /* OPENGV_QUATERNION_HPP_ */

83
thirdparty/opengv/include/opengv/math/roots.hpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file roots.hpp
* \brief Closed-form solutions for computing the roots of a polynomial.
*/
#ifndef OPENGV_ROOTS_HPP_
#define OPENGV_ROOTS_HPP_
#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace of the math tools.
*/
namespace math
{
/**
* \brief The roots of a third-order polynomial.
*
* \param[in] p The polynomial coefficients (poly = p[0]*x^3 + p[1]*x^2 ...).
* \return The roots of the polynomial (only real ones).
*/
std::vector<double> o3_roots( const std::vector<double> & p );
/**
* \brief Ferrari's method for computing the roots of a fourth order polynomial.
*
* \param[in] p The polynomial coefficients (poly = p(0,0)*x^4 + p(1,0)*x^3 ...).
* \return The roots of the polynomial (only real ones).
*/
std::vector<double> o4_roots( const Eigen::MatrixXd & p );
/**
* \brief Ferrari's method for computing the roots of a fourth order polynomial.
* With a different interface.
*
* \param[in] p The polynomial coefficients (poly = p[0]*x^4 + p[1]*x^3 ...).
* \return The roots of the polynomial (only real ones).
*/
std::vector<double> o4_roots( const std::vector<double> & p );
}
}
#endif /* OPENGV_ROOTS_HPP_ */

109
thirdparty/opengv/include/opengv/point_cloud/MAPointCloud.hpp vendored Normal file
View File

@@ -0,0 +1,109 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MAPointCloud.hpp
* \brief Adapter-class for passing 3D point correspondences. Maps
* matlab types to opengv types.
*/
#ifndef OPENGV_MAPOINTCLOUD_HPP_
#define OPENGV_MAPOINTCLOUD_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/point_cloud/PointCloudAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the point-cloud alignment methods.
*/
namespace point_cloud
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a PointCloudAdapter. This child-class is used for holding
* data in form of pointers to matlab-data.
*/
class MAPointCloud : public PointCloudAdapterBase
{
private:
using PointCloudAdapterBase::_t12;
using PointCloudAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MAPointCloud(
const double * points1,
const double * points2,
int numberPoints1,
int numberPoints2 );
/**
* Destructor
*/
virtual ~MAPointCloud();
//Access of correspondences
/** See parent-class */
virtual opengv::point_t getPoint1( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
private:
/** A pointer to the points in frame 1 */
const double * _points1;
/** A pointer to the points in frame 2 */
const double * _points2;
/** The number of points in frame 1 */
int _numberPoints1;
/** The number of points in frame 2 */
int _numberPoints2;
};
}
}
#endif /* OPENGV_MAPOINTCLOUD_HPP_ */

117
thirdparty/opengv/include/opengv/point_cloud/PointCloudAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,117 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file PointCloudAdapter.hpp
* \brief Adapter-class for passing 3D point correspondences. Maps
* opengv types back to opengv types.
*/
#ifndef OPENGV_POINTCLOUDADAPTER_HPP_
#define OPENGV_POINTCLOUDADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/point_cloud/PointCloudAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the point-cloud alignment methods.
*/
namespace point_cloud
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a PointCloudAdapter. This child-class is used for holding
* data in form of references to opengv-types.
*/
class PointCloudAdapter : public PointCloudAdapterBase
{
private:
using PointCloudAdapterBase::_t12;
using PointCloudAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
PointCloudAdapter(
const points_t & points1,
const points_t & points2 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
PointCloudAdapter(
const points_t & points1,
const points_t & points2,
const rotation_t & R12 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
PointCloudAdapter(
const points_t & points1,
const points_t & points2,
const translation_t & t12,
const rotation_t & R12 );
/**
* Destructor
*/
virtual ~PointCloudAdapter();
//Access of correspondences
/** See parent-class */
virtual opengv::point_t getPoint1( size_t index ) const;
/** See parent-class */
virtual opengv::point_t getPoint2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
private:
/** Reference to the 3D-points in frame 1 */
const points_t & _points1;
/** Reference to the 3D-points in frame 2 */
const points_t & _points2;
};
}
}
#endif /* OPENGV_POINTCLOUDADAPTER_HPP_ */

161
thirdparty/opengv/include/opengv/point_cloud/PointCloudAdapterBase.hpp vendored Normal file
View File

@@ -0,0 +1,161 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file PointCloudAdapter.hpp
* \brief Adapter-class for passing 3D point correspondences.
*/
#ifndef OPENGV_POINTCLOUDADAPTERBASE_HPP_
#define OPENGV_POINTCLOUDADAPTERBASE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the point-cloud alignment methods.
*/
namespace point_cloud
{
/**
* The PointCloudAdapterBase is the base-class for the visitors to the
* point-cloud alignment methods. It provides a unified interface to
* opengv-methods to access point correspondences and priors or known variables
* for the alignment. Derived classes may hold the data in any user-specific
* format, and adapt to opengv-types.
*/
class PointCloudAdapterBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor.
*/
PointCloudAdapterBase( ) :
_t12(Eigen::Vector3d::Zero()),
_R12(Eigen::Matrix3d::Identity()) {};
/**
* \brief Constructor.
* \param[in] R12 A prior or known value for the rotation from frame 2 to
* frame 1.
*/
PointCloudAdapterBase( const rotation_t & R12 ) :
_t12(Eigen::Vector3d::Zero()),
_R12(R12) {};
/**
* \brief Constructor.
* \param[in] t12 A prior or known value for the position of frame 2 seen
* from frame 1.
* \param[in] R12 A prior or known value for the rotation from frame 2
* to frame 1.
*/
PointCloudAdapterBase( const translation_t & t12, const rotation_t & R12 ) :
_t12(t12),
_R12(R12) {};
/**
* \brief Destructor.
*/
virtual ~PointCloudAdapterBase() {};
//Access of correspondences
/**
* \brief Retrieve the 3D-point of a correspondence in frame 1.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding 3D-point.
*/
virtual opengv::point_t getPoint1( size_t index ) const = 0;
/**
* \brief Retrieve the 3D-point of a correspondence in frame 2.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding 3D-point.
*/
virtual opengv::point_t getPoint2( size_t index ) const = 0;
/**
* \brief Retrieve the number of correspondences.
* \return The number of correspondences.
*/
virtual size_t getNumberCorrespondences() const = 0;
/**
* \brief Retrieve the weight of a correspondence. The weight is supposed to
* reflect the quality of a correspondence, and typically is between
* 0 and 1.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding weight.
*/
virtual double getWeight( size_t index ) const = 0;
//Access of priors or known values
/**
* \brief Retrieve the prior or known value for the relative position.
* \return The prior or known value for the position.
*/
virtual opengv::translation_t gett12() const { return _t12; };
/**
* \brief Set the prior or known value for the relative position.
* \param[in] t12 The prior or known value for the position.
*/
virtual void sett12(const translation_t & t12) { _t12 = t12; };
/**
* \brief Retrieve the prior or known value for the relative rotation.
* \return The prior or known value for the rotation.
*/
virtual opengv::rotation_t getR12() const { return _R12; };
/**
* \brief Set the prior or known value for the relative rotation.
* \param[in] R12 The prior or known value for the rotation.
*/
virtual void setR12(const rotation_t & R12) { _R12 = R12; };
public:
/** Prior or known value for the position of frame 2 seen from frame 1.
* Initialized to zero if not provided.
*/
translation_t _t12;
/** Prior or known value for the rotation from frame 2 to frame 1.
* Initialized to identity if not provided.
*/
rotation_t _R12;
};
}
}
#endif /* OPENGV_POINTCLOUDADAPTERBASE_HPP_ */

120
thirdparty/opengv/include/opengv/point_cloud/methods.hpp vendored Normal file
View File

@@ -0,0 +1,120 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file point_cloud/methods.hpp
* \brief Methods for computing the transformation between two frames that
* contain point-clouds.
*/
#ifndef OPENGV_POINT_CLOUD_METHODS_HPP_
#define OPENGV_POINT_CLOUD_METHODS_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/point_cloud/PointCloudAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the point-cloud alignment methods.
*/
namespace point_cloud
{
/**
* \brief Compute the transformation between two frames containing point clouds,
* following Arun's method [13]. Using all available correspondences.
*
* \param[in] adapter Visitor holding world-point correspondences.
* \return Transformation from frame 2 back to frame 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
* frame 1, and \f$ \mathbf{R} \f$ being the rotation from
* frame 2 to frame 1).
*/
transformation_t threept_arun( const PointCloudAdapterBase & adapter );
/**
* \brief Compute the transformation between two frames containing point clouds,
* following Arun's method [13].
*
* \param[in] adapter Visitor holding world-point correspondences.
* \param[in] indices Indices of the correspondences used for deriving the
* transformation.
* \return Transformation from frame 2 back to frame 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
* frame 1, and \f$ \mathbf{R} \f$ being the rotation from
* frame 2 to frame 1).
*/
transformation_t threept_arun(
const PointCloudAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the transformation between two frames containing point clouds
* using nonlinear optimization. Using all available correspondences.
*
* \param[in] adapter Visitor holding world-point correspondences, plus the
* initial values.
* \return Transformation from frame 2 back to frame 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
* frame 1, and \f$ \mathbf{R} \f$ being the rotation from
* frame 2 to frame 1).
*/
transformation_t optimize_nonlinear( PointCloudAdapterBase & adapter );
/**
* \brief Compute the transformation between two frames containing point clouds.
* Using nonlinear optimization.
*
* \param[in] adapter Visitor holding world-point correspondences, plus the
* initial values.
* \param[in] indices Indices of the correspondences used for optimization.
* \return Transformation from frame 2 back to frame 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
* frame 1, and \f$ \mathbf{R} \f$ being the rotation from
* frame 2 to frame 1).
*/
transformation_t optimize_nonlinear(
PointCloudAdapterBase & adapter,
const std::vector<int> & indices );
}
}
#endif /* OPENGV_POINT_CLOUD_METHODS_HPP_ */

126
thirdparty/opengv/include/opengv/relative_pose/CentralRelativeAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,126 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file CentralRelativeAdapter.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* central relative-pose algorithms. Maps opengv types back to
* opengv types.
*/
#ifndef OPENGV_RELATIVE_POSE_CENTRALRELATIVEADAPTER_HPP_
#define OPENGV_RELATIVE_POSE_CENTRALRELATIVEADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeAdapter. This child-class is for the central case and holds data
* in form of references to opengv-types.
*/
class CentralRelativeAdapter : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const rotation_t & R12 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const translation_t & t12,
const rotation_t & R12 );
/**
* \brief Destructor.
*/
virtual ~CentralRelativeAdapter();
//Access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1( size_t index ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset1( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation1( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset2( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation2( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** Reference to bearing-vectors expressed in viewpoint 1. */
const bearingVectors_t & _bearingVectors1;
/** Reference to bearing-vectors expressed in viewpoint 2. */
const bearingVectors_t & _bearingVectors2;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_CENTRALRELATIVEADAPTER_HPP_ */

138
thirdparty/opengv/include/opengv/relative_pose/CentralRelativeMultiAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,138 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file CentralRelativeMultiAdapter.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* central relative-pose algorithms. Maps opengv types
* back to opengv types. For multi-central-viewpoint. Manages multiple
* match-lists for pairs of cameras (e.g. pairs of central viewpoints).
*/
#ifndef OPENGV_RELATIVE_POSE_CENTRALRELATIVEMULTIADAPTER_HPP_
#define OPENGV_RELATIVE_POSE_CENTRALRELATIVEMULTIADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <memory>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeMultiAdapter. This child-class is for the multi-central case and
* holds data in form of references to opengv-types. It is meant to be used for
* problems involving more than two central viewpoints. This is experimental!
*/
class CentralRelativeMultiAdapter : public RelativeMultiAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeMultiAdapter(
std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors1,
std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors2 );
/**
* \brief Destructor.
*/
virtual ~CentralRelativeMultiAdapter();
//camera-pair-wise access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual double getWeight( size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual translation_t getCamOffset( size_t pairIndex ) const;
/** See parent-class */
virtual rotation_t getCamRotation( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberPairs() const;
//Conversion to and from serialized indices
/** See parent-class */
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const;
/** See parent-class */
virtual int convertMultiIndex(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual int multiPairIndex( size_t index ) const;
/** See parent-class */
virtual int multiCorrespondenceIndex( size_t index ) const;
protected:
/** References to multiple sets of bearing-vectors (the ones from camera 1 of
* each pair, and expressed in there
*/
std::vector<std::shared_ptr<bearingVectors_t> > _bearingVectors1;
/** References to multiple sets of bearing-vectors (the ones from camera 1 of
* each pair, and expressed in there).
*/
std::vector<std::shared_ptr<bearingVectors_t> > _bearingVectors2;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiPairIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiKeypointIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> singleIndexOffsets;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_CENTRALRELATIVEMULTIADAPTER_HPP_ */

133
thirdparty/opengv/include/opengv/relative_pose/CentralRelativeWeightingAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,133 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file CentralRelativeWeightingAdapter.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* central relative-pose algorithms. Maps opengv types back to
* opengv types. Also contains "weights" reflecting the quality of
* a feature correspondence (typically between 0 and 1)
*/
#ifndef OPENGV_RELATIVE_POSE_CENTRALRELATIVEWEIGHTINGADAPTER_HPP_
#define OPENGV_RELATIVE_POSE_CENTRALRELATIVEWEIGHTINGADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeAdapter. This child-class is for the central case and holds data
* in form of references to opengv-types. It also includes weights for the
* correspondences.
*/
class CentralRelativeWeightingAdapter : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeWeightingAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const std::vector<double> & weights );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeWeightingAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const std::vector<double> & weights,
const rotation_t & R12 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
CentralRelativeWeightingAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const std::vector<double> & weights,
const translation_t & t12,
const rotation_t & R12 );
/**
* \brief Destructor.
*/
virtual ~CentralRelativeWeightingAdapter();
//Access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1( size_t index ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset1( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation1( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset2( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation2( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** Reference to bearing-vectors expressed in viewpoint 1. */
const bearingVectors_t & _bearingVectors1;
/** Reference to bearing-vectors expressed in viewpoint 2. */
const bearingVectors_t & _bearingVectors2;
/** Reference to an array of weights. Indexing follows _bearingVectors2. */
const std::vector<double> & _weights;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_CENTRALRELATIVEWEIGHTINGADAPTER_HPP_ */

117
thirdparty/opengv/include/opengv/relative_pose/MACentralRelative.hpp vendored Normal file
View File

@@ -0,0 +1,117 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MACentralRelative.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* central relative-pose algorithms. Maps matlab types to opengv types.
*/
#ifndef OPENGV_RELATIVE_POSE_MACENTRALRELATIVE_HPP_
#define OPENGV_RELATIVE_POSE_MACENTRALRELATIVE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeAdapter. This child-class is for the central case and holds data
* in form of pointers to matlab data.
*/
class MACentralRelative : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MACentralRelative(
const double * bearingVectors1,
const double * bearingVectors2,
int numberBearingVectors1,
int numberBearingVectors2 );
/**
* \brief Destructor.
*/
virtual ~MACentralRelative();
//Access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1( size_t index ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset1( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation1( size_t index ) const;
/** See parent-class. Returns zero for this adapter. */
virtual translation_t getCamOffset2( size_t index ) const;
/** See parent-class. Returns identity for this adapter. */
virtual rotation_t getCamRotation2( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** A pointer to the bearing-vectors in viewpoint 1 */
const double * _bearingVectors1;
/** A pointer to the bearing-vectors in viewpoint 2 */
const double * _bearingVectors2;
/** The number of bearing-vectors in viewpoint 1 */
int _numberBearingVectors1;
/** The number of bearing-vectors in viewpoint 2 */
int _numberBearingVectors2;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_MACENTRALRELATIVE_HPP_ */

118
thirdparty/opengv/include/opengv/relative_pose/MANoncentralRelative.hpp vendored Normal file
View File

@@ -0,0 +1,118 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MANoncentralRelative.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* non-central relative-pose algorithms. Maps matlab types
* to opengv types.
*/
#ifndef OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVE_HPP_
#define OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeAdapter. This child-class is for the non-central case and holds
* data in form of pointers to matlab-types.
*/
class MANoncentralRelative : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MANoncentralRelative(
const double * bearingVectors1,
const double * bearingVectors2,
int numberBearingVectors1,
int numberBearingVectors2 );
/**
* \brief Destructor.
*/
virtual ~MANoncentralRelative();
//Access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1( size_t index ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual translation_t getCamOffset1( size_t index ) const;
/** See parent-class */
virtual rotation_t getCamRotation1( size_t index ) const;
/** See parent-class */
virtual translation_t getCamOffset2( size_t index ) const;
/** See parent-class */
virtual rotation_t getCamRotation2( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** A pointer to the bearing-vectors in viewpoint 1 */
const double * _bearingVectors1;
/** A pointer to the bearing-vectors in viewpoint 2 */
const double * _bearingVectors2;
/** The number of bearing-vectors in viewpoint 1 */
int _numberBearingVectors1;
/** The number of bearing-vectors in viewpoint 2 */
int _numberBearingVectors2;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVE_HPP_ */

144
thirdparty/opengv/include/opengv/relative_pose/MANoncentralRelativeMulti.hpp vendored Normal file
View File

@@ -0,0 +1,144 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MANoncentralRelativeMulti.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* non-central relative-pose algorithms. Maps Matlab types
* to opengv types. Manages multiple match-lists for pairs of cameras.
* This allows to draw samples homogeneously over the cameras.
*/
#ifndef OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVEMULTI_HPP_
#define OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVEMULTI_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeMultiAdapter. This child-class is for the relative non-central case
* and holds data in form of pointers to matlab-types. It is meant to be used
* for problems involving two non-central viewpoints, but in the special case
* where correspondences result from two cameras with equal transformation
* to their two viewpoints.
*/
class MANoncentralRelativeMulti : public RelativeMultiAdapterBase
{
protected:
using RelativeMultiAdapterBase::_t12;
using RelativeMultiAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
MANoncentralRelativeMulti(
const std::vector<double*> & bearingVectors1,
const std::vector<double*> & bearingVectors2,
const double * camOffsets,
const std::vector<int> & numberBearingVectors );
/**
* \brief Destructor.
*/
virtual ~MANoncentralRelativeMulti();
//camera-pair-wise access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual double getWeight( size_t camIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual translation_t getCamOffset( size_t pairIndex ) const;
/** See parent-class */
virtual rotation_t getCamRotation( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberPairs() const;
//Conversion to and from serialized indices
/** See parent-class */
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const;
/** See parent-class */
virtual int convertMultiIndex(
size_t camIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual int multiPairIndex( size_t index ) const;
/** See parent-class */
virtual int multiCorrespondenceIndex( size_t index ) const;
protected:
/** A pointer to the bearing-vectors in viewpoint 1 */
std::vector<double *> _bearingVectors1;
/** A pointer to the bearing-vectors in viewpoint 2 */
std::vector<double *> _bearingVectors2;
/** The offset from the viewpoint origin of each vector */
const double * _camOffsets;
/** The number of bearing-vectors in each camera */
std::vector<int> _numberBearingVectors;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiPairIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiKeypointIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> singleIndexOffsets;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_MANONCENTRALRELATIVEMULTI_HPP_ */

168
thirdparty/opengv/include/opengv/relative_pose/NoncentralRelativeAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,168 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file NoncentralRelativeAdapter.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* non-central relative-pose algorithms. Maps opengv types
* back to opengv types.
*/
#ifndef OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEADAPTER_HPP_
#define OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeAdapter. This child-class is for the non-central case and holds
* data in form of references to opengv-types.
*/
class NoncentralRelativeAdapter : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** A type defined for the camera-correspondences, see protected
* class-members
*/
typedef std::vector<int> camCorrespondences_t;
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const camCorrespondences_t & camCorrespondences1,
const camCorrespondences_t & camCorrespondences2,
const translations_t & camOffsets,
const rotations_t & camRotations );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const camCorrespondences_t & camCorrespondences1,
const camCorrespondences_t & camCorrespondences2,
const translations_t & camOffsets,
const rotations_t & camRotations,
const rotation_t & R12 );
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralRelativeAdapter(
const bearingVectors_t & bearingVectors1,
const bearingVectors_t & bearingVectors2,
const camCorrespondences_t & camCorrespondences1,
const camCorrespondences_t & camCorrespondences2,
const translations_t & camOffsets,
const rotations_t & camRotations,
const translation_t & t12,
const rotation_t & R12 );
/**
* \brief Destructor.
*/
virtual ~NoncentralRelativeAdapter();
//Access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1( size_t index ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2( size_t index ) const;
/** See parent-class */
virtual double getWeight( size_t index ) const;
/** See parent-class */
virtual translation_t getCamOffset1( size_t index ) const;
/** See parent-class */
virtual rotation_t getCamRotation1( size_t index ) const;
/** See parent-class */
virtual translation_t getCamOffset2( size_t index ) const;
/** See parent-class */
virtual rotation_t getCamRotation2( size_t index ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences() const;
protected:
/** Reference to bearing-vectors in viewpoint 1.
* (expressed in their individual cameras)
*/
const bearingVectors_t & _bearingVectors1;
/** Reference to bearing-vectors in viewpoint 2.
* (expressed in their individual cameras)
*/
const bearingVectors_t & _bearingVectors2;
/** Reference to an array of camera-indices for the bearing vectors in
* viewpoint 1. Length equals to number of bearing-vectors in viewpoint 1,
* and elements are indices of cameras in the _camOffsets and _camRotations
* arrays.
*/
const camCorrespondences_t & _camCorrespondences1;
/** Reference to an array of camera-indices for the bearing vectors in
* viewpoint 2. Length equals to number of bearing-vectors in viewpoint 2,
* and elements are indices of cameras in the _camOffsets and _camRotations
* arrays.
*/
const camCorrespondences_t & _camCorrespondences2;
/** Reference to positions of the different cameras seen from their
* viewpoint.
*/
const translations_t & _camOffsets;
/** Reference to rotations from the different cameras back to their
* viewpoint.
*/
const rotations_t & _camRotations;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEADAPTER_HPP_ */

151
thirdparty/opengv/include/opengv/relative_pose/NoncentralRelativeMultiAdapter.hpp vendored Normal file
View File

@@ -0,0 +1,151 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file NoncentralRelativeMultiAdapter.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* non-central relative-pose algorithms. Maps opengv types
* back to opengv types. Manages multiple match-lists for pairs of cameras.
* This allows to draw samples homogeneously over the cameras.
*/
#ifndef OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEMULTIADAPTER_HPP_
#define OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEMULTIADAPTER_HPP_
#include <stdlib.h>
#include <vector>
#include <memory>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Check the documentation of the parent-class to understand the meaning of
* a RelativeMultiAdapter. This child-class is for the relative non-central case
* and holds data in form of references to opengv-types. It is meant to be used
* for problems involving two non-central viewpoints, but in the special case
* where correspondences result from two cameras with equal transformation
* to their two viewpoints.
*/
class NoncentralRelativeMultiAdapter : public RelativeMultiAdapterBase
{
protected:
using RelativeMultiAdapterBase::_t12;
using RelativeMultiAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor. See protected class-members to understand parameters
*/
NoncentralRelativeMultiAdapter(
std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors1,
std::vector<std::shared_ptr<bearingVectors_t> > bearingVectors2,
const translations_t & camOffsets,
const rotations_t & camRotations );
/**
* \brief Destructor.
*/
virtual ~NoncentralRelativeMultiAdapter();
//camera-pair-wise access of correspondences
/** See parent-class */
virtual bearingVector_t getBearingVector1(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual bearingVector_t getBearingVector2(
size_t pairIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual double getWeight( size_t camIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual translation_t getCamOffset( size_t pairIndex ) const;
/** See parent-class */
virtual rotation_t getCamRotation( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberCorrespondences( size_t pairIndex ) const;
/** See parent-class */
virtual size_t getNumberPairs() const;
//Conversion to and from serialized indices
/** See parent-class */
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const;
/** See parent-class */
virtual int convertMultiIndex(
size_t camIndex, size_t correspondenceIndex ) const;
/** See parent-class */
virtual int multiPairIndex( size_t index ) const;
/** See parent-class */
virtual int multiCorrespondenceIndex( size_t index ) const;
protected:
/** References to multiple sets of bearing-vectors (the ones from camera 1 of
* each pair, and expressed in there).
*/
std::vector<std::shared_ptr<bearingVectors_t> > _bearingVectors1;
/** References to multiple sets of bearing-vectors (the ones from camera 2 of
* each pair, and expressed in there).
*/
std::vector<std::shared_ptr<bearingVectors_t> > _bearingVectors2;
/** Reference to positions of the different cameras seen from the
* viewpoints.
*/
const translations_t & _camOffsets;
/** Reference to rotations from the different cameras back to the
* viewpoints.
*/
const rotations_t & _camRotations;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiPairIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> multiKeypointIndices;
/** Initialized in constructor, used for (de)-serialiaztion of indices */
std::vector<int> singleIndexOffsets;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_NONCENTRALRELATIVEMULTIADAPTER_HPP_ */

195
thirdparty/opengv/include/opengv/relative_pose/RelativeAdapterBase.hpp vendored Normal file
View File

@@ -0,0 +1,195 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file RelativeAdapterBase.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* relative-pose algorithms.
*/
#ifndef OPENGV_RELATIVE_POSE_RELATIVEADAPTERBASE_HPP_
#define OPENGV_RELATIVE_POSE_RELATIVEADAPTERBASE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* The RelativeAdapterBase is the base-class for the visitors to the central
* and non-central relative pose algorithms. It provides a unified interface to
* opengv-methods to access bearing-vector correspondences, priors or known
* variables for the relative pose, and the multi-camera configuration in the
* non-central case. Derived classes may hold the data in any user-specific
* format, and adapt to opengv-types.
*/
class RelativeAdapterBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor.
*/
RelativeAdapterBase( ) :
_t12(Eigen::Vector3d::Zero()),
_R12(Eigen::Matrix3d::Identity()) {};
/**
* \brief Constructor.
* \param[in] R12 A prior or known value for the rotation from viewpoint 2
* to viewpoint 1.
*/
RelativeAdapterBase( const rotation_t & R12 ) :
_t12(Eigen::Vector3d::Zero()),
_R12(R12) {};
/**
* \brief Constructor.
* \param[in] t12 A prior or known value for the position of viewpoint 2 seen
* from viewpoint 1.
* \param[in] R12 A prior or known value for the rotation from viewpoint 2
* to viewpoint 1.
*/
RelativeAdapterBase( const translation_t & t12, const rotation_t & R12 ) :
_t12(t12),
_R12(R12) {};
/**
* \brief Destructor.
*/
virtual ~RelativeAdapterBase() {};
//Access of correspondences
/**
* \brief Retrieve the bearing vector of a correspondence in viewpoint 1.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding bearing vector.
*/
virtual opengv::bearingVector_t getBearingVector1( size_t index ) const = 0;
/**
* \brief Retrieve the bearing vector of a correspondence in viewpoint 2.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding bearing vector.
*/
virtual opengv::bearingVector_t getBearingVector2( size_t index ) const = 0;
/**
* \brief Retrieve the weight of a correspondence. The weight is supposed to
* reflect the quality of a correspondence, and typically is between
* 0 and 1.
* \param[in] index The serialized index of the correspondence.
* \return The corresponding weight.
*/
virtual double getWeight( size_t index ) const = 0;
/**
* \brief Retrieve the position of a camera of a correspondence in viewpoint
* 1 seen from the origin of the viewpoint.
* \param[in] index The serialized index of the correspondence.
* \return The position of the corresponding camera seen from the viewpoint
* origin.
*/
virtual opengv::translation_t getCamOffset1( size_t index ) const = 0;
/**
* \brief Retrieve the rotation from a camera of a correspondence in
* viewpoint 1 to the viewpoint origin.
* \param[in] index The serialized index of the correspondence.
* \return The rotation from the corresponding camera back to the viewpoint
* origin.
*/
virtual opengv::rotation_t getCamRotation1( size_t index ) const = 0;
/**
* \brief Retrieve the position of a camera of a correspondence in viewpoint
* 2 seen from the origin of the viewpoint.
* \param[in] index The serialized index of the correspondence.
* \return The position of the corresponding camera seen from the viewpoint
* origin.
*/
virtual opengv::translation_t getCamOffset2( size_t index ) const = 0;
/**
* \brief Retrieve the rotation from a camera of a correspondence in
* viewpoint 2 to the viewpoint origin.
* \param[in] index The serialized index of the correspondence.
* \return The rotation from the corresponding camera back to the viewpoint
* origin.
*/
virtual opengv::rotation_t getCamRotation2( size_t index ) const = 0;
/**
* \brief Retrieve the number of correspondences.
* \return The number of correspondences.
*/
virtual size_t getNumberCorrespondences() const = 0;
//Access of priors or known values
/**
* \brief Retrieve the prior or known value for the relative position.
* \return The prior or known value for the position.
*/
opengv::translation_t gett12() const { return _t12; };
/**
* \brief Set the prior or known value for the relative position.
* \param[in] t12 The prior or known value for the position.
*/
void sett12(const opengv::translation_t & t12) { _t12 = t12; };
/**
* \brief Retrieve the prior or known value for the relative rotation.
* \return The prior or known value for the rotation.
*/
opengv::rotation_t getR12() const { return _R12; };
/**
* \brief Set the prior or known value for the relative rotation.
* \param[in] R12 The prior or known value for the rotation.
*/
void setR12(const opengv::rotation_t & R12) { _R12 = R12; };
protected:
/** Prior or known value for the position of viewpoint 2 seen from
* viewpoint 1. Initialized to zero if not provided.
*/
opengv::translation_t _t12;
/** Prior or known value for the rotation from viewpoint 2 back to
* viewpoint 1. Initialized to identity if not provided.
*/
opengv::rotation_t _R12;
};
}
}
#endif /* OPENGV_RELATIVE_POSE_RELATIVEADAPTERBASE_HPP_ */

222
thirdparty/opengv/include/opengv/relative_pose/RelativeMultiAdapterBase.hpp vendored Normal file
View File

@@ -0,0 +1,222 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file RelativeMultiAdapterBase.hpp
* \brief Adapter-class for passing bearing-vector correspondences to the
* relative-pose algorithms. Intended for multi-central-viewpoint or
* relative non-central-viewpoint problems. Access of correspondences
* etc. via an additional pair-index referring to the pairs of cameras.
*/
#ifndef OPENGV_RELATIVE_POSE_RELATIVEMULTIADAPTERBASE_HPP_
#define OPENGV_RELATIVE_POSE_RELATIVEMULTIADAPTERBASE_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* See the documentation of RelativeAdapterBase to understand the meaning of
* a RelativeAdapter. RelativeMultiAdapterBase extends the interface of
* RelativeAdapterBase by an additional pair-index for referring to pairs
* of cameras. Intended for special central multi-viewpoint or non-central
* relative viewpoint problems, allowing "camera-pair"-wise grouping of
* correspondences. Derived classes need to implement functionalities for
* deriving unique serialization of multi-indices.
*/
class RelativeMultiAdapterBase : public RelativeAdapterBase
{
protected:
using RelativeAdapterBase::_t12;
using RelativeAdapterBase::_R12;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** See parent-class */
RelativeMultiAdapterBase() :
RelativeAdapterBase() {};
/** See parent-class */
RelativeMultiAdapterBase( const rotation_t & R12 ) :
RelativeAdapterBase( R12 ) {};
/** See parent-class */
RelativeMultiAdapterBase( const translation_t & t12, const rotation_t & R12 ) :
RelativeAdapterBase( t12, R12 ) {};
/** See parent-class */
virtual ~RelativeMultiAdapterBase() {};
//camera-pair-wise access of correspondences
/**
* \brief Retrieve the bearing vector of a correspondence in camera 1 of a pair.
* \param[in] pairIndex Index of the camera-pair.
* \param[in] correspondenceIndex Index of the correspondence in the camera-pair.
* \return The corresponding bearing vector.
*/
virtual bearingVector_t getBearingVector1(
size_t pairIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the bearing vector of a correspondence in camera 2 of a pair.
* \param[in] pairIndex Index of the camera-pair.
* \param[in] correspondenceIndex Index of the correspondence in the camera-pair.
* \return The corresponding bearing vector.
*/
virtual bearingVector_t getBearingVector2(
size_t pairIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the weight of a correspondence. The weight is supposed to
* reflect the quality of a correspondence, and typically is between
* 0 and 1.
* \param[in] pairIndex Index of the camera-pair.
* \param[in] correspondenceIndex Index of the correspondence in the camera-pair.
* \return The corresponding weight.
*/
virtual double getWeight(
size_t pairIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Retrieve the position of the cameras of a camera-pair seen from the
* origin of the viewpoints (assumed to be the same in both
* viewpoints).
* \param[in] pairIndex Index of the camera-pair.
* \return The position of the camera seen from the viewpoint origin.
*/
virtual translation_t getCamOffset( size_t pairIndex ) const = 0;
/**
* \brief Retrieve the rotation from the cameras of a camera-pair back to the
* origin of the viewpoints (assumed to be the same in both
* viewpoints).
* \param[in] pairIndex Index of the camera-pair.
* \return The rotation from the camera back to the viewpoint origin.
*/
virtual rotation_t getCamRotation( size_t pairIndex ) const = 0;
/**
* \brief Retrieve the number of correspondences for a camera-pair.
* \param[in] pairIndex Index of the camera-pair.
* \return The number of correspondences in this camera-pair.
*/
virtual size_t getNumberCorrespondences( size_t pairIndex ) const = 0;
/**
* \brief Retrieve the number of camera-pairs.
* \return The number of camera-pairs.
*/
virtual size_t getNumberPairs() const = 0;
//Conversion to and from serialized indices
/**
* \brief Convert an array of (pairIndex,correspondenceIndex)-pairs into an
* array of serialized indices.
* \param[in] multiIndices Array of (pairIndex,correspondenceIndex)-pairs.
* \return Array of single serialized indices referring uniquely to
* (pairIndex,correspondenceIndex)-pairs.
*/
virtual std::vector<int> convertMultiIndices(
const std::vector<std::vector<int> > & multiIndices ) const = 0;
/**
* \brief Convert a (pairIndex,correspondenceIndex)-pair into a serialized index.
* \param[in] pairIndex The index of the camera-pair.
* \param[in] correspondenceIndex The index of the keypoint in the camera-pair.
* \return Array of single serialized indices referring uniquely to
* (pairIndex,correspondenceIndex)-pairs.
*/
virtual int convertMultiIndex(
size_t pairIndex, size_t correspondenceIndex ) const = 0;
/**
* \brief Get the camera-pair-index corresponding to a serialized index.
* \param[in] index The serialized index.
* \return The camera-pair index.
*/
virtual int multiPairIndex( size_t index ) const = 0;
/**
* \brief Get the keypoint-index in a camera-pair for a serialized index.
* \param[in] index The serialized index.
* \return The keyopint-index in the camera-pair.
*/
virtual int multiCorrespondenceIndex( size_t index ) const = 0;
//the classic interface (with serialized indices, used by the opengv-methods)
/** See parent-class (no need to overload) */
virtual bearingVector_t getBearingVector1( size_t index ) const
{
return getBearingVector1(
multiPairIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual bearingVector_t getBearingVector2( size_t index ) const
{
return getBearingVector2(
multiPairIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual double getWeight( size_t index ) const
{
return getWeight(
multiPairIndex(index), multiCorrespondenceIndex(index) );
}
/** See parent-class (no need to overload) */
virtual translation_t getCamOffset1( size_t index ) const
{ return getCamOffset( multiPairIndex(index) ); }
/** See parent-class (no need to overload) */
virtual rotation_t getCamRotation1( size_t index ) const
{ return getCamRotation( multiPairIndex(index) ); }
/** See parent-class (no need to overload) */
virtual translation_t getCamOffset2( size_t index ) const
{ return getCamOffset( multiPairIndex(index) ); }
/** See parent-class (no need to overload) */
virtual rotation_t getCamRotation2( size_t index ) const
{ return getCamRotation( multiPairIndex(index) ); }
/** See parent-class (no need to overload) */
virtual size_t getNumberCorrespondences() const
{
size_t numberCorrespondences = 0;
for(size_t i = 0; i < getNumberPairs(); i++)
numberCorrespondences += getNumberCorrespondences(i);
return numberCorrespondences;
}
};
}
}
#endif /* OPENGV_RELATIVE_POSE_RELATIVEMULTIADAPTERBASE_HPP_ */

507
thirdparty/opengv/include/opengv/relative_pose/methods.hpp vendored Normal file
View File

@@ -0,0 +1,507 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file relative_pose/methods.hpp
* \brief A collection of methods for computing the relative pose between two
* calibrated central or non-central viewpoints.
*/
#ifndef OPENGV_RELATIVE_POSE_METHODS_HPP_
#define OPENGV_RELATIVE_POSE_METHODS_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* \brief Compute the translation between two central viewpoints with known
* relative rotation, and using two correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences and known
* relative rotation.
* \param[in] unrotate Set to true if known rotation should be used to
* unrotate bearing vectors from viewpoint 2 (not required
* in case of identity rotation).
* \param[in] indices Indices of the two correspondences used for deriving the
* translation.
* \return Position of viewpoint 2 seen from viewpoint 1.
*/
translation_t twopt(
const RelativeAdapterBase & adapter,
bool unrotate,
const std::vector<int> & indices );
/**
* \brief Compute the translation between two central viewpoints with known
* relative rotation, and using two correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences and known
* relative rotation.
* \param[in] unrotate Set to true if known rotation should be used to
* unrotate bearing vectors from viewpoint 2 (not required
* in case of identity rotation).
* \param[in] index0 Index of the first correspondence used for deriving the
* translation (use default value if only two provided
* anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* translation (use default vector if only two provided
* anyway).
* \return Position of viewpoint 2 seen from viewpoint 1.
*/
translation_t twopt(
const RelativeAdapterBase & adapter,
bool unrotate,
size_t index0 = 0,
size_t index1 = 1 );
/**
* \brief Compute the rotation between two central viewpoints with pure
* rotation change, using only two correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the two correspondences used for deriving the
* rotation.
* \return Rotation from viewpoint 2 to viewpoint 1.
*/
rotation_t twopt_rotationOnly(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the rotation between two central viewpoints with pure
* rotation change, using only two correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] index0 Index of the first correspondence used for deriving the
* rotation (use default value if only two provided anyway).
* \param[in] index1 Index of the second correspondence used for deriving the
* rotation (use default value if only two provided anyway).
* \return Rotation from viewpoint 2 to viewpoint 1.
*/
rotation_t twopt_rotationOnly(
const RelativeAdapterBase & adapter,
size_t index0 = 0,
size_t index1 = 1 );
/**
* \brief Compute the rotation between two central viewpoints with pure
* rotation change using Arun's method [13]. Using all available
* correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \return Rotation from viewpoint 2 to viewpoint 1.
*/
rotation_t rotationOnly( const RelativeAdapterBase & adapter );
/**
* \brief Compute the rotation between two central viewpoints with pure
* rotation change using Arun's method [13].
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation.
* \return Rotation from viewpoint 2 to viewpoint 1.
*/
rotation_t rotationOnly(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the complex essential matrices between two central viewpoints
* using Stewenius' method [5]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \return Complex essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum of 10 solutions).
*/
complexEssentials_t fivept_stewenius( const RelativeAdapterBase & adapter );
/**
* \brief Compute the complex essential matrices between two central viewpoints
* using Stewenius' method [5].
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the essential matrices.
* \return Complex essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum of 10 solutions).
*/
complexEssentials_t fivept_stewenius(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the essential matrices between two central viewpoints using
* Nister's method [6]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \return Real essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum of 10 solutions).
*/
essentials_t fivept_nister( const RelativeAdapterBase & adapter );
/**
* \brief Compute the essential matrices between two central viewpoints using
* Nister's method [6].
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the essential matrices.
* \return Real essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum of 10 solutions).
*/
essentials_t fivept_nister(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the rotation matrices between two central viewpoints using
* Kneips's method [7]. Only minimal case.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrices.
* \return Rotation matrices from viewpoint 2 to viewpoint 1
* (maximum 20 solutions).
*/
rotations_t fivept_kneip(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the essential matrices between two central viewpoints using
* the seven-point algorithm [8]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \return Real essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum 3 solutions).
*/
essentials_t sevenpt( const RelativeAdapterBase & adapter );
/**
* \brief Compute the essential matrices between two central viewpoints using
* the seven-point algorithm [8].
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the essential matrices.
* \return Real essential matrices
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1, maximum 3 solutions).
*/
essentials_t sevenpt(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the essential matrix between two central viewpoints using
* the eight-point algorithm [9,10]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \return Real essential matrix
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1).
*/
essential_t eightpt( const RelativeAdapterBase & adapter );
/**
* \brief Compute the essential matrix between two central viewpoints using
* the eight-point algorithm [9,10].
*
* \param[in] adapter Visitor holding bearing-vector correspondences.
* \param[in] indices Indices of the correspondences used for deriving
* the essential matrix.
* \return Real essential matrix
* (\f$ \mathbf{E}= \f$ skew\f$(\mathbf{t})\mathbf{R}\f$, where
* \f$ \mathbf{t} \f$ is the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ is the rotation from viewpoint
* 2 back to viewpoint 1).
*/
essential_t eightpt(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the rotation matrix between two central viewpoints as an
* iterative eigenproblem [11]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* initial rotation for the optimization.
* \param[out] output Returns more complete information (position of viewpoint
* 2 seen from viewpoint 1, eigenvectors and eigenvalues)
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t eigensolver(
const RelativeAdapterBase & adapter,
eigensolverOutput_t & output,
bool useWeights = false );
/**
* \brief Compute the rotation matrix between two central viewpoints as an
* iterative eigenproblem [11].
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* initial rotation for the optimization.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrix.
* \param[out] output Returns more complete information (position of viewpoint
* 2 seen from viewpoint 1, eigenvectors and eigenvalues)
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t eigensolver(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices,
eigensolverOutput_t & output,
bool useWeights = false );
/**
* \brief Compute the rotation matrix between two central viewpoints as an
* iterative eigenproblem [11]. Using all available correspondences.
* Outputs only the rotation.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* initial rotation for the optimization.
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t eigensolver(
const RelativeAdapterBase & adapter,
bool useWeights = false );
/**
* \brief Compute the rotation matrix between two central viewpoints as an
* iterative eigenproblem [11]. Outputs only the rotation.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* initial rotation for the optimization.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrix.
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t eigensolver(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices,
bool useWeights = false );
/**
* \brief Compute the relative rotation between two non-central viewpoints
* following Stewenius' method [16]. Assuming exactly 6 correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* multi-camera configuration.
* \return Rotations from viewpoint 2 back to viewpoint 1
*/
rotations_t sixpt(
const RelativeAdapterBase & adapter );
/**
* \brief Compute the relative rotation between two non-central viewpoints
* following Stewenius' method using 6 correspondences [16].
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* multi-camera configuration.
* \param[in] indices Indices of the six correspondences used for deriving
* the rotation matrix.
* \return Rotations from viewpoint 2 back to viewpoint 1
*/
rotations_t sixpt(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the rotation matrix between two non-central viewpoints as an
* iterative eigenproblem. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the multi-
* camera configuration, plus the initial rotation for the
* optimization.
* \param[out] output Returns more complete information (position of viewpoint
* 2 seen from viewpoint 1, eigenvectors and eigenvalues)
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t ge(
const RelativeAdapterBase & adapter,
geOutput_t & output,
bool useWeights = false );
/**
* \brief Compute the rotation matrix between two non-central viewpoints as an
* iterative eigenproblem.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the multi-
* camera configuration, plus the initial rotation for the
* optimization.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrix.
* \param[out] output Returns more complete information (position of viewpoint
* 2 seen from viewpoint 1, eigenvectors and eigenvalues)
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t ge(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices,
geOutput_t & output,
bool useWeights = false );
/**
* \brief Compute the rotation matrix between two non-central viewpoints as an
* iterative eigenproblem. Using all available correspondences.
* Outputs only the rotation.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the multi-
* camera configuration, plus the initial rotation for the
* optimization.
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t ge( const RelativeAdapterBase & adapter, bool useWeights = false );
/**
* \brief Compute the rotation matrix between two non-central viewpoints as an
* iterative eigenproblem. Outputs only the rotation.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the multi-
* camera configuration, plus the initial rotation for the
* optimization.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrix.
* \param[in] useWeights Use weights to weight the summation terms?
* \return Rotation matrix from viewpoint 2 to viewpoint 1.
*/
rotation_t ge(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices,
bool useWeights = false );
/**
* \brief Compute the relative pose between two non-central viewpoints
* following Li's method [12]. Using all available correspondences.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* multi-camera configuration.
* \return Pose of viewpoint 2 seen from viewpoint 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ being the rotation from
* viewpoint 2 to viewpoint 1).
*/
transformation_t seventeenpt( const RelativeAdapterBase & adapter );
/**
* \brief Compute the relative pose between two non-central viewpoints
* following Li's method [12].
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* multi-camera configuration.
* \param[in] indices Indices of the correspondences used for deriving
* the rotation matrix.
* \return Pose of viewpoint 2 seen from viewpoint 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ being the rotation from
* viewpoint 2 to viewpoint 1).
*/
transformation_t seventeenpt(
const RelativeAdapterBase & adapter,
const std::vector<int> & indices );
/**
* \brief Compute the pose between two viewpoints using nonlinear optimization.
* Using all available correspondences. Works for both central and
* non-central case.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the
* multi-camera configuration, plus the initial values.
* \return Pose of viewpoint 2 seen from viewpoint 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ being the rotation from
* viewpoint 2 to viewpoint 1).
*/
transformation_t optimize_nonlinear( RelativeAdapterBase & adapter );
/**
* \brief Compute the pose between two viewpoints using nonlinear optimization.
* Works for both central and non-central case.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, the
* multi-camera configuration, plus the initial values.
* \param[in] indices Indices of the correspondences used for deriving the pose.
* \return Pose of viewpoint 2 seen from viewpoint 1 (
* \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
* with \f$ \mathbf{t} \f$ being the position of viewpoint 2 seen from
* viewpoint 1, and \f$ \mathbf{R} \f$ being the rotation from
* viewpoint 2 to viewpoint 1).
*/
transformation_t optimize_nonlinear(
RelativeAdapterBase & adapter,
const std::vector<int> & indices );
}
}
#endif /* OPENGV_RELATIVE_POSE_METHODS_HPP_ */

94
thirdparty/opengv/include/opengv/relative_pose/modules/eigensolver/modules.hpp vendored Normal file
View File

@@ -0,0 +1,94 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace eigensolver
{
double getSmallestEV(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const cayley_t & cayley,
Eigen::Matrix3d & M );
double getSmallestEVwithJacobian(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const cayley_t & cayley,
Eigen::Matrix<double,1,3> & jacobian);
Eigen::Matrix3d composeM(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const cayley_t & cayley);
Eigen::Matrix3d composeMwithJacobians(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const cayley_t & cayley,
Eigen::Matrix3d & M_jac1,
Eigen::Matrix3d & M_jac2,
Eigen::Matrix3d & M_jac3 );
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_EIGENSOLVER_MODULES_HPP_ */

161
thirdparty/opengv/include/opengv/relative_pose/modules/fivept_kneip/modules.hpp vendored Normal file
View File

@@ -0,0 +1,161 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_FIVEPT_KNEIP_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_FIVEPT_KNEIP_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <vector>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace fivept_kneip
{
Eigen::Matrix<double,1,197> initEpncpRowR(
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > & c123_1,
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > & c123_2);
void initMatrix( Eigen::Matrix<double,66,197> & groebnerMatrix );
void computeBasis( Eigen::Matrix<double,66,197> & groebnerMatrix );
void sPolynomial30( Eigen::Matrix<double,66,197> & groebnerMatrix );
void sPolynomial31( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow30_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial32( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow31_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial33( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow32_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial34( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow33_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial35( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow34_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial36( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow35_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial37( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow36_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial38( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow37_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial39( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow38_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial40( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow38_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow39_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow39_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial41( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow40_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow33_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow34_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow35_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow36_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow37_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial42( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow41_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow32_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial43( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow42_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial44( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow43_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow31_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial45( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow44_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow30_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial46( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow45_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial47( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow46_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial48( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow47_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial49( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow48_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial50( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow49_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial51( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow50_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial52( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow32_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow33_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow51_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial53( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow52_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial54( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow30_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow31_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow53_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial55( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow39_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow54_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial56( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow38_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow55_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial57( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow37_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow56_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial58( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow36_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow57_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial59( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow35_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow58_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial60( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow34_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow59_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial61( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow33_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow60_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial62( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow61_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow61_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow61_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial63( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow32_000100000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow62_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow62_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow62_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial64( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow63_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow63_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void sPolynomial65( Eigen::Matrix<double,66,197> & groebnerMatrix );
void groebnerRow63_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow64_010000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow64_100000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
void groebnerRow64_000000000_f( Eigen::Matrix<double,66,197> & groebnerMatrix, int targetRow );
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_KNEIP_MODULES_HPP_ */

82
thirdparty/opengv/include/opengv/relative_pose/modules/fivept_nister/modules.hpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace fivept_nister
{
void composeA(
const Eigen::Matrix<double,9,4> & EE,
Eigen::Matrix<double,10,20> & A);
double polyVal(const Eigen::MatrixXd & p, double x);
void computeSeventhOrderPolynomial(
const Eigen::Matrix<double,1,5> & p1,
const Eigen::Matrix<double,1,4> & p2,
Eigen::Matrix<double,1,8> & p_out );
void computeSixthOrderPolynomial(
const Eigen::Matrix<double,1,4> & p1,
const Eigen::Matrix<double,1,4> & p2,
Eigen::Matrix<double,1,7> & p_out );
void computeTenthOrderPolynomialFrom73(
const Eigen::Matrix<double,1,8> & p1,
const Eigen::Matrix<double,1,4> & p2,
Eigen::Matrix<double,1,11> & p_out );
void computeTenthOrderPolynomialFrom64(
const Eigen::Matrix<double,1,7> & p1,
const Eigen::Matrix<double,1,5> & p2,
Eigen::Matrix<double,1,11> & p_out );
void pollishCoefficients(
const Eigen::Matrix<double,10,20> & A,
double & x,
double & y,
double & z);
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_NISTER_MODULES_HPP_ */

60
thirdparty/opengv/include/opengv/relative_pose/modules/fivept_stewenius/modules.hpp vendored Normal file
View File

@@ -0,0 +1,60 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace fivept_stewenius
{
void composeA(
const Eigen::Matrix<double,9,4> & EE,
Eigen::Matrix<double,10,20> & A);
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ */

174
thirdparty/opengv/include/opengv/relative_pose/modules/ge/modules.hpp vendored Normal file
View File

@@ -0,0 +1,174 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_GE_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_GE_MODULES_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace ge
{
void getEV(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley,
Eigen::Vector4d & roots );
double getCost(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley,
int step );
double getCostWithJacobian(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley,
Eigen::Matrix<double,1,3> & jacobian,
int step );
void getQuickJacobian(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley,
double currentValue,
Eigen::Matrix<double,1,3> & jacobian,
int step );
Eigen::Matrix4d composeG(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley);
Eigen::Matrix4d composeGwithJacobians(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & cayley,
Eigen::Matrix4d & G_jac1,
Eigen::Matrix4d & G_jac2,
Eigen::Matrix4d & G_jac3 );
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_GE_MODULES_HPP_ */

128
thirdparty/opengv/include/opengv/relative_pose/modules/main.hpp vendored Normal file
View File

@@ -0,0 +1,128 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
void fivept_stewenius_main(
const Eigen::Matrix<double,9,4> & EE,
complexEssentials_t & complexEssentials );
void fivept_nister_main(
const Eigen::Matrix<double,9,4> & EE,
essentials_t & essentials );
void fivept_kneip_main(
const Eigen::Matrix<double,3,5> & f1,
const Eigen::Matrix<double,3,5> & f2,
rotations_t & rotations );
void eigensolver_main(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
eigensolverOutput_t & output );
void sixpt_main(
Eigen::Matrix<double,6,6> & L1,
Eigen::Matrix<double,6,6> & L2,
rotations_t & solutions);
void ge_main(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & startingPoint,
geOutput_t & output );
void ge_main2(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
const cayley_t & startingPoint,
geOutput_t & output );
void ge_plot(
const Eigen::Matrix3d & xxF,
const Eigen::Matrix3d & yyF,
const Eigen::Matrix3d & zzF,
const Eigen::Matrix3d & xyF,
const Eigen::Matrix3d & yzF,
const Eigen::Matrix3d & zxF,
const Eigen::Matrix<double,3,9> & x1P,
const Eigen::Matrix<double,3,9> & y1P,
const Eigen::Matrix<double,3,9> & z1P,
const Eigen::Matrix<double,3,9> & x2P,
const Eigen::Matrix<double,3,9> & y2P,
const Eigen::Matrix<double,3,9> & z2P,
const Eigen::Matrix<double,9,9> & m11P,
const Eigen::Matrix<double,9,9> & m12P,
const Eigen::Matrix<double,9,9> & m22P,
geOutput_t & output );
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_ */

78
thirdparty/opengv/include/opengv/relative_pose/modules/sixpt/modules.hpp vendored Normal file
View File

@@ -0,0 +1,78 @@
/******************************************************************************
* 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. *
******************************************************************************/
#ifndef OPENGV_RELATIVE_POSE_MODULES_SIXPT_MODULES_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_SIXPT_MODULES_HPP_
#define EQS 60
#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>
namespace opengv
{
namespace relative_pose
{
namespace modules
{
namespace sixpt
{
//my solver
void fillRow1(
const Eigen::Matrix<double,6,1> & l01,
const Eigen::Matrix<double,6,1> & l02,
const Eigen::Matrix<double,6,1> & l11,
const Eigen::Matrix<double,6,1> & l12,
std::vector<double> & c0,
std::vector<double> & c1,
std::vector<double> & c2 );
void fillRow2(
Eigen::Matrix<double,EQS,84> & M1,
int row,
const std::vector<double> (&m0)[3],
const std::vector<double> (&m1)[3],
const std::vector<double> (&m2)[3] );
void setupAction(
const std::vector<Eigen::Matrix<double,6,1>,Eigen::aligned_allocator< Eigen::Matrix<double,6,1> > > & L1,
const std::vector<Eigen::Matrix<double,6,1>,Eigen::aligned_allocator< Eigen::Matrix<double,6,1> > > & L2,
Eigen::Matrix<double,64,64> & Action );
}
}
}
}
#endif /* OPENGV_RELATIVE_POSE_MODULES_FIVEPT_STEWENIUS_MODULES_HPP_ */

99
thirdparty/opengv/include/opengv/sac/Lmeds.hpp vendored Normal file
View File

@@ -0,0 +1,99 @@
/******************************************************************************
* Authors: Johannes Mikulasch *
* 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. *
******************************************************************************/
//Note: has been derived from Ransac which has been derived from ROS
/**
* \file Lmeds.hpp
* \brief Implementation of the Lmeds algorithm
*/
#ifndef OPENGV_SAC_LMEDS_HPP_
#define OPENGV_SAC_LMEDS_HPP_
#include <vector>
#include <opengv/sac/SampleConsensus.hpp>
#include <cstdio>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* The LMedS (Least Median of Squares) sample consensus method
*/
template<typename PROBLEM_T>
class Lmeds : public SampleConsensus<PROBLEM_T>
{
public:
/** A child of SampleConsensusProblem */
typedef PROBLEM_T problem_t;
/** The model we trying to fit */
typedef typename problem_t::model_t model_t;
using SampleConsensus<problem_t>::max_iterations_;
using SampleConsensus<problem_t>::threshold_;
using SampleConsensus<problem_t>::iterations_;
using SampleConsensus<problem_t>::sac_model_;
using SampleConsensus<problem_t>::model_;
using SampleConsensus<problem_t>::model_coefficients_;
using SampleConsensus<problem_t>::inliers_;
using SampleConsensus<problem_t>::probability_;
/**
* \brief Constructor.
*/
Lmeds(
int maxIterations = 1000,
double threshold = 1.0,
double probability = 0.99);
/**
* \brief Destructor.
*/
virtual ~Lmeds();
/**
* \brief Fit the model.
*/
bool computeModel( int debug_verbosity_level = 0 );
};
} // namespace sac
} // namespace opengv
#include "implementation/Lmeds.hpp"
#endif /* OPENGV_SAC_LMEDS_HPP_ */

104
thirdparty/opengv/include/opengv/sac/MultiRansac.hpp vendored Normal file
View File

@@ -0,0 +1,104 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file MultiRansac.hpp
* \brief Implementation of the Ransac algorithm as outlined in [15]. This
* version is intended for use with the RelativeMultiAdapterBase, and
* attempts to do sampling in multiple camera-pairs in each hypothesis
* instantiation.
*/
#ifndef OPENGV_SAC_MULTIRANSAC_HPP_
#define OPENGV_SAC_MULTIRANSAC_HPP_
#include <vector>
#include <opengv/sac/MultiSampleConsensus.hpp>
#include <cstdio>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* The Ransac sample consensus method, as outlined in [15]. This one is using
* multi-indices for homogeneous sampling over groups of samples.
*/
template<typename PROBLEM_T>
class MultiRansac : public MultiSampleConsensus<PROBLEM_T>
{
public:
/** A child of MultiSampleConsensusProblem */
typedef PROBLEM_T problem_t;
/** The model we trying to fit */
typedef typename problem_t::model_t model_t;
using MultiSampleConsensus<problem_t>::max_iterations_;
using MultiSampleConsensus<problem_t>::threshold_;
using MultiSampleConsensus<problem_t>::iterations_;
using MultiSampleConsensus<problem_t>::sac_model_;
using MultiSampleConsensus<problem_t>::model_;
using MultiSampleConsensus<problem_t>::model_coefficients_;
using MultiSampleConsensus<problem_t>::inliers_;
using MultiSampleConsensus<problem_t>::probability_;
/**
* \brief Constructor.
*/
MultiRansac(
int maxIterations = 1000,
double threshold = 1.0,
double probability = 0.99 );
/**
* \brief Destructor.
*/
virtual ~MultiRansac();
/**
* \brief Fit the model.
*/
bool computeModel( int debug_verbosity_level = 0 );
};
} // namespace sac
} // namespace opengv
#include "implementation/MultiRansac.hpp"
#endif /* OPENGV_SAC_MULTIRANSAC_HPP_ */

120
thirdparty/opengv/include/opengv/sac/MultiSampleConsensus.hpp vendored Normal file
View File

@@ -0,0 +1,120 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file MultiSampleConsensus.hpp
* \brief This is a base class for sample consensus methods such as Ransac.
* Derivatives call the three basic functions of a sample-consensus
* problem (sample drawing, computation of a hypothesis, and verification
* of a hypothesis). This version is intended for use with the
* RelativeMultiAdapterBase, and attempts to do sampling in multiple
* camera-pairs in each hypothesis instantiation.
*/
#ifndef OPENGV_SAC_MULTISAMPLECONSENSUS_HPP_
#define OPENGV_SAC_MULTISAMPLECONSENSUS_HPP_
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* Super-class for sample consensus methods, such as Ransac. This one is using
* multi-indices for homogeneous sampling over groups of samples.
*/
template<typename PROBLEM_T>
class MultiSampleConsensus
{
public:
/** A child of MultiSampleConsensusProblem */
typedef PROBLEM_T problem_t;
/** The model we trying to fit */
typedef typename problem_t::model_t model_t;
/**
* \brief Constructor.
* \param[in] maxIterations The maximum number of hypothesis generations
* \param[in] threshold Some threshold value for classifying samples as
* an inlier or an outlier.
* \param[in] probability The probability of being able to draw at least one
* sample that is free of outliers (see [15])
*/
MultiSampleConsensus(
int maxIterations = 1000,
double threshold = 1.0,
double probability = 0.99 );
/**
* \brief Destructor
*/
virtual ~MultiSampleConsensus();
/**
* \brief Fit the model to the data.
* \param[in] debug_verbosity_level Sets the verbosity level.
* \return bool True if success.
*/
virtual bool computeModel( int debug_verbosity_level = 0 ) = 0;
// \todo accessors
//private:
/** the maximum number of iterations */
int max_iterations_;
/** the current number of iterations */
int iterations_;
/** the threshold for classifying inliers */
double threshold_;
/** the current probability (defines remaining iterations) */
double probability_;
/** the currently best model coefficients */
model_t model_coefficients_;
/** the group-wise indices for the currently best hypothesis */
std::vector< std::vector<int> > model_;
/** the group-wise indices of the samples that have been clasified as inliers */
std::vector< std::vector<int> > inliers_;
/** the multi-sample-consensus problem we are trying to solve */
std::shared_ptr<PROBLEM_T> sac_model_;
};
} // namespace sac
} // namespace opengv
#include "implementation/MultiSampleConsensus.hpp"
#endif /* OPENGV_MULTISAMPLECONSENSUS_HPP_ */

245
thirdparty/opengv/include/opengv/sac/MultiSampleConsensusProblem.hpp vendored Normal file
View File

@@ -0,0 +1,245 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file MultiSampleConsensusProblem.hpp
* \brief Basis-class for Sample-consensus problems. Contains declarations for
* the three basic functions of a sample-consensus problem (sample
* drawing, computation of a hypothesis, and verification of a
* hypothesis). This version is intended for use with the
* RelativeMultiAdapterBase, and attempts to do sampling in multiple
* camera-pairs in each hypothesis instantiation.
*/
#ifndef OPENGV_SAC_MULTISAMPLECONSENSUSPROBLEM_HPP_
#define OPENGV_SAC_MULTISAMPLECONSENSUSPROBLEM_HPP_
#include <stdlib.h>
#include <stdio.h>
#include <memory>
#include <functional>
#include <random>
#include <ctime>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* Basis-class for Sample-consensus problems containing the three basic
* functions for model-fitting. This one is using multi-indices for homogeneous
* sampling over groups of samples.
*/
template<typename MODEL_T>
class MultiSampleConsensusProblem
{
public:
/** The model we are trying to fit */
typedef MODEL_T model_t;
/**
* \brief Contructor.
* \param[in] randomSeed Setting the seed of the random number generator with
* something unique, namely the current time.
*/
MultiSampleConsensusProblem( bool randomSeed = true );
/**
* \brief Destructor.
*/
virtual ~MultiSampleConsensusProblem();
/**
* \brief Get samples for hypothesis generation.
* \param[in] iterations We won't try forever to get a good sample, this
* parameter keeps track of the iterations.
* \param[out] samples The multi-indices of the samples we attempt to use.
*/
virtual void getSamples(
int &iterations,
std::vector< std::vector<int> > &samples );
/**
* \brief Check if a set of samples for model generation is degenerate
* \param[in] sample The multi-indices of the samples we attempt to use for
* model instantiation.
* \return Is this set of samples ok?
*/
virtual bool isSampleGood(
const std::vector< std::vector<int> > & sample ) const;
/**
* \brief Get a pointer to the vector of multi-indices used.
* \return A pointer to the vector of multi-indices used.
*/
std::shared_ptr< std::vector< std::vector<int> > > getIndices() const;
/**
* \brief Sub-function for getting samples for hypothesis generation.
* \param[out] sample The multi-indices of the samples we attempt to use.
*/
void drawIndexSample( std::vector< std::vector<int> > & sample );
/**
* \brief Get the number of samples needed for a hypothesis generation.
* Needs implementation in the child class.
* \return The number of samples in each group needed for hypothesis generation.
*/
virtual std::vector<int> getSampleSizes() const = 0;
/**
* \brief Compute a model from a set of samples. Needs implementation in the
* child-class.
* \param[in] indices The multi-indices of the samples we use for the hypothesis.
* \param[out] outModel The computed model.
* \return Success?
*/
virtual bool computeModelCoefficients(
const std::vector< std::vector<int> > & indices,
model_t & outModel) const = 0;
/**
* \brief Refine the model coefficients over a given set (inliers). Needs
* implementation in the child-class.
* \param[in] inliers The multi-indices of the inlier samples supporting the model.
* \param[in] model_coefficients The initial guess for the model coefficients.
* \param[out] optimized_coefficients The resultant refined coefficients.
*/
virtual void optimizeModelCoefficients(
const std::vector< std::vector<int> > & inliers,
const model_t & model_coefficients,
model_t & optimized_coefficients ) = 0;
/**
* \brief Compute the distances of all samples whith respect to given model
* coefficients. Needs implementation in the child-class.
* \param[in] model The coefficients of the model hypothesis.
* \param[in] indices The multi-indices of the samples of which we compute distances.
* \param[out] scores The resultant distances of the selected samples. Low
* distances mean a good fit.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector< std::vector<int> > & indices,
std::vector< std::vector<double> > & scores ) const = 0;
/**
* \brief Compute the distances of all samples which respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[out] distances The resultant distances of all samples. Low distances
* mean a good fit.
*/
virtual void getDistancesToModel(
const model_t & model_coefficients,
std::vector< std::vector<double> > &distances );
/**
* \brief Select all the inlier samples whith respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[in] threshold A maximum admissible distance threshold for
* determining the inliers and outliers.
* \param[out] inliers The resultant multi-indices of inlier samples.
*/
virtual void selectWithinDistance(
const model_t &model_coefficients,
const double threshold,
std::vector< std::vector<int> > &inliers );
/**
* \brief Count all the inlier samples whith respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[in] threshold A maximum admissible distance threshold for
* determining the inliers and outliers.
* \return The resultant number of inliers
*/
virtual int countWithinDistance(
const model_t &model_coefficients,
const double threshold);
/**
* \brief Set the indices_ variable (see member-description).
* \param[in] indices The multi-indices we want to use.
*/
void setIndices(const std::vector< std::vector<int> > & indices);
/**
* \brief Use this method if you want to use all samples.
* \param[in] N The number of samples in each group.
*/
void setUniformIndices(std::vector<int> N);
/**
* \brief Get a random number.
* \return A random number.
*/
int rnd();
/** The maximum number of times we try to extract a valid set of samples */
int max_sample_checks_;
/** The multi-indices of the samples we are using for solving the entire
* problem. These are not the multi-indices for generating a hypothesis, but
* all indices for model verification
*/
std::shared_ptr< std::vector< std::vector<int> > > indices_;
/** A shuffled version of the multi-indices used for random sample drawing */
std::vector< std::vector<int> > shuffled_indices_;
/** \brief std-based random number generator algorithm. */
std::mt19937 rng_alg_;
/** \brief std-based random number generator distribution. */
std::shared_ptr< std::uniform_int_distribution<> > rng_dist_;
/** \brief std-based random number generator. */
std::shared_ptr< std::function<int()> > rng_gen_;
};
} // namespace sac
} // namespace opengv
#include "implementation/MultiSampleConsensusProblem.hpp"
#endif /* OPENGV_SAC_MULTISAMPLECONSENSUS_PROBLEM_HPP_ */

100
thirdparty/opengv/include/opengv/sac/Ransac.hpp vendored Normal file
View File

@@ -0,0 +1,100 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file Ransac.hpp
* \brief Implementation of the Ransac algorithm as outlined in [15].
*/
#ifndef OPENGV_SAC_RANSAC_HPP_
#define OPENGV_SAC_RANSAC_HPP_
#include <vector>
#include <opengv/sac/SampleConsensus.hpp>
#include <cstdio>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* The Ransac sample consensus method, as outlined in [15].
*/
template<typename PROBLEM_T>
class Ransac : public SampleConsensus<PROBLEM_T>
{
public:
/** A child of SampleConsensusProblem */
typedef PROBLEM_T problem_t;
/** The model we trying to fit */
typedef typename problem_t::model_t model_t;
using SampleConsensus<problem_t>::max_iterations_;
using SampleConsensus<problem_t>::threshold_;
using SampleConsensus<problem_t>::iterations_;
using SampleConsensus<problem_t>::sac_model_;
using SampleConsensus<problem_t>::model_;
using SampleConsensus<problem_t>::model_coefficients_;
using SampleConsensus<problem_t>::inliers_;
using SampleConsensus<problem_t>::probability_;
/**
* \brief Constructor.
*/
Ransac(
int maxIterations = 1000,
double threshold = 1.0,
double probability = 0.99 );
/**
* \brief Destructor.
*/
virtual ~Ransac();
/**
* \brief Fit the model.
*/
bool computeModel( int debug_verbosity_level = 0 );
};
} // namespace sac
} // namespace opengv
#include "implementation/Ransac.hpp"
#endif /* OPENGV_SAC_RANSAC_HPP_ */

121
thirdparty/opengv/include/opengv/sac/SampleConsensus.hpp vendored Normal file
View File

@@ -0,0 +1,121 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file SampleConsensus.hpp
* \brief This is a base class for sample consensus methods such as Ransac.
* Derivatives call the three basic functions of a sample-consensus
* problem (sample drawing, computation of a hypothesis, and verification
* of a hypothesis).
*/
#ifndef OPENGV_SAC_SAMPLECONSENSUS_HPP_
#define OPENGV_SAC_SAMPLECONSENSUS_HPP_
#include <memory>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* Super-class for sample consensus methods, such as Ransac.
*/
template<typename PROBLEM_T>
class SampleConsensus
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** A child of SampleConsensusProblem */
typedef PROBLEM_T problem_t;
/** The model we trying to fit */
typedef typename problem_t::model_t model_t;
/**
* \brief Constructor.
* \param[in] maxIterations The maximum number of hypothesis generations
* \param[in] threshold Some threshold value for classifying samples as
* an inlier or an outlier.
* \param[in] probability The probability of being able to draw at least one
* sample that is free of outliers (see [15])
*/
SampleConsensus(
int maxIterations = 1000,
double threshold = 1.0,
double probability = 0.99 );
/**
* \brief Destructor
*/
virtual ~SampleConsensus();
/**
* \brief Fit the model to the data.
* \param[in] debug_verbosity_level Sets the verbosity level.
* \return bool True if success.
*/
virtual bool computeModel(
int debug_verbosity_level = 0 ) = 0;
// \todo accessors
//private:
/** the maximum number of iterations */
int max_iterations_;
/** the current number of iterations */
int iterations_;
/** the threshold for classifying inliers */
double threshold_;
/** the current probability (defines remaining iterations) */
double probability_;
/** the currently best model coefficients */
model_t model_coefficients_;
/** the indices for the currently best hypothesis */
std::vector<int> model_;
/** the indices of the samples that have been clasified as inliers */
std::vector<int> inliers_;
/** the sample-consensus problem we are trying to solve */
std::shared_ptr<PROBLEM_T> sac_model_;
};
} // namespace sac
} // namespace opengv
#include "implementation/SampleConsensus.hpp"
#endif /* OPENGV_SAC_SAMPLECONSENSUS_HPP_ */

239
thirdparty/opengv/include/opengv/sac/SampleConsensusProblem.hpp vendored Normal file
View File

@@ -0,0 +1,239 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
/**
* \file SampleConsensusProblem.hpp
* \brief Basis-class for Sample-consensus problems. Contains declarations for
* the three basic functions of a sample-consensus problem (sample
* drawing, computation of a hypothesis, and verification of a
* hypothesis).
*/
#ifndef OPENGV_SAC_SAMPLECONSENSUSPROBLEM_HPP_
#define OPENGV_SAC_SAMPLECONSENSUSPROBLEM_HPP_
#include <stdlib.h>
#include <stdio.h>
#include <random>
#include <functional>
#include <memory>
#include <ctime>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus methods.
*/
namespace sac
{
/**
* Basis-class for Sample-consensus problems containing the three basic
* functions for model-fitting.
*/
template<typename MODEL_T>
class SampleConsensusProblem
{
public:
/** The model we are trying to fit */
typedef MODEL_T model_t;
/**
* \brief Contructor.
* \param[in] randomSeed Setting the seed of the random number generator with
* something unique, namely the current time.
*/
SampleConsensusProblem( bool randomSeed = true );
/**
* \brief Destructor.
*/
virtual ~SampleConsensusProblem();
/**
* \brief Get samples for hypothesis generation.
* \param[in] iterations We won't try forever to get a good sample, this
* parameter keeps track of the iterations.
* \param[out] samples The indices of the samples we attempt to use.
*/
virtual void getSamples( int &iterations, std::vector<int> &samples );
/**
* \brief Check if a set of samples for model generation is degenerate
* \param[in] sample The indices of the samples we attempt to use for
* model instantiation.
* \return Is this set of samples ok?
*/
virtual bool isSampleGood( const std::vector<int> & sample ) const;
/**
* \brief Get a pointer to the vector of indices used.
* \return A pointer to the vector of indices used.
*/
std::shared_ptr< std::vector<int> > getIndices() const;
/**
* \brief Sub-function for getting samples for hypothesis generation.
* \param[out] sample The indices of the samples we attempt to use.
*/
void drawIndexSample( std::vector<int> & sample );
/**
* \brief Get the number of samples needed for a hypothesis generation.
* Needs implementation in the child class.
* \return The number of samples needed for hypothesis generation.
*/
virtual int getSampleSize() const = 0;
/**
* \brief Compute a model from a set of samples. Needs implementation in the
* child-class.
* \param[in] indices The indices of the samples we use for the hypothesis.
* \param[out] outModel The computed model.
* \return Success?
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const = 0;
/**
* \brief Refine the model coefficients over a given set (inliers). Needs
* implementation in the child-class.
* \param[in] inliers The indices of the inlier samples supporting the model.
* \param[in] model_coefficients The initial guess for the model coefficients.
* \param[out] optimized_coefficients The resultant refined coefficients.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model_coefficients,
model_t & optimized_coefficients ) = 0;
/**
* \brief Compute the distances of all samples whith respect to given model
* coefficients. Needs implementation in the child-class.
* \param[in] model The coefficients of the model hypothesis.
* \param[in] indices The indices of the samples of which we compute distances.
* \param[out] scores The resultant distances of the selected samples. Low
* distances mean a good fit.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores ) const = 0;
/**
* \brief Compute the distances of all samples which respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[out] distances The resultant distances of all samples. Low distances
* mean a good fit.
*/
virtual void getDistancesToModel(
const model_t & model_coefficients,
std::vector<double> &distances );
/**
* \brief Select all the inlier samples whith respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[in] threshold A maximum admissible distance threshold for
* determining the inliers and outliers.
* \param[out] inliers The resultant indices of inlier samples.
*/
virtual void selectWithinDistance(
const model_t &model_coefficients,
const double threshold,
std::vector<int> &inliers );
/**
* \brief Count all the inlier samples whith respect to given model
* coefficients.
* \param[in] model_coefficients The coefficients of the model hypothesis.
* \param[in] threshold A maximum admissible distance threshold for
* determining the inliers and outliers.
* \return The resultant number of inliers
*/
virtual int countWithinDistance(
const model_t &model_coefficients,
const double threshold );
/**
* \brief Set the indices_ variable (see member-description).
* \param[in] indices The indices we want to use.
*/
void setIndices( const std::vector<int> & indices );
/**
* \brief Use this method if you want to use all samples.
* \param[in] N The number of samples.
*/
void setUniformIndices( int N );
/**
* \brief Get a random number.
* \return A random number.
*/
int rnd();
/** The maximum number of times we try to extract a valid set of samples */
int max_sample_checks_;
/** The indices of the samples we are using for solving the entire
* problem. These are not the indices for generating a hypothesis, but
* all indices for model verification
*/
std::shared_ptr< std::vector<int> > indices_;
/** A shuffled version of the indices used for random sample drawing */
std::vector<int> shuffled_indices_;
/** \brief std-based random number generator algorithm. */
std::mt19937 rng_alg_;
/** \brief std-based random number generator distribution. */
std::shared_ptr< std::uniform_int_distribution<> > rng_dist_;
/** \brief std-based random number generator. */
std::shared_ptr< std::function<int()> > rng_gen_;
};
} // namespace sac
} // namespace opengv
#include "implementation/SampleConsensusProblem.hpp"
#endif /* OPENGV_SAC_SAMPLECONSENSUSPROBLEM_HPP_ */

195
thirdparty/opengv/include/opengv/sac/implementation/Lmeds.hpp vendored Normal file
View File

@@ -0,0 +1,195 @@
/******************************************************************************
* Authors: Johannes Mikulasch *
* 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. *
******************************************************************************/
//Note: has been derived from PCL and from Ransac.hpp which has been derived from ROS
template<typename P>
opengv::sac::Lmeds<P>::Lmeds(
int maxIterations, double threshold, double probability) :
SampleConsensus<P>(maxIterations, threshold, probability)
{}
template<typename P>
opengv::sac::Lmeds<P>::~Lmeds(){}
template<typename PROBLEM_T>
bool
opengv::sac::Lmeds<PROBLEM_T>::computeModel(int debug_verbosity_level)
{
typedef PROBLEM_T problem_t;
typedef typename problem_t::model_t model_t;
// Warn and exit if no threshold was set
if (threshold_ == std::numeric_limits<double>::max())
{
fprintf(stderr,"[sm::LeastMedianSquares::computeModel] No threshold set!\n");
return (false);
}
iterations_ = 0;
double d_best_penalty = std::numeric_limits<double>::max();
std::vector<int> best_model;
std::vector<int> selection;
model_t model_coefficients;
std::vector<double> distances;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// suppress infinite loops by just allowing 10 x maximum allowed iterations for
// invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
if(debug_verbosity_level > 1)
fprintf(stdout,
"[sm::LeastMedianSquares::computeModel] Starting Least Median of Squares\n"
"max_iterations: %d\n"
"max_skip: %d\n",
max_iterations_, max_skip);
// Iterate
while(iterations_ < max_iterations_ && skipped_count < max_skip)
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples(iterations_, selection);
if(selection.empty())
{
fprintf(stderr, "[sm::LeastMedianSquares::computeModel] No samples could be selected!\n");
break;
}
if(!sac_model_->computeModelCoefficients(selection, model_coefficients))
{
++ skipped_count;
continue;
}
double d_cur_penalty = 0;
// Iterate through the 3d points and calculate the distances from them to the model
distances.clear();
sac_model_->getDistancesToModel(model_coefficients, distances);
// No distances? The model must not respect the user given constraints
if (distances.empty ())
{
++skipped_count;
continue;
}
// Clip distances smaller than 0. Square the distances
for (std::size_t i = 0; i < distances.size(); ++i) {
if (distances[i] < 0) {
distances[i] = 0;
}
distances[i] = distances[i] * distances[i];
}
std::sort (distances.begin(), distances.end());
size_t mid = sac_model_->getIndices()->size() / 2;
if (mid >= distances.size())
{
++skipped_count;
continue;
}
// Do we have a "middle" point or should we "estimate" one ?
if (sac_model_->getIndices()->size() % 2 == 0) {
d_cur_penalty = (distances[mid-1] + distances[mid]) / 2;
} else {
d_cur_penalty = distances[mid];
}
// Better match ?
if(d_cur_penalty < d_best_penalty)
{
d_best_penalty = d_cur_penalty;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
}
++iterations_;
if(debug_verbosity_level > 1)
fprintf(stdout,
"[sm::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is: %f so far. Current penalty is: %f\n",
iterations_, max_iterations_, d_best_penalty, d_cur_penalty);
}
if(model_.empty())
{
if (debug_verbosity_level > 0)
fprintf(stdout,"[sm::LeastMedianSquares::computeModel] Unable to find a solution!\n");
return (false);
}
// Classify the data points into inliers and outliers
// Sigma = 1.4826 * (1 + 5 / (n-d)) * sqrt (M)
// @note: See "Robust Regression Methods for Computer Vision: A Review"
//double sigma = 1.4826 * (1 + 5 / (sac_model_->getIndices ()->size () - best_model.size ())) * sqrt (d_best_penalty);
//double threshold = 2.5 * sigma;
// Iterate through the 3d points and calculate the distances from them to the model again
distances.clear();
sac_model_->getDistancesToModel(model_coefficients_, distances);
// No distances? The model must not respect the user given constraints
if (distances.empty())
{
fprintf(stderr,"[sm::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n");
return (false);
}
std::vector<int> &indices = *sac_model_->getIndices();
if (distances.size () != indices.size ())
{
fprintf(stderr,"[sm::LeastMedianSquares::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ());
return false;
}
inliers_.resize(distances.size());
// Get the inliers for the best model found
n_inliers_count = 0;
for (size_t i = 0; i < distances.size(); ++i)
if (distances[i] <= threshold_)
inliers_[n_inliers_count++] = indices[i];
// Resize the inliers vector
inliers_.resize(n_inliers_count);
if (debug_verbosity_level > 0)
fprintf(stdout,"[sm::LeastMedianSquares::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count);
return (true);
}

159
thirdparty/opengv/include/opengv/sac/implementation/MultiRansac.hpp vendored Normal file
View File

@@ -0,0 +1,159 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
template<typename P>
opengv::sac::MultiRansac<P>::MultiRansac(
int maxIterations, double threshold, double probability) :
MultiSampleConsensus<P>(maxIterations, threshold, probability)
{}
template<typename P>
opengv::sac::MultiRansac<P>::~MultiRansac()
{}
template<typename PROBLEM_T>
bool
opengv::sac::MultiRansac<PROBLEM_T>::computeModel(
int debug_verbosity_level )
{
typedef PROBLEM_T problem_t;
typedef typename problem_t::model_t model_t;
iterations_ = 0;
int n_best_inliers_count = -INT_MAX;
double k = 1.0;
std::vector<std::vector<int> > selection;
model_t model_coefficients;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for
// invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while( iterations_ < k && skipped_count < max_skip )
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples( iterations_, selection );
if( selection.empty() )
{
fprintf( stderr,
"[sm::RandomSampleConsensus::computeModel] No samples could be selected!\n" );
break;
}
// Search for inliers in the point cloud for the current plane model M
if(!sac_model_->computeModelCoefficients( selection, model_coefficients ))
{
//++iterations_;
++ skipped_count;
continue;
}
// Select the inliers that are within threshold_ from the model
//sac_model_->selectWithinDistance(model_coefficients, threshold_, inliers);
//if(inliers.empty() && k > 1.0)
// continue;
n_inliers_count = sac_model_->countWithinDistance( model_coefficients, threshold_ );
// Better match ?
if( n_inliers_count > n_best_inliers_count )
{
n_best_inliers_count = n_inliers_count;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
//MultiRansac preparation for probability computation
std::shared_ptr<std::vector< std::vector<int> > > multiIndices =
sac_model_->getIndices();
size_t multiIndicesNumber = 0;
for( size_t multiIter = 0; multiIter < multiIndices->size(); multiIter++ )
multiIndicesNumber += multiIndices->at(multiIter).size();
size_t multiSelectionSize = 0;
for( size_t multiIter = 0; multiIter < selection.size(); multiIter++ )
multiSelectionSize += selection[multiIter].size();
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_best_inliers_count) /
static_cast<double> (multiIndicesNumber);
double p_no_outliers = 1.0 - pow(w, static_cast<double> (multiSelectionSize));
p_no_outliers =
(std::max) (std::numeric_limits<double>::epsilon(), p_no_outliers);
// Avoid division by -Inf
p_no_outliers =
(std::min) (1.0 - std::numeric_limits<double>::epsilon(), p_no_outliers);
// Avoid division by 0.
k = log(1.0 - probability_) / log(p_no_outliers);
}
++iterations_;
if(debug_verbosity_level > 1)
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n",
iterations_, k, n_inliers_count, n_best_inliers_count );
if(iterations_ > max_iterations_)
{
if(debug_verbosity_level > 0)
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
break;
}
}
if(debug_verbosity_level > 0)
{
size_t multiModelSize = 0;
for( size_t modelIter = 0; modelIter < model_.size(); modelIter++ )
multiModelSize += model_[modelIter].size();
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n",
multiModelSize, n_best_inliers_count );
}
if(model_.empty())
{
inliers_.clear();
return (false);
}
// Get the set of inliers that correspond to the best model found so far
sac_model_->selectWithinDistance( model_coefficients_, threshold_, inliers_ );
return (true);
}

45
thirdparty/opengv/include/opengv/sac/implementation/MultiSampleConsensus.hpp vendored Normal file
View File

@@ -0,0 +1,45 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
template<typename P>
opengv::sac::MultiSampleConsensus<P>::MultiSampleConsensus(
int maxIterations,
double threshold,
double probability) :
max_iterations_(maxIterations),
threshold_(threshold),
probability_(probability)
{}
template<typename P>
opengv::sac::MultiSampleConsensus<P>::~MultiSampleConsensus()
{}

236
thirdparty/opengv/include/opengv/sac/implementation/MultiSampleConsensusProblem.hpp vendored Normal file
View File

@@ -0,0 +1,236 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
#include <ctime>
template<typename M>
opengv::sac::MultiSampleConsensusProblem<M>::MultiSampleConsensusProblem(
bool randomSeed) :
max_sample_checks_(10)
{
rng_dist_.reset(new std::uniform_int_distribution<>( 0, std::numeric_limits<int>::max () ));
// Create a random number generator object
if (randomSeed)
rng_alg_.seed(static_cast<unsigned>(time(0)) + static_cast<unsigned>(clock()) );
else
rng_alg_.seed(12345u);
rng_gen_.reset(new std::function<int()>(std::bind(*rng_dist_, rng_alg_)));
}
template<typename M>
opengv::sac::MultiSampleConsensusProblem<M>::~MultiSampleConsensusProblem()
{}
template<typename M>
bool
opengv::sac::MultiSampleConsensusProblem<M>::isSampleGood(
const std::vector< std::vector<int> > & sample) const
{
// Default implementation
return true;
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::drawIndexSample(
std::vector< std::vector<int> > & sample)
{
for( size_t subIter = 0; subIter < sample.size(); subIter++ )
{
size_t sample_size = sample[subIter].size();
size_t index_size = shuffled_indices_[subIter].size();
for( unsigned int i = 0; i < sample_size; ++i )
{
// The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly
// distributed and for small modulo elements, that does not matter
// (and nowadays, random number generators are good)
//std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
std::swap(
shuffled_indices_[subIter][i],
shuffled_indices_[subIter][i + (rnd() % (index_size - i))] );
}
std::copy(
shuffled_indices_[subIter].begin (),
shuffled_indices_[subIter].begin () + sample_size,
sample[subIter].begin () );
}
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::getSamples(
int &iterations, std::vector< std::vector<int> > &samples )
{
std::vector<int> sampleSizes = getSampleSizes();
samples.resize( sampleSizes.size() );
for( size_t subIter = 0; subIter < samples.size(); subIter++ )
{
// We're assuming that indices_ have already been set in the constructor
if( (*indices_)[subIter].size() < (size_t) sampleSizes[subIter] )
{
fprintf( stderr,
"[sm::SampleConsensusModel::getSamples] Can not select %d unique points out of %zu!\n",
sampleSizes[subIter], (*indices_)[subIter].size() );
// one of these will make it stop :)
samples.clear();
iterations = std::numeric_limits<int>::max();
return;
}
// Get a second point which is different than the first
samples[subIter].resize( sampleSizes[subIter] );
}
for( int iter = 0; iter < max_sample_checks_; ++iter )
{
drawIndexSample(samples);
// If it's a good sample, stop here
if (isSampleGood (samples))
return;
}
size_t multiSampleSize = 0;
for( size_t multiIter = 0; multiIter < samples.size(); multiIter++ )
multiSampleSize += samples[multiIter].size();
fprintf( stdout,
"[sm::SampleConsensusModel::getSamples] WARNING: Could not select %zu sample points in %d iterations!\n",
multiSampleSize, max_sample_checks_ );
samples.clear();
}
template<typename M>
std::shared_ptr< std::vector< std::vector<int> > >
opengv::sac::MultiSampleConsensusProblem<M>::getIndices() const
{
return indices_;
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::getDistancesToModel(
const model_t & model_coefficients,
std::vector<std::vector<double> > & distances )
{
getSelectedDistancesToModel( model_coefficients, *indices_, distances );
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::setUniformIndices(
std::vector<int> N)
{
indices_.reset( new std::vector< std::vector<int> >() );
indices_->resize(N.size());
for( size_t j = 0; j < N.size(); j++ )
{
(*indices_)[j].resize(N[j]);
for( int i = 0; i < N[j]; ++i )
(*indices_)[j][i] = i;
}
shuffled_indices_ = *indices_;
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::setIndices(
const std::vector< std::vector<int> > & indices )
{
indices_.reset( new std::vector<std::vector<int> >(indices) );
shuffled_indices_ = *indices_;
}
template<typename M>
int
opengv::sac::MultiSampleConsensusProblem<M>::rnd()
{
return ((*rng_gen_) ());
}
template<typename M>
void
opengv::sac::MultiSampleConsensusProblem<M>::selectWithinDistance(
const model_t & model_coefficients,
const double threshold,
std::vector<std::vector<int> > &inliers )
{
std::vector<std::vector<double> > dist;
dist.resize(indices_->size());
inliers.clear();
inliers.resize(indices_->size());
for( size_t j = 0; j < indices_->size(); j++ )
dist[j].reserve((*indices_)[j].size());
getDistancesToModel( model_coefficients, dist );
for( size_t j = 0; j < indices_->size(); j++ )
{
inliers[j].clear();
inliers[j].reserve((*indices_)[j].size());
for(size_t i = 0; i < dist[j].size(); ++i)
{
if( dist[j][i] < threshold )
inliers[j].push_back( (*indices_)[j][i] );
}
}
}
template<typename M>
int
opengv::sac::MultiSampleConsensusProblem<M>::countWithinDistance(
const model_t & model_coefficients, const double threshold )
{
std::vector< std::vector<double> > dist;
dist.resize(indices_->size());
for( size_t j = 0; j < indices_->size(); j++ )
dist[j].reserve((*indices_)[j].size());
getDistancesToModel( model_coefficients, dist );
int count = 0;
for( size_t j = 0; j < indices_->size(); j++ )
{
for( size_t i = 0; i < dist[j].size(); ++i )
{
if( dist[j][i] < threshold )
++count;
}
}
return count;
}

144
thirdparty/opengv/include/opengv/sac/implementation/Ransac.hpp vendored Normal file
View File

@@ -0,0 +1,144 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
template<typename P>
opengv::sac::Ransac<P>::Ransac(
int maxIterations, double threshold, double probability) :
SampleConsensus<P>(maxIterations, threshold, probability)
{}
template<typename P>
opengv::sac::Ransac<P>::~Ransac(){}
template<typename PROBLEM_T>
bool
opengv::sac::Ransac<PROBLEM_T>::computeModel(
int debug_verbosity_level)
{
typedef PROBLEM_T problem_t;
typedef typename problem_t::model_t model_t;
iterations_ = 0;
int n_best_inliers_count = -INT_MAX;
double k = 1.0;
std::vector<int> selection;
model_t model_coefficients;
int n_inliers_count = 0;
unsigned skipped_count = 0;
// supress infinite loops by just allowing 10 x maximum allowed iterations for
// invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;
// Iterate
while( iterations_ < k && skipped_count < max_skip )
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples( iterations_, selection );
if(selection.empty())
{
fprintf(stderr,
"[sm::RandomSampleConsensus::computeModel] No samples could be selected!\n");
break;
}
// Search for inliers in the point cloud for the current plane model M
if(!sac_model_->computeModelCoefficients( selection, model_coefficients ))
{
//++iterations_;
++ skipped_count;
continue;
}
// Select the inliers that are within threshold_ from the model
//sac_model_->selectWithinDistance( model_coefficients, threshold_, inliers );
//if(inliers.empty() && k > 1.0)
// continue;
n_inliers_count = sac_model_->countWithinDistance(
model_coefficients, threshold_ );
// Better match ?
if(n_inliers_count > n_best_inliers_count)
{
n_best_inliers_count = n_inliers_count;
// Save the current model/inlier/coefficients selection as being the best so far
model_ = selection;
model_coefficients_ = model_coefficients;
// Compute the k parameter (k=log(z)/log(1-w^n))
double w = static_cast<double> (n_best_inliers_count) /
static_cast<double> (sac_model_->getIndices()->size());
double p_no_outliers = 1.0 - pow(w, static_cast<double> (selection.size()));
p_no_outliers =
(std::max) (std::numeric_limits<double>::epsilon(), p_no_outliers);
// Avoid division by -Inf
p_no_outliers =
(std::min) (1.0 - std::numeric_limits<double>::epsilon(), p_no_outliers);
// Avoid division by 0.
k = log(1.0 - probability_) / log(p_no_outliers);
}
++iterations_;
if(debug_verbosity_level > 1)
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n",
iterations_, k, n_inliers_count, n_best_inliers_count );
if(iterations_ > max_iterations_)
{
if(debug_verbosity_level > 0)
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
break;
}
}
if(debug_verbosity_level > 0)
fprintf(stdout,
"[sm::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n",
model_.size(), n_best_inliers_count );
if(model_.empty())
{
inliers_.clear();
return (false);
}
// Get the set of inliers that correspond to the best model found so far
sac_model_->selectWithinDistance( model_coefficients_, threshold_, inliers_ );
return (true);
}

42
thirdparty/opengv/include/opengv/sac/implementation/SampleConsensus.hpp vendored Normal file
View File

@@ -0,0 +1,42 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
template<typename P>
opengv::sac::SampleConsensus<P>::SampleConsensus(
int maxIterations, double threshold, double probability) :
max_iterations_(maxIterations),
threshold_(threshold),
probability_(probability)
{}
template<typename P>
opengv::sac::SampleConsensus<P>::~SampleConsensus(){}

203
thirdparty/opengv/include/opengv/sac/implementation/SampleConsensusProblem.hpp vendored Normal file
View File

@@ -0,0 +1,203 @@
/******************************************************************************
* Authors: Laurent Kneip & Paul Furgale *
* 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. *
******************************************************************************/
//Note: has been derived from ROS
#include <functional>
#include <ctime>
template<typename M>
opengv::sac::SampleConsensusProblem<M>::SampleConsensusProblem(
bool randomSeed) :
max_sample_checks_(10)
{
rng_dist_.reset(new std::uniform_int_distribution<>( 0, std::numeric_limits<int>::max() ));
// Create a random number generator object
if(randomSeed)
rng_alg_.seed(static_cast<unsigned>(time(0)) + static_cast<unsigned>(clock()) );
else
rng_alg_.seed(12345u);
rng_gen_.reset(new std::function<int()>(std::bind(*rng_dist_, rng_alg_)));
}
template<typename M>
opengv::sac::SampleConsensusProblem<M>::~SampleConsensusProblem()
{}
template<typename M>
bool opengv::sac::SampleConsensusProblem<M>::isSampleGood(
const std::vector<int> & sample) const
{
// Default implementation
return true;
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::drawIndexSample(
std::vector<int> & sample)
{
size_t sample_size = sample.size();
size_t index_size = shuffled_indices_.size();
for( unsigned int i = 0; i < sample_size; ++i )
{
// The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly
// distributed and for small modulo elements, that does not matter
// (and nowadays, random number generators are good)
//std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
std::swap(
shuffled_indices_[i],
shuffled_indices_[i + (rnd() % (index_size - i))] );
}
std::copy(
shuffled_indices_.begin(),
shuffled_indices_.begin() + sample_size,
sample.begin() );
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::getSamples(
int &iterations, std::vector<int> &samples)
{
// We're assuming that indices_ have already been set in the constructor
if (indices_->size() < (size_t)getSampleSize())
{
fprintf( stderr,
"[sm::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
(size_t) getSampleSize(), indices_->size() );
// one of these will make it stop :)
samples.clear();
iterations = std::numeric_limits<int>::max();
return;
}
// Get a second point which is different than the first
samples.resize( getSampleSize() );
for( int iter = 0; iter < max_sample_checks_; ++iter )
{
drawIndexSample(samples);
// If it's a good sample, stop here
if(isSampleGood(samples))
return;
}
fprintf( stdout,
"[sm::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize(), max_sample_checks_ );
samples.clear();
}
template<typename M>
std::shared_ptr< std::vector<int> >
opengv::sac::SampleConsensusProblem<M>::getIndices() const
{
return indices_;
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::getDistancesToModel(
const model_t & model_coefficients, std::vector<double> & distances )
{
getSelectedDistancesToModel( model_coefficients, *indices_, distances );
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::setUniformIndices(int N)
{
indices_.reset( new std::vector<int>() );
indices_->resize(N);
for( int i = 0; i < N; ++i )
(*indices_)[i] = i;
shuffled_indices_ = *indices_;
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::setIndices(
const std::vector<int> & indices )
{
indices_.reset( new std::vector<int>(indices) );
shuffled_indices_ = *indices_;
}
template<typename M>
int
opengv::sac::SampleConsensusProblem<M>::rnd()
{
return ((*rng_gen_)());
}
template<typename M>
void
opengv::sac::SampleConsensusProblem<M>::selectWithinDistance(
const model_t & model_coefficients,
const double threshold,
std::vector<int> &inliers )
{
std::vector<double> dist;
dist.reserve(indices_->size());
getDistancesToModel( model_coefficients, dist );
inliers.clear();
inliers.reserve(indices_->size());
for( size_t i = 0; i < dist.size(); ++i )
{
if( dist[i] < threshold )
inliers.push_back( (*indices_)[i] );
}
}
template<typename M>
int
opengv::sac::SampleConsensusProblem<M>::countWithinDistance(
const model_t & model_coefficients, const double threshold)
{
std::vector<double> dist;
dist.reserve(indices_->size());
getDistancesToModel( model_coefficients, dist );
int count = 0;
for( size_t i = 0; i < dist.size(); ++i )
{
if( dist[i] < threshold )
++count;
}
return count;
}

168
thirdparty/opengv/include/opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,168 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file AbsolutePoseSacProblem.hpp
* \brief Functions for fitting an absolute-pose model to a set of
* bearing-vector-point correspondences, using different algorithms
* (central and non-central one). Used in a sample-consenus paradigm for
* rejecting outlier correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_ABSOLUTEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_ABSOLUTEPOSESACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Provides functions for fitting an absolute-pose model to a set of
* bearing-vector to point correspondences, using different algorithms (central
* and non-central ones). Used in a sample-consenus paradigm for rejecting
* outlier correspondences.
*/
class AbsolutePoseSacProblem :
public sac::SampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::absolute_pose::AbsoluteAdapterBase adapter_t;
/** The possible algorithms for solving this problem */
typedef enum Algorithm
{
TWOPT = 0, // central, with rotation prior
KNEIP = 1, // central [1]
GAO = 2, // central [2]
EPNP = 3, // central [4]
GP3P = 4 // non-central [3]
} algorithm_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vectors, world points, etc.
* \param[in] algorithm The algorithm we want to use.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
AbsolutePoseSacProblem(adapter_t & adapter, algorithm_t algorithm,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vectors, world points, etc.
* \param[in] algorithm The algorithm we want to use.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
AbsolutePoseSacProblem(
adapter_t & adapter,
algorithm_t algorithm,
const std::vector<int> & indices,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~AbsolutePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data */
adapter_t & _adapter;
/** The algorithm we are using */
algorithm_t _algorithm;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_ABSOLUTEPOSESACPROBLEM_HPP_

155
thirdparty/opengv/include/opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,155 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MultiNoncentralAbsolutePoseSacProblem.hpp
* \brief Functions for fitting an absolute-pose model to a set of
* bearing-vector-point correspondences, using different algorithms
* (central and non-central one). Used in a sample-consenus paradigm for
* rejecting outlier correspondences, which does homogeneous sampling
*/
#ifndef OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_MULTINONCENTRALABSOLUTEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_MULTINONCENTRALABSOLUTEPOSESACPROBLEM_HPP_
#include <opengv/sac/MultiSampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/absolute_pose/AbsoluteMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the absolute pose methods.
*/
namespace absolute_pose
{
/**
* Provides functions for fitting an absolute-pose model to a set of
* bearing-vector to point correspondences, using different algorithms (central
* and non-central ones). Used in a sample-consenus paradigm for rejecting
* outlier correspondences.
*/
class MultiNoncentralAbsolutePoseSacProblem :
public sac::MultiSampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::absolute_pose::AbsoluteMultiAdapterBase adapter_t;
//This multisacproblem uses either gp3p or p3p for finding the relative pose
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vectors, world points, etc.
*/
MultiNoncentralAbsolutePoseSacProblem(adapter_t & adapter, bool asCentral = false) :
sac::MultiSampleConsensusProblem<model_t> (),
_adapter(adapter),
_asCentral(asCentral)
{
std::vector<int> numberCorrespondences;
for(size_t i = 0; i < adapter.getNumberFrames(); i++)
numberCorrespondences.push_back(adapter.getNumberCorrespondences(i));
setUniformIndices(numberCorrespondences);
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vectors, world points, etc.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
*/
MultiNoncentralAbsolutePoseSacProblem(
adapter_t & adapter,
const std::vector<std::vector<int> > & indices,
bool asCentral = false ) :
sac::MultiSampleConsensusProblem<model_t> (),
_adapter(adapter),
_asCentral(asCentral)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~MultiNoncentralAbsolutePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector< std::vector<int> > & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<std::vector<int> > & indices,
std::vector<std::vector<double> > & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<std::vector<int> > & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual std::vector<int> getSampleSizes() const;
protected:
/** The adapter holding all input data */
adapter_t & _adapter;
/** Use the central algorithm? (only one camera?). */
bool _asCentral;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_ABSOLUTE_POSE_MULTINONCENTRALABSOLUTEPOSESACPROBLEM_HPP_

149
thirdparty/opengv/include/opengv/sac_problems/point_cloud/PointCloudSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,149 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file PointCloudSacProblem.hpp
* \brief Functions for fitting a transformation model between two frames
* containing point-clouds. Used in a random sample paradigm for
* rejecting outliers in the correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_POINT_CLOUD_POINTCLOUDSACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_POINT_CLOUD_POINTCLOUDSACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/point_cloud/PointCloudAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the point-cloud alignment methods.
*/
namespace point_cloud
{
/**
* Functions for fitting a transformation model between two frames containing
* point-clouds. Using treept_arun. Used in a random sample paradigm for
* rejecting outliers in the correspondences.
*/
class PointCloudSacProblem :
public sac::SampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::point_cloud::PointCloudAdapterBase adapter_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
PointCloudSacProblem(adapter_t & adapter, bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
PointCloudSacProblem(
adapter_t & adapter,
const std::vector<int> & indices,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~PointCloudSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_POINT_CLOUD_POINTCLOUDSACPROBLEM_HPP_

167
thirdparty/opengv/include/opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,167 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file CentralRelativePoseSacProblem.hpp
* \brief Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences, using different algorithms (central). Used with a
* random sample paradigm for rejecting outlier correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_CENTRALRELATIVEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_CENTRALRELATIVEPOSESACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Provides functions for fitting an absolute-pose model to a set of
* bearing-vector to point correspondences, using different algorithms (only
* central case). Used in a sample-consenus paradigm for rejecting
* outlier correspondences.
*/
class CentralRelativePoseSacProblem :
public sac::SampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeAdapterBase adapter_t;
/** The possible algorithms for solving this problem */
typedef enum Algorithm
{
STEWENIUS = 0, // [5]
NISTER = 1, // [6]
SEVENPT = 2, // [8]
EIGHTPT = 3 // [9,10]
} algorithm_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm we want to use.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
CentralRelativePoseSacProblem(adapter_t & adapter, algorithm_t algorithm,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm we want to use.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
CentralRelativePoseSacProblem(
adapter_t & adapter,
algorithm_t algorithm,
const std::vector<int> & indices,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~CentralRelativePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
/** The algorithm we are using. */
algorithm_t _algorithm;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_CENTRALRELATIVEPOSESACPROBLEM_HPP_

156
thirdparty/opengv/include/opengv/sac_problems/relative_pose/EigensolverSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,156 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file EigensolverSacProblem.hpp
* \brief Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences, using the eigensolver algorithm. Used with a random
* sample paradigm for rejecting outlier correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_EIGENSOLVERSACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_EIGENSOLVERSACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences, using the eigensolver algorithm [11]. Used with a random
* sample paradigm for rejecting outlier correspondences.
*/
class EigensolverSacProblem :
public sac::SampleConsensusProblem<eigensolverOutput_t>
{
public:
/** The model we are trying to fit (eigensolverOutput) */
typedef eigensolverOutput_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeAdapterBase adapter_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] sampleSize Number of correspondences used for generating hypotheses.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
EigensolverSacProblem(adapter_t & adapter, size_t sampleSize,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_sampleSize(sampleSize)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] sampleSize Number of correspondences used for generating hypotheses.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
EigensolverSacProblem(
adapter_t & adapter,
size_t sampleSize,
const std::vector<int> & indices,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_sampleSize(sampleSize)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~EigensolverSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> &indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
/** The number of samples we are using for generating a hypothesis. */
size_t _sampleSize;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_EIGENSOLVERSACPROBLEM_HPP_

160
thirdparty/opengv/include/opengv/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,160 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MultiCentralRelativePoseSacProblem.hpp
* \brief Functions for fitting a multi relative-pose model to sets of
* bearing-vector correspondences between multiple viewpoints. Exploits
* mutual agreement. Used within a multi random-sample paradigm for
* rejecting outlier correspondences. Experimental!
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTICENTRALRELATIVEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTICENTRALRELATIVEPOSESACPROBLEM_HPP_
#include <opengv/sac/MultiSampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a multi relative-pose model to sets of
* bearing-vector correspondences between multiple viewpoints. Exploits mutual
* agreement. Used within a multi random-sample paradigm for rejecting outlier
* correspondences. Experimental! Currently using eightpt [9,10].
*/
class MultiCentralRelativePoseSacProblem :
public sac::MultiSampleConsensusProblem< transformations_t >
{
public:
/** The model we are trying to fit (multiple transformations) */
typedef transformations_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeMultiAdapterBase adapter_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] sampleSize The number of samples for each "sub"-hypothesis.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
MultiCentralRelativePoseSacProblem(adapter_t & adapter, int sampleSize,
bool randomSeed = true) :
sac::MultiSampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_sampleSize(sampleSize)
{
std::vector<int> numberCorrespondences;
for(size_t i = 0; i < adapter.getNumberPairs(); i++)
numberCorrespondences.push_back(adapter.getNumberCorrespondences(i));
setUniformIndices(numberCorrespondences);
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] indices A vector of multi-indices to be used from all available
* correspondences.
* \param[in] sampleSize The number of samples for each "sub"-hypothesis.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
MultiCentralRelativePoseSacProblem(
adapter_t & adapter,
const std::vector<std::vector<int> > & indices,
int sampleSize, bool randomSeed = true) :
sac::MultiSampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_sampleSize(sampleSize)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~MultiCentralRelativePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector< std::vector<int> > & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<std::vector<int> > & indices,
std::vector<std::vector<double> > & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<std::vector<int> > & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual std::vector<int> getSampleSizes() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
/** The number of samples for each "sub"-hypothesis. */
int _sampleSize;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTICENTRALRELATIVEPOSESACPROBLEM_HPP_

183
thirdparty/opengv/include/opengv/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,183 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file MultiNoncentralRelativePoseSacProblem.hpp
* \brief Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences from a multi-camera system (non-central). Uses
* RelativeMultiAdapterBase for managing correspondence-lists between
* individual pairs of views, enabling homogeneous sampling over the
* views. Used in a random-sample paradigm for rejecting outlier
* correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTINONCENTRALRELATIVEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTINONCENTRALRELATIVEPOSESACPROBLEM_HPP_
#include <opengv/sac/MultiSampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeMultiAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences from a multi-camera system (non-central). Can switch back to
* central as well in case only one camera is present. Otherwise using
* seventeenpt [12]. Uses RelativeMultiAdapterBase for managing
* correspondence-lists between individual pairs of views, enabling homogeneous
* sampling over the views. Used in a random-sample paradigm for rejecting
* outlier correspondences.
*/
class MultiNoncentralRelativePoseSacProblem :
public sac::MultiSampleConsensusProblem< transformation_t >
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeMultiAdapterBase adapter_t;
/** The possible algorithms for solving this problem */
typedef enum Algorithm
{
SIXPT = 0, // [16]
GE = 1, // []
SEVENTEENPT = 2 // [12]
} algorithm_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm to use.
* \param[in] asCentral Solve problem with only one camera?
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
MultiNoncentralRelativePoseSacProblem(
adapter_t & adapter, algorithm_t algorithm, bool asCentral = false,
bool randomSeed = true) :
sac::MultiSampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm),
_asCentral(asCentral)
{
std::vector<int> numberCorrespondences;
for(size_t i = 0; i < adapter.getNumberPairs(); i++)
numberCorrespondences.push_back(adapter.getNumberCorrespondences(i));
setUniformIndices(numberCorrespondences);
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm to use.
* \param[in] indices A vector of multi-indices to be used from all available
* correspondences.
* \param[in] asCentral Solve problem with only one camera?
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
MultiNoncentralRelativePoseSacProblem(
adapter_t & adapter,
algorithm_t algorithm,
const std::vector<std::vector<int> > & indices,
bool asCentral = false,
bool randomSeed = true) :
sac::MultiSampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm),
_asCentral(asCentral)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~MultiNoncentralRelativePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector< std::vector<int> > & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<std::vector<int> > & indices,
std::vector<std::vector<double> > & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<std::vector<int> > & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual std::vector<int> getSampleSizes() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
/** The algorithm we are using. */
algorithm_t _algorithm;
/** Use the central algorithm? (only one camera?). */
bool _asCentral;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_MULTINONCENTRALRELATIVEPOSESACPROBLEM_HPP_

174
thirdparty/opengv/include/opengv/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,174 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file NoncentralRelativePoseSacProblem.hpp
* \brief Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences from a multi-camera system (non-central). Used with
* a random-sample paradigm for rejecting outlier-correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_NONCENTRALRELATIVEPOSESACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_NONCENTRALRELATIVEPOSESACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences from a multi-camera system (non-central). Can switch back to
* central as well in case only one camera is present. Otherwise using
* seventeenpt [12]. Used with a random-sample paradigm for rejecting
* outlier-correspondences.
*/
class NoncentralRelativePoseSacProblem :
public sac::SampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeAdapterBase adapter_t;
/** The possible algorithms for solving this problem */
typedef enum Algorithm
{
SIXPT = 0, // [16]
GE = 1, // []
SEVENTEENPT = 2 // [12]
} algorithm_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm to use.
* \param[in] asCentral Solve problem with only one camera?
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
NoncentralRelativePoseSacProblem(
adapter_t & adapter, algorithm_t algorithm, bool asCentral = false,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm),
_asCentral(asCentral)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] algorithm The algorithm to use
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] asCentral Solve problem with only one camera?
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
NoncentralRelativePoseSacProblem(
adapter_t & adapter,
algorithm_t algorithm,
const std::vector<int> & indices,
bool asCentral = false,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter),
_algorithm(algorithm),
_asCentral(asCentral)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~NoncentralRelativePoseSacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
/** The algorithm we are using. */
algorithm_t _algorithm;
/** Use the central algorithm? (only one camera?). */
bool _asCentral;
};
}
}
}
#endif /* OPENGV_SAC_PROBLEMS_RELATIVE_POSE_NONCENTRALRELATIVEPOSESACPROBLEM_HPP_ */

150
thirdparty/opengv/include/opengv/sac_problems/relative_pose/RotationOnlySacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,150 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file RotationOnlySacProblem.hpp
* \brief Functions for fitting a rotation-only model to a set of bearing-vector
* correspondences. The viewpoints are assumed to be separated by
* rotation only. Used within a random-sample paradigm for rejecting
* outlier correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_ROTATIONONLYSACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_ROTATIONONLYSACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a rotation-only model to a set of bearing-vector
* correspondences (using twopt_rotationOnly). The viewpoints are assumed to
* be separated by rotation only. Used within a random-sample paradigm for
* rejecting outlier correspondences.
*/
class RotationOnlySacProblem :
public sac::SampleConsensusProblem<rotation_t>
{
public:
/** The model we are trying to fit (rotation) */
typedef rotation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeAdapterBase adapter_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
RotationOnlySacProblem(adapter_t & adapter, bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
RotationOnlySacProblem(
adapter_t & adapter,
const std::vector<int> & indices, bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~RotationOnlySacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_ROTATIONONLYSACPROBLEM_HPP_

150
thirdparty/opengv/include/opengv/sac_problems/relative_pose/TranslationOnlySacProblem.hpp vendored Normal file
View File

@@ -0,0 +1,150 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file TranslationOnlySacProblem.hpp
* \brief Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences. The viewpoints are assumed to be separated by
* translation only. Used within a random-sample paradigm for rejecting
* outlier correspondences.
*/
#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_TRANSLATIONONLYSACPROBLEM_HPP_
#define OPENGV_SAC_PROBLEMS_RELATIVE_POSE_TRANSLATIONONLYSACPROBLEM_HPP_
#include <opengv/sac/SampleConsensusProblem.hpp>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the sample consensus problems.
*/
namespace sac_problems
{
/**
* \brief The namespace for the relative pose methods.
*/
namespace relative_pose
{
/**
* Functions for fitting a relative-pose model to a set of bearing-vector
* correspondences (using twopt). The viewpoints are assumed to be separated by
* translation only. Used within a random-sample paradigm for rejecting outlier
* correspondences.
*/
class TranslationOnlySacProblem :
public sac::SampleConsensusProblem<transformation_t>
{
public:
/** The model we are trying to fit (transformation) */
typedef transformation_t model_t;
/** The type of adapter that is expected by the methods */
typedef opengv::relative_pose::RelativeAdapterBase adapter_t;
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
TranslationOnlySacProblem(adapter_t & adapter, bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setUniformIndices(adapter.getNumberCorrespondences());
};
/**
* \brief Constructor.
* \param[in] adapter Visitor holding bearing vector correspondences etc.
* \param[in] indices A vector of indices to be used from all available
* correspondences.
* \param[in] randomSeed Whether to seed the random number generator with
* the current time.
*/
TranslationOnlySacProblem(
adapter_t & adapter,
const std::vector<int> & indices,
bool randomSeed = true) :
sac::SampleConsensusProblem<model_t> (randomSeed),
_adapter(adapter)
{
setIndices(indices);
};
/**
* Destructor.
*/
virtual ~TranslationOnlySacProblem() {};
/**
* \brief See parent-class.
*/
virtual bool computeModelCoefficients(
const std::vector<int> & indices,
model_t & outModel) const;
/**
* \brief See parent-class.
*/
virtual void getSelectedDistancesToModel(
const model_t & model,
const std::vector<int> & indices,
std::vector<double> & scores) const;
/**
* \brief See parent-class.
*/
virtual void optimizeModelCoefficients(
const std::vector<int> & inliers,
const model_t & model,
model_t & optimized_model);
/**
* \brief See parent-class.
*/
virtual int getSampleSize() const;
protected:
/** The adapter holding all input data. */
adapter_t & _adapter;
};
}
}
}
#endif //#ifndef OPENGV_SAC_PROBLEMS_RELATIVE_POSE_TRANSLATIONONLYSACPROBLEM_HPP_

83
thirdparty/opengv/include/opengv/triangulation/methods.hpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file triangulation/methods.hpp
* \brief Some triangulation methods. Not exhaustive.
*/
#ifndef OPENGV_TRIANGULATION_METHODS_HPP_
#define OPENGV_TRIANGULATION_METHODS_HPP_
#include <stdlib.h>
#include <vector>
#include <opengv/types.hpp>
#include <opengv/relative_pose/RelativeAdapterBase.hpp>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/**
* \brief The namespace for the triangulation methods.
*/
namespace triangulation
{
/**
* \brief Compute the position of a 3D point seen from two viewpoints. Linear
* Method.
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* relative transformation.
* \param[in] index The index of the correspondence being triangulated.
* \return The 3D point expressed in the first viewpoint.
*/
point_t triangulate(
const relative_pose::RelativeAdapterBase & adapter, size_t index );
/**
* \brief Compute the position of a 3D point seen from two viewpoints. Fast
* non-linear approximation (closed-form).
*
* \param[in] adapter Visitor holding bearing-vector correspondences, plus the
* relative transformation.
* \param[in] index The index of the correspondence being triangulated.
* \return The 3D point expressed in the first viewpoint.
*/
point_t triangulate2(
const relative_pose::RelativeAdapterBase & adapter, size_t index );
}
}
#endif /* OPENGV_TRIANGULATION_METHODS_HPP_ */

176
thirdparty/opengv/include/opengv/types.hpp vendored Normal file
View File

@@ -0,0 +1,176 @@
/******************************************************************************
* 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. *
******************************************************************************/
/**
* \file types.hpp
* \brief A collection of variables used in geometric vision for the
* computation of calibrated absolute and relative pose.
*/
#ifndef OPENGV_TYPES_HPP_
#define OPENGV_TYPES_HPP_
#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
/**
* \brief The namespace of this library.
*/
namespace opengv
{
/** A 3-vector of unit length used to describe landmark observations/bearings
* in camera frames (always expressed in camera frames)
*/
typedef Eigen::Vector3d
bearingVector_t;
/** An array of bearing-vectors */
typedef std::vector<bearingVector_t, Eigen::aligned_allocator<bearingVector_t> >
bearingVectors_t;
/** A 3-vector describing a translation/camera position */
typedef Eigen::Vector3d
translation_t;
/** An array of translations */
typedef std::vector<translation_t, Eigen::aligned_allocator<translation_t> >
translations_t;
/** A rotation matrix */
typedef Eigen::Matrix3d
rotation_t;
/** An array of rotation matrices as returned by fivept_kneip [7] */
typedef std::vector<rotation_t, Eigen::aligned_allocator<rotation_t> >
rotations_t;
/** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
* translation \f$ \mathbf{t} \f$ as follows:
* \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right) \f$
*/
typedef Eigen::Matrix<double,3,4>
transformation_t;
/** An array of transformations */
typedef std::vector<transformation_t, Eigen::aligned_allocator<transformation_t> >
transformations_t;
/** A 3-vector containing the cayley parameters of a rotation matrix */
typedef Eigen::Vector3d
cayley_t;
/** A 4-vector containing the quaternion parameters of rotation matrix */
typedef Eigen::Vector4d
quaternion_t;
/** Essential matrix \f$ \mathbf{E} \f$ between two viewpoints:
*
* \f$ \mathbf{E} = \f$ skew(\f$\mathbf{t}\f$) \f$ \mathbf{R} \f$,
*
* where \f$ \mathbf{t} \f$ describes the position of viewpoint 2 seen from
* viewpoint 1, and \f$\mathbf{R}\f$ describes the rotation from viewpoint 2
* to viewpoint 1.
*/
typedef Eigen::Matrix3d
essential_t;
/** An array of essential matrices */
typedef std::vector<essential_t, Eigen::aligned_allocator<essential_t> >
essentials_t;
/** An essential matrix with complex entires (as returned from
* fivept_stewenius [5])
*/
typedef Eigen::Matrix3cd
complexEssential_t;
/** An array of complex-type essential matrices */
typedef std::vector< complexEssential_t, Eigen::aligned_allocator< complexEssential_t> >
complexEssentials_t;
/** A 3-vector describing a point in 3D-space */
typedef Eigen::Vector3d
point_t;
/** An array of 3D-points */
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
points_t;
/** A 3-vector containing the Eigenvalues of matrix \f$ \mathbf{M} \f$ in the
* eigensolver-algorithm (described in [11])
*/
typedef Eigen::Vector3d
eigenvalues_t;
/** A 3x3 matrix containing the eigenvectors of matrix \f$ \mathbf{M} \f$ in the
* eigensolver-algorithm (described in [11])
*/
typedef Eigen::Matrix3d
eigenvectors_t;
/** EigensolverOutput holds the output-parameters of the eigensolver-algorithm
* (described in [11])
*/
typedef struct EigensolverOutput
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** Position of viewpoint 2 seen from viewpoint 1 (unscaled) */
translation_t translation;
/** Rotation from viewpoint 2 back to viewpoint 1 */
rotation_t rotation;
/** The eigenvalues of matrix \f$ \mathbf{M} \f$ */
eigenvalues_t eigenvalues;
/** The eigenvectors of matrix matrix \f$ \mathbf{M} \f$ */
eigenvectors_t eigenvectors;
} eigensolverOutput_t;
/** GeOutput holds the output-parameters of ge
*/
typedef struct GeOutput
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** Homogeneous position of viewpoint 2 seen from viewpoint 1 */
Eigen::Vector4d translation;
/** Rotation from viewpoint 2 back to viewpoint 1 */
rotation_t rotation;
/** The eigenvalues of matrix \f$ \mathbf{G} \f$ */
Eigen::Vector4d eigenvalues;
/** The eigenvectors of matrix matrix \f$ \mathbf{G} \f$ */
Eigen::Matrix4d eigenvectors;
} geOutput_t;
}
#endif /* OPENGV_TYPES_HPP_ */