C++ VectorXd::cols方法代碼示例

  /** 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;
		}
//.........這裏部分代碼省略.........