void PlaneSupportedCuboidEstimator::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_INFO("cloudCallback");
if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
JSK_NODELET_WARN("Not yet polygon is available");
return;
}
if (!tracker_) {
NODELET_INFO("initTracker");
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons_.push_back(polygon);
}
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
ParticleCloud::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
support_plane_updated_ = false;
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
else {
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() > fast_cloud_threshold_) {
estimate(msg);
}
}
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> SnapIt::createConvexes(
const std::string& frame_id, const ros::Time& stamp,
jsk_recognition_msgs::PolygonArray::ConstPtr polygons)
{
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> result;
try
{
for (size_t i = 0; i < polygons->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
jsk_recognition_utils::Vertices vertices;
tf::StampedTransform transform = lookupTransformWithDuration(
tf_listener_,
polygon.header.frame_id, frame_id, stamp, ros::Duration(5.0));
for (size_t j = 0; j < polygon.polygon.points.size(); j++) {
Eigen::Vector4d p;
p[0] = polygon.polygon.points[j].x;
p[1] = polygon.polygon.points[j].y;
p[2] = polygon.polygon.points[j].z;
p[3] = 1;
Eigen::Affine3d eigen_transform;
tf::transformTFToEigen(transform, eigen_transform);
Eigen::Vector4d transformed_pointd = eigen_transform.inverse() * p;
Eigen::Vector3f transformed_point;
transformed_point[0] = transformed_pointd[0];
transformed_point[1] = transformed_pointd[1];
transformed_point[2] = transformed_pointd[2];
vertices.push_back(transformed_point);
}
std::reverse(vertices.begin(), vertices.end());
jsk_recognition_utils::ConvexPolygon::Ptr convex(new jsk_recognition_utils::ConvexPolygon(vertices));
result.push_back(convex);
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
catch (tf2::ExtrapolationException &e)
{
JSK_NODELET_ERROR("Transform error: %s", e.what());
}
return result;
}
void PointCloudLocalization::tfTimerCallback(
const ros::TimerEvent& event)
{
boost::mutex::scoped_lock lock(tf_mutex_);
try {
ros::Time stamp = event.current_real;
if (initialize_from_tf_ && first_time_) {
// Update localize_transform_ to points initialize_tf
tf::StampedTransform transform = lookupTransformWithDuration(
tf_listener_,
initialize_tf_, odom_frame_, stamp, ros::Duration(1.0));
localize_transform_ = transform;
}
tf_broadcast_.sendTransform(tf::StampedTransform(localize_transform_,
stamp,
global_frame_,
odom_frame_));
}
catch (tf2::TransformException& e) {
JSK_NODELET_FATAL("Failed to lookup transformation: %s", e.what());
}
}
void PlaneSupportedCuboidEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygon->decomposeToTriangles();
polygons_.push_back(polygon);
}
try {
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
if (!tracker_) {
NODELET_INFO("initTracker");
pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
}
else {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
tracker_->setInputCloud(cloud);
// Before call compute() method, we prepare candidate_cloud_ for
// weight phase.
candidate_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
std::set<int> all_indices;
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism_extract;
pcl::PointCloud<pcl::PointXYZ>::Ptr
boundaries (new pcl::PointCloud<pcl::PointXYZ>);
polygon->boundariesToPointCloud<pcl::PointXYZ>(*boundaries);
boundaries->points.push_back(boundaries->points[0]);
prism_extract.setInputCloud(cloud);
prism_extract.setViewPoint(viewpoint_[0], viewpoint_[1], viewpoint_[2]);
prism_extract.setHeightLimits(init_local_position_z_min_, init_local_position_z_max_);
prism_extract.setInputPlanarHull(boundaries);
pcl::PointIndices output_indices;
prism_extract.segment(output_indices);
for (size_t i = 0; i < output_indices.indices.size(); i++) {
all_indices.insert(output_indices.indices[i]);
}
}
pcl::ExtractIndices<pcl::PointXYZ> ex;
ex.setInputCloud(cloud);
pcl::PointIndices::Ptr indices (new pcl::PointIndices);
indices->indices = std::vector<int>(all_indices.begin(), all_indices.end());
ex.setIndices(indices);
ex.filter(*candidate_cloud_);
sensor_msgs::PointCloud2 ros_candidate_cloud;
pcl::toROSMsg(*candidate_cloud_, ros_candidate_cloud);
ros_candidate_cloud.header = msg->header;
pub_candidate_cloud_.publish(ros_candidate_cloud);
// check the number of candidate points
if (candidate_cloud_->points.size() == 0) {
JSK_NODELET_ERROR("No candidate cloud");
return;
}
tree_.setInputCloud(candidate_cloud_);
if (support_plane_updated_) {
// Compute assignment between particles and polygons
NODELET_INFO("polygon updated");
updateParticlePolygonRelationship(tracker_->getParticles());
}
ROS_INFO("start tracker_->compute()");
tracker_->compute();
ROS_INFO("done tracker_->compute()");
Particle result = tracker_->getResult();
jsk_recognition_msgs::BoundingBoxArray box_array;
box_array.header = msg->header;
box_array.boxes.push_back(result.toBoundingBox());
box_array.boxes[0].header = msg->header;
pub_result_.publish(box_array);
}
support_plane_updated_ = false;
ParticleCloud::Ptr particles = tracker_->getParticles();
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
catch (tf2::TransformException& e) {
JSK_ROS_ERROR("tf exception");
}
}
void PointCloudLocalization::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
//JSK_NODELET_INFO("cloudCallback");
latest_cloud_ = cloud_msg;
if (localize_requested_){
JSK_NODELET_INFO("localization is requested");
try {
pcl::PointCloud<pcl::PointNormal>::Ptr
local_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*latest_cloud_, *local_cloud);
JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
latest_cloud_->header.frame_id.c_str(),
global_frame_.c_str());
if (tf_listener_->waitForTransform(
latest_cloud_->header.frame_id,
global_frame_,
latest_cloud_->header.stamp,
ros::Duration(1.0))) {
pcl::PointCloud<pcl::PointNormal>::Ptr
input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl_ros::transformPointCloudWithNormals(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
else {
pcl_ros::transformPointCloud(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
applyDownsampling(input_cloud, *input_downsampled_cloud);
if (isFirstTime()) {
all_cloud_ = input_downsampled_cloud;
first_time_ = false;
}
else {
// run ICP
ros::ServiceClient client
= pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
jsk_pcl_ros::ICPAlign icp_srv;
if (clip_unseen_pointcloud_) {
// Before running ICP, remove pointcloud where we cannot see
// First, transform reference pointcloud, that is all_cloud_, into
// sensor frame.
// And after that, remove points which are x < 0.
tf::StampedTransform global_sensor_tf_transform
= lookupTransformWithDuration(
tf_listener_,
global_frame_,
sensor_frame_,
cloud_msg->header.stamp,
ros::Duration(1.0));
Eigen::Affine3f global_sensor_transform;
tf::transformTFToEigen(global_sensor_tf_transform,
global_sensor_transform);
pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*all_cloud_,
*sensor_cloud,
global_sensor_transform.inverse());
// Remove negative-x points
pcl::PassThrough<pcl::PointNormal> pass;
pass.setInputCloud(sensor_cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(0.0, 100.0);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pass.filter(*filtered_cloud);
JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
// Convert the pointcloud to global frame again
pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*filtered_cloud,
*global_filtered_cloud,
global_sensor_transform);
pcl::toROSMsg(*global_filtered_cloud,
icp_srv.request.target_cloud);
}
else {
pcl::toROSMsg(*all_cloud_,
icp_srv.request.target_cloud);
}
pcl::toROSMsg(*input_downsampled_cloud,
icp_srv.request.reference_cloud);
if (client.call(icp_srv)) {
Eigen::Affine3f transform;
tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
Eigen::Vector3f transform_pos(transform.translation());
float roll, pitch, yaw;
pcl::getEulerAngles(transform, roll, pitch, yaw);
JSK_NODELET_INFO("aligned parameter --");
JSK_NODELET_INFO(" - pos: [%f, %f, %f]",
transform_pos[0],
transform_pos[1],
transform_pos[2]);
JSK_NODELET_INFO(" - rot: [%f, %f, %f]", roll, pitch, yaw);
pcl::PointCloud<pcl::PointNormal>::Ptr
transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl::transformPointCloudWithNormals(*input_cloud,
*transformed_input_cloud,
transform);
}
else {
pcl::transformPointCloud(*input_cloud,
*transformed_input_cloud,
transform);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>);
*concatenated_cloud = *all_cloud_ + *transformed_input_cloud;
// update *all_cloud
applyDownsampling(concatenated_cloud, *all_cloud_);
// update localize_transform_
tf::Transform icp_transform;
tf::transformEigenToTF(transform, icp_transform);
{
boost::mutex::scoped_lock tf_lock(tf_mutex_);
localize_transform_ = localize_transform_ * icp_transform;
}
}
else {
JSK_NODELET_ERROR("Failed to call ~icp_align");
return;
}
}
localize_requested_ = false;
}
else {
JSK_NODELET_WARN("No tf transformation is available");
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
}
}
