/**
* @function fillEq_PLP
* @brief Fill PLP equations ( 3 Eq - 2 DOF )
*/
void trifocalTensor::fillEq_PLP( cv::Point3f _A,
cv::Point3f _B,
cv::Point3f _C ) {
Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
std::cout << "Correspondence PLP: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;
// 3 equations
for( int s = 0; s < 3; ++s ) {
// T0, T1, T2
for( int i = 0; i < 3; ++i ) {
for( int j = 0; j < 3; ++j ) {
for( int q = 0; q < 3; ++q ) {
for( int k = 0; k < 3; ++k ) {
if( epsilon(k,q,s) != 0 ) {
mEq( mPointer, 9*i + 3*j + q) = A(i)*B(j)*C(k)*epsilon(k,q,s);
}
} // k
} // q
} // j
} // i
mPointer++;
} // s
}
/**
* @function fillEq_LLL
* @brief Fill LLL equations ( 3 Equations, 2 DOF )
*/
void trifocalTensor::fillEq_LLL( cv::Point3f _A,
cv::Point3f _B,
cv::Point3f _C ) {
Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
std::cout << "[LLL] Correspondence: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;
// 3 equations
for( int s = 0; s < 3; ++s ) {
// T0, T1, T2
for( int i = 0; i < 3; ++i ) {
for( int j = 0; j < 3; ++j ) {
for( int k = 0; k < 3; ++k ) {
for( int r = 0; r < 3; ++r ) {
mEq( mPointer, 9*i + 3*j + k) += A(r)*B(j)*C(k)*epsilon(r,i,s);
} // r
} // k
} // j
} // i
mPointer++;
} // s
}
/** \brief Computes the gradient parameters of the patch (patch gradient components dx_ dy_, Hessian H_, Shi-Tomasi Score s_, Eigenvalues of the Hessian e0_ and e1_).
* The expanded patch patchWithBorder_ must be set.
* Sets validGradientParameters_ afterwards to true.
*/
void computeGradientParameters() const{
if(!validGradientParameters_){
H_.setZero();
const int refStep = patchSize+2;
float* it_dx = dx_;
float* it_dy = dy_;
const float* it;
Eigen::Vector3f J;
J.setZero();
for(int y=0; y<patchSize; ++y){
it = patchWithBorder_ + (y+1)*refStep + 1;
for(int x=0; x<patchSize; ++x, ++it, ++it_dx, ++it_dy){
J[0] = 0.5 * (it[1] - it[-1]);
J[1] = 0.5 * (it[refStep] - it[-refStep]);
J[2] = 1;
*it_dx = J[0];
*it_dy = J[1];
H_ += J*J.transpose();
}
}
const float dXX = H_(0,0)/(patchSize*patchSize);
const float dYY = H_(1,1)/(patchSize*patchSize);
const float dXY = H_(0,1)/(patchSize*patchSize);
e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
s_ = e0_;
validGradientParameters_ = true;
}
}
float robotDepth()
{
// Two interesting points
float alpha1 = (x + (sx-height)/2)/(float)height*2*tan(VFOV/2.);
float alpha2 = (x - (sx+height)/2)/(float)height*2*tan(VFOV/2.);
float beta = (y - width/2)/(float)width*2*tan(HFOV/2.);
// Vectors
Eigen::Vector3f v1(1,-beta,-alpha1);
Eigen::Vector3f v2(1,-beta,-alpha2);
// Normalization
v1 = v1/v1.norm();
v2 = v2/v2.norm();
// Center
Eigen::Vector3f c = (v1+v2)/2.;
float c_norm = c.norm();
// Projection
Eigen::MatrixXf proj_mat = c.transpose()*v1;
float proj = proj_mat(0,0);
// Orthogonal part in v1
Eigen::Vector3f orth = v1 - proj/c_norm*c;
// Norm
float orth_norm = orth.norm();
// Approximate depth
float d = H/2.*proj/orth_norm;
return d;
}
/**
* @function fillEq_PLL
* @brief Fill PLL equations ( 1 Equation - 1 DOF )
*/
void trifocalTensor::fillEq_PLL( cv::Point3f _A,
cv::Point3f _B,
cv::Point3f _C ) {
Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
std::cout << "PLL Correspondence: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;
// T0, T1, T2
for( int i = 0; i < 3; ++i ) {
for( int j = 0; j < 3; ++j ) {
for( int k = 0; k < 3; ++k ) {
mEq( mPointer, 9*i + 3*j + k) += A(i)*B(j)*C(k);
} // k
} // j
} // i
mPointer++;
}
void GenericDistanceConstraint::gradientFct(
const unsigned int i,
const unsigned int numberOfParticles,
const float mass[],
const Eigen::Vector3f x[],
void *userData,
Eigen::Matrix<float, 1, 3> &jacobian)
{
Eigen::Vector3f n = x[i] - x[1 - i];
n.normalize();
jacobian = n.transpose();
}
Eigen::Matrix4f lookAt(const Eigen::Vector3f &eye,
const Eigen::Vector3f ¢er,
const Eigen::Vector3f &up) {
Eigen::Vector3f f = (center - eye).normalized();
Eigen::Vector3f s = f.cross(up).normalized();
Eigen::Vector3f u = s.cross(f);
Eigen::Matrix4f Result = Eigen::Matrix4f::Identity();
Result(0, 0) = s(0);
Result(0, 1) = s(1);
Result(0, 2) = s(2);
Result(1, 0) = u(0);
Result(1, 1) = u(1);
Result(1, 2) = u(2);
Result(2, 0) = -f(0);
Result(2, 1) = -f(1);
Result(2, 2) = -f(2);
Result(0, 3) = -s.transpose() * eye;
Result(1, 3) = -u.transpose() * eye;
Result(2, 3) = f.transpose() * eye;
return Result;
}
Eigen::Matrix3f
ElementHex::GetDeformationGrad(const Eigen::Vector3f & p,
const std::vector<Eigen::Vector3f> & X,
const std::vector<Eigen::Vector3f> & u)
{
Eigen::Matrix3f F = Eigen::Matrix3f::Identity();
for(int ii = 0;ii<8;ii++){
int idx = GetNodeIndex(ii);
Eigen::Vector3f gradN = ShapeFunGrad(ii,p,X);
//outer product
F += u[idx] * gradN.transpose();
}
return F;
}
void OCLSLAM<CR, CW>::display ()
{
double angle = 180.0 / M_PI * 2.0 * std::atan2 (q_g.vec ().norm (), q_g.w ()); // in degrees
Eigen::Vector3f axis = ((angle == 0.0) ? Eigen::Vector3f::Zero () : q_g.vec ().normalized ());
std::cout << " Time step : " << timeStep << std::endl;
std::cout << " Latency : " << timer.stop () << " [ms]" << std::endl;
std::cout << " ICP iterations : " << icp.k << std::endl;
std::cout << " ICP latency : " << lICP << " [ms]" << std::endl;
std::cout << " Localization " << std::endl;
std::cout << " - Translation vector : " << t_g.transpose () << " [mm]" << std::endl;
std::cout << " - Rotation axis : " << axis.transpose () << std::endl;
std::cout << " - Rotation angle : " << angle << " [degrees]" << std::endl;
std::cout << "=========================== " << std::endl;
timer.start ();
}
wcFloat wcCostEstimator::cost( const wcMetaVector& meta )
{
const Eigen::Vector3f V1(Eigen::Vector3f().Ones());
/*
for( wcPos idx = 0; idx < 3; idx++ )
if(meta(idx)<WC_PRECISION) return 1.0f;
wcMetaVector delta0 = (meta - optima);
wcMetaVector delta1 = (meta - V1);
wcFloat maxQR = ((V1.transpose()*Qc*V1 + V1.transpose()*Rc*V1)(0));
return (delta0.transpose()*Qc*delta0 + delta1.transpose()*Rc*delta1)(0)/maxQR;
*/
for( wcPos idx = 0; idx < 3; idx++ )
if(meta(idx)<WC_PRECISION) return 1.0f;
wcMetaVector delta0 = (meta - optima);
wcFloat maxQR = ((V1.transpose()*Qc*V1))(0);
return (delta0.transpose()*Qc*delta0)(0)/maxQR;
}
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
const pcl::PointCloud <PointInT> &cloud, const std::vector <int> &indices,
const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
{
if (indices.size () < 3)
{
gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
return;
}
Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
Eigen::Vector3f b = Eigen::Vector3f::Zero ();
for (size_t i_point = 0; i_point < indices.size (); ++i_point)
{
PointInT p = cloud.points[indices[i_point]];
if (!pcl_isfinite (p.x) ||
!pcl_isfinite (p.y) ||
!pcl_isfinite (p.z) ||
!pcl_isfinite (intensity_ (p)))
continue;
p.x -= point[0];
p.y -= point[1];
p.z -= point[2];
intensity_.demean (p, mean_intensity);
A (0, 0) += p.x * p.x;
A (0, 1) += p.x * p.y;
A (0, 2) += p.x * p.z;
A (1, 1) += p.y * p.y;
A (1, 2) += p.y * p.z;
A (2, 2) += p.z * p.z;
b[0] += p.x * intensity_ (p);
b[1] += p.y * intensity_ (p);
b[2] += p.z * intensity_ (p);
}
// Fill in the lower triangle of A
A (1, 0) = A (0, 1);
A (2, 0) = A (0, 2);
A (2, 1) = A (1, 2);
//*
Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
/*/
Eigen::Vector3f eigen_values;
Eigen::Matrix3f eigen_vectors;
eigen33 (A, eigen_vectors, eigen_values);
b = eigen_vectors.transpose () * b;
if ( eigen_values (0) != 0)
b (0) /= eigen_values (0);
else
b (0) = 0;
if ( eigen_values (1) != 0)
b (1) /= eigen_values (1);
else
b (1) = 0;
if ( eigen_values (2) != 0)
b (2) /= eigen_values (2);
else
b (2) = 0;
Eigen::Vector3f x = eigen_vectors * b;
// if (A.col (0).squaredNorm () != 0)
// x [0] /= A.col (0).squaredNorm ();
// b -= x [0] * A.col (0);
//
//
// if (A.col (1).squaredNorm () != 0)
// x [1] /= A.col (1).squaredNorm ();
// b -= x[1] * A.col (1);
//
// x [2] = b.dot (A.col (2));
// if (A.col (2).squaredNorm () != 0)
// x[2] /= A.col (2).squaredNorm ();
// Fit a hyperplane to the data
//*/
// std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
// std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
// Project the gradient vector, x, onto the tangent plane
gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
}
});
auto isAtTarget = [this](){
Eigen::Vector3f lookat = glwidget_->camera_.getRotation() * -Eigen::Vector3f::UnitZ();
Eigen::Vector3f look_proj = lookat;
look_proj[1] = 0;
look_proj.normalize();
float rads = acos(lookat.dot(look_proj));
//qDebug() << "rads" << rads;
Eigen::Vector3f pos = glwidget_->camera_.getPosition();
std::cout << "target: " << target_.transpose() << std::endl;
std::cout << "pos: " << pos.transpose() << std::endl;
float dist = (pos - target_).norm();
distance_text_->setText(QString::number(dist));
// qDebug() << "dist" << dist;
return dist < 2 && fabs(rads) < 0.2;
};
// Start when moving the camera
connect(&glwidget_->camera_, &Camera::modified, [this, isAtTarget, elapsed_time_text](){
if(!running_){
if(ready_to_start_){
time_.restart();
running_ = true;
} else {
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
{
const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());
std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
std::vector <float> triangle_area (number_of_triangles);
std::vector <float> distance_weight (number_of_triangles);
float total_area = 0.0f;
const float coeff = 1.0f / 12.0f;
const float coeff_1_div_3 = 1.0f / 3.0f;
Eigen::Vector3f feature_point (point.x, point.y, point.z);
std::set <unsigned int>::const_iterator it;
unsigned int i_triangle = 0;
for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
triangle_area[i_triangle] = curr_area;
total_area += curr_area;
distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);
Eigen::Matrix3f curr_scatter_matrix;
curr_scatter_matrix.setZero ();
for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
{
Eigen::Vector3f vec = pt[i_pt] - feature_point;
curr_scatter_matrix += vec * (vec.transpose ());
for (unsigned int j_pt = 0; j_pt < 3; j_pt++)
curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ());
}
scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
}
if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
total_area = 1.0f / total_area;
else
total_area = 1.0f;
Eigen::Matrix3f overall_scatter_matrix;
overall_scatter_matrix.setZero ();
std::vector<float> total_weight (number_of_triangles);
const float denominator = 1.0f / 6.0f;
for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
{
float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
overall_scatter_matrix += factor * scatter_matrices[i_triangle];
total_weight[i_triangle] = factor * denominator;
}
Eigen::Vector3f v1, v2, v3;
computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
float h1 = 0.0f;
float h3 = 0.0f;
for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
float factor1 = 0.0f;
float factor3 = 0.0f;
for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
{
Eigen::Vector3f vec = pt[i_pt] - feature_point;
factor1 += vec.dot (v1);
factor3 += vec.dot (v3);
}
h1 += total_weight[i_triangle] * factor1;
h3 += total_weight[i_triangle] * factor3;
}
if (h1 < 0.0f) v1 = -v1;
if (h3 < 0.0f) v3 = -v3;
v2 = v3.cross (v1);
lrf_matrix.row (0) = v1;
lrf_matrix.row (1) = v2;
lrf_matrix.row (2) = v3;
}
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
PointInTPtr & grid, pcl::PointIndices & indices)
{
Eigen::Vector3f plane_normal;
plane_normal[0] = -centroid[0];
plane_normal[1] = -centroid[1];
plane_normal[2] = -centroid[2];
Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
plane_normal.normalize ();
Eigen::Vector3f axis = plane_normal.cross (z_vector);
double rotation = -asin (axis.norm ());
axis.normalize ();
Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
grid->points.resize (processed->points.size ());
for (size_t k = 0; k < processed->points.size (); k++)
grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();
pcl::transformPointCloud (*grid, *grid, transformPC);
Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
centroid4f = transformPC * centroid4f;
normal_centroid4f = transformPC * normal_centroid4f;
Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
Eigen::Vector4f farthest_away;
pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
farthest_away[3] = 0;
float max_dist = (farthest_away - centroid4f).norm ();
pcl::demeanPointCloud (*grid, centroid4f, *grid);
Eigen::Matrix4f center_mat;
center_mat.setIdentity (4, 4);
center_mat (0, 3) = -centroid4f[0];
center_mat (1, 3) = -centroid4f[1];
center_mat (2, 3) = -centroid4f[2];
Eigen::Matrix3f scatter;
scatter.setZero ();
float sum_w = 0.f;
//for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
float d_k = (pvector).norm ();
float w = (max_dist - d_k);
Eigen::Vector3f diff = (pvector);
Eigen::Matrix3f mat = diff * diff.transpose ();
scatter = scatter + mat * w;
sum_w += w;
}
scatter /= sum_w;
Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
Eigen::Vector3f evx = svd.matrixV ().col (0);
Eigen::Vector3f evy = svd.matrixV ().col (1);
Eigen::Vector3f evz = svd.matrixV ().col (2);
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
Eigen::Vector3f evzminus = evz * -1;
float s_xplus, s_xminus, s_yplus, s_yminus;
s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
//disambiguate rf using all points
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
float dist_x, dist_y;
dist_x = std::abs (evx.dot (pvector));
dist_y = std::abs (evy.dot (pvector));
if ((pvector).dot (evx) >= 0)
s_xplus += dist_x;
else
s_xminus += dist_x;
if ((pvector).dot (evy) >= 0)
s_yplus += dist_y;
else
s_yminus += dist_y;
}
if (s_xplus < s_xminus)
evx = evxminus;
if (s_yplus < s_yminus)
evy = evyminus;
//select the axis that could be disambiguated more easily
float fx, fy;
float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
fx = (min_x / max_x);
fy = (min_y / max_y);
Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
if (normal3f.dot (evz) < 0)
evz = evzminus;
//if fx/y close to 1, it was hard to disambiguate
//what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
float max_axis = std::max (fx, fy);
float min_axis = std::min (fx, fy);
if ((min_axis / max_axis) > axis_ratio_)
{
PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");
Eigen::Vector3f evy_copy = evy;
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
if (min_axis > min_axis_value_)
{
//combination of all possibilities
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evxminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evyminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
else
{
//1-st case (evx selected)
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
//2-nd case (evy selected)
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
}
else
{
if (fy < fx)
{
evx = evy;
fx = fy;
}
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
return true;
}
template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
cob_3d_features::OrganizedCurvatureEstimation<PointInT,PointNT,PointLabelT,PointOutT>::computePointCurvatures (
const NormalCloudIn &normals,
const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_distances,
float &pcx, float &pcy, float &pcz, float &pc1, float &pc2,
int &label_out)
{
Eigen::Vector3f n_idx(normals.points[index].normal);
Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - n_idx * n_idx.transpose(); // projection matrix
std::vector<Eigen::Vector3f> normals_projected;
normals_projected.reserve(n_points_);
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
Eigen::Vector3f p_idx = surface_->points[index].getVector3fMap();
float angle = 0.0; // to discriminate convex and concave surface
int prob_concave = 0, prob_convex = 0;
for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it)
{
PointNT const* n_i = &(normals.points[*it]);
if ( pcl_isnan(n_i->normal[2]) ) continue;
normals_projected.push_back( M * Eigen::Vector3f(n_i->normal) );
centroid += normals_projected.back();
if ( (surface_->points[*it].getVector3fMap() - p_idx).normalized().dot(n_idx) > 0.0) ++prob_concave;
else ++prob_convex;
}
if (normals_projected.size() <=1) return;
float num_p_inv = 1.0f / normals_projected.size();
centroid *= num_p_inv;
Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
{
std::vector<Eigen::Vector3f>::iterator n_it = normals_projected.begin();
std::vector<float>::const_iterator d_it = sqr_distances.begin();
for (; n_it != normals_projected.end(); ++n_it, ++d_it)
{
Eigen::Vector3f demean = *n_it - centroid;
//cov += 1.0f / sqrt(*d_it) * demean * demean.transpose();
cov += demean * demean.transpose();
}
}
Eigen::Vector3f eigenvalues;
Eigen::Matrix3f eigenvectors;
pcl::eigen33(cov, eigenvectors, eigenvalues);
pcx = eigenvectors (0,2);
pcy = eigenvectors (1,2);
pcz = eigenvectors (2,2);
if (prob_concave < prob_convex) // if convex, make eigenvalues negative
num_p_inv *= surface_->points[index].z * (-1);
//num_p_inv *= 1.0 / (10.0*centroid.norm()) * surface_->points[index].z * (-1);
else
num_p_inv *= surface_->points[index].z;
pc1 = eigenvalues (2) * num_p_inv;
pc2 = eigenvalues (1) * num_p_inv;
//normals_->points[index].curvature = curvatures_->points[index].pc1;
if (std::abs(pc1) >= edge_curvature_threshold_ && label_out == 0)
label_out = I_EDGE;
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
{
float bad_point = std::numeric_limits<float>::quiet_NaN ();
const int width = input_->width;
const int height = input_->height;
// ==============================================================
if (normal_estimation_method_ == COVARIANCE_MATRIX)
{
if (!init_covariance_matrix_)
initCovarianceMatrixMethod ();
const int start_x = pos_x - rect_width_2_;
const int start_y = pos_y - rect_height_2_;
const int end_x = start_x + rect_width_;
const int end_y = start_y + rect_height_;
unsigned count = 0;
sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count);
// no valid points within the rectangular reagion?
if (count == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
return;
}
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector3f center;
typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
typename IntegralImage2D<float, 3>::ElementType tmp_center;
typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;
center[0] = 0;
center[1] = 0;
center[2] = 0;
tmp_center[0] = 0;
tmp_center[1] = 0;
tmp_center[2] = 0;
so_elements[0] = 0;
so_elements[1] = 0;
so_elements[2] = 0;
so_elements[3] = 0;
so_elements[4] = 0;
so_elements[5] = 0;
sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center);
sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements);
center[0] = float (tmp_center[0]);
center[1] = float (tmp_center[1]);
center[2] = float (tmp_center[2]);
covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
float eigen_value;
Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
normal.getNormalVector3fMap () = eigen_vector;
// Compute the curvature surface change
if (eigen_value > 0.0)
normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
else
normal.curvature = 0;
return;
}
// =======================================================
else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
{
if (!init_average_3d_gradient_)
initAverage3DGradientMethod ();
const int start_x = pos_x - rect_width_2_;
const int start_y = pos_y - rect_height_2_;
const int end_x = start_x + rect_width_;
const int end_y = start_y + rect_height_;
unsigned count_x = 0;
unsigned count_y = 0;
sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x);
sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y);
if (count_x == 0 || count_y == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
return;
}
Eigen::Vector3d gradient_x (0, 0, 0);
Eigen::Vector3d gradient_y (0, 0, 0);
sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x);
sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y);
Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
double normal_length = normal_vector.squaredNorm ();
if (normal_length == 0.0f)
{
normal.getNormalVector3fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
normal_vector /= sqrt (normal_length);
float nx = static_cast<float> (normal_vector [0]);
float ny = static_cast<float> (normal_vector [1]);
float nz = static_cast<float> (normal_vector [2]);
flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
normal.normal_x = nx;
normal.normal_y = ny;
normal.normal_z = nz;
normal.curvature = bad_point;
return;
}
// ======================================================
else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
{
if (!init_depth_change_)
initAverageDepthChangeMethod ();
int point_index_L_x = pos_x - rect_width_4_ - 1;
int point_index_L_y = pos_y;
int point_index_R_x = pos_x + rect_width_4_ + 1;
int point_index_R_y = pos_y;
int point_index_U_x = pos_x - 1;
int point_index_U_y = pos_y - rect_height_4_;
int point_index_D_x = pos_x + 1;
int point_index_D_y = pos_y + rect_height_4_;
if (point_index_L_x < 0)
point_index_L_x = -point_index_L_x;
if (point_index_U_x < 0)
point_index_U_x = -point_index_U_x;
if (point_index_U_y < 0)
point_index_U_y = -point_index_U_y;
if (point_index_R_x >= width)
point_index_R_x = width-(point_index_R_x-(width-1));
if (point_index_D_x >= width)
point_index_D_x = width-(point_index_D_x-(width-1));
if (point_index_D_y >= height)
point_index_D_y = height-(point_index_D_y-(height-1));
const int start_x_L = pos_x - rect_width_2_;
const int start_y_L = pos_y - rect_height_4_;
const int end_x_L = start_x_L + rect_width_2_;
const int end_y_L = start_y_L + rect_height_2_;
const int start_x_R = pos_x + 1;
const int start_y_R = pos_y - rect_height_4_;
const int end_x_R = start_x_R + rect_width_2_;
const int end_y_R = start_y_R + rect_height_2_;
const int start_x_U = pos_x - rect_width_4_;
const int start_y_U = pos_y - rect_height_2_;
const int end_x_U = start_x_U + rect_width_2_;
const int end_y_U = start_y_U + rect_height_2_;
const int start_x_D = pos_x - rect_width_4_;
const int start_y_D = pos_y + 1;
const int end_x_D = start_x_D + rect_width_2_;
const int end_y_D = start_y_D + rect_height_2_;
unsigned count_L_z = 0;
unsigned count_R_z = 0;
unsigned count_U_z = 0;
unsigned count_D_z = 0;
sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z);
sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z);
sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z);
sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z);
if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
return;
}
float mean_L_z = 0;
float mean_R_z = 0;
float mean_U_z = 0;
float mean_D_z = 0;
sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z);
sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z);
sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z);
sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z);
mean_L_z /= float (count_L_z);
mean_R_z /= float (count_R_z);
mean_U_z /= float (count_U_z);
mean_D_z /= float (count_D_z);
PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];
const float mean_x_z = mean_R_z - mean_L_z;
const float mean_y_z = mean_D_z - mean_U_z;
const float mean_x_x = pointR.x - pointL.x;
const float mean_x_y = pointR.y - pointL.y;
const float mean_y_x = pointD.x - pointU.x;
const float mean_y_y = pointD.y - pointU.y;
float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
if (normal_length == 0.0f)
{
normal.getNormalVector3fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
const float scale = 1.0f / sqrtf (normal_length);
normal.normal_x = normal_x * scale;
normal.normal_y = normal_y * scale;
normal.normal_z = normal_z * scale;
normal.curvature = bad_point;
return;
}
// ========================================================
else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
{
PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
}
normal.getNormalVector3fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal (
const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
{
float bad_point = std::numeric_limits<float>::quiet_NaN ();
if (normal_estimation_method_ == COVARIANCE_MATRIX)
{
if (!init_covariance_matrix_)
initCovarianceMatrixMethod ();
unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);
// no valid points within the rectangular reagion?
if (count == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
return;
}
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector3f center;
typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).cast<float> ();
so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
float eigen_value;
Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
//pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector);
pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
normal.getNormalVector3fMap () = eigen_vector;
// Compute the curvature surface change
if (eigen_value > 0.0)
normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
else
normal.curvature = 0;
return;
}
else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
{
if (!init_average_3d_gradient_)
initAverage3DGradientMethod ();
unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
if (count_x == 0 || count_y == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
return;
}
Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
double normal_length = normal_vector.squaredNorm ();
if (normal_length == 0.0f)
{
normal.getNormalVector4fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
normal_vector /= sqrt (normal_length);
float nx = static_cast<float> (normal_vector [0]);
float ny = static_cast<float> (normal_vector [1]);
float nz = static_cast<float> (normal_vector [2]);
//pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
normal.normal_x = nx;
normal.normal_y = ny;
normal.normal_z = nz;
normal.curvature = bad_point;
return;
}
else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
{
if (!init_depth_change_)
initAverageDepthChangeMethod ();
// unsigned count = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
// if (count == 0)
// {
// normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
// return;
// }
// const float mean_L_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ - 1, pos_y - rect_height_2_ , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
// const float mean_R_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ + 1, pos_y - rect_height_2_ , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
// const float mean_U_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ , pos_y - rect_height_2_ - 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
// const float mean_D_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ , pos_y - rect_height_2_ + 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
// width and height are at least 3 x 3
unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_);
if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
{
normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
return;
}
float mean_L_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z);
float mean_R_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1 , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z);
float mean_U_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z);
float mean_D_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1 , rect_width_2_, rect_height_2_) / count_D_z);
PointInT pointL = input_->points[point_index - rect_width_4_ - 1];
PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1];
const float mean_x_z = mean_R_z - mean_L_z;
const float mean_y_z = mean_D_z - mean_U_z;
const float mean_x_x = pointR.x - pointL.x;
const float mean_x_y = pointR.y - pointL.y;
const float mean_y_x = pointD.x - pointU.x;
const float mean_y_y = pointD.y - pointU.y;
float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;
const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);
if (normal_length == 0.0f)
{
normal.getNormalVector4fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
const float scale = 1.0f / sqrt (normal_length);
normal.normal_x = normal_x * scale;
normal.normal_y = normal_y * scale;
normal.normal_z = normal_z * scale;
normal.curvature = bad_point;
return;
}
else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
{
if (!init_simple_3d_gradient_)
initSimple3DGradientMethod ();
// this method does not work if lots of NaNs are in the neighborhood of the point
Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);
Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
double normal_length = normal_vector.squaredNorm ();
if (normal_length == 0.0f)
{
normal.getNormalVector4fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
normal_vector /= sqrt (normal_length);
float nx = static_cast<float> (normal_vector [0]);
float ny = static_cast<float> (normal_vector [1]);
float nz = static_cast<float> (normal_vector [2]);
//pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
normal.normal_x = nx;
normal.normal_y = ny;
normal.normal_z = nz;
normal.curvature = bad_point;
return;
}
normal.getNormalVector4fMap ().setConstant (bad_point);
normal.curvature = bad_point;
return;
}
int
main (int argc, char** argv)
{
// Get input object and scene
if (argc < 2)
{
pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
return (1);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if(argc<3)
{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
pcl::console::print_error ("Error loading first file!\n");
*cloud_out = *cloud_in;
//transform cloud
Eigen::Affine3f transformation;
transformation.setIdentity();
transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
float roll, pitch, yaw;
roll = 0.02; pitch = 1.2; yaw = 0;
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
transformation.rotate(q);
pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
}else{
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
{
pcl::console::print_error ("Error loading files!\n");
return (1);
}
}
// Fill in the CloudIn data
// cloud_in->width = 100;
// cloud_in->height = 1;
// cloud_in->is_dense = false;
// cloud_in->points.resize (cloud_in->width * cloud_in->height);
// for (size_t i = 0; i < cloud_in->points.size (); ++i)
// {
// cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
// cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
// }
std::cout << "size:" << cloud_out->points.size() << std::endl;
{
pcl::ScopeTime("icp proces");
pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGBA> Final;
icp.setMaximumIterations(1000000);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
//translation, rotation
Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
std::cout << "rotation: " << euler.transpose() << std::endl;
std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
}
return (0);
}
template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBox(
const PointCloudPtr& cloud,
const pcl::PointIndices& indices,
const Eigen::Vector3f& plane_normal,
const Eigen::Vector3f& plane_point,
Eigen::Vector3f& position,
Eigen::Quaternion<float>& orientation,
Eigen::Vector3f& size)
{
// transform to table coordinate frame and project points on X-Y, save max height
Eigen::Affine3f tf;
pcl::getTransformationFromTwoUnitVectorsAndOrigin(plane_normal.unitOrthogonal(), plane_normal, plane_point, tf);
pcl::PointCloud<pcl::PointXYZ>::Ptr pc2d(new pcl::PointCloud<pcl::PointXYZ>);
float height = 0.0;
for(std::vector<int>::const_iterator it=indices.indices.begin(); it != indices.indices.end(); ++it)
{
Eigen::Vector3f tmp = tf * (*cloud)[*it].getVector3fMap();
height = std::max<float>(height, fabs(tmp(2)));
pc2d->push_back(pcl::PointXYZ(tmp(0),tmp(1),0.0));
}
// create convex hull of projected points
#ifdef PCL_MINOR_VERSION >= 6
pcl::PointCloud<pcl::PointXYZ>::Ptr conv_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setDimension(2);
chull.setInputCloud(pc2d);
chull.reconstruct(*conv_hull);
#endif
/*for(int i=0; i<conv_hull->size(); ++i)
std::cout << (*conv_hull)[i].x << "," << (*conv_hull)[i].y << std::endl;*/
// find the minimal bounding rectangle in 2D and table coordinates
Eigen::Vector2f p1, p2, p3;
cob_3d_mapping::MinimalRectangle2D mr2d;
mr2d.setConvexHull(conv_hull->points);
mr2d.rotatingCalipers(p2, p1, p3);
/*std::cout << "BB: \n" << p1[0] << "," << p1[1] <<"\n"
<< p2[0] << "," << p2[1] <<"\n"
<< p3[0] << "," << p3[1] <<"\n ---" << std::endl;*/
// compute center of rectangle
position[0] = 0.5f*(p1[0] + p3[0]);
position[1] = 0.5f*(p1[1] + p3[1]);
position[2] = 0.0f;
// transform back
Eigen::Affine3f inv_tf = tf.inverse();
position = inv_tf * position;
// set size of bounding box
float norm_1 = (p3-p2).norm();
float norm_2 = (p1-p2).norm();
// BoundingBox coordinates: X:= main direction, Z:= table normal
Eigen::Vector3f direction; // y direction
if (norm_1 < norm_2)
{
direction = Eigen::Vector3f(p3[0]-p2[0], p3[1]-p2[1], 0) / (norm_1);
size[0] = norm_2 * 0.5f;
size[1] = norm_1 * 0.5f;
}
else
{
direction = Eigen::Vector3f(p1[0]-p2[0], p1[1]-p2[1], 0) / (norm_2);
size[0] = norm_1 * 0.5f;
size[1] = norm_2 * 0.5f;
}
size[2] = -height;
direction = inv_tf.rotation() * direction;
orientation = pcl::getTransformationFromTwoUnitVectors(direction, plane_normal).rotation(); // (y, z-direction)
return;
Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - plane_normal * plane_normal.transpose();
Eigen::Vector3f xn = M * Eigen::Vector3f::UnitX(); // X-axis project on normal
Eigen::Vector3f xxn = M * direction;
float cos_phi = acos(xn.normalized().dot(xxn.normalized())); // angle between xn and main direction
cos_phi = cos(0.5f * cos_phi);
float sin_phi = sqrt(1.0f-cos_phi*cos_phi);
//orientation.w() = cos_phi;
//orientation.x() = sin_phi * plane_normal(0);
//orientation.y() = sin_phi * plane_normal(1);
//orientation.z() = sin_phi * plane_normal(2);
}
bool Pcl_grabing::isBlock()
{
update_kinect_points();
int npts = transformed_pclKinect_clr_ptr_->points.size();
Eigen::Vector3f pt;
Eigen::Vector3f dist;
vector<int> index;
index.clear();
double distance = 1;
BlockColor<<0,0,0;
for (int i = 0; i < npts; i++)
{
pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
dist = pt - TableCentroid;
dist[2]=0;
distance = dist.norm();
if(distance < TableRadius)
if(pt[2]>(TableHeight+0.003) && pt[2]<BlockMaxHeight)
{
index.push_back(i);
BlockColor(0)+=transformed_pclKinect_clr_ptr_->points[i].r;
BlockColor(1)+=transformed_pclKinect_clr_ptr_->points[i].g;
BlockColor(2)+=transformed_pclKinect_clr_ptr_->points[i].b;
}
}
int n_block_points = index.size();
if(n_block_points<10)
{
ROS_INFO("There is no block on the stool.");
return 0;
}
ROS_INFO("There is a block with %d points", n_block_points);
BlockColor/=n_block_points;
ROS_INFO_STREAM("The block color:"<<BlockColor.transpose());
display_ptr_->header = transformed_pclKinect_clr_ptr_->header;
display_ptr_->is_dense = transformed_pclKinect_clr_ptr_->is_dense;
display_ptr_->width = n_block_points; //transformed_pclKinect_clr_ptr_->width;
display_ptr_->height = transformed_pclKinect_clr_ptr_->height;
display_ptr_->points.resize(n_block_points);
for (int i = 0; i < n_block_points; i++)
{
display_ptr_->points[i].getVector3fMap() = transformed_pclKinect_clr_ptr_->points[index[i]].getVector3fMap();
}
display_points(*display_ptr_);
Eigen::Vector3f BlockCentroid;
BlockCentroid =pcl_wsn.compute_centroid(display_ptr_);
ROS_INFO_STREAM("The centroid of the block:"<<BlockCentroid.transpose());
vector<int> block_index;
block_index.clear();
for (int i = 0; i < n_block_points; i++)
{
pt=display_ptr_->points[i].getVector3fMap();
dist = pt - BlockCentroid;
dist[2]=0;
distance = dist.norm();
if(distance < BlockTopRadius)
{
block_index.push_back(i);
}
}
int n_block_top = block_index.size();
ROS_INFO("There are %d points around the block's top center",n_block_top);
pcl::PointCloud<pcl::PointXYZ>::Ptr block_ptr_(new PointCloud<pcl::PointXYZ>);
block_ptr_->header=display_ptr_->header;
block_ptr_->is_dense=display_ptr_->is_dense;
block_ptr_->width=n_block_top;
block_ptr_->height=display_ptr_->height;
block_ptr_->points.resize(n_block_top);
for (int i = 0; i < n_block_top; i++)
{
block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
}
BlockTopCentroid = pcl_wsn.compute_centroid(block_ptr_);
ROS_INFO_STREAM("The centroid of the block's top:"<<BlockTopCentroid.transpose());
//display_points(*block_ptr_);
block_index.clear();
for(int i = 0; i < n_block_points; i++)
{
pt=display_ptr_->points[i].getVector3fMap();
if(abs(pt[2]-BlockTopCentroid[2])<0.002)
{
block_index.push_back(i);
}
}
n_block_top = block_index.size();
block_ptr_->header=display_ptr_->header;
block_ptr_->is_dense=display_ptr_->is_dense;
block_ptr_->width=n_block_top;
block_ptr_->height=display_ptr_->height;
block_ptr_->points.resize(n_block_top);
for (int i = 0; i < n_block_top; i++)
{
block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
}
double block_dist;
pcl_wsn.fit_points_to_plane(block_ptr_,Block_Normal,block_dist);
Block_Major = pcl_wsn.get_major_axis();
ROS_INFO_STREAM("The major vector of the block's top:"<<Block_Major.transpose());
//display_points(*block_ptr_);
return true;
}
Eigen::Matrix3f Homography::calcHomography( const Eigen::Matrix3f &R, const Eigen::Vector3f& t, const Eigen::Vector3f& n, float d ) const
{
Eigen::Matrix3f A = R + (t*n.transpose()) / d;
return A;
}
