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); }