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

template <typename PointT> int
pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return (0);
  }

  int nr_p = 0;

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) < threshold)
      nr_p++;
  }
  return (nr_p);
}

template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) < threshold)
    {
      // Returns the indices of the points whose distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      nr_p++;
    }
  }
  inliers.resize (nr_p);
}

	virtual double gradient( const Eigen::VectorXf & x, Eigen::VectorXf & dx ) {
		int p = 0;
		if (unary_) {
			crf_.setUnaryParameters( x.segment( p, initial_u_param_.rows() ) );
			p += initial_u_param_.rows();
		}
		if (pairwise_) {
			crf_.setLabelCompatibilityParameters( x.segment( p, initial_lbl_param_.rows() ) );
			p += initial_lbl_param_.rows();
		}
		if (kernel_)
			crf_.setKernelParameters( x.segment( p, initial_knl_param_.rows() ) );
		
		Eigen::VectorXf du = 0*initial_u_param_, dl = 0*initial_u_param_, dk = 0*initial_knl_param_;
		double r = crf_.gradient( NIT_, objective_, unary_?&du:NULL, pairwise_?&dl:NULL, kernel_?&dk:NULL );
		dx.resize( unary_*du.rows() + pairwise_*dl.rows() + kernel_*dk.rows() );
		dx << -(unary_?du:Eigen::VectorXf()), -(pairwise_?dl:Eigen::VectorXf()), -(kernel_?dk:Eigen::VectorXf());
		r = -r;
		if( l2_norm_ > 0 ) {
			dx += l2_norm_ * x;
			r += 0.5*l2_norm_ * (x.dot(x));
		}
		
		return r;
	}

float felli(const std::vector<float>& xx) {
  Eigen::VectorXf x = Eigen::VectorXf::Zero(xx.size());
  for (size_t i = 0; i < xx.size(); ++i)
    x[i] = xx[i];
  Eigen::VectorXf v = Eigen::VectorXf::Zero(x.size());
  for (size_t i = 0; i < v.size(); ++i)
    v[i] = powf(1e6, i / (x.size() - 1.0f));
  return v.dot((x.array() * x.array()).matrix());
}

std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::QWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
    // compute error from joint deviation
    const float e = diff.transpose()*_Q*diff;

    Eigen::MatrixXf deriv = Eigen::MatrixXf::Zero(diff.size(), 1);

    for(unsigned int i=0; i<diff.size(); i++) {
        // original derivation
        //deriv(i) = diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
        // negative direction, this works
        //deriv(i) = - diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
        deriv(i) = - ( diff.dot(_Q.row(i) + _Q.col(i).transpose()) - (diff[i]*_Q(i,i)) );
    }

    // Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
    // For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
    const Eigen::MatrixXf J = (diff.array()==0).select(0, deriv);

    const Eigen::VectorXf JTe = J.array()*e;

    return std::make_tuple(J, JTe);
}

template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
    if (!compute_done_)
        initCompute ();
    if (!compute_done_)
        PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");

    Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
    const size_t n = eigenvectors_.cols ();// number of eigen vectors
    Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
    Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
    Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
    Eigen::VectorXf h = y - input;
    if (h.norm() > 0)
        h.normalize ();
    else
        h.setZero ();
    float gamma = h.dot(input - mean_.head<3>());
    Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
    D.block(0,0,n,n) = a * a.transpose();
    D /=  float(n)/float((n+1) * (n+1));
    for(std::size_t i=0; i < a.size(); i++) {
        D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
        D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
        D(i,D.cols()-1) = D(D.rows()-1,i);
        D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
    }

    Eigen::MatrixXf R(D.rows(), D.cols());
    Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
    Eigen::VectorXf alphap = D_evd.eigenvalues().real();
    eigenvalues_.resize(eigenvalues_.size() +1);
    for(std::size_t i=0; i<eigenvalues_.size(); i++) {
        eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
        R.col(i) = D.col(D.cols()-i-1);
    }
    Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
    Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
    Up.rightCols<1>() = h;
    eigenvectors_ = Up*R;
    if (!basis_only_) {
        Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
        coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
        for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
            coefficients_(coefficients_.rows()-1,i) = 0;
            coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
        }
        a.resize(a.size()+1);
        a(a.size()-1) = 0;
        coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
    }
    mean_.head<3>() = meanp;
    switch (flag)
    {
    case increase:
        if (eigenvectors_.rows() >= eigenvectors_.cols())
            break;
    case preserve:
        if (!basis_only_)
            coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
        eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
        eigenvalues_.resize(eigenvalues_.size()-1);
        break;
    default:
        PCL_ERROR("[pcl::PCA] unknown flag\n");
    }
}

float LinearSvmModel::compute_score(const Eigen::VectorXf &feature_vector) const
{
    assert(w.size() > 0);
    return feature_vector.dot(w) - bias;
}