void TestMatrixExtras::test_mult_SST() {
size_t n = 10;
size_t k = 8;
size_t m = 9;
size_t repetitions = 300;
for (size_t r = 0; r < repetitions; r++) {
Matrix A = MatrixFactory::MakeRandomSparse(n, k, std::ceil(n * k / 2), 2.0, 1.0);
Matrix B = MatrixFactory::MakeRandomSparse(m, k, std::ceil(m * k / 2), 2.0, 1.0);
Matrix C = MatrixFactory::MakeRandomSparse(n, m, std::ceil(n * m / 2), 2.0, 1.0);
B.transpose();
double alpha = -2.0 + 2.0 * static_cast<double> (std::rand()) / static_cast<double> (RAND_MAX);
double gamma = -2.0 + 3.0 * static_cast<double> (std::rand()) / static_cast<double> (RAND_MAX);
Matrix C_orig(C);
int status = Matrix::mult(C, alpha, A, B, gamma);
_ASSERT_EQ(ForBESUtils::STATUS_OK, status);
C_orig *= gamma;
Matrix AB = A * B;
AB *= alpha;
C_orig += AB;
_ASSERT_EQ(C_orig, C);
}
}
void TestMatrixExtras::test_mult_SX() {
size_t repetitions = 300;
for (size_t r = 0; r < repetitions; r++) {
size_t n = 10;
size_t k = 8;
size_t nnz = std::ceil(n * k / 2);
Matrix A = MatrixFactory::MakeRandomSparse(n, k, nnz, 2.0, 1.0);
Matrix B = MatrixFactory::MakeRandomMatrix(k, k, 0.0, 1.0, Matrix::MATRIX_DIAGONAL);
Matrix C = MatrixFactory::MakeRandomSparse(n, k, nnz, 2.0, 1.0);
Matrix D = MatrixFactory::MakeRandomMatrix(n, k, 0.0, 1.0);
double alpha = -2.0 + 2.0 * static_cast<double> (std::rand()) / static_cast<double> (RAND_MAX);
double gamma = -2.0 + 3.0 * static_cast<double> (std::rand()) / static_cast<double> (RAND_MAX);
Matrix D_orig(D);
Matrix C_orig(C);
int status = Matrix::mult(C, alpha, A, B, gamma);
_ASSERT_EQ(ForBESUtils::STATUS_OK, status);
status = Matrix::mult(D, alpha, A, B, gamma);
_ASSERT_EQ(ForBESUtils::STATUS_OK, status);
C_orig *= gamma;
D_orig *= gamma;
Matrix AB = A * B;
AB *= alpha;
C_orig += AB;
D_orig += AB;
_ASSERT_EQ(C_orig, C);
_ASSERT_EQ(D_orig, D);
}
}
bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
// first build plane model
geometry_msgs::PolygonStamped target_plane = req.request.target_plane;
// check the size of the points
if (target_plane.polygon.points.size() < 3) {
NODELET_ERROR("not enough points included in target_plane");
return false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
EigenVector3fVector points;
Eigen::Vector3f n, p;
if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*points_inside_pole);
publishPointCloud(debug_candidate_points_pub_, points_inside_pole);
// estimate plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a planar model for the given dataset.");
return false;
}
// extract plane points
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (plane_inliers);
proj.setInputCloud (points_inside_pole);
proj.setModelCoefficients (plane_coefficients);
proj.filter (*plane_points);
publishPointCloud(debug_candidate_points_pub3_, plane_points);
// next, compute convexhull and centroid
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (plane_points);
chull.setDimension(2);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
if (cloud_hull->points.size() < 3) {
NODELET_ERROR("failed to estimate convex hull");
return false;
}
publishConvexHullMarker(cloud_hull);
Eigen::Vector4f C_new_4f;
pcl::compute3DCentroid(*cloud_hull, C_new_4f);
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = C_new_4f[i];
}
Eigen::Vector3f n_prime;
n_prime[0] = plane_coefficients->values[0];
n_prime[1] = plane_coefficients->values[1];
n_prime[2] = plane_coefficients->values[2];
plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm();
n_prime.normalize();
if (n_prime.dot(n) < 0) {
n_prime = - n_prime;
plane_coefficients->values[3] = - plane_coefficients->values[3];
}
Eigen::Vector3f n_cross = n.cross(n_prime);
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
// compute C
Eigen::Vector3f C_orig(0, 0, 0);
for (size_t i = 0; i < points.size(); i++) {
C_orig = C_orig + points[i];
}
C_orig = C_orig / points.size();
// compute C
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = target_plane.header;
debug_centroid_pub_.publish(centroid);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new[0];
centroid_transformed.point.y = C_new[1];
centroid_transformed.point.z = C_new[2];
centroid_transformed.header = target_plane.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
return true;
}
