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

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

float rttemp1( VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda, VectorXf initial){
  //Calculate the cost of the given warping
  // t_reg  : time grid of y_reg
  // curvei : query curve
  // curvek : reference curve
  // nknots : number of knots
  // lambda : time distortion penalty parameter
  // initial: position of knots on the simplex 
    
  int N = t_reg.size();
  VectorXf struct_ = VectorXf::LinSpaced( nknots+2, t_reg(0)  , t_reg.maxCoeff() ); 
  VectorXf hik(N);
  VectorXf Q(2+ initial.size()) ;           //Solution with the placement of the knots on the simplex
  Q(0) = t_reg(0) ; Q(1+ initial.size()) = t_reg.maxCoeff()  ; Q.segment(1, initial.size()) = initial;
  hik =  fnvalspapi( struct_, Q , t_reg);   // compute the new internal time-scale  
  
  //cout << "hik: " << hik.transpose() << endl;
  //cout << "Monotonicity Checked on Hik: " << MonotonicityCheck(hik) << endl;
  //if(  MonotonicityCheck(hik) == -1) { cout <<" Q.transpose()  is :"<< Q.transpose()  << endl;}
  
  return ( (fnvalspapi(t_reg,curvei,hik)-fnvalspapi(t_reg,curvek ,t_reg)).array().pow(2).sum() + lambda * (hik - t_reg).array().pow(2).sum() );
}

rthink_Output rthik_SA(VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda){
  // Random Search solver for the optimization problem of pairwise warping
  // t_reg : time_grid
  // curvei: query curve
  // curvek: reference curve 
  // nknots : number of knots
  // lambda : time distortion penalty parameter
    
  int k=0; float OldSol, bk; 
  VectorXf xk(nknots);   
  bk = (t_reg.maxCoeff() - t_reg(0))/(1+ float(nknots ));   //Distance between adjacent knots and edges-knots
  xk  = VectorXf::LinSpaced(nknots , t_reg(0) + bk  ,  - bk + t_reg.maxCoeff()  ); //Initial candidate solution with equispaced knots 
  VectorXf xn(nknots);   VectorXf help(2+nknots); 
 
  float NewSol; 
  OldSol =  rttemp1(t_reg, curvei, curvek, nknots , lambda, xk);  //Cost of initial solution
  int z= 99*nknots;                                               //Number of random search to do (proportional to the # of knots)
  //srand(1);                                                     //Fix the seed to have reproducable behaviour
  VectorXf Steps(z); Steps = (ArrayXf::Random(z)+1.)/2.;          //Generate possible random pertubations magnitude
  VectorXi Posit(z); for (int u=0; u <z; u++ ) Posit(u) =1+ rand()%(nknots+0); //Generate list of positions to purturb
 
  k=0;
  while((OldSol > .0001) && (k<z)) {
    xn = NewSolution(xk,Steps(k), Posit(k), t_reg );              //Get a new solution
    NewSol = rttemp1(t_reg, curvei, curvek, nknots, lambda, xn);  //Cost of new solution
    if (  (NewSol < OldSol)  ) {                                  //If it's better than the old one, use it.
      OldSol= NewSol; xk= xn; 
    }
  k++;
}

  VectorXf x3 =  VectorXf::LinSpaced(2+nknots, t_reg(0), t_reg( t_reg.size()-1)); 
  help(0) =  t_reg(0) ; help(nknots+1) = t_reg( t_reg.size()-1) ; help.segment(1,nknots) = xk;  

  rthink_Output G;
  G.Val = OldSol;
  G.Mapping = fnvalspapi(  x3 , help  , t_reg);
  return G ;
}

VectorXf NewSolution( VectorXf x0 , float Step, int point, VectorXf t_reg){ 
    //generate new solution on the simplex defined in [t_reg(0)-x0-t_reg(N-1)] using a displacement of size Step
    // x0   : initial solution
    // Step : displacement size
    // point: knot to perturb
    // t_reg: time_grid
    
    VectorXf InitConf (2+ x0.size());
    InitConf(0) = t_reg(0); InitConf( x0.size()+1) = t_reg.maxCoeff()  ; InitConf.segment(1, x0.size()) = x0; 
    float LowBou =  InitConf(point-1);
    float UppBou =  InitConf(point+1);
    float New_State = (UppBou - LowBou) * Step + LowBou; 
    InitConf(point) = New_State; 
    //if (  MonotonicityCheck( InitConf.segment(1, x0.size()) ) == -1) { 
    //    cout << "We generated a unacceptable solutiion" << endl << "Initial seed was : " << x0.transpose() 
    //         << endl << " and we produced :"<<InitConf.segment(1, x0.size()).transpose() << endl;}
    return (InitConf.segment(1, x0.size()) ) ;
  } 

void wavePlotter::autoScaleRange(VectorXf &data)
{
	lowRange = data.minCoeff();//Utils::ofMin(&data(0), DATA_SIZE);
	highRange = data.maxCoeff();//Utils::ofMax(&data(0), DATA_SIZE);
}

void D3DCloudProjector::projectCloud(int id, const sensor_msgs::PointCloud& data, const std::vector<int>& interest_region_indices) {
  MatrixXf& oriented = orienter_->oriented_clouds_[id];

  // -- Get a copy of the projected points.
  MatrixXf projected(oriented.rows(), 2);
  int c=0;
  for(int i=0; i<3; ++i) {
    if(i == axis_of_projection_)
      continue;
    projected.col(c) = oriented.col(i);
    ++c;
  }

  // -- Transform into pixel units.  projected is currently in meters, centered at 0.
  //projected *= pixels_per_meter_;
  for(int i=0; i<projected.rows(); ++i) {
    projected(i, 0) *= pixels_per_meter_;
    projected(i, 1) *= pixels_per_meter_;
  }
  
  
  // -- Find min and max of u and v.  TODO: noise sensitivity?
  // u is the col number in the image plane, v is the row number.
  float min_v = FLT_MAX;
  float min_u = FLT_MAX;
  float max_v = -FLT_MAX;
  float max_u = -FLT_MAX;
  for(int i=0; i<projected.rows(); ++i) {
    float u = projected(i, 0);
    float v = projected(i, 1);
    if(u < min_u)
      min_u = u;
    if(u > max_u)
      max_u = u;
    if(v < min_v)
      min_v = v;
    if(v > max_v)
      max_v = v;
  }

  // -- Translate to coordinate system where (0,0) is the upper right of the image.
  for(int i=0; i<projected.rows(); ++i) {
    projected(i, 0) -= min_u;
    projected(i, 1) = max_v - projected(i, 1);
  }
  
  // -- Get the max depth.
  float max_depth = -FLT_MAX;
  float min_depth = FLT_MAX;
  for(int i=0; i<oriented.rows(); ++i) {
    if(oriented(i, axis_of_projection_) > max_depth)
      max_depth = oriented(i, axis_of_projection_);
    if(oriented(i, axis_of_projection_) < min_depth)
      min_depth = oriented(i, axis_of_projection_);
  }

  // -- Compute the normalized depths.  Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest.
  VectorXf depths = oriented.col(axis_of_projection_);
  if(axis_of_projection_ == 1)
    depths = -depths;
  depths = depths.cwise() - depths.minCoeff();
  depths = depths / depths.maxCoeff();
  
      
  
  // -- Fill the IplImages.
  assert(sizeof(float) == 4);
  CvSize size = cvSize(ceil(max_u - min_u), ceil(max_v - min_v));
  IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1);
  IplImage* intensity = cvCreateImage(size, IPL_DEPTH_32F, 1);
  IplImage* depth = cvCreateImage(size, IPL_DEPTH_32F, 1);
  cvSetZero(acc);
  cvSetZero(depth);
  cvSetZero(intensity);
  assert(projected.rows() == (int)interest_region_indices.size());
  for(int i=0; i<projected.rows(); ++i) {
    int row = floor(projected(i, 1));
    int col = floor(projected(i, 0));

    // Update accumulator.
    assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
    ((float*)(acc->imageData + row * acc->widthStep))[col]++;

    // Add to intensity values.
    float val = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
    assert(val <= 1.0 && val >= 0.0);
    ((float*)(intensity->imageData + row * intensity->widthStep))[col] += val;

    // Add to depth values.
    ((float*)(depth->imageData + row * depth->widthStep))[col] += depths(i); //
  }
  
  // -- Normalize by the number of points falling in each pixel.
  for(int v=0; v<acc->height; ++v) {
    float* intensity_ptr = (float*)(intensity->imageData + v * intensity->widthStep);
    float* depth_ptr = (float*)(depth->imageData + v * depth->widthStep);
    float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
    for(int u=0; u<acc->width; ++u) {
      if(*acc_ptr == 0) {
	*intensity_ptr = 0;
//.........这里部分代码省略.........

void CloudProjector::_compute() {
    assert(orienter_);
    assert(orienter_->getOutputCloud());
    assert(!depth_projection_);
    assert(!intensity_projection_);

    MatrixXf& oriented = *orienter_->getOutputCloud();
    VectorXf& intensities = *orienter_->getOutputIntensity();

    // -- Get a copy of the projected points.
    MatrixXf projected(oriented.rows(), 2);
    int c=0;
    for(int i=0; i<3; ++i) {
        if(i == axis_of_projection_)
            continue;
        projected.col(c) = oriented.col(i);
        ++c;
    }

    // -- Transform into pixel units.  projected is currently in meters, centered at 0.
    //projected *= pixels_per_meter_;
    for(int i=0; i<projected.rows(); ++i) {
        projected(i, 0) *= pixels_per_meter_;
        projected(i, 1) *= pixels_per_meter_;
    }


    // -- Find min and max of u and v.  TODO: noise sensitivity?
    // u is the col number in the image plane, v is the row number.
    float min_v = FLT_MAX;
    float min_u = FLT_MAX;
    float max_v = -FLT_MAX;
    float max_u = -FLT_MAX;
    for(int i=0; i<projected.rows(); ++i) {
        float u = projected(i, 0);
        float v = projected(i, 1);
        if(u < min_u)
            min_u = u;
        if(u > max_u)
            max_u = u;
        if(v < min_v)
            min_v = v;
        if(v > max_v)
            max_v = v;
    }

    // -- Translate to coordinate system where (0,0) is the upper right of the image.
    for(int i=0; i<projected.rows(); ++i) {
        projected(i, 0) -= min_u;
        projected(i, 1) = max_v - projected(i, 1);
    }

    // -- Get the max depth.
    float max_depth = -FLT_MAX;
    float min_depth = FLT_MAX;
    for(int i=0; i<oriented.rows(); ++i) {
        if(oriented(i, axis_of_projection_) > max_depth)
            max_depth = oriented(i, axis_of_projection_);
        if(oriented(i, axis_of_projection_) < min_depth)
            min_depth = oriented(i, axis_of_projection_);
    }

    // -- Compute the normalized depths.  Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest.
    VectorXf depths = oriented.col(axis_of_projection_);
    if(axis_of_projection_ == 1)
        depths = -depths;
    depths = (depths.array() - depths.minCoeff()).matrix();
    depths = depths / depths.maxCoeff();


    // -- Fill the IplImages.
    assert(sizeof(float) == 4);
    CvSize size = cvSize(ceil(max_u - min_u) + 1, ceil(max_v - min_v) + 1);
    float pad_width = 0;
    if(min_width_ > 0 && size.width < min_width_) {
        pad_width = (float)(min_width_ - size.width) / 2.;
        size.width = min_width_;
    }
    float pad_height = 0;
    if(min_height_ > 0 && size.height < min_height_) {
        pad_height = (float)(min_height_ - size.height) / 2.;
        size.height = min_height_;
    }
    IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1);
    intensity_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
    depth_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSetZero(acc);
    cvSetZero(depth_projection_);
    cvSetZero(intensity_projection_);
    assert(intensities.rows() == projected.rows());
    for(int i=0; i<projected.rows(); ++i) {
        int row = floor(projected(i, 1) + pad_height);
        int col = floor(projected(i, 0) + pad_width);

        assert(row < size.height && row >= 0 && col < size.width && col >= 0);

        // Update accumulator.
        ((float*)(acc->imageData + row * acc->widthStep))[col]++;

        // Update intensity values.
//.........这里部分代码省略.........