cisstNumerical API

cisstNumerical provides numerical algorithms including least-squares solvers, polynomial fitting, SVD, and constraint optimization.

Key classes

class nmrConstraintOptimizer

nmrConstraintOptimizer: A class that makes using the constraint control algorithm more efficient

This is a container for constrained control optimizer. It provides high level functions to add common functionality. Solves the LSI problem arg min || C x - d ||, s.t. E x = f and A x >= b.

Public Types

enum STATUS

enum used for solver results. 0 Both equality and inequality constraints are compatible and have been satisfied. 1 Equality constraints are contradictory. A generalized inverse solution of EX=F was used to minimize the residual vector length F-EX. In this sense, the solution is still meaningful. 2 Inequality constraints are contradictory. 3 Both equality and inequality constraints are contradictory. 4 Input has a NaN or INF

Values:

enumerator NMR_OK

enumerator NMR_EQ_CONTRADICTION

enumerator NMR_INEQ_CONTRADICTION

enumerator NMR_BOTH_CONTRADICTION

enumerator NMR_MALFORMED

enumerator NMR_EMPTY

Public Functions

inline nmrConstraintOptimizer(): Constructor

inline ~nmrConstraintOptimizer(): Destructor

nmrConstraintOptimizer(const size_t n)

Initialize control optimizer

Parameters:: n – Number of variables

STATUS Solve(vctDoubleVec &dq)

Calls the solver and stores the result in dq.

Solve

Parameters:: dq – Vector of current joint values
Returns:: STATUS enum that dictates whether the solver worked or there was a problem

void ResetIndices(void)

Clear all indices.

Reset

void Allocate(void)

Allocate memory indicated by indices.

allocate

void Allocate(const size_t CRows, const size_t CCols, const size_t ARows, const size_t ACols, const size_t ERows, const size_t ECols)

Allocate memory indicated by input.

allocate

void ReserveSpace(const size_t CRows, const size_t ARows, const size_t ERows, const size_t num_slacks)

Reserves space in the tableau.

ReserveSpace

Parameters:

CRows – Number of rows needed for the objective
ARows – Number of rows needed for the inequality constraint
ERows – Number of rows needed for the equality constraint
num_slacks – The number of slacks needed

void SetRefs(const size_t CRows, const size_t ARows, const size_t ERows, const size_t NumSlacks, vctDynamicMatrixRef<double> &CData, vctDynamicMatrixRef<double> &CSlacks, vctDynamicVectorRef<double> &dData, vctDynamicMatrixRef<double> &AData, vctDynamicMatrixRef<double> &ASlacks, vctDynamicVectorRef<double> &bData, vctDynamicVectorRef<double> &bSlacks, vctDynamicMatrixRef<double> &EData, vctDynamicVectorRef<double> &fData)

Returns references to spaces in the tableau.

GetObjectiveSpace

Parameters:

CRows – Number of rows needed for the objective data
ARows – Number of rows needed for the inequality constraint data
ERows – Number of rows needed for the equality constraint data
SlackRows – The number of slacks
CData – A reference to the data portion of the objective matrix
CSlacks – A reference to the slack portion of the objective matrix
dData – A reference to the objective vector
AData – A reference to the data portion of the inequality constraint matrix
bData – A reference to the inequality constraint vector
bSlacks – A reference to the inequality constraint vector for slack portion (slack limit)
EData – A reference to the data portion of the equality constraint matrix
fData – A reference to the equality constraint vector

size_t GetNumVars(void) const

Returns the number of variables.

GetNumVars

Returns:: size_t Number of variables

size_t GetSlackIndex(void) const

Gets the current slack index.

GetSlackIndex

Returns:: size_t The slack index

size_t GetSlacks(void) const

Gets the number of slacks.

GetSlacks

Returns:: size_t The number of slacks

size_t GetObjectiveRows(void) const

Gets the number of rows for the objective expression.

GetObjectiveRows

Returns:: size_t The number of objective rows

size_t GetObjectiveIndex(void) const

Gets the objective index.

GetObjectiveIndex

Returns:: size_t The objective index

size_t GetIneqConstraintRows(void) const

Gets the number of rows for the inequality constraint.

GetIneqConstraintRows

Returns:: size_t The number of inequality constraint rows

size_t GetIneqConstraintIndex(void) const

Gets the inequality constraint index.

GetIneqConstraintIndex

Returns:: size_t The inequality constraint index

size_t GetEqConstraintRows(void) const

Gets the number of rows for the equality constraint.

GetEqConstraintRows

Returns:: size_t The number of rows for the equality constraint

size_t GetEqConstraintIndex(void) const

Gets the equality constraint index.

GetEqConstraintIndex

Returns:: size_t The equality constraint index

const vctDoubleMat &GetObjectiveMatrix(void) const

Gets the objective matrix.

GetObjectiveMatrix

Returns:: vctDoubleMat The objective matrix

const vctDoubleVec &GetObjectiveVector(void) const

Gets the objective vector.

GetObjectiveVector

Returns:: vctDoubleVec The objective vector

const vctDoubleMat &GetIneqConstraintMatrix(void) const

Gets the inequality constraint matrix.

GetIneqConstraintMatrix

Returns:: vctDoubleMat The inequality constraint matrix

const vctDoubleVec &GetIneqConstraintVector(void) const

Gets the inequality constraint vector.

GetIneqConstraintVector

Returns:: vctDoubleVec The inequality constraint vector

const vctDoubleMat &GetEqConstraintMatrix(void) const

Gets the equality constraint matrix.

GetEqConstraintMatrix

Returns:: vctDoubleMat The equality constraint matrix

const vctDoubleVec &GetEqConstraintVector(void) const

Gets the equality constraint vector.

GetEqConstraintVector

Returns:: vctDoubleVec The equality constraint vector

const std::string GetStatusString(STATUS status) const

Helper function for converting status enum to a string message.

GetStatusString

Parameters:: status – A STATUS enum variable
Returns:: A string representation of that STATUS

class nmrBernsteinPolynomial : public nmrDynAllocPolynomialContainer

class nmrBernsteinPolynomial defines a polynomial in Bernstein basis. In a Bernstein polynomial, all the terms are of equal degree (that is, the sum of powers is a constant), and the variables sum up to a unity. Each term is associated with a scalar coefficient and with a multinomial factor, which reflects the relative weight of the term in the expression:

\(1 = (x_0 + ... + x_{n-1}) ^ d = \sum_{(p_0 + ... + p_{n-1}) = d} \choose{d}{p_0 p_1 ... p_{n-1}} x_0^{p_0} ... x_{n-1}^{p_{n-1}}\)

To make repeated evaluations quicker, we cache the multinomial factor along with the coefficient of the term in a BernsteinTermInfo object. Appropriate accessors are defined.

Since the sum of the variables is 1, one of the variables depends on the others. Typically, it is either the last or the first, but the user can choose any variable to be the `implicit’ variable. Once the implicit variable has been determined, its value cannot be updated directly, and instead it is reassigned every time one of the other variables is set. However, the user of the class still has the flexibility to replace the choice of the implicit variable at any time. The only thing that matters is that the variables sum to 1.

Note: We use dynamic allocation for the term information (BernsteinTermInfo), which is separate from the STL provided dynamic allocation for the container elements. We store the pointer to the dynamically allocated term info in the container. Therefore:

We had to override the base-class’s RemoveTerm() and Clear() methods.
Do not even try to make a copy of a nmrBernsteinPolynomial using copy-ctor or operator= (yeah, like you would). Just for safety, we declare these operations protected, and do not provide implementation.
A better solution may be to define a common base type for TermInfo, and have it declare a virtual dtor. Go for it, if you have time.

Public Types

typedef nmrDynAllocPolynomialContainer BaseType

typedef std::pair<nmrPolynomialBase::CoefficientType, nmrPolynomialTermPowerIndex::MultinomialCoefficientType> BernsteinTermInfo

Public Functions

inline nmrBernsteinPolynomial(VariableIndexType numVariables, PowerType degree)

Constructor determines the number of variables and the degree of the polynomial. Note that Bernstein polynomial contains an additional implicit variable. The argument numVariables must include the implicit variable, since we want to be consistent with the GetNumVariables() method. The degree defines both maximum and minimum degrees for the terms, as all terms are of equal degree. The constructor initializes the free variables to zero, and the implicit variable to 1, so that they all sum up to

The constructor sets the implicit variable to be the last one (the numVariable - 1) by default. The user can change this setting by calling SetImplicitVarIndex().

virtual InsertStatus SetCoefficient(const nmrPolynomialTermPowerIndex &where, CoefficientType coefficient): Overriding nmrPolynomial::SetCoefficient(). This implementation computes the multinomial factor for the term.

virtual InsertStatus SetCoefficient(TermIteratorType &where, CoefficientType coefficient): Set a coefficient for the given term iterator. Implemented from nmrPolynomialContainer.

virtual CoefficientType GetCoefficient(const nmrPolynomialTermPowerIndex &where) const: Retrieve the value of the user defined coefficient for a given term.

inline virtual CoefficientType GetCoefficient(const TermConstIteratorType &where) const: Retrieve the value of the user defined coefficient for a term given by iterator.

inline virtual CoefficientType GetCoefficient(const TermIteratorType &where) const

virtual void RemoveTerm(TermIteratorType &where): Remove a term from the polynomial. The term is given by iterator. The function also reclaims the space allocated for the term.

virtual void Clear(): Remove all the terms to make an empty (zero) polynomial We cannot use nmrPolynomialContainer::Clear(), since it does not reclaim the space allocated for the term. This implementation does.

virtual ValueType EvaluateBasis(const nmrPolynomialTermPowerIndex &where, const nmrMultiVariablePowerBasis &variables) const: Evaluate the basis function for a term. This DOES NOT include the term’s coefficient, but DOES include the multinomial factor.

virtual ValueType EvaluateBasis(const TermConstIteratorType &where, const nmrMultiVariablePowerBasis &variables) const

virtual ValueType EvaluateBasis(const TermIteratorType &where, const nmrMultiVariablePowerBasis &variables) const

virtual void SerializeTermInfo(std::ostream &output, const TermConstIteratorType &termIterator) const: This function is overridden to store the term coefficient to the stream. It is called from nmrPolynomialContainer::SerializeRaw().

virtual void DeserializeTermInfo(std::istream &input, TermIteratorType &termIterator): This function is overridden to allocate memory for the multinomial factor and the term coefficient, read the latter from the stream and calculate the former from the term power index. It is called from nmrPolynomialContainer::DeserializeRaw().

virtual void AddConstant(CoefficientType shiftAmount): Overloaded from nmrPolynomialBase. This implementation goes over all the possible for this polynomial, reads their current coefficient (which may be zero if the term is not included so far) then adds the given shift amount. It may not be the most efficient method, but it should not be called too many times.

virtual void AddConstantToCoefficients(CoefficientType coefficients[], CoefficientType shiftAmount) const: Overloaded from nmrPolynomialBase. This implementation asserts that this polynomial contains all the possible terms, and then iterates on all the terms and adds the shift amount to the corresponding external coefficient.

inline const nmrPolynomialTermPowerIndex::MultinomialCoefficientType &GetMultinomialCoefficient(const TermIteratorType &where) const

inline const nmrPolynomialTermPowerIndex::MultinomialCoefficientType &GetMultinomialCoefficient(const TermConstIteratorType &where) const

template<class _elementType> class nmrLinearRegressionSolver

This class provides a linear regression solver using a least-squares solution. It is primarily designed for ongoing use, where samples arrive at discrete points in time. The basic approach is as follows:

nmrLinearRegressionSolver<double> lr;
double x,y;
double slope, yint, mse;

while (!done) {
   // Get and sample new data (x,y)
   lr.Sample(x,y);
}
lr.Estimate(slope, yint, &mse);

In addition to the Estimate method, there is also an EstimateAsFractions method that returns slope_num, yint_num, denom, and tse_num (note that the types may be different from the input data types due to type promotion). In other words:

\( y = (slope_num/denom)*x + (yint_num/denom) \)

which can be rewritten as:

\( denom*y = slope_num*x + yint_num \)

tse_num is the numerator for the total square error. To get mean square error:

\( mse = tse_num/(N*denom) \)

where N is the number of points.

There are two good reasons for this method:

The results are always defined (i.e., there is no possible division by 0), so the caller can use whatever tolerance is desired. For near vertical lines, it is possible to instead compute the parameters of the line \( x = (denom/slope_num)*y - (yint_num/slope_num) \).
When this templated class is instantiated for fixed point numbers, such as int, the estimated slope and yint are also fixed point numbers. But, by returning them as fractions, the caller can decide whether to compute the final results as floating point numbers, or whether to perform both fixed point multiplication and division.

The Sample method is overloaded so that it can also accept input vectors, either a fixed size vector of size 2 that contains both x and y, or dynamic vectors of multiple x and y values.

All methods return a boolean flag to indicate success or failure. Possible reasons for failure include inconsistent vector sizes and near infinite slopes. Note that the check for near infinite slopes is based on a specified tolerance value. The default tolerances are obtained from cmnTypeTraits. These defaults may be too large for some applications (e.g., the default for float is currently 1e-5 and for double it is 1e-9). It is possible to specify a new tolerance value in the class constructor, or via the SetTolerance method.

This class has virtual methods to enable derivation. One possible derivation would be to create a class that checks for impending overflow on the accumulators (Sx, Sy, Sxx, Sxy, Syy) and returns false in that case. This can be done using cmnTypeTraits<_elementType>::MaxPositiveValue and MinNegativeValue. For example, statements such as the following can be added to Sample:

if ((Sx > (cmnTypeTraits<_elementType>::MaxPositiveValue()-x)) ||
    (Sx < cmnTypeTraits<_elementType>::MinNegativeValue()+x)) return false;
if (Sxx > (cmnTypeTraits<_elementType>::MaxPositiveValue()-x*x)) return false;

Subclassed by nmrLinearRegressionWindowSolver< _elementType >

Public Types

typedef _elementType ElementType

typedef cmnTypeTraits<_elementType>::VaArgPromotion SummationType

Public Functions

inline nmrLinearRegressionSolver(_elementType tol = cmnTypeTraits<_elementType>::DefaultTolerance)

Constructor

Parameters:: tol – Tolerance to use when checking for infinite slope (default from cmnTypeTraits)

inline virtual ~nmrLinearRegressionSolver()

inline size_t NumPoints() const: Returns number of sampled points

inline _elementType GetTolerance() const: Returns the tolerance used when checking for infinite slope.

inline void SetTolerance(_elementType tol): Set the tolerance used when checking for infinite slope.

inline virtual void Clear(): Initialize the accumulators (i.e., clear all sample points)

virtual bool Sample(const _elementType &x, const _elementType &y): Sample the specified x,y pair. Always returns true, but derived classes could return false (e.g., if checking for overflow)

inline virtual bool Sample(const vctFixedSizeVector<_elementType, 2> &in): Sample the specified x,y pair, provided as a vector.

inline virtual bool Sample(const vctFixedSizeConstVectorRef<_elementType, 2, 1> &in): Sample the specified x,y pair, provided as a vector ref.

inline virtual bool Sample(const vctDynamicVector<_elementType> &x, const vctDynamicVector<_elementType> &y): Sample multiple x and y values, provided as two vctDynamicVectors or two vctDynamicVectorRefs. Returns true unless vector sizes are not equal.

inline virtual bool Sample(const vctDynamicConstVectorRef<_elementType> &x, const vctDynamicConstVectorRef<_elementType> &y)

inline virtual bool Sample(const std::vector<_elementType> &x, const std::vector<_elementType> &y): Sample multiple x and y values, provided as two std::vector objects. Returns true unless vector sizes are not equal.

virtual bool EstimateAsFractions(SummationType &slope_num, SummationType &yint_num, SummationType &denom, SummationType *tse_num = 0): Estimate the slope and y intercept as fractions: slope = slope_num/denom yint = yint_num/denom tse = tse_num/denom (total square error, divide by numpts to get mse)

virtual bool Estimate(_elementType &slope, _elementType &yint, _elementType *mse = 0): Estimate the slope and y intercept and (optionally) return the mean square error (MSE). RMS error is the square root of MSE.