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

void CCIPCA::update(const float *pp) {

    const int D = m_mean.cols();
    const int K = m_eigenvecs.cols();

    m_num_data_points++;

    //< TODO amnesic weights
    const float w1 = (m_num_data_points-1)/(float)m_num_data_points;
    const float w2 = (1)/(float)m_num_data_points;

    VectorXf vec_pp = Map<const VectorXf>(pp, D) - m_mean.transpose();
    for (int k = 0; k < K; k++) {
        VectorXf v = m_eigenvecs.col(k);
        m_eigenvecs.col(k) = w1 * v + (v.dot(vec_pp))*vec_pp*(w2/m_eigenvecs_norms(k));
        m_eigenvecs_norms(k) = m_eigenvecs.col(k).norm();
        VectorXf nrm_v = m_eigenvecs.col(k).normalized();
        vec_pp -= (vec_pp.dot(nrm_v))*nrm_v;
    }
}

float Sphere::fit_eval ( const VectorXf &fitpar, const void  *user_data)
{
    /*
    * Calculate the cost function value
    * Optimize for the radius inside here
    */
    const fitUserNew& user = (fitUserNew)user_data;
    const VectorXf& r0 = fitpar;

    float F;

    MatrixXf diff = user->rr.rowwise() - r0.transpose();
    VectorXf one = diff.rowwise().norm();

    float sum = one.sum();
    float sum2 = one.dot(one);

    F = sum2 - sum*sum/user->rr.rows();

    if(user->report)
        std::cout << "r0: " << 1000*r0[0] << ", r1: " << 1000*r0[1] << ", r2: " << 1000*r0[2] << "; R: " << 1000*sum/user->rr.rows() << "; fval: "<<F<<std::endl;

    return F;
}

IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
					     const std::vector<int>& interest_region_indices)
{
  // -- Put cluster points into matrix form.
  MatrixXf points(interest_region_indices.size(), 3);
  for(size_t i=0; i<interest_region_indices.size(); ++i) {
    points(i, 0) = data.points[interest_region_indices[i]].x;
    points(i, 1) = data.points[interest_region_indices[i]].y;
    points(i, 2) = data.points[interest_region_indices[i]].z;
  }

  // -- Subtract off the mean and flatten to z=0 to prepare for PCA.
  MatrixXf X = points;
  X.col(2) = VectorXf::Zero(X.rows());
  VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
  for(int i=0; i<X.rows(); ++i) {
    X.row(i) -= pt_mean.transpose();
  }
  MatrixXf Xt = X.transpose();
  
  // -- Find the long axis.
  // Start with a random vector.
  VectorXf pc = VectorXf::Zero(3);
  pc(0) = 1; //Chosen by fair dice roll.
  pc(1) = 1;
  pc.normalize();
  
  // Power method.
  VectorXf prev = pc;
  double thresh = 1e-4;
  int ctr = 0;
  while(true) { 
    prev = pc;
    pc =  Xt * (X * pc);
    pc.normalize();
    ctr++;
    if((pc - prev).norm() < thresh)
      break;
  }
  assert(abs(pc(2)) < 1e-4);
  
  // -- Find the short axis.
  VectorXf shrt = VectorXf::Zero(3);
  shrt(1) = -pc(0);
  shrt(0) = pc(1);
  assert(abs(shrt.norm() - 1) < 1e-4);
  assert(abs(shrt.dot(pc)) < 1e-4);
  
  // -- Build the basis of normalized coordinates.
  MatrixXf basis = MatrixXf::Zero(3,3);
  basis.col(0) = pc;
  basis.col(1) = shrt;
  basis(2,2) = -1.0;
  assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
  assert(abs(basis.col(0).norm() - 1) < 1e-4);
  assert(abs(basis.col(1).norm() - 1) < 1e-4);
  assert(abs(basis.col(2).norm() - 1) < 1e-4);


  // -- Put the cluster into normalized coordinates, and choose which axis to project on.
  MatrixXf projected_basis(3, 2);
  if(axis_ == 0) { 
    projected_basis.col(0) = basis.col(1);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 1) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 2) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(1);
  }
  MatrixXf projected = points * projected_basis;
    
  // -- Transform into pixel units.
  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?
  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;
  }

  // -- Shift the origin based on {u,v}_offset_pct. 
//.........这里部分代码省略.........

MatrixXf D3DCloudOrienter::orientCloud(const sensor_msgs::PointCloud& data,
				    const std::vector<int>& interest_region_indices)
{
  // -- Put cluster points into matrix form.
  MatrixXf points(interest_region_indices.size(), 3);
  for(size_t i=0; i<interest_region_indices.size(); ++i) {
    points(i, 0) = data.points[interest_region_indices[i]].x;
    points(i, 1) = data.points[interest_region_indices[i]].y;
    points(i, 2) = data.points[interest_region_indices[i]].z;
  }

  // -- Subtract off the mean of the points.
  VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
  for(int i=0; i<points.rows(); ++i)
    points.row(i) -= pt_mean.transpose();

  // -- Flatten to z == 0.
  MatrixXf X = points;
  X.col(2) = VectorXf::Zero(X.rows());
  MatrixXf Xt = X.transpose();
  
  // -- Find the long axis.
  // Start with a random vector.
  VectorXf pc = VectorXf::Zero(3);
  pc(0) = 1; //Chosen by fair dice roll.
  pc(1) = 1;
  pc.normalize();
  
  // Power method.
  VectorXf prev = pc;
  double thresh = 1e-4;
  int ctr = 0;
  while(true) { 
    prev = pc;
    pc =  Xt * (X * pc);
    pc.normalize();
    ctr++;
    if((pc - prev).norm() < thresh)
      break;
  }
  assert(abs(pc(2)) < 1e-4);
  
  // -- Find the short axis.
  VectorXf shrt = VectorXf::Zero(3);
  shrt(1) = -pc(0);
  shrt(0) = pc(1);
  assert(abs(shrt.norm() - 1) < 1e-4);
  assert(abs(shrt.dot(pc)) < 1e-4);
  
  // -- Build the basis of normalized coordinates.
  MatrixXf basis = MatrixXf::Zero(3,3);
  basis.col(0) = pc;
  basis.col(1) = shrt;
  basis(2,2) = 1.0;
  assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
  assert(abs(basis.col(0).norm() - 1) < 1e-4);
  assert(abs(basis.col(1).norm() - 1) < 1e-4);
  assert(abs(basis.col(2).norm() - 1) < 1e-4);

  // -- Rotate and return.
  MatrixXf oriented = points * basis;
  return oriented;
}

void HoughCloudOrienter::_compute() {
    assert(input_cloud_);
    assert(input_intensities_);
    assert(!output_cloud_);
    //cout << input_intensities_->rows() << " " << input_cloud_->rows() << endl;
    assert(input_cloud_->rows() == input_intensities_->rows());
    assert(input_cloud_->rows() > 2);

    // -- Subtract off the mean of the points.
    MatrixXf& points = *input_cloud_;
    VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
    for(int i=0; i<points.rows(); ++i)
        points.row(i) -= pt_mean.transpose();

    // -- Find the principal axis.
    static const int num_bins = 12;
    static const int max_samples = 100;
    static unsigned int count[num_bins];
    static double angle_total[num_bins];

    for (int i=0; i < num_bins; i++) {
        count[i] = 0;
        angle_total[i] = 0;
    }

    int num_points = points.rows();
    int num_samples = 0;
    unsigned int max_count = 0;
    int max_index = 0;
    while (num_samples < max_samples) {
        int ix = rand() % num_points;
        int iy = rand() % num_points;
        while (ix == iy)
            iy = rand() % num_points;

        VectorXf p1 = points.row(ix);
        VectorXf p2 = points.row(iy);
        double dy = (p1(1) - p2(1));
        double dx = (p1(0) - p2(0));
        if ((fabs(dy) < 0.001) && ( fabs(dx) < 0.001))
            continue;
        double y = atan2(dy, dx);

        // wrap into range
        if (y < 0) y += M_PI;
        if (y >= M_PI_2) y -= M_PI_2;

        int idx = (num_bins * y / M_PI_2);
        if (idx >= num_bins) {
            idx = 0;
            y = 0.0;
        }
        angle_total[idx] += y;
        count[idx]++;
        if (count[idx] > max_count) {
            max_count = count[idx];
            max_index = idx;
        }

        num_samples++;
    }
    double angle = angle_total[max_index] / max_count;

    VectorXf pc = VectorXf::Zero(3);
    pc(0) = sin(angle);
    pc(1) = cos(angle);
    pc(2) = 0.0;

    assert(abs(pc(2)) < 1e-4);

    // -- Find the short axis.
    VectorXf shrt = VectorXf::Zero(3);
    shrt(1) = -pc(0);
    shrt(0) = pc(1);
    assert(abs(shrt.norm() - 1) < 1e-4);
    assert(abs(shrt.dot(pc)) < 1e-4);

    // -- Build the basis of normalized coordinates.
    MatrixXf basis = MatrixXf::Zero(3,3);
    basis.col(0) = pc;
    basis.col(1) = shrt;
    basis(2,2) = 1.0;
    assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
    assert(abs(basis.col(0).norm() - 1) < 1e-4);
    assert(abs(basis.col(1).norm() - 1) < 1e-4);
    assert(abs(basis.col(2).norm() - 1) < 1e-4);

    // -- Rotate and set the output_cloud_.
    output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
    *output_cloud_ = points * basis;
    assert(output_cloud_->rows() == input_cloud_->rows());
}