inline void VectorAverage<double, 3>::getEigenVector1(Eigen::Matrix<double, 3, 1>& eigen_vector1) const
{
//cout << "Using specialized 3x3 version of doPCA!\n";
Eigen::Vector3d::Scalar eigen_value;
Eigen::Vector3d eigen_vector;
eigen33(covariance_, eigen_value, eigen_vector);
eigen_vector1 = eigen_vector;
}
inline void VectorAverage<double, 3>::getEigenVector1(Eigen::Matrix<double, 3, 1>& eigen_vector1) const
{
//cout << "Using specialized 3x3 version of doPCA!\n";
Eigen::Matrix<double, 3, 1> eigen_values;
Eigen::Matrix<double, 3, 3> eigen_vectors;
eigen33(covariance_, eigen_vectors, eigen_values);
eigen_vector1 = eigen_vectors.col(0);
}
template <typename PointT> bool
pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon)
{
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients[0] = eigen_vector [0];
model_coefficients[1] = eigen_vector [1];
model_coefficients[2] = eigen_vector [2];
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
float distance_to_plane = model_coefficients[0] * point.x +
model_coefficients[1] * point.y +
model_coefficients[2] * point.z +
model_coefficients[3];
PointT ppoint;
// Calculate the projection of the point on the plane
ppoint.x = point.x - distance_to_plane * model_coefficients[0];
ppoint.y = point.y - distance_to_plane * model_coefficients[1];
ppoint.z = point.z - distance_to_plane * model_coefficients[2];
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> xy_polygon;
xy_polygon.points.resize (polygon.points.size ());
for (size_t i = 0; i < polygon.points.size (); ++i)
{
Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0);
xy_polygon.points[i].x = pt[k1];
xy_polygon.points[i].y = pt[k2];
xy_polygon.points[i].z = 0;
}
PointT xy_point;
xy_point.z = 0;
Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
xy_point.x = pt[k1];
xy_point.y = pt[k2];
return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon));
}
inline void VectorAverage<float, 3>::doPCA(Eigen::Matrix<float, 3, 1>& eigen_values, Eigen::Matrix<float, 3, 1>& eigen_vector1,
Eigen::Matrix<float, 3, 1>& eigen_vector2, Eigen::Matrix<float, 3, 1>& eigen_vector3) const
{
//cout << "Using specialized 3x3 version of doPCA!\n";
Eigen::Matrix<float, 3, 3> eigen_vectors;
eigen33(covariance_, eigen_vectors, eigen_values);
eigen_vector1 = eigen_vectors.col(0);
eigen_vector2 = eigen_vectors.col(1);
eigen_vector3 = eigen_vectors.col(2);
}
// 二次曲線当てはめの微分係数行列から２次曲線係数を計算する
// 戻り値：固有値の数
int
fitConic(double sum[5][5], // 二次曲線当てはめの微分係数行列
double coef[3][6], // 二次曲線係数
double* offset) // 重心のオフセット
{
double P00[3][3] = {
{sum[4][0], sum[3][1], sum[2][2]},
{sum[3][1], sum[2][2], sum[1][3]},
{sum[2][2], sum[1][3], sum[0][4]}
};
double P01[3][3] = {
{sum[3][0], sum[2][1], sum[2][0]},
{sum[2][1], sum[1][2], sum[1][1]},
{sum[1][2], sum[0][3], sum[0][2]}
};
double P10[3][3] = {
{sum[3][0], sum[2][1], sum[1][2]},
{sum[2][1], sum[1][2], sum[0][3]},
{sum[2][0], sum[1][1], sum[0][2]}
};
double P11[3][3] = {
{sum[2][0], sum[1][1], sum[1][0]},
{sum[1][1], sum[0][2], sum[0][1]},
{sum[1][0], sum[0][1], sum[0][0]}
};
double TMP1[3][3];
CvMat vP10 = cvMat(3, 3, CV_64FC1, P10);
CvMat vP11 = cvMat(3, 3, CV_64FC1, P11);
CvMat vTMP1 = cvMat(3, 3, CV_64FC1, TMP1);
double ret = cvSolve(&vP11, &vP10, &vTMP1);
if (ret == 0)
{
return -1;
}
double TMP2[3][3];
mulM33(P01, TMP1, TMP2);
double TMP3[3][3];
subM33(P00, TMP2, TMP3);
double e[3]; // eigen value
double ev[3][3]; // eigen vectors
// 固有値の計算
const int nEigen = eigen33(e, ev, TMP3, VISION_EPS);
int i;
double newcoef3;
double newcoef4;
double newcoef5;
for (i = 0; i < nEigen; i++)
{
copyV3(ev[i], coef[i]);
mulM33V3(TMP1, ev[i], &(coef[i][3]));
mulV3S(-1.0, &(coef[i][3]), &(coef[i][3]));
// オフセットを考慮した計算
newcoef3 = -2.0 * coef[i][0] * offset[0] - coef[i][1] * offset[1] + coef[i][3];
newcoef4 = -coef[i][1] * offset[0] - 2.0 * coef[i][2] * offset[1] + coef[i][4];
newcoef5 = coef[i][0] * offset[0] * offset[0] + coef[i][1] * offset[0] * offset[1]
+ coef[i][2] * offset[1] * offset[1] - coef[i][3] * offset[0]
- coef[i][4] * offset[1] + coef[i][5];
coef[i][3] = newcoef3;
coef[i][4] = newcoef4;
coef[i][5] = newcoef5;
}
return nEigen;
}
template <typename PointT> void
pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output)
{
output.header = input_->header;
if (!initCompute ())
{
output.indices.clear ();
return;
}
if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_)
{
PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ());
output.indices.clear ();
return;
}
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid);
// Compute the model coefficients
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
eigen33 (covariance_matrix, eigen_value, eigen_vector);
model_coefficients[0] = eigen_vector [0];
model_coefficients[1] = eigen_vector [1];
model_coefficients[2] = eigen_vector [2];
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
// See if we need to flip any plane normals
vp -= planar_hull_->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
if (cos_theta < 0)
{
model_coefficients *= -1;
model_coefficients[3] = 0;
// Hessian form (D = nc . p_plane (centroid here) + p)
model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ()));
}
// Project all points
PointCloud projected_points;
SampleConsensusModelPlane<PointT> sacmodel (input_);
sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false);
// Create a X-Y projected representation for within bounds polygonal checking
int k0, k1, k2;
// Determine the best plane to project points onto
k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1;
k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2;
k1 = (k0 + 1) % 3;
k2 = (k0 + 2) % 3;
// Project the convex hull
pcl::PointCloud<PointT> polygon;
polygon.points.resize (planar_hull_->points.size ());
for (size_t i = 0; i < planar_hull_->points.size (); ++i)
{
Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0);
polygon.points[i].x = pt[k1];
polygon.points[i].y = pt[k2];
polygon.points[i].z = 0;
}
PointT pt_xy;
pt_xy.z = 0;
output.indices.resize (indices_->size ());
int l = 0;
for (size_t i = 0; i < projected_points.points.size (); ++i)
{
// Check the distance to the user imposed limits from the table planar model
double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients);
if (distance < height_limit_min_ || distance > height_limit_max_)
continue;
// Check what points are inside the hull
Eigen::Vector4f pt (projected_points.points[i].x,
projected_points.points[i].y,
projected_points.points[i].z, 0);
pt_xy.x = pt[k1];
pt_xy.y = pt[k2];
if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon))
continue;
output.indices[l++] = (*indices_)[i];
}
output.indices.resize (l);
deinitCompute ();
}
