C++ MatrixXf::array方法代码示例

	virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const {
		if (ktype_ == CONST_KERNEL)
			return VectorXf();
		MatrixXf fg = featureGradient( a, b );
		if (ktype_ == DIAG_KERNEL)
			return (f_.array()*fg.array()).rowwise().sum();
		else {
			MatrixXf p = fg*f_.transpose();
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}

	MatrixXf featureGradient( const MatrixXf & a, const MatrixXf & b ) const {
		if (ntype_ == NO_NORMALIZATION )
			return kernelGradient( a, b );
		else if (ntype_ == NORMALIZE_SYMMETRIC ) {
			MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
			MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
			MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
			VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
			MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
		}
		else if (ntype_ == NORMALIZE_AFTER ) {
			MatrixXf fb = lattice_.compute( b );
			
			MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
			VectorXf norm2 = norm_.array()*norm_.array();
			MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b );
		}
		else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
			MatrixXf fa = lattice_.compute( a, true );
			
			MatrixXf ones = MatrixXf::Ones( a.rows(), a.cols() );
			VectorXf norm2 = norm_.array()*norm_.array();
			MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
			return -r+kernelGradient( a, b*norm_.asDiagonal() );
		}
	}

void normalizePts(MatrixXf &mat, Matrix3f &T) {

  float cx = mat.col(0).mean();
  float cy = mat.col(1).mean();
  mat.array().col(0) -= cx;
  mat.array().col(1) -= cy;
  
  float sqrt_2 = sqrt(2);
  float meandist = (mat.array().col(0)*mat.array().col(0) + mat.array().col(1)*mat.array().col(1)).sqrt().mean();
  float scale = sqrt_2/meandist;
  mat.leftCols<2>().array() *= scale; 

  T << scale, 0, -scale*cx, 0, scale, -scale*cy, 0, 0, 1;
}

VectorXf EMclustering::logsumexp(MatrixXf x, int dim)
{
	int r = x.rows();
	int c = x.cols();

	VectorXf y(r);
	MatrixXf tmp1(r,c);
	VectorXf tmp2(r);
	VectorXf s(r);

	y = x.rowwise().maxCoeff();//cerr<<"y"<<y<<endl<<endl;
	x = x.colwise() - y;	
	//cerr<<"x"<<x<<endl<<endl;
	tmp1 = x.array().exp();	
	//cerr<<"t"<<tmp1<<endl<<endl;
	tmp2 = tmp1.rowwise().sum();	
	//cerr<<"t"<<tmp2<<endl<<endl;
	s = y.array() + tmp2.array().log();

	for(int i=0;i<s.size();i++)
	{
		if(!isfinite(s(i)))
		{
			s(i) = y(i);
		}
	}

	y.resize(0);
	tmp1.resize(0,0);
	tmp2.resize(0);
	
	return s;
}

void MapOptimizer::precondition_direction (MatrixXf & dH) const
{
  if (logging) LOG("  preconditioning direction");

  // Transform descent direction from H(y,x) coords to log(H(y,x)) coords,
  // which are equivalent to log(J(y,x)) coords (so hereafter dH = dJ).

  const MatrixXf & H = m_conditional;

  dH.array() *= H.array();
}

bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
  int maxdegen = 10;
  int dataSize = data.rows();
  int psize = 4;
  MatrixXf x1 = data.block(0, 0, data.rows(), 3);
  MatrixXf x2 = data.block(0, 3, data.rows(), 3);
  vector<int> sample;
  MatrixXf pts1(4, 3);
  MatrixXf pts2(4, 3);
  int maxInlier = -1;
  MatrixXf bestResidue;
  for (int m = 0; m < M; m++) {
    int degencount = 0;
    int isdegen = 1;
    while (isdegen==1 && degencount < maxdegen) {
      degencount ++;
      RandomSampling(psize, dataSize, sample);
      for (int i = 0; i < psize; i++) {
        pts1.row(i) = x1.row(sample[i]);
        pts2.row(i) = x2.row(sample[i]);
      }
      if (sampleValidTest(pts1, pts2))
          isdegen = 0;
    }
    if (isdegen) {
      cout << "Cannot find valid p-subset" << endl;
      return false;
    }
    Matrix3f local_H;
    MatrixXf local_A;
    fitHomography(pts1, pts2, local_H, local_A);

    MatrixXf residue;
    computeHomographyResidue(x1, x2, local_H, residue);
    int inlierCount = (residue.array() < THRESHOLD).count();
    if (inlierCount > maxInlier) {
      maxInlier = inlierCount;
      bestResidue = residue;
    }
  }
  inlier.resize(maxInlier, data.cols());
  int transferCounter = 0;
  for (int i = 0; i < dataSize; i++) {
    if (bestResidue(i) < THRESHOLD) {
      inlier.row(transferCounter) = data.row(i);
      transferCounter++;
    }
  }
  if (transferCounter != maxInlier) {
    cout << "RANSAC result size does not match!!!!" << endl;
    return false;
  }
  return true;
}

// Compute the KL-divergence of a set of marginals
double DenseCRF::klDivergence( const MatrixXf & Q ) const {
	double kl = 0;
	// Add the entropy term
	for( int i=0; i<Q.cols(); i++ )
		for( int l=0; l<Q.rows(); l++ )
			kl += Q(l,i)*log(std::max( Q(l,i), 1e-20f) );
	// Add the unary term
	if( unary_ ) {
		MatrixXf unary = unary_->get();
		for( int i=0; i<Q.cols(); i++ )
			for( int l=0; l<Q.rows(); l++ )
				kl += unary(l,i)*Q(l,i);
	}
	
	// Add all pairwise terms
	MatrixXf tmp;
	for( unsigned int k=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->apply( tmp, Q );
		kl += (Q.array()*tmp.array()).sum();
	}
	return kl;
}

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

    float accum_y_dJ = 0;

    for (size_t y = 0; y < Y; ++y) {

      float dJ_xy = dJ_x_[y];

      accum_y_dJ += dJ_xy;
      accum_x_dJ_[y] += dJ_xy;
    }

    project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
  }

  for (size_t y = 0; y < Y; ++y) {
    project_x_[y] = project_scale * accum_x_dJ_[y] / sum_x_J_[y];
    accum_x_dJ_[y] = 0;
  }

  // apply previous projection and accumulate next projection
  for (size_t iter = 0; iter < 100; ++iter) {

    float error = 0;

    for (size_t x = 0; x < X; ++x) {

      const float * restrict J_x_ = J_ + Y * x;
      float * restrict dJ_x_ = dJ_ + Y * x;

      float accum_y_dJ = 0;

      for (size_t y = 0; y < Y; ++y) {

        float dJ_xy = dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);

        accum_y_dJ += dJ_xy;
        accum_x_dJ_[y] += dJ_xy;
      }

      project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
      imax(error, max(-accum_y_dJ, accum_y_dJ));
    }

    for (size_t y = 0; y < Y; ++y) {

      float accum_x_dJ_y = accum_x_dJ_[y];
      accum_x_dJ_[y] = 0;

      project_x_[y] = project_scale * accum_x_dJ_y / sum_x_J_[y];
      imax(error, max(-accum_x_dJ_y, accum_x_dJ_y));
    }

    if (error < tol) {
      if (logging) {
        LOG("   after " << (1+iter) << " iterations, error < " << error);
      }
      break;
    }
  }

  // apply final projection
  for (size_t x = 0; x < X; ++x) {

    const float * restrict J_x_ = J_ + Y * x;
    float * restrict dJ_x_ = dJ_ + Y * x;

    for (size_t y = 0; y < Y; ++y) {
      dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);
    }
  }

  if (debug) {

    sum_y_dJ = dJ.colwise().sum();
    sum_x_dJ = dJ.rowwise().sum();
    float sum_xy_dJ = sum_x_dJ.sum();

    DEBUG("max constraint errors = "
        << sqrt(sum_x_dJ.array().square().maxCoeff())<< ", "
        << sqrt(sum_y_dJ.array().square().maxCoeff())<< ", "
        << sum_xy_dJ);

    sum_y_dJ.array() /= sum_y_J.array();
    sum_x_dJ.array() /= sum_x_J.array();
    sum_xy_dJ /= sum_xy_J;

    DEBUG("max relative constraints errors = "
        << sqrt(sum_x_dJ.array().square().maxCoeff()) << ", "
        << sqrt(sum_y_dJ.array().square().maxCoeff()) << ", "
        << sum_xy_dJ);

    DEBUG("max(|dJ|) = " << dJ.array().abs().maxCoeff()
        << ", rms(dJ) = " << sqrt(dJ.array().square().mean()));
    DEBUG("max(J) / min(J) = " << (J.maxCoeff() / J.minCoeff()));
    DEBUG("max(sum x. J) / min(sum x. J) = "
        << (sum_x_J.maxCoeff() / sum_x_J.minCoeff()));
    DEBUG("max(sum y. J) / min(sum y. J) = "
        << (sum_y_J.maxCoeff() / sum_y_J.minCoeff()));
  }
}

int main(void)
{
  cout << "Eigen v" << EIGEN_WORLD_VERSION << "." << EIGEN_MAJOR_VERSION << "." << EIGEN_MINOR_VERSION << endl;
  static const int R = 288;
  static const int N = R*(R+1)/2;
  static const int M = 63;
  static const int HALF_M = M/2;
  static const float nsigma = 2.5f;

  MatrixXf data = MatrixXf::Random(M, N);
  MatrixXf mask = MatrixXf::Zero(M, N);
  MatrixXf result = MatrixXf::Zero(1, N);
  VectorXf std = VectorXf::Zero(N);
  VectorXf centroid = VectorXf::Zero(N);
  VectorXf mean = VectorXf::Zero(N);
  VectorXf minval = VectorXf::Zero(N);
  VectorXf maxval = VectorXf::Zero(N);

  cout << "computing..." << flush;
  double t = GetRealTime();

  // computes the exact median
  if (M&1)
  {
#pragma omp parallel for
    for (int i = 0; i < N; i++)
    {
      vector<float> row(data.data()+i*M, data.data()+(i+1)*M);
      nth_element(row.begin(), row.begin()+HALF_M, row.end());
      centroid(i) = row[HALF_M];
    }
  }
  // nth_element guarantees x_0,...,x_{n-1} < x_n
  else
  {
#pragma omp parallel for
    for (int i = 0; i < N; i++)
    {
      vector<float> row(data.data()+i*M, data.data()+(i+1)*M);
      nth_element(row.begin(), row.begin()+HALF_M, row.end());
      centroid(i) = row[HALF_M];
      centroid(i) += *max_element(row.begin(), row.begin()+HALF_M);
      centroid(i) *= 0.5f;
    }
  }

  // compute the mean
  mean = data.colwise().mean();

  // compute std (x) = sqrt ( 1/N SUM_i (x(i) - mean(x))^2 )
  std = (((data.rowwise() - mean.transpose()).array().square()).colwise().sum() *
         (1.0f / M))
            .array()
            .sqrt();

  // compute n sigmas from centroid
  minval = centroid - std * nsigma;
  maxval = centroid + std * nsigma;
  
  // compute clip mask
  for (int i = 0; i < N; i++)
  {
    mask.col(i) = (data.col(i).array() > minval(i)).select(VectorXf::Ones(M), 0.0f);
    mask.col(i) = (data.col(i).array() < maxval(i)).select(VectorXf::Ones(M), 0.0f);
  }

  // apply clip mask to data
  data.array() *= mask.array();

  // compute mean such that we ignore clipped data, this is our final result
  result = data.colwise().sum().array() / mask.colwise().sum().array();

  t = GetRealTime() - t;
  cout << "[done]" << endl << endl;

  size_t bytes = data.size()*sizeof(float);
  cout << "data: " << M << "x" << N << endl;
  cout << "size: " << bytes*1e-6f << " MB" << endl;
  cout << "rate: " << bytes/(1e6f*t) << " MB/s" << endl;
  cout << "time: " << t << " s" << endl;

  return 0;
}