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

void NonRigid::computeInflateDir(VectorXf &p_vec, VectorXf &g_vec)
{
    VectorXf arap_g = g_vec;
    VectorXf inflate_g = g_vec;
    VectorXf userCrsp_g = g_vec;
    double f_e = 0;
    computeArap(p_vec, arap_g);
    computeUserCrsp(p_vec, userCrsp_g);

    Vector3f cur_v(0,0,0);
    Vector3f cur_g(0,0,0);
    size_t P_Num = p_vec.size()/3;
    for (size_t i = 0; i < P_Num; ++i)
    {
        cur_v << p_vec(i + 0*P_Num), p_vec(i + 1*P_Num), p_vec(i + 2*P_Num);
        if (computeVertexGrad(cur_v, cur_g) > 0.0)
        {
            inflate_g(i + 0*P_Num) = cur_g(0);
            inflate_g(i + 1*P_Num) = cur_g(1);
            inflate_g(i + 2*P_Num) = cur_g(2);
        }
        else
        {
            inflate_g(i + 0*P_Num) = -P_Prime_N(0, i);
            inflate_g(i + 1*P_Num) = -P_Prime_N(1, i);
            inflate_g(i + 2*P_Num) = -P_Prime_N(2, i);
        }

    }

    g_vec = lamd_inflate*inflate_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g;
    g_vec.normalize();
}

double NonRigid::computeGradEnergy(VectorXf &p_vec, VectorXf &g_vec)
{
    VectorXf arap_g = g_vec;
    VectorXf dist_g = g_vec;
    VectorXf userCrsp_g = g_vec;
    double f_e = 0;
    computeArap(p_vec, arap_g);

    f_e += computeDistEnergy(p_vec, dist_g);

    computeUserCrsp(p_vec, userCrsp_g);

    g_vec = lamd_dist*dist_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g;
    g_vec.normalize();
    return f_e;
}

bool Energy::iterate() {
    if (step_size <= step_size_end) { return true; }

    step_size = update_step_size(step_size, step_size_end);
    if (step_size <= step_size_end) { return true; }

    // every coordinate of a vector will all equal coordinates and a norm of
    // step_size has the following value:
    float dparam = step_size;

    float energy_now = calc_energy();

    VectorXf *diff = new VectorXf(numparams);
    VectorXf *chg  = new VectorXf(numparams);

    for (int np = 0; np < numparams; np++) {
        (*diff)[np] = 0.0f;
    }

    bool *done = new bool[numparams];

    for (int np = 0; np < numparams; np++) {
        (*chg )[np] = 0;
        done[np] = false;

        (*diff)[np] = dparam;
        apply_change(diff);
        float energy_large = calc_energy();

        (*diff)[np] = -2 * dparam;
        apply_change(diff);
        float energy_small = calc_energy();

        (*diff)[np] = dparam;
        apply_change(diff);

        (*diff)[np] = 0;

        if (!boost::math::isnan(energy_small) &&
            !boost::math::isnan(energy_large)) {
            if (!is_improvement(energy_now, energy_large) &&
                !is_improvement(energy_now, energy_small)) {
                if (step_size <= step_size_end) { done[np] = true; }
                continue;
            }

            (*chg)[np] = energy_small - energy_large;
        }
        else {
            (*chg)[np] = 0.0f;
        }
    }

    bool chg_all_zeros = true;
    for (int np = 0; np < numparams; np++) {
        chg_all_zeros = chg_all_zeros && ((*chg)[np] == 0.0f);
    }

    if (!chg_all_zeros) {
        chg->normalize();
        (*chg) *= step_size * speed;
        apply_change(chg);

        float energy_new = calc_energy();

        if (!is_improvement(energy_now, energy_new) &&
            step_size > step_size_end) {
            (*chg) *= -1;
            apply_change(chg);
            step_size /= 2;
        }

        log_iteration(step_size);
    }
    else if (step_size > step_size_end) {
        step_size /= 2;
    }

    log_energies();
    
    delete chg ;
    delete diff;

    for (int np = 0; np < numparams; np++) {
        if (!done[np]) { delete done; return false; }
    }
    delete done;
    return true;
}

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 CloudOrienter::_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();

    // -- 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;
        // -- In some degenerate cases, it is possible for the vector
        //    to never settle down to the first PC.
        if(ctr > 100)
            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 set the output_cloud_.
    output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
    *output_cloud_ = points * basis;
    assert(output_cloud_->rows() == input_cloud_->rows());
}