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); }
template <typename PointT> void pcl::SampleConsensusModelPlane<PointT>::getDistancesToModel ( const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) { // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); return; } distances.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| /*distances[i] = fabs (model_coefficients[0] * input_->points[(*indices_)[i]].x + model_coefficients[1] * input_->points[(*indices_)[i]].y + model_coefficients[2] * input_->points[(*indices_)[i]].z + model_coefficients[3]);*/ Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1); distances[i] = fabs (model_coefficients.dot (pt)); } }
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()); }
template <typename PointT> void pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients ( const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) { // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ()); optimized_coefficients = model_coefficients; return; } // Need at least 3 points to estimate a plane if (inliers.size () < 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ()); optimized_coefficients = model_coefficients; return; } Eigen::Vector4f plane_parameters; // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; // Estimate the XYZ centroid compute3DCentroid (*input_, inliers, xyz_centroid); xyz_centroid[3] = 0; // Compute the 3x3 covariance matrix computeCovarianceMatrix (*input_, inliers, xyz_centroid, covariance_matrix); // Compute the model coefficients EIGEN_ALIGN16 Eigen::Vector3f eigen_values; EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values); // Hessian form (D = nc . p_plane (centroid here) + p) optimized_coefficients.resize (4); optimized_coefficients[0] = eigen_vectors (0, 0); optimized_coefficients[1] = eigen_vectors (1, 0); optimized_coefficients[2] = eigen_vectors (2, 0); optimized_coefficients[3] = 0; optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid); }
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> bool pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel ( const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) { // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); return (false); } for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) { Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 1); if (fabs (model_coefficients.dot (pt)) > threshold) return (false); } return (true); }
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; }