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;
}
