/** Sets the current goal acceleration */
bool CTaskOpPosition::setGoalAcc(const Eigen::VectorXd & arg_goal)
{
if((data_->dof_task_ == arg_goal.cols() && 1 == arg_goal.rows()) ||
(1 == arg_goal.cols() && data_->dof_task_ == arg_goal.rows()) )
{
data_->ddx_goal_ = arg_goal;
return true;
}
#ifdef DEBUG
else
{
std::cerr<<"\nCTaskOpPosition::setGoalAcc() : Error : Goal vector's size != data_->dof_task_"<<std::flush;
assert(false);
}
#endif
return false;
}
void addVectorBlock( Eigen::VectorXd &matrix, const int block_length ){
assert( 1==matrix.cols() );
int old_size = matrix.rows();
matrix.conservativeResize( old_size+block_length, 1 );
matrix.block( old_size, 0, block_length, 1 ) = Eigen::MatrixXd::Zero( block_length, 1);
}
/** Compute the potential of a node according to the weights and its
* features.
* \param features: features of the node.
* \return potentials of that node.
*/
Eigen::VectorXd computePotentials( Eigen::VectorXd &features )
{
// Check that features are a column vector, and transpose it otherwise
if ( features.cols() > 1 )
features = features.transpose();
assert ( features.rows() == m_nFeatures );
return (*m_computePotentialsFunction)(m_weights,features);
}
Params SMC::calculatePosteriorParams( int currTime,\
StateProgression * currState,\
Params params,\
vector< vector<double> > * cloudData,
int curDataPoint){
MatrixXd data_t= getDataOfCluster(curDataPoint, & currState->assignments, cloudData);
Eigen::MatrixXd clusteredData(data_t.rows()+ params.auxiliaryNum, data_t.cols() );
if(clusteredData.rows()>0 && currState->stateProg[currTime-timeOffset].size()>0){
SufficientStatistics ss = currState->stateProg[currTime-timeOffset][curDataPoint];
VectorXd mean(ss.mean.size());
mean(0) = ss.mean[0];
mean(1) = ss.mean[1];
mean(2) = ss.mean[2];
//Format: params = {crp, del, #aux, tau0, v0, mu0, k0, q0, _,_,_<-#colorbins?}
Eigen::MatrixXd auxGausSamples = ut.sampleMultivariateNormal(mean,ss.covar, params.auxiliaryNum, 3);
Eigen::MatrixXd auxMultSamples = ut.sampleMultinomial( ss.categorical, params.auxiliaryNum);
Eigen::VectorXd auxExpSamples = ut.exprnd(ss.exponential , params.auxiliaryNum);
MatrixXd C(auxGausSamples.transpose().rows(),\
auxExpSamples.cols()*3\
+ auxGausSamples.transpose().cols()\
+ auxMultSamples.cols());
C << auxGausSamples.transpose() , auxExpSamples,auxExpSamples,auxExpSamples, auxMultSamples;
clusteredData << data_t, C;
return updateParams(clusteredData, params, 3);
}else if(currState->stateProg[currTime-timeOffset].size()>0){
SufficientStatistics ss = currState->stateProg[currTime-timeOffset][curDataPoint];
VectorXd mean(ss.mean.size());
mean(0) = ss.mean[0];
mean(1) = ss.mean[1];
mean(2) = ss.mean[2];
Eigen::MatrixXd auxGausSamples = ut.sampleMultivariateNormal(mean,ss.covar, params.auxiliaryNum, 3);
Eigen::MatrixXd auxMultSamples = ut.sampleMultinomial( ss.categorical, params.auxiliaryNum);
Eigen::VectorXd auxExpSamples = ut.exprnd(ss.exponential , params.auxiliaryNum);
MatrixXd C( auxGausSamples.transpose().rows(), auxExpSamples.cols()*3 +\
auxGausSamples.transpose().cols() + auxMultSamples.cols());
C << auxGausSamples.transpose() , auxExpSamples,auxExpSamples,auxExpSamples, auxMultSamples;
return updateParams(C, params, 3);
}else
return updateParams(clusteredData, params, 3);
}
ukf::ukf(int states, int measurements, int controls, double alpha, double beta, //Assumes that Kappa=0
std::chrono::milliseconds dT,
Eigen::VectorXd initState, Eigen::MatrixXd initCovar, vFunc transform, vFunc measure,
Eigen::MatrixXd Q, Eigen::MatrixXd R){
if(states > 0){ _states = states; }
else{
std::cerr<<"In ukf(~): <1 value of given states\n"; states = 1;
}
if(measurements > 0){ _measurements = measurements; }
else{
std::cerr<<"In ukf(~): <1 value of given measurements\n"; states = 1;
}
if(controls > 0){ _controls = controls; }
else{
std::cerr<<"In ukf(~): <1 value of given controls\n"; controls = 1;
}
this->setQ(Q);
this->setR(R);
this->setMeasure(measure);
this->setTransform(transform);
_alpha = 0; _beta = 0; _kappa = 0;
this->setAlpha(alpha);
this->setBeta(beta);
this->setKappa(0);
if(initCovar.cols() == initCovar.rows() && initCovar.rows() == _states){
_prevCovar = initCovar;
_covar = initCovar;
}
else{
std::cerr<<"In ukf(~): incorrect size of initial covariance matrix\n";
_prevCovar = Eigen::MatrixXd::Zero(_states, _states); //_prevCovar.resize(_states, _states); _prevCovar.zero();
_covar = Eigen::MatrixXd::Zero(_states, _states); //_covar.resize(_states, _states); _covar.zero();
}
if(initState.cols() == 1 && initState.rows() == _states){
_prevState = initState;
_state = initState;
}
else{
std::cerr<<"In ukf(~): incorrect size of initial state vector\n";
_prevState = Eigen::VectorXd::Zero(_states); //_prevState.resize(_states, 1); _prevState.zero();
_state = Eigen::VectorXd::Zero(_states); //_state.resize(_states, 1); _state.zero();
}
_sigmaPoints = Eigen::MatrixXd::Constant(_states, _states*2+1, 0);
_measureSigmaPoints = Eigen::MatrixXd::Constant(_states, _states*2+1, 0);
}
void deleteVectorBlock( Eigen::VectorXd &matrix, const int block_start_index, const int block_length ){
assert( 1==matrix.cols() );
assert( matrix.rows()>=block_start_index+block_length );
int size = matrix.rows();
/*
R
D
M
*/
matrix.block( block_start_index, 0, size-block_start_index-block_length, 1 ) = matrix.block( block_start_index+block_length, 0, size-block_start_index-block_length, 1 );
matrix.conservativeResize( size-block_length, 1 );
}
bool PinholeCamera<DISTORTION_T>::setIntrinsics(
const Eigen::VectorXd & intrinsics)
{
if (intrinsics.cols() != NumIntrinsics) {
return false;
}
intrinsics_ = intrinsics;
fu_ = intrinsics[0]; //< focalLengthU
fv_ = intrinsics[1]; //< focalLengthV
cu_ = intrinsics[2]; //< imageCenterU
cv_ = intrinsics[3]; //< imageCenterV
distortion_.setParameters(
intrinsics.tail<distortion_t::NumDistortionIntrinsics>());
one_over_fu_ = 1.0 / fu_; //< 1.0 / fu_
one_over_fv_ = 1.0 / fv_; //< 1.0 / fv_
fu_over_fv_ = fu_ / fv_; //< fu_ / fv_
return true;
}
/// Base exponential function with logarithmic term
double IncompressibleFluid::baseLogexponential(IncompressibleData data, double y, double ybase){
Eigen::VectorXd coeffs = makeVector(data.coeffs);
size_t r=coeffs.rows(),c=coeffs.cols();
if (strict && (r!=3 || c!=1) ) throw ValueError(format("%s (%d): You have to provide a 3,1 matrix of coefficients, not (%d,%d).",__FILE__,__LINE__,r,c));
return exp( (double) ( log( (double) (1.0/((y-ybase)+coeffs[0]) + 1.0/((y-ybase)+coeffs[0])/((y-ybase)+coeffs[0]) ) ) *coeffs[1]+coeffs[2] ) );
}
static lbfgsfloatval_t evaluate(
void *instance,
const lbfgsfloatval_t *x,
lbfgsfloatval_t *g,
const int n,
const lbfgsfloatval_t step
){
double fx = 0;
timeutil t;
optimization_instance_t* optInst = (optimization_instance_t*)instance;
Eigen::Map<Eigen::VectorXd> param((double*)x,
n, 1);
Eigen::Map<Eigen::VectorXd> grad((double*)g,
n, 1);
// price for sparse A*x
t.tic();
Eigen::VectorXd xw = (*(optInst->samples_))*param;
if( (xw.cols() != 1) || (xw.rows() != optInst->samples_->rows())){
std::cerr << "Error, column size must be 1" << std::endl;
std::cerr << "product of Xw is " << xw.rows()
<< " by " << xw.cols() << std::endl;
std::exit(-1);
}
int effective_cnt = 0;
Eigen::ArrayXd xwy(xw.rows());
xwy.setZero();
for(int i=0; i<xw.rows(); ++i){
double z = xw.coeff(i)*(optInst->labels_->coeff(i));
xwy.coeffRef(i) = z;
if( z < 1 ){
fx += (1-z)*(1-z);
++effective_cnt;
}
}
// fx /= optInst->labels_->rows();
// l2 regularization value
for(int i = optInst->prog_params_->l2start;
i <= optInst->prog_params_->l2end;
++i){
fx += 0.5*optInst->prog_params_->l2c * std::pow(param.coeff(i), 2);
}
grad.setZero();
for (int k = 0; k < optInst->samples_->outerSize(); ++k){
double z = xwy.coeff(k);
if(z >= 1){
continue;
}
double factor = -2 * optInst->labels_->coeff(k) * (1-z);
for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it( *(optInst->samples_) ,k);
it; ++it){
int colIdx = it.col();
grad.coeffRef(colIdx) += factor*it.value();
}
}
// grad /= optInst->samples_->rows();
//l2 regularization
for(int i = optInst->prog_params_->l2start;
i <= optInst->prog_params_->l2end;
++i){
grad.coeffRef(i) += optInst->prog_params_->l2c * param.coeff(i);
}
std::cout << "One computation of function value & gradient costs " << t.toc() << " seconds"
<< std::endl;
return fx;
}
void state::compute(int want, FitContext *fc)
{
state *st = (state*) this;
auto *oo = this;
for (auto c1 : components) {
if (c1->fitFunction) {
omxFitFunctionCompute(c1->fitFunction, want, fc);
} else {
omxRecompute(c1, fc);
}
}
if (!(want & FF_COMPUTE_FIT)) return;
int nrow = components[0]->rows;
for (auto c1 : components) {
if (c1->rows != nrow) {
mxThrow("%s: component '%s' has %d rows but component '%s' has %d rows",
oo->name(), components[0]->name(), nrow, c1->name(), c1->rows);
}
}
Eigen::VectorXd expect;
Eigen::VectorXd rowResult;
int numC = components.size();
Eigen::VectorXd tp(numC);
double lp=0;
for (int rx=0; rx < nrow; ++rx) {
if (expectation->loadDefVars(rx) || rx == 0) {
omxExpectationCompute(fc, expectation, NULL);
if (!st->transition || rx == 0) {
EigenVectorAdaptor Einitial(st->initial);
expect = Einitial;
if (expect.rows() != numC || expect.cols() != 1) {
omxRaiseErrorf("%s: initial prob matrix must be %dx%d not %dx%d",
name(), numC, 1, expect.rows(), expect.cols());
return;
}
}
if (st->transition && (st->transition->rows != numC || st->transition->cols != numC)) {
omxRaiseErrorf("%s: transition prob matrix must be %dx%d not %dx%d",
name(), numC, numC, st->transition->rows, st->transition->cols);
return;
}
}
for (int cx=0; cx < int(components.size()); ++cx) {
EigenVectorAdaptor Ecomp(components[cx]);
tp[cx] = Ecomp[rx];
}
if (st->verbose >= 4) {
mxPrintMat("tp", tp);
}
if (st->transition) {
EigenMatrixAdaptor Etransition(st->transition);
expect = (Etransition * expect).eval();
}
rowResult = tp.array() * expect.array();
double rowp = rowResult.sum();
rowResult /= rowp;
lp += log(rowp);
if (st->transition) expect = rowResult;
}
oo->matrix->data[0] = Global->llScale * lp;
if (st->verbose >= 2) mxLog("%s: fit=%f", oo->name(), lp);
}
void ba81NormalQuad::setup(double Qwidth, int Qpoints, double *means,
Eigen::MatrixXd &priCov, Eigen::VectorXd &sVar)
{
quadGridSize = Qpoints;
numSpecific = sVar.rows() * sVar.cols();
primaryDims = priCov.rows();
maxDims = primaryDims + (numSpecific? 1 : 0);
maxAbilities = primaryDims + numSpecific;
if (maxAbilities == 0) {
setup0();
return;
}
totalQuadPoints = 1;
for (int dx=0; dx < maxDims; dx++) {
totalQuadPoints *= quadGridSize;
}
if (int(Qpoint.size()) != quadGridSize) {
Qpoint.clear();
Qpoint.reserve(quadGridSize);
double qgs = quadGridSize-1;
for (int px=0; px < quadGridSize; ++px) {
Qpoint.push_back(px * 2 * Qwidth / qgs - Qwidth);
}
}
std::vector<double> tmpWherePrep(totalQuadPoints * maxDims);
Eigen::VectorXi quad(maxDims);
for (int qx=0; qx < totalQuadPoints; qx++) {
double *wh = tmpWherePrep.data() + qx * maxDims;
decodeLocation(qx, maxDims, quad.data());
pointToWhere(quad.data(), wh, maxDims);
}
totalPrimaryPoints = totalQuadPoints;
weightTableSize = totalQuadPoints;
if (numSpecific) {
totalPrimaryPoints /= quadGridSize;
speQarea.resize(quadGridSize * numSpecific);
weightTableSize *= numSpecific;
}
std::vector<double> tmpPriQarea;
tmpPriQarea.reserve(totalPrimaryPoints);
{
Eigen::VectorXd where(primaryDims);
for (int qx=0; qx < totalPrimaryPoints; qx++) {
decodeLocation(qx, primaryDims, quad.data());
pointToWhere(quad.data(), where.data(), primaryDims);
double den = exp(dmvnorm(primaryDims, where.data(), means, priCov.data()));
tmpPriQarea.push_back(den);
}
}
std::vector<int> priOrder;
priOrder.reserve(totalPrimaryPoints);
for (int qx=0; qx < totalPrimaryPoints; qx++) {
priOrder.push_back(qx);
}
if (0) {
sortAreaHelper priCmp(tmpPriQarea);
std::sort(priOrder.begin(), priOrder.end(), priCmp);
}
priQarea.clear();
priQarea.reserve(totalPrimaryPoints);
double totalArea = 0;
for (int qx=0; qx < totalPrimaryPoints; qx++) {
double den = tmpPriQarea[priOrder[qx]];
priQarea.push_back(den);
//double prevTotalArea = totalArea;
totalArea += den;
// if (totalArea == prevTotalArea) {
// mxLog("%.4g / %.4g = %.4g", den, totalArea, den / totalArea);
// }
}
for (int qx=0; qx < totalPrimaryPoints; qx++) {
priQarea[qx] *= One;
priQarea[qx] /= totalArea;
//mxLog("%.5g,", priQarea[qx]);
}
for (int sgroup=0; sgroup < numSpecific; sgroup++) {
totalArea = 0;
double mean = means[primaryDims + sgroup];
double var = sVar(sgroup);
for (int qx=0; qx < quadGridSize; qx++) {
double den = exp(dmvnorm(1, &Qpoint[qx], &mean, &var));
speQarea[sIndex(sgroup, qx)] = den;
totalArea += den;
}
for (int qx=0; qx < quadGridSize; qx++) {
speQarea[sIndex(sgroup, qx)] /= totalArea;
}
}
//pda(speQarea.data(), numSpecific, quadGridSize);
for (int sx=0; sx < int(speQarea.size()); ++sx) {
speQarea[sx] *= One;
}
//pda(speQarea.data(), numSpecific, quadGridSize);
wherePrep.clear();
wherePrep.reserve(totalQuadPoints * maxDims);
if (numSpecific == 0) {
for (int qx=0; qx < totalPrimaryPoints; qx++) {
int sortq = priOrder[qx] * maxDims;
for (int dx=0; dx < maxDims; ++dx) {
wherePrep.push_back(tmpWherePrep[sortq + dx]);
}
}
} else {
for (int qx=0; qx < totalPrimaryPoints; ++qx) {
int sortq = priOrder[qx] * quadGridSize;
for (int sx=0; sx < quadGridSize; ++sx) {
int base = (sortq + sx) * maxDims;
for (int dx=0; dx < maxDims; ++dx) {
wherePrep.push_back(tmpWherePrep[base + dx]);
}
}
}
}
// recompute whereGram because the order might have changed
whereGram.resize(triangleLoc1(maxDims), totalQuadPoints);
for (int qx=0; qx < totalQuadPoints; qx++) {
double *wh = wherePrep.data() + qx * maxDims;
gramProduct(wh, maxDims, &whereGram.coeffRef(0, qx));
}
}
