void pointCloudModifier::manualTableDetection(pcl::PointCloud<pointT>::Ptr cloud_in,
pcl::PointCloud<pointT>::Ptr cloud_out,
std::vector<pointT> &points){
// Compute the plane coefficients given 3 points
computePlaneCoefficients(points);
// Move the coordenate system of the point cloud to the table plane
// 1st - Set the translation to the first picked point
Eigen::Vector3f point_on_plane_negated(-points[0].x, -points[0].y, -points[0].z);
Eigen::Translation3f translate((Eigen::Vector3f)(point_on_plane_negated));
// 2nd - Find the rotation
Eigen::Vector3f normal_vector(
_planeCoefficients->values[0],
_planeCoefficients->values[1],
_planeCoefficients->values[2]);
Eigen::Vector3f z_vector(0,0,1);
Eigen::Vector3f k_vector = normal_vector.cross(z_vector);
k_vector.normalize();
float angle = acos(normal_vector.dot(z_vector));
Eigen::Affine3f rotate = (Eigen::Affine3f)Eigen::AngleAxisf(angle, k_vector);
// 3rd - Calculate the final transformation
Eigen::Affine3f total_transformation = rotate*translate;
// 4th - Transform the point cloud
pcl::transformPointCloud(
*cloud_in,
*cloud_out,
(Eigen::Affine3f)total_transformation);
}
/**
@param point Un point donne
@param line Un segment de droite donnee
@param intersection si ce pointeur est different de 0, le QPointF ainsi
designe contiendra les coordonnees du projete orthogonal, meme si celui-ci
n'appartient pas au segment de droite
@return true si le projete orthogonal du point sur la droite appartient au
segment de droite.
*/
bool QET::orthogonalProjection(const QPointF &point, const QLineF &line, QPointF *intersection) {
// recupere le vecteur normal de `line'
QLineF line_normal_vector(line.normalVector());
QPointF normal_vector(line_normal_vector.dx(), line_normal_vector.dy());
// cree une droite perpendiculaire a `line' passant par `point'
QLineF perpendicular_line(point, point + normal_vector);
// determine le point d'intersection des deux droites = le projete orthogonal
QPointF intersection_point;
QLineF::IntersectType it = line.intersect(perpendicular_line, &intersection_point);
// ne devrait pas arriver (mais bon...)
if (it == QLineF::NoIntersection) return(false);
// fournit le point d'intersection a l'appelant si necessaire
if (intersection) {
*intersection = intersection_point;
}
// determine si le point d'intersection appartient au segment de droite
if (QET::lineContainsPoint(line, intersection_point)) {
return(true);
}
return(false);
}
geometry_msgs::Pose transformPose(geometry_msgs::Pose pose_in, Eigen::Matrix4f transf){
geometry_msgs::Pose pose_out;
// Obtener rotación desde matriz de transformación
Eigen::Matrix4f rot;
rot = transf;
rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
// Crear normal desde quaternion
tf::Quaternion tf_q;
tf::quaternionMsgToTF(pose_in.orientation, tf_q);
tf::Vector3 normal(1, 0, 0);
normal = tf::quatRotate(tf_q, normal);
normal.normalize();
// Rotar normal
Eigen::Vector3f normal_vector (normal.x(), normal.y(), normal.z());
Eigen::Vector3f normal_rotated = transformVector(normal_vector, transf);
normal_rotated.normalize();
// Obtener quaternion desde normal rotada
pose_out.orientation = coefsToQuaternionMsg(normal_rotated[0], normal_rotated[1], normal_rotated[2]);
// Transportar posición
pose_out.position = transformPoint(pose_in.position, transf);
return pose_out;
}
bool UnitSquare::intersect( Ray3D& ray, const Matrix4x4& worldToModel,
const Matrix4x4& modelToWorld ) {
// TODO: implement intersection code for UnitSquare, which is
// defined on the xy-plane, with vertices (0.5, 0.5, 0),
// (-0.5, 0.5, 0), (-0.5, -0.5, 0), (0.5, -0.5, 0), and normal
// (0, 0, 1).
//
// Your goal here is to fill ray.intersection with correct values
// should an intersection occur. This includes intersection.point,
// intersection.normal, intersection.none, intersection.t_value.
//
// HINT: Remember to first transform the ray into object space
// to simplify the intersection test.
Vector3D d = worldToModel * ray.dir;
Point3D a = worldToModel * ray.origin;
double lambda = - a[2] / d[2];
Point3D p = a + (lambda * d);
if (ray.intersection.none || ray.intersection.t_value > lambda){
if(p[0] > 0.5 || p[0] < -0.5 || p[1] > 0.5 || p[1] < -0.5){
return false;
}
Vector3D normal_vector(0, 0, 1);
ray.intersection.t_value = lambda;
ray.intersection.normal = transNorm(worldToModel, normal_vector);
ray.intersection.normal.normalize();
ray.intersection.point = modelToWorld * p;
ray.intersection.none = false;
return true;
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::Update()
{
// Get Time Difference dt
common::Time cur_time = world->GetSimTime();
double dt = (cur_time - last_time).Double();
if (last_time + update_period > cur_time) return;
boost::mutex::scoped_lock scoped_lock(lock);
// Get Pose/Orientation
math::Pose pose = link->GetWorldPose();
// get Acceleration and Angular Rates
// the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
//accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
accel = pose.rot.RotateVectorReverse((temp - velocity) / dt);
velocity = temp;
// GetRelativeAngularVel() sometimes return nan?
//rate = link->GetRelativeAngularVel(); // get angular rate in body frame
math::Quaternion delta = pose.rot - orientation;
orientation = pose.rot;
rate.x = 2.0 * (-orientation.x * delta.w + orientation.w * delta.x + orientation.z * delta.y - orientation.y * delta.z) / dt;
rate.y = 2.0 * (-orientation.y * delta.w - orientation.z * delta.x + orientation.w * delta.y + orientation.x * delta.z) / dt;
rate.z = 2.0 * (-orientation.z * delta.w + orientation.y * delta.x - orientation.x * delta.y + orientation.w * delta.z) / dt;
// get Gravity
gravity = world->GetPhysicsEngine()->GetGravity();
gravity_body = orientation.RotateVectorReverse(gravity);
double gravity_length = gravity.GetLength();
ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
// add gravity vector to body acceleration
accel = accel - gravity_body;
// update sensor models
accel = accel + accelModel.update(dt);
rate = rate + rateModel.update(dt);
headingModel.update(dt);
ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "Current errors: accel = [%g %g %g], rate = [%g %g %g], heading = %g",
accelModel.getCurrentError().x, accelModel.getCurrentError().y, accelModel.getCurrentError().z,
rateModel.getCurrentError().x, rateModel.getCurrentError().y, rateModel.getCurrentError().z,
headingModel.getCurrentError());
// apply offset error to orientation (pseudo AHRS)
double normalization_constant = (gravity_body + accelModel.getCurrentError()).GetLength() * gravity_body.GetLength();
double cos_alpha = (gravity_body + accelModel.getCurrentError()).Dot(gravity_body)/normalization_constant;
math::Vector3 normal_vector(gravity_body.Cross(accelModel.getCurrentError()));
normal_vector *= sqrt((1 - cos_alpha)/2)/normalization_constant;
math::Quaternion attitudeError(sqrt((1 + cos_alpha)/2), normal_vector.x, normal_vector.y, normal_vector.z);
math::Quaternion headingError(cos(headingModel.getCurrentError()/2),0,0,sin(headingModel.getCurrentError()/2));
pose.rot = attitudeError * pose.rot * headingError;
// copy data into pose message
imuMsg.header.frame_id = frameId;
imuMsg.header.stamp.sec = cur_time.sec;
imuMsg.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
imuMsg.orientation.x = pose.rot.x;
imuMsg.orientation.y = pose.rot.y;
imuMsg.orientation.z = pose.rot.z;
imuMsg.orientation.w = pose.rot.w;
// pass angular rates
imuMsg.angular_velocity.x = rate.x;
imuMsg.angular_velocity.y = rate.y;
imuMsg.angular_velocity.z = rate.z;
// pass accelerations
imuMsg.linear_acceleration.x = accel.x;
imuMsg.linear_acceleration.y = accel.y;
imuMsg.linear_acceleration.z = accel.z;
// fill in covariance matrix
imuMsg.orientation_covariance[8] = headingModel.gaussian_noise*headingModel.gaussian_noise;
if (gravity_length > 0.0) {
imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
} else {
imuMsg.orientation_covariance[0] = -1;
imuMsg.orientation_covariance[4] = -1;
}
// publish to ros
pub_.publish(imuMsg);
// debug output
#ifdef DEBUG_OUTPUT
if (debugPublisher) {
geometry_msgs::PoseStamped debugPose;
debugPose.header = imuMsg.header;
debugPose.header.frame_id = "/map";
debugPose.pose.orientation.w = imuMsg.orientation.w;
debugPose.pose.orientation.x = imuMsg.orientation.x;
debugPose.pose.orientation.y = imuMsg.orientation.y;
debugPose.pose.orientation.z = imuMsg.orientation.z;
math::Pose pose = link->GetWorldPose();
debugPose.pose.position.x = pose.pos.x;
debugPose.pose.position.y = pose.pos.y;
debugPose.pose.position.z = pose.pos.z;
debugPublisher.publish(debugPose);
}
#endif // DEBUG_OUTPUT
// save last time stamp
last_time = cur_time;
}
int main (int argc, char** argv)
{
//---------------------------------------------------------------------------------------------------
//-- Initialization stuff
//---------------------------------------------------------------------------------------------------
//-- Command-line arguments
float ransac_threshold = 0.02;
float hsv_s_threshold = 0.30;
float hsv_v_threshold = 0.35;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
if (pcl::console::find_switch(argc, argv, "--ransac-threshold"))
pcl::console::parse_argument(argc, argv, "--ransac-threshold", ransac_threshold);
else
{
std::cerr << "RANSAC theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-s-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-s-threshold", hsv_s_threshold);
else
{
std::cerr << "Saturation theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-v-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-v-threshold", hsv_v_threshold);
else
{
std::cerr << "Value theshold not specified, using default value..." << std::endl;
}
//-- Get point cloud file from arguments
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames.size() != 1)
{
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames.size() != 1)
{
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Load point cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Load point cloud data (with color)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << ransac_threshold << std::endl
<< "\tColor point threshold: " << hsv_s_threshold << std::endl
<< "\tColor region threshold: " << hsv_v_threshold << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//--------------------------------------------------------------------------------------------------------
//-- Program does actual work from here
//--------------------------------------------------------------------------------------------------------
Debug debug;
debug.setAutoShow(false);
debug.setEnabled(false);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(source_cloud_color, Debug::COLOR_ORIGINAL);
debug.show("Original with color");
//-- Downsample the dataset prior to plane detection (using a leaf size of 1cm)
//-----------------------------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(source_cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f);
voxel_grid.filter(*cloud_filtered);
std::cout << "Initially PointCloud has: " << source_cloud->points.size () << " data points." << std::endl;
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
//-- Detect all possible planes
//-----------------------------------------------------------------------------------
std::vector<pcl::ModelCoefficientsPtr> all_planes;
pcl::SACSegmentation<pcl::PointXYZ> ransac_segmentation;
ransac_segmentation.setOptimizeCoefficients(true);
ransac_segmentation.setModelType(pcl::SACMODEL_PLANE);
ransac_segmentation.setMethodType(pcl::SAC_RANSAC);
ransac_segmentation.setDistanceThreshold(ransac_threshold);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr current_plane(new pcl::ModelCoefficients);
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
ransac_segmentation.setInputCloud(cloud_filtered);
ransac_segmentation.segment(*inliers, *current_plane);
if (inliers->indices.size() == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
//-- Save plane
pcl::ModelCoefficients::Ptr copy_current_plane(new pcl::ModelCoefficients);
*copy_current_plane = *current_plane;
all_planes.push_back(copy_current_plane);
//-- Debug stuff
debug.setEnabled(false);
debug.plotPlane(*current_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(cloud_filtered, Debug::COLOR_RED);
debug.show("Plane segmentation");
}
//-- Filter planes to obtain garment plane
//-----------------------------------------------------------------------------------
pcl::ModelCoefficients::Ptr garment_plane(new pcl::ModelCoefficients);
float min_height = FLT_MAX;
pcl::PointXYZ garment_projected_center;
for(int i = 0; i < all_planes.size(); i++)
{
//-- Check orientation
Eigen::Vector3f normal_vector(all_planes[i]->values[0],
all_planes[i]->values[1],
all_planes[i]->values[2]);
normal_vector.normalize();
Eigen::Vector3f good_orientation(0, -1, -1);
good_orientation.normalize();
std::cout << "Checking vector with dot product: " << std::abs(normal_vector.dot(good_orientation)) << std::endl;
if (std::abs(normal_vector.dot(good_orientation)) >= 0.9)
{
//-- Check "height" (height is defined in the local frame of reference in the yz direction)
//-- With this frame, it is approximately equal to the norm of the vector OO' (being O the
//-- center of the old frame and O' the projection of that center onto the plane).
//-- Project center point onto given plane:
pcl::PointCloud<pcl::PointXYZ>::Ptr center_to_be_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
center_to_be_projected_cloud->points.push_back(pcl::PointXYZ(0,0,0));
pcl::PointCloud<pcl::PointXYZ>::Ptr center_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> project_inliners;
project_inliners.setModelType(pcl::SACMODEL_PLANE);
project_inliners.setInputCloud(center_to_be_projected_cloud);
project_inliners.setModelCoefficients(all_planes[i]);
project_inliners.filter(*center_projected_cloud);
pcl::PointXYZ projected_center = center_projected_cloud->points[0];
Eigen::Vector3f projected_center_vector(projected_center.x, projected_center.y, projected_center.z);
float height = projected_center_vector.norm();
if (height < min_height)
{
min_height = height;
*garment_plane = *all_planes[i];
garment_projected_center = projected_center;
}
}
}
if (!(min_height < FLT_MAX))
{
std::cerr << "Garment plane not found!" << std::endl;
return -3;
}
else
{
std::cout << "Found closest plane with h=" << min_height << std::endl;
//-- Debug stuff
debug.setEnabled(true);
debug.plotPlane(*garment_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(source_cloud, Debug::COLOR_RED);
debug.show("Garment plane");
}
//-- Reorient cloud to origin (with color point cloud)
//-----------------------------------------------------------------------------------
//-- Translating to center
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f translation_transform = Eigen::Affine3f::Identity();
translation_transform.translation() << -garment_projected_center.x, -garment_projected_center.y, -garment_projected_center.z;
//pcl::transformPointCloud(*source_cloud_color, *centered_cloud, translation_transform);
//-- Orient using the plane normal
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f normal_vector(garment_plane->values[0], garment_plane->values[1], garment_plane->values[2]);
//-- Check normal vector orientation
if (normal_vector.dot(Eigen::Vector3f::UnitZ()) >= 0 && normal_vector.dot(Eigen::Vector3f::UnitY()) >= 0)
normal_vector = -normal_vector;
Eigen::Quaternionf rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(normal_vector, Eigen::Vector3f::UnitZ());
//pcl::transformPointCloud(*centered_cloud, *oriented_cloud, Eigen::Vector3f(0,0,0), rotation_quaternion);
Eigen::Transform<float, 3, Eigen::Affine> t(rotation_quaternion * translation_transform);
pcl::transformPointCloud(*source_cloud_color, *oriented_cloud, t);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform1.txt"), translation_transform, rotation_quaternion);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_cloud, Debug::COLOR_GREEN);
debug.show("Oriented");
//-- Filter points under the garment table
//-----------------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr garment_table_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> passthrough_filter;
passthrough_filter.setInputCloud(oriented_cloud);
passthrough_filter.setFilterFieldName("z");
passthrough_filter.setFilterLimits(-ransac_threshold/2.0f, FLT_MAX);
passthrough_filter.setFilterLimitsNegative(false);
passthrough_filter.filter(*garment_table_cloud);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(garment_table_cloud, Debug::COLOR_GREEN);
debug.show("Table cloud (filtered)");
//-- Color segmentation of the garment
//-----------------------------------------------------------------------------------
//-- HSV thresholding
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloudXYZRGBtoXYZHSV(*garment_table_cloud, *hsv_cloud);
for (int i = 0; i < hsv_cloud->points.size(); i++)
{
if (isnan(hsv_cloud->points[i].x) || isnan(hsv_cloud->points[i].y || isnan(hsv_cloud->points[i].z)))
continue;
if (hsv_cloud->points[i].s > hsv_s_threshold && hsv_cloud->points[i].v > hsv_v_threshold)
filtered_garment_cloud->push_back(garment_table_cloud->points[i]);
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(filtered_garment_cloud, Debug::COLOR_GREEN);
debug.show("Garment cloud");
//-- Euclidean Clustering of the resultant cloud
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtered_garment_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_custering;
euclidean_custering.setClusterTolerance(0.005);
euclidean_custering.setMinClusterSize(100);
euclidean_custering.setSearchMethod(tree);
euclidean_custering.setInputCloud(filtered_garment_cloud);
euclidean_custering.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr largest_color_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
int largest_cluster_size = 0;
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back(filtered_garment_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Found cluster of " << cloud_cluster->points.size() << " points." << std::endl;
if (cloud_cluster->points.size() > largest_cluster_size)
{
largest_cluster_size = cloud_cluster->points.size();
*largest_color_cluster = *cloud_cluster;
}
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(largest_color_cluster, Debug::COLOR_GREEN);
debug.show("Filtered garment cloud");
//-- Centering the point cloud before saving it
//-----------------------------------------------------------------------------------
//-- Find bounding box
pcl::MomentOfInertiaEstimation<pcl::PointXYZRGB> feature_extractor;
pcl::PointXYZRGB min_point_AABB, max_point_AABB;
pcl::PointXYZRGB min_point_OBB, max_point_OBB;
pcl::PointXYZRGB position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(largest_color_cluster);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//-- Translating to center
pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f garment_translation_transform = Eigen::Affine3f::Identity();
garment_translation_transform.translation() << -position_OBB.x, -position_OBB.y, -position_OBB.z;
pcl::transformPointCloud(*largest_color_cluster, *centered_garment_cloud, garment_translation_transform);
//-- Orient using the principal axes of the bounding box
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f principal_axis_x(max_point_OBB.x - min_point_OBB.x, 0, 0);
Eigen::Quaternionf garment_rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(principal_axis_x, Eigen::Vector3f::UnitX()); //-- This transformation is wrong (I guess)
Eigen::Transform<float, 3, Eigen::Affine> t2 = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
t2.rotate(rotational_matrix_OBB.inverse());
//pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, Eigen::Vector3f(0,0,0), garment_rotation_quaternion);
pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, t2);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform2.txt"), garment_translation_transform, Eigen::Quaternionf(t2.rotation()));
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_garment_cloud, Debug::COLOR_GREEN);
debug.plotBoundingBox(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB, Debug::COLOR_YELLOW);
debug.show("Oriented garment patch");
//-- Save point cloud in file to process it in Python
pcl::io::savePCDFileBinary(argv[filenames[0]]+std::string("-output.pcd"), *oriented_garment_cloud);
return 0;
}
void pointCloudModifier::automaticTableDetection(pcl::PointCloud<pointT>::Ptr cloud_in,
pcl::PointCloud<pointT>::Ptr cloud_out){
pcl::PointCloud<pointT>::Ptr cloudProjected (new pcl::PointCloud<pointT>);
// Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
// Stores the coefficents to the plane (a * x + b * y + c * z = d)
seg.setInputCloud (cloud_in);
seg.segment (*inliers, *_planeCoefficients);
// Project the model inliers
pcl::ProjectInliers<pointT> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_in);
proj.setModelCoefficients (_planeCoefficients);
proj.filter (*cloudProjected);
// Move the coordinate system of the point cloud to the table plane
// 1st - Set the translation to the center of the plane negated
Eigen::Vector3f cloud_plane_center_negated(0,0,0);
for (size_t i = 0; i < cloudProjected->points.size(); ++i)
{
cloud_plane_center_negated[0] -= cloudProjected->points[i].x/cloudProjected->points.size();
cloud_plane_center_negated[1] -= cloudProjected->points[i].y/cloudProjected->points.size();
cloud_plane_center_negated[2] -= cloudProjected->points[i].z/cloudProjected->points.size();
}
Eigen::Translation3f translation((Eigen::Vector3f)(cloud_plane_center_negated));
// 2nd - Find the rotation
Eigen::Vector3f normal_vector(
_planeCoefficients->values[0],
_planeCoefficients->values[1],
_planeCoefficients->values[2]);
Eigen::Vector3f z_vector(0,0,1);
Eigen::Vector3f k_vector = normal_vector.cross(z_vector);
k_vector.normalize();
float angle = acos(normal_vector.dot(z_vector));
Eigen::Affine3f rotation = (Eigen::Affine3f)Eigen::AngleAxisf(angle, k_vector);
// 3rd - Calculate the final transformation
Eigen::Affine3f total_transformation = rotation*translation;
// 4th - Transform the point cloud
pcl::transformPointCloud(
*cloud_in,
*cloud_out,
(Eigen::Affine3f)total_transformation);
}
