C++ DblVec类代码示例

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

 size_t NeedleCollisionHash::hash(const DblVec& x) {
   DblVec extended_x = x;
   for (int i = 0; i < helper->pis.size(); ++i) {
     for (int j = 0; j < helper->pis[i]->local_configs.size(); ++j) {
       DblVec state = toDblVec(logDown(helper->pis[i]->local_configs[j]->pose));
       extended_x.insert(extended_x.end(), state.begin(), state.end());
     }
   }
   return boost::hash_range(extended_x.begin(), extended_x.end());
 }

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

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

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

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

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

void InitInfo::fromJson(const Json::Value& v) {
  string type_str;
  childFromJson(v, type_str, "type");
  int n_steps = gPCI->basic_info.n_steps;
  int n_dof = gPCI->rad->GetDOF();

  if (type_str == "stationary") {
    data = toVectorXd(gPCI->rad->GetDOFValues()).transpose().replicate(n_steps, 1);
  }
  else if (type_str == "given_traj") {
    FAIL_IF_FALSE(v.isMember("data"));
    const Value& vdata = v["data"];
    if (vdata.size() != n_steps) {
      PRINT_AND_THROW("given initialization traj has wrong length");
    }
    data.resize(n_steps, n_dof);
    for (int i=0; i < n_steps; ++i) {
      DblVec row;
      fromJsonArray(vdata[i], row, n_dof);
      data.row(i) = toVectorXd(row);
    }
  }
  else if (type_str == "straight_line") {
    FAIL_IF_FALSE(v.isMember("endpoint"));
    DblVec endpoint;
    childFromJson(v, endpoint, "endpoint");
    if (endpoint.size() != n_dof) {
      PRINT_AND_THROW(boost::format("wrong number of dof values in initialization. expected %i got %j")%n_dof%endpoint.size());
    }
    data = TrajArray(n_steps, n_dof);
    DblVec start = gPCI->rad->GetDOFValues();
    for (int idof = 0; idof < n_dof; ++idof) {
      data.col(idof) = VectorXd::LinSpaced(n_steps, start[idof], endpoint[idof]);
    }
  }

}

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