C++ VectorXf::maxCoeff方法代码示例

float mmf::OptSO3MMFvMF::conjugateGradientCUDA_impl(Matrix3f& R, float res0,
    uint32_t n, uint32_t maxIter) {
  float f = 0;

  std::cout << this->cld_.xSums() << std::endl;
  for (uint32_t k=0; k<K(); ++k) {
    Eigen::Matrix3f N = Eigen::Matrix3f::Zero();
    for (uint32_t j=k*6; j<6*(k+1); ++j) { 
      Eigen::Vector3f m = Eigen::Vector3f::Zero();
      m((j%6)/2) = (j%6)%2==0?-1.:1.;
      N += m*this->cld_.xSums().col(j).transpose();
    }
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(N,Eigen::ComputeThinU|Eigen::ComputeThinV);
    Rs_[k] = svd.matrixV()*svd.matrixU().transpose();
    if (Rs_[k].determinant() < 0.) Rs_[k] *= -1.;
    f += (N*R).trace();
    std::cout << N << std::endl;
    std::cout << Rs_[k] << std::endl;
  }

  Eigen::VectorXf mfCounts = Eigen::VectorXf::Zero(K());
  for (uint32_t k=0; k<K()*6; ++k) {
    mfCounts(k/6) += this->cld_.counts()(k);
  }
  cout<<"counts: "<<endl<<this->cld_.counts().transpose()<<endl;
  std::cout << " mf counts: " << mfCounts.transpose() << std::endl;
  uint32_t iMax = 0;
  mfCounts.maxCoeff(&iMax);
  R = Rs_[iMax];
//  std::cout << "N" << std::endl << N << std::endl;
//  std::cout << "R" << std::endl << R << std::endl;
  return f;
}

///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

Eigen::MatrixXf
powerspectrum::from_pcm(const Eigen::VectorXf& pcm_samples)
{
    MINILOG(logTRACE) << "Powerspectrum computation. input samples="
            << pcm_samples.size();
    // check if inputs are sane
    if ((pcm_samples.size() < win_size) || (hop_size > win_size)) {
        return Eigen::MatrixXf(0, 0);
    }
    size_t frames = (pcm_samples.size() - (win_size-hop_size)) / hop_size;
    size_t freq_bins = win_size/2 + 1;

    // initialize power spectrum
    Eigen::MatrixXf ps(freq_bins, frames);

    // peak normalization value
    float pcm_scale = std::max(fabs(pcm_samples.minCoeff()),
            fabs(pcm_samples.maxCoeff()));

    // scale signal to 96db (16bit)
    pcm_scale =  std::pow(10.0f, 96.0f/20.0f) / pcm_scale;

    // compute the power spectrum
    for (size_t i = 0; i < frames; i++) {

        // fill pcm
        for (int j = 0; j < win_size; j++) {
            kiss_pcm[j] = pcm_samples(i*hop_size+j) * pcm_scale * win_funct(j);
        }

        // fft
        kiss_fftr(kiss_status, kiss_pcm, kiss_freq);

        // save powerspectrum frame
        Eigen::MatrixXf::ColXpr psc(ps.col(i));
        for (int j = 0; j < win_size/2+1; j++) {
            psc(j) =
                    std::pow(kiss_freq[j].r, 2) + std::pow(kiss_freq[j].i, 2);
        }
    }

    MINILOG(logTRACE) << "Powerspectrum finished. size=" << ps.rows() << "x"
            << ps.cols();
    return ps;
}

//.........这里部分代码省略.........
	}

	ROS_INFO("Action server started, sending goal.");

	// send a goal to the action
	for (int i = 0; i < workers; i++) {
		ac_list[i]->sendGoal(goals[i]);
	}

	//wait for the action to return
	std::vector<bool> finished;
	for (int i = 0; i < workers; i++) {
		bool finished_before_timeout = ac_list[i]->waitForResult(
				ros::Duration(30.0));
		finished.push_back(finished_before_timeout);
	}

	bool success = true;
	for (int i = 0; i < workers; i++) {
		success = finished[i] && success;
	}

	Eigen::MatrixXf acc_JtJ;
	acc_JtJ.setZero(size * 6, size * 6);
	Eigen::VectorXf acc_Jte;
	acc_Jte.setZero(size * 6);

	if (success) {

		for (int i = 0; i < workers; i++) {
			Eigen::MatrixXf JtJ;
			JtJ.setZero(size * 6, size * 6);
			Eigen::VectorXf Jte;
			Jte.setZero(size * 6);

			rm_multi_mapper::Vector rosJte = ac_list[i]->getResult()->Jte;
			rm_multi_mapper::Matrix rosJtJ = ac_list[i]->getResult()->JtJ;

			vector2eigen(rosJte, Jte);

			matrix2eigen(rosJtJ, JtJ);

			acc_JtJ += JtJ;
			acc_Jte += Jte;

		}

	} else {
		ROS_INFO("Action did not finish before the time out.");
		std::exit(0);
	}

	Eigen::VectorXf update = -acc_JtJ.ldlt().solve(acc_Jte);

	float iteration_max_update = std::max(std::abs(update.maxCoeff()),
			std::abs(update.minCoeff()));

	ROS_INFO("Max update %f", iteration_max_update);

	/*for (int i = 0; i < (int)frames.size(); i++) {

	 frames[i]->get_pos() = Sophus::SE3f::exp(update.segment<6>(i))
	 * frames[i]->get_pos();

	 std::string query = "UPDATE `positions` SET `q0` = " + 
	 boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[0]) +
	 ", `q1` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[1]) +
	 ", `q2` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[2]) +
	 ", `q3` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[3]) +
	 ", `t0` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[0]) +
	 ", `t1` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[1]) +
	 ", `t2` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[2]) +
	 ", `int0` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[0]) +
	 ", `int1` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[1]) +
	 ", `int2` = " +
	 boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[2]) +
	 " WHERE `id` = " +
	 boost::lexical_cast<std::string>(i) +
	 ";";

	 res = U.sql_query(query);
	 delete res;
	 

	 }*/
	timestamp_t t1 = get_timestamp();

	double secs = (t1 - t0) / 1000000.0L;
	std::cout << secs << std::endl;
	return 0;

}

int main(int argc, char *argv[])
{

  // Load a mesh
  igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);

  printf("--Initialization--\n");
  V_border = igl::is_border_vertex(V,F);
  igl::adjacency_list(F, VV);
  igl::vertex_triangle_adjacency(V,F,VF,VFi);
  igl::triangle_triangle_adjacency(F,TT,TTi);
  igl::edge_topology(V,F,E,F2E,E2F);

  // Generate "subdivided" mesh for visualization of curl terms
  igl::false_barycentric_subdivision(V, F, Vbs, Fbs);

  // Compute scale for visualizing fields
  global_scale =  .2*igl::avg_edge_length(V, F);

  //Compute scale for visualizing texture
  uv_scale = 0.6/igl::avg_edge_length(V, F);

  // Compute face barycenters
  igl::barycenter(V, F, B);

  // Compute local basis for faces
  igl::local_basis(V,F,B1,B2,B3);

  //Generate random vectors for constraints
  generate_constraints();

  // Interpolate a 2-PolyVector field to be used as the original field
  printf("--Initial solution--\n");
  igl::n_polyvector(V, F, b, bc, two_pv_ori);

  // Post process original field
  // Compute curl_minimizing matchings and curl
  Eigen::MatrixXi match_ab, match_ba;  // matchings across interior edges
  printf("--Matchings and curl--\n");
  double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
  double maxCurl = curl_ori.maxCoeff();
  printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl,  avgCurl);

  printf("--Singularities--\n");
  // Compute singularities
  igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
  printf("original #singularities: %ld\n", singularities.rows());

  printf("--Cuts--\n");
 // Get mesh cuts based on singularities
  igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);

  printf("--Combing--\n");
// Comb field
  Eigen::MatrixXd combed;
  igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);

  printf("--Cut mesh--\n");
  // Reconstruct integrable vector fields from combed field
  igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);

  printf("--Poisson--\n");
  double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
  double maxPoisson = poisson_error_ori.maxCoeff();
  printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);


  // Set the curl-free 2-PolyVector to equal the original field
  two_pv = two_pv_ori;
  singularities = singularities_ori;
  curl = curl_ori;
  cuts = cuts_ori;
  two_pv_poisson = two_pv_poisson_ori;
  poisson_error = poisson_error_ori;
  Vcut = Vcut_ori;
  Fcut = Fcut_ori;
  scalars = scalars_ori;

  printf("--Integrable - Precomputation--\n");
  // Precompute stuff for solver
  igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);

  cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = &key_down;
  viewer.core.show_lines = false;
  key_down(viewer,'2',0);

  // Replace the standard texture with an integer shift invariant texture
  line_texture(texture_R, texture_G, texture_B);

  viewer.launch();

  return 0;
}

bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{

  if (key == '1')
  {
    display_mode = 1;
    update_display(viewer);
  }

  if (key == '2')
  {
    display_mode = 2;
    update_display(viewer);
  }

  if (key == '3')
  {
    display_mode = 3;
    update_display(viewer);
  }

  if (key == '4')
  {
    display_mode = 4;
    update_display(viewer);
  }

  if (key == '5')
  {
    display_mode = 5;
    update_display(viewer);
  }

  if (key == '6')
  {
    display_mode = 6;
    update_display(viewer);
  }

  if (key == '7')
  {
    display_mode = 7;
    update_display(viewer);
  }

  if (key == '8')
  {
    display_mode = 8;
    update_display(viewer);
  }

  if (key == '9')
  {
    display_mode = 9;
    update_display(viewer);
  }

  if (key == '0')
  {
    display_mode = 0;
    update_display(viewer);
  }

  if (key == 'A')
  {
    //do a batch of iterations
    printf("--Improving Curl--\n");
    for (int bi = 0; bi<5; ++bi)
    {
      printf("\n\n **** Batch %d ****\n", iter);
      igl::integrable_polyvector_fields_solve(ipfdata, params, two_pv, iter ==0);
      iter++;
      params.wSmooth *= params.redFactor_wsmooth;
    }
    // Post process current field
    // Compute curl_minimizing matchings and curl
    printf("--Matchings and curl--\n");
    Eigen::MatrixXi match_ab, match_ba;  // matchings across interior edges
    double avgCurl = igl::polyvector_field_matchings(two_pv, V, F, true, match_ab, match_ba, curl);
    double maxCurl = curl.maxCoeff();
    printf("curl -- max: %.5g, avg: %.5g\n", maxCurl,  avgCurl);
    // Compute singularities
    printf("--Singularities--\n");
    igl::polyvector_field_singularities_from_matchings(V, F, match_ab, match_ba, singularities);
    printf("#singularities: %ld\n", singularities.rows());
    // Get mesh cuts based on singularities
    printf("--Cuts--\n");
    igl::polyvector_field_cut_mesh_with_singularities(V, F, singularities, cuts);
    // Comb field
    printf("--Combing--\n");
    Eigen::MatrixXd combed;
    igl::polyvector_field_comb_from_matchings_and_cuts(V, F, two_pv, match_ab, match_ba, cuts, combed);
    // Reconstruct integrable vector fields from combed field
    printf("--Cut mesh--\n");
    igl::cut_mesh(V, F, cuts, Vcut, Fcut);
    printf("--Poisson--\n");
    double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut, Fcut, combed, scalars, two_pv_poisson, poisson_error);
    double maxPoisson = poisson_error.maxCoeff();
    printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);

//.........这里部分代码省略.........