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

void Layouter::laplacianEigMap( const MatrixXd& laplacian, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor /*= 1.f*/ )
{
	if (laplacian.rows() <= 0 || laplacian.cols() <= 0 || radiusVec.size() <= 0)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = 0;
		return;
	}
	if (laplacian.rows() == 1 && laplacian.cols() == 1 && radiusVec.size() == 1)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = radiusVec[0];
		return;
	}
	MatrixXd pos2D, finalPosd;
	LaplacianSolver::compute(laplacian, pos2D); 

	VectorXd minPos, maxPos;
	MDSPostProcesser m_postProcessor(500, 1.0f, 1.0, 1.02, radiusVec.minCoeff());
	m_postProcessor.setSparseFactor(sparseFactor);
	m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID);
	m_postProcessor.compute();
	m_postProcessor.getFinalPos(finalPosd);
	m_postProcessor.getFinalBoundingRect(minPos, maxPos);
	finalPos2D = finalPosd.cast<float>();
	// 	finalPos2D = pos2D.cast<float>();
	// 	minPos = pos2D.colwise().minCoeff();
	// 	maxPos = pos2D.colwise().maxCoeff();
	//  VectorXd size = maxPos - minPos;
	finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f;
}

void Layouter::mds(const MatrixXd& distMat, 
						   const VectorXf& radiusVec, 
						   const VectorXi& hashID,
						   MatrixXf& finalPos2D, 
						   float& finalRadius,
						   float  sparseFactor,
						   float  paddingRatio)
{
	if (distMat.rows() <= 0 || distMat.cols() <= 0 || radiusVec.size() <= 0)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = 0;
		return;
	}
	if (distMat.rows() == 1 && distMat.cols() == 1 && radiusVec.size() == 1)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = radiusVec[0];
		return;
	}
	MatrixXd pos2D, finalPosd;
	ClassicalMDSSolver::compute(distMat, pos2D); 
	
	VectorXd minPos, maxPos;
	float minPadding = radiusVec.minCoeff();
	MDSPostProcesser m_postProcessor(5000, sparseFactor, 1.0, paddingRatio, minPadding);
	m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID);
	m_postProcessor.compute();
	m_postProcessor.getFinalPos(finalPosd);
	m_postProcessor.getFinalBoundingRect(minPos, maxPos);
	finalPos2D = finalPosd.cast<float>();
	// 	finalPos2D = pos2D.cast<float>();
	// 	minPos = pos2D.colwise().minCoeff();
	// 	maxPos = pos2D.colwise().maxCoeff();
	//  VectorXd size = maxPos - minPos;
	finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f;
}

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 constrain_marginals_bp (
    MatrixXf & joint,
    const VectorXf & prior_dom,
    const VectorXf & prior_cod,
    VectorXf & temp_dom,
    VectorXf & temp_cod,
    float tol,
    size_t max_steps,
    bool logging)
{
  // Enforce simultaineous constraints on a joint PMF
  //
  //   /\x. sum y. J(y,x) = p(x)
  //   /\y. sum x. J(y,x) = q(y)

  ASSERT_EQ(prior_dom.size(), joint.cols());
  ASSERT_EQ(prior_cod.size(), joint.rows());
  ASSERT_LT(0, prior_dom.minCoeff());
  ASSERT_LT(0, prior_cod.minCoeff());

  if (logging) LOG("  constraining marginals via full BP");

  const size_t X = joint.cols();
  const size_t Y = joint.rows();

  const Vector<float> p = as_vector(prior_dom);
  const Vector<float> q = as_vector(prior_cod);

  Vector<float> J = as_vector(joint);
  Vector<float> sum_y_J = as_vector(temp_dom);
  Vector<float> sum_x_J = as_vector(temp_cod);

  float stepsize = 0;
  size_t steps = 0;
  while (steps < max_steps) {
    ++steps;
    if (logging) cout << "   step " << steps << "/" << max_steps << flush;

    stepsize = 0;

    // constrain sum y. J(y,x) = 1 first,
    // in case joint is initalized with conditional

    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_y_J[x] = sum(J_x);
    }
    ASSERT_LT(0, min(sum_y_J)); // XXX error here
    imax(stepsize, sqrtf(max_dist_squared(sum_y_J, p)));
    idiv_store_rhs(p, sum_y_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_y_J[x];
    }

    sum_x_J.zero();
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_x_J += J_x;
    }
    ASSERT_LT(0, min(sum_x_J));
    imax(stepsize, sqrtf(max_dist_squared(sum_x_J, q)));
    idiv_store_rhs(q, sum_x_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_x_J;
    }

    if (logging) LOG(", stepsize = " << stepsize);
    if (stepsize < tol) break;
  }
}

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.
//.........这里部分代码省略.........