void BSplineMotionError<SPLINE_T>::initialize(spline_t * /* splineDV */, Eigen::MatrixXd /* W */, unsigned int errorTermOrder) {
// check spline order:
int splineOrder = _splineDV->spline().splineOrder();
if(splineOrder <= (int)errorTermOrder && splineOrder >= 2) {
errorTermOrder = splineOrder-1;
std::cout << "! Invalid ErrorTermOrder reduced to " << errorTermOrder << std::endl;
}
sbm_t Qsp = _splineDV->spline().curveQuadraticIntegralSparse(_W, errorTermOrder);
// set spline design variables
unsigned int numberOfSplineDesignVariables = _splineDV->spline().numVvCoefficients();
_coefficientVectorLength = _splineDV->spline().coefficients().rows() * numberOfSplineDesignVariables;
Qsp.cloneInto(_Q);
// std::cout << "next:" << std::endl;
// std::cout << Q.cols() << ":" << Q.rows() << std::endl;
// std::cout << _Q->cols() << ":" << _Q->rows() << std::endl;
// Tell the super class about the design variables:
// loop the design variables and add to vector:
std::vector<aslam::backend::DesignVariable*> dvV;
for ( unsigned int i = 0; i < numberOfSplineDesignVariables; i++) {
dvV.push_back(_splineDV->designVariable(i));
}
setDesignVariables(dvV);
}
void ErrorTerm::setDesignVariables(DesignVariable* dv1, DesignVariable* dv2)
{
std::vector<DesignVariable*> v;
v.push_back(dv1);
v.push_back(dv2);
setDesignVariables(v);
}
MarginalizationPriorErrorTerm::MarginalizationPriorErrorTerm(const std::vector<aslam::backend::DesignVariable*>& designVariables,
const Eigen::VectorXd& d, const Eigen::MatrixXd& R)
: aslam::backend::ErrorTermDs(R.rows()), _designVariables(designVariables), _d(d), _R(R), _dimensionDesignVariables(R.cols())
{
SM_ASSERT_GT(aslam::InvalidArgumentException, designVariables.size(), 0, "The prior error term doesn't make much sense with zero design variables.");
// PTF: Hrm...here is a big weakness of the current optimizer code. We should have
// different base classes for different uncertainty types (scalar, diagonal, matrix, none)
// to avoid big matrix multiplications during optimization.
setInvR(Eigen::MatrixXd::Identity(R.rows(), R.rows()));
//_M = Eigen::MatrixXd::Identity(R.cols(), R.cols());
// set all design variables
for(vector<aslam::backend::DesignVariable*>::iterator it = _designVariables.begin(); it != _designVariables.end(); ++it)
{
Eigen::MatrixXd values;
(*it)->getParameters(values);
_designVariableValuesAtMarginalization.push_back(values);
}
setDesignVariables(designVariables);
}
