C++ DblVec::size方法代码示例

void SingleTimestepCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists, DblVec& weights) {
  DblVec dofvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(dofvals);
  vector<Collision> collisions;
  GetCollisionsCached(dofvals, collisions);
  dists.reserve(dists.size() + collisions.size());
  weights.reserve(weights.size() + collisions.size());
  BOOST_FOREACH(const Collision& col, collisions) {
    Link2Int::iterator itA = m_link2ind.find(col.linkA),
                       itB = m_link2ind.find(col.linkB);
    if (itA != m_link2ind.end() || itB != m_link2ind.end()) {
      dists.push_back(col.distance);
      weights.push_back(col.weight);
    }
  }

void displayGradient(DblVec& gradientP){
	cout << "DEBUG DISPLAY GRADIENT\n";
	for(size_t i = 0; i < gradientP.size(); i++){
		if(gradientP[i] != 0) cout << i << " : " << gradientP[i] << endl;
	}
	cout << "DEBUG DISPLAY GRADIENT OVER\n";
}

double cal_score(vector <size_t> instance){
	//f(x)=(UW)(TV)' + Px
	double score = 0.0;
	DblVec UW;
	DblVec TV;
	int dimLatent = W.size() / getUserFeaCount();
	for (size_t j = 0 ; j < dimLatent; j++){
		UW.push_back(0);
		TV.push_back(0);
	}
	for (size_t j = 0; j < instance.size(); j++){
		score += P[instance[j]] * 1.0;
		if(instance[j] < getAdFeaCount()){
			for(size_t k = 0; k < dimLatent; k++){
				int v_index = instance[j] * dimLatent + k;
				TV[k]+=V[v_index];
			}
		} 
		else if(instance[j] < getAdFeaCount() + getUserFeaCount()){
			for(size_t k = 0; k < dimLatent; k++){
				int w_index = (instance[j] - getAdFeaCount())*dimLatent + k;
				UW[k] += W[w_index];
			}
		}
	}
	
	for (size_t j = 0; j < dimLatent; j++){
		score += UW[j]*TV[j];
	}

	return score;
}

ConvexConstraintsPtr ZMPConstraint::convex(const DblVec& x, Model* model) {
  DblVec curvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(curvals);
  DblMatrix jacmoment = DblMatrix::Zero(3, curvals.size());
  OR::Vector moment(0,0,0);
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      OR::Vector cm = link->GetGlobalCOM();
      moment += cm * link->GetMass();
      jacmoment += m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  jacmoment /= totalmass;

  AffExpr x_expr = AffExpr(moment.x) + varDot(jacmoment.row(0), m_vars) - jacmoment.row(0).dot(toVectorXd(curvals));
  AffExpr y_expr = AffExpr(moment.y) + varDot(jacmoment.row(1), m_vars) - jacmoment.row(1).dot(toVectorXd(curvals));

  ConvexConstraintsPtr out(new ConvexConstraints(model));
  for (int i=0; i < m_ab.rows(); ++i) {
    out->addIneqCnt(m_ab(i,0) * x_expr + m_ab(i,1) * y_expr + m_c(i));
  }
  return out;
}

double OptimizerState::dotProduct(const DblVec& a, const DblVec& b){
	double result = 0;
	for(size_t i=0; i < a.size(); i++){
		result += a[i] * b[i];
	}
	return result;
}

double LogisticRegressionObjective::EvalLocal(const DblVec& input, DblVec& gradient){
	double loss = 0.0;
	for (size_t i = 0; i < input.size(); i++){
		loss += 0.5 * input[i] * input[i] * l2weight;
		gradient[i] = l2weight * input[i];
	}
	

	for (size_t i = 0; i < problem.NumInstances(); i++){
		double score = problem.ScoreOf(i, input);
		double insLoss, insProb;
		if (score < -30){
			insLoss = -score;
			insProb = 0;
		}else if (score > 30){
			insLoss = 0;
			insProb = 1;
		}else {
			double temp = 1.0 + exp(-score);
			insLoss = log(temp);
			insProb = 1.0/ temp;
		}
		loss += insLoss;
		problem.AddMultTo(i, 1.0 - insProb, gradient);	
	}
	return loss ;
}

void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type,
  const DblVec& init, const DblVec& sol) {
    OptProbPtr prob;
    size_t n = init.size();
    assert (sol.size() == n);
    setupProblem(prob, n);
    prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true)));
    prob->addConstraint(ConstraintPtr(new ConstraintFromFunc(g, prob->getVars(), VectorXd(), cnt_type,"g")));
    BasicTrustRegionSQP solver(prob);
    solver.max_iter_ = 1000;
    solver.min_trust_box_size_ = 1e-5;
    solver.min_approx_improve_ = 1e-10;
    solver.merit_error_coeff_ = 1;
    
    solver.initialize(init);
    OptStatus status = solver.optimize();
    EXPECT_EQ(status, OPT_CONVERGED);
    expectAllNear(solver.x(), sol, .01);
}

VectorXd RotationError::operator()(const VectorXd& a) const {
    DblVec phis;
    DblVec Deltas;
    phis = toDblVec(a);
    VectorXd ret(1);
    ret(0) = -this->total_rotation_limit;
    for (int i = 0; i < phis.size(); ++i) {
        ret(0) += fabs(phis[i]);
    }
    return ret;
}

double calSparsity(const DblVec& vec, size_t dimLatent){
	int num = vec.size() / dimLatent;
	int sparseNum = 0;
	double sum = 0;
	for(int i = 0; i < num; i++){ 
		sum = 0;
		for(int j = 0; j < dimLatent; j++)
		sum += vec[i*dimLatent+j] * vec[i*dimLatent+j];
		if(sum < 1e-10) sparseNum++;
	}
	return (double)sparseNum / num;
} 

void printVector(const DblVec &vec, const char* filename) {
	ofstream outfile(filename);
	if (!outfile.good()) {
		cerr << "error opening matrix file " << filename << endl;
		exit(1);
	}

	for (size_t i=0; i<vec.size(); i++) {
		outfile << vec[i] << endl;
	}
	outfile.close();
}

double LogisticRegressionObjective::EvalLocalMultiThread(const DblVec& input, DblVec& gradient){
	
	/*
	create 24 thread;
	each thread calculate a loss and gradient
	*/
	int threadNum = 24;
	double lossList[threadNum];
	DblVec *gradList = new DblVec[threadNum];
	for(int i = 0; i < threadNum; i++){
		gradList[i] = DblVec(gradient.size());
	}
	threadList = new pthread_t[threadNum];
	
	for(int i = 0; i < threadNum; i++){
		Parameter*p = new Parameter(*this, input, gradList[i], lossList[i], i, threadNum);
		pthread_create(&threadList[i], NULL, ThreadEvalLocal, p);
	}
	
	for(int i = 0; i < threadNum; i++){
		pthread_join(threadList[i], NULL);
	}
	
	double loss = 0.0;
	for(int j = 0; j < gradient.size(); j++){
		gradient[j] = 0.0;
	}
	for(int i = 0; i < threadNum; i++){
		loss += lossList[i];
		for(int j = 0; j < gradient.size(); j++){
			gradient[j] += gradList[i][j];
		}
	}
	delete []gradList;
	delete []threadList;
	return loss;
	
}

double Mse(const DblVec &rec, const char* filename) {
	static DblVec temp(rec.size());
	ifstream infile(filename);
	if (!infile.good()) {
		cerr << "error opening matrix file " << filename << endl;
		exit(1);
	}
	// outfile << "%%MatrixMarket matrix array real general" << endl;
	// outfile << "1 " << vec.size() << endl;
	for (size_t i=0; i<rec.size(); i++) {
		float val;
		infile >> val;
		temp[i] = val;
	}
	infile.close();

	double final_mse = 0;
	for (size_t i=0; i<rec.size();i++){
		final_mse += (rec[i]-temp[i])*(rec[i]-temp[i]);
	}
	final_mse /=rec.size();
	return final_mse;
}

double LogisticRegressionObjective::Eval(const DblVec& input, DblVec& gradient){
//	clock_t start,finish;
//	double totaltime;

	DblVec localInput = input;
	DblVec localGradient = gradient;
	MPI_Bcast(&localInput[0], localInput.size(), MPI_DOUBLE, 0, MPI_COMM_WORLD);	
	MPI_Bcast(&localGradient[0], localGradient.size(), MPI_DOUBLE, 0, MPI_COMM_WORLD);

	

	double loss = EvalLocalMultiThread(localInput, localGradient);
	
	
//	start=clock();
//	double loss = EvalLocal(localInput, localGradient);	
	double gloss = 0.0;
	MPI_Reduce(&localGradient[0], &gradient[0], input.size(), MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
	MPI_Reduce(&loss, &gloss, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
//	finish=clock();
//	totaltime=(double)(finish-start)/CLOCKS_PER_SEC;
//	cout<<"\nReduce Time: "<<totaltime<<"seconds"<<endl;
	return gloss;
}

 double NeedleCollisionClearanceCost::value(const vector<double>& x, Model* model) {
   DblVec dists;
   EnvironmentBasePtr env = helper->pis[0]->local_configs[0]->GetEnv();
   BOOST_FOREACH(const CollisionEvaluatorPtr& cc, this->ccs) {
     DblVec tmp_dists;
     cc->CalcDists(x, tmp_dists);
     for (int i = 0; i < tmp_dists.size(); ++i) {
       dists.push_back(-tmp_dists[i]);
     }
   }
   if (dists.size() > 0) {
     return vecMax(dists) * this->coeff;
   } else {
     return - helper->collision_clearance_threshold * this->coeff;
   }
 }

void CirclesSeparation::initialize(DblVec &x, DblVec &y, DblVec r, DblVec m) {
	_N = (int)x.size();
	double minx = 10000, maxx = -10000, miny = 10000, maxy = -10000;
	int i;
	for (i = 0; i < _N; ++i) {
		minx = min(minx, x[i]);
		maxx = max(maxx, x[i]);
		miny = min(miny, y[i]);
		maxy = max(maxy, y[i]);
	}
	_cenx = (minx+maxx) * 0.5;
	_ceny = (miny+maxy) * 0.5;
	for (i = 0; i < _N; ++i) {
		x[i] -= _cenx, y[i] -= _ceny;
	}
	_x = x, _y = y, _r = r, _m = m, _ox = x, _oy = y;
}