void ConnectionBasedNodelet::cameraConnectionBaseCallback()
{
if (verbose_connection_) {
JSK_NODELET_INFO("New image connection or disconnection is detected");
}
if (!always_subscribe_) {
boost::mutex::scoped_lock lock(connection_mutex_);
for (size_t i = 0; i < camera_publishers_.size(); i++) {
image_transport::CameraPublisher pub = camera_publishers_[i];
if (pub.getNumSubscribers() > 0) {
if (!ever_subscribed_) {
ever_subscribed_ = true;
}
if (connection_status_ != SUBSCRIBED) {
if (verbose_connection_) {
JSK_NODELET_INFO("Subscribe input topics");
}
subscribe();
connection_status_ = SUBSCRIBED;
}
return;
}
}
if (connection_status_ == SUBSCRIBED) {
if (verbose_connection_) {
JSK_NODELET_INFO("Unsubscribe input topics");
}
unsubscribe();
connection_status_ = NOT_SUBSCRIBED;
}
}
}
void ConnectionBasedNodelet::connectionCallback(const ros::SingleSubscriberPublisher& pub)
{
if (verbose_connection_) {
JSK_NODELET_INFO("New connection or disconnection is detected");
}
if (!always_subscribe_) {
boost::mutex::scoped_lock lock(connection_mutex_);
for (size_t i = 0; i < publishers_.size(); i++) {
ros::Publisher pub = publishers_[i];
if (pub.getNumSubscribers() > 0) {
if (!ever_subscribed_) {
ever_subscribed_ = true;
}
if (connection_status_ != SUBSCRIBED) {
if (verbose_connection_) {
JSK_NODELET_INFO("Subscribe input topics");
}
subscribe();
connection_status_ = SUBSCRIBED;
}
return;
}
}
if (connection_status_ == SUBSCRIBED) {
if (verbose_connection_) {
JSK_NODELET_INFO("Unsubscribe input topics");
}
unsubscribe();
connection_status_ = NOT_SUBSCRIBED;
}
}
}
void DepthCalibration::printModel()
{
JSK_NODELET_INFO("C2(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients2_[0], coefficients2_[1], coefficients2_[2], coefficients2_[3], coefficients2_[4]);
JSK_NODELET_INFO("C1(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients1_[0], coefficients1_[1], coefficients1_[2], coefficients1_[3], coefficients1_[4]);
JSK_NODELET_INFO("C0(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients0_[0], coefficients0_[1], coefficients0_[2], coefficients0_[3], coefficients0_[4]);
if (use_abs_) {
JSK_NODELET_INFO("use_abs: True");
}
else {
JSK_NODELET_INFO("use_abs: False");
}
}
void FindObjectOnPlane::generateStartPoints(
const cv::Point2f& point_2d,
const image_geometry::PinholeCameraModel& model,
const pcl::ModelCoefficients::Ptr& coefficients,
std::vector<cv::Point3f>& search_points_3d,
std::vector<cv::Point2f>& search_points_2d)
{
JSK_NODELET_INFO("generateStartPoints");
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(coefficients->values));
cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
const double resolution_step = 0.01;
const int resolution = 5;
search_points_3d.clear();
search_points_2d.clear();
for (int i = - resolution; i < resolution; ++i) {
for (int j = - resolution; j < resolution; ++j) {
double x_diff = resolution_step * i;
double y_diff = resolution_step * j;
Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
Eigen::Vector3f projected_moved_point;
plane->project(moved_point, projected_moved_point);
cv::Point3f projected_moved_point_cv(projected_moved_point[0],
projected_moved_point[1],
projected_moved_point[2]);
search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
search_points_2d.push_back(p2d);
}
}
}
bool PointCloudLocalization::localizationRequest(
std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res)
{
JSK_NODELET_INFO("localize!");
boost::mutex::scoped_lock lock(mutex_);
localize_requested_ = true;
return true;
}
bool CollisionDetector::checkCollision(const sensor_msgs::JointState& joint,
const geometry_msgs::PoseStamped& pose)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_NODELET_DEBUG("checkCollision is called.");
// calculate the sensor transformation
tf::StampedTransform sensor_to_world_tf;
try {
tf_listener_.lookupTransform(world_frame_id_, cloud_frame_id_, cloud_stamp_, sensor_to_world_tf);
} catch (tf::TransformException& ex) {
JSK_NODELET_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
return false;
}
// transform point cloud
Eigen::Matrix4f sensor_to_world;
pcl_ros::transformAsMatrix(sensor_to_world_tf, sensor_to_world);
pcl::transformPointCloud(cloud_, cloud_, sensor_to_world);
self_mask_->assumeFrameFromJointAngle(joint, pose);
// check containment for all point cloud
bool contain_flag = false;
pcl::PointXYZ p;
for (size_t i = 0; i < cloud_.size(); i++) {
p = cloud_.at(i);
if (finite(p.x) && finite(p.y) && finite(p.z) &&
self_mask_->getMaskContainment(p.x, p.y, p.z) == robot_self_filter::INSIDE) {
contain_flag = true;
break;
}
}
if (contain_flag) {
JSK_NODELET_INFO("collision!");
} else {
JSK_NODELET_INFO("no collision!");
}
return contain_flag;
}
void DepthImageError::calcError(const sensor_msgs::Image::ConstPtr& depth_image,
const geometry_msgs::PointStamped::ConstPtr& uv_point,
const sensor_msgs::CameraInfo::ConstPtr& camera_info)
{
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_32FC1);
cv::Mat cv_depth_image = cv_ptr->image;
double depth_from_depth_sensor = cv_depth_image.at<float>((int)uv_point->point.y, (int)uv_point->point.x);
JSK_NODELET_INFO("timestamp diff is %f", (depth_image->header.stamp - uv_point->header.stamp).toSec());
JSK_NODELET_INFO("(u, v) = (%d, %d)", (int)uv_point->point.x, (int)uv_point->point.y);
JSK_NODELET_INFO("(z, d) = (%f, %f)", uv_point->point.z, depth_from_depth_sensor);
if (! isnan(depth_from_depth_sensor)) {
jsk_recognition_msgs::DepthErrorResult result;
result.header.frame_id = depth_image->header.frame_id;
result.header.stamp = depth_image->header.stamp;
result.u = (int)uv_point->point.x;
result.v = (int)uv_point->point.y;
result.center_u = camera_info->P[2];
result.center_v = camera_info->P[6];
result.true_depth = uv_point->point.z;
result.observed_depth = depth_from_depth_sensor;
depth_error_publisher_.publish(result);
}
}
void jsk_pcl_ros::DepthImageCreator::onInit () {
JSK_NODELET_INFO("[%s::onInit]", getName().c_str());
ConnectionBasedNodelet::onInit();
tf_listener_ = TfListenerSingleton::getInstance();
// scale_depth
pnh_->param("scale_depth", scale_depth, 1.0);
JSK_ROS_INFO("scale_depth : %f", scale_depth);
// use fixed transform
pnh_->param("use_fixed_transform", use_fixed_transform, false);
JSK_ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
pnh_->param("use_service", use_service, false);
JSK_ROS_INFO("use_service : %d", use_service);
pnh_->param("use_asynchronous", use_asynchronous, false);
JSK_ROS_INFO("use_asynchronous : %d", use_asynchronous);
pnh_->param("use_approximate", use_approximate, false);
JSK_ROS_INFO("use_approximate : %d", use_approximate);
pnh_->param("info_throttle", info_throttle_, 0);
info_counter_ = 0;
pnh_->param("max_queue_size", max_queue_size_, 3);
// set transformation
std::vector<double> trans_pos(3, 0);
std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
if (pnh_->hasParam("translation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
}
if (pnh_->hasParam("rotation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
}
tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
fixed_transform.setOrigin(btp);
fixed_transform.setRotation(btq);
pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_queue_size_);
pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_queue_size_);
pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_queue_size_);
if (use_service) {
service_ = pnh_->advertiseService("set_point_cloud",
&DepthImageCreator::service_cb, this);
}
onInitPostProcess();
}
void GeometricConsistencyGrouping::recognize(
const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
return;
}
pcl::PointCloud<pcl::SHOT352>::Ptr
scene_feature (new pcl::PointCloud<pcl::SHOT352>);
pcl::PointCloud<pcl::PointNormal>::Ptr
scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud (reference_feature_);
for (size_t i = 0; i < scene_feature->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
continue;
}
int found_neighs
= match_search.nearestKSearch(scene_feature->at(i), 1,
neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// (SHOT descriptor distances are between 0 and 1 by design)
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
gc_clusterer.setGCSize(gc_size_);
gc_clusterer.setGCThreshold(gc_thresh_);
gc_clusterer.setInputCloud(reference_cloud_);
gc_clusterer.setSceneCloud(scene_cloud);
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
std::vector<pcl::Correspondences> clustered_corrs;
std::vector<Eigen::Matrix4f,
Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
gc_clusterer.recognize(rototranslations, clustered_corrs);
if (rototranslations.size() > 0) {
JSK_NODELET_INFO("detected %lu objects", rototranslations.size());
Eigen::Matrix4f result_mat = rototranslations[0];
Eigen::Affine3f affine(result_mat);
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(affine, ros_pose.pose);
ros_pose.header = scene_cloud_msg->header;
pub_output_.publish(ros_pose);
}
else {
JSK_NODELET_WARN("Failed to find object");
}
}
void ColorHistogramLabelMatch::match(
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::Image::ConstPtr& label_msg,
const sensor_msgs::Image::ConstPtr& mask_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (histogram_.empty()) {
JSK_NODELET_DEBUG("no reference histogram is available");
return;
}
cv::Mat image
= cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
cv::Mat label
= cv_bridge::toCvShare(label_msg, label_msg->encoding)->image;
cv::Mat whole_mask
= cv_bridge::toCvShare(mask_msg, mask_msg->encoding)->image;
cv::Mat coefficient_image = cv::Mat::zeros(
image_msg->height, image_msg->width, CV_32FC1);
std::vector<int> labels;
getLabels(label, labels);
cv::Mat coefficients_heat_image = cv::Mat::zeros(
image_msg->height, image_msg->width, CV_8UC3); // BGR8
int hist_size = histogram_.cols;
float range[] = { min_value_, max_value_ };
const float* hist_range = { range };
double min_coef = DBL_MAX;
double max_coef = - DBL_MAX;
for (size_t i = 0; i < labels.size(); i++) {
int label_index = labels[i];
cv::Mat label_mask = cv::Mat::zeros(label.rows, label.cols, CV_8UC1);
getMaskImage(label, label_index, label_mask);
double coef = 0.0;
// get label_mask & whole_mask and check is it all black or not
cv::Mat masked_label;
label_mask.copyTo(masked_label, whole_mask);
if (isMasked(label_mask, masked_label)) {
coef = masked_coefficient_;
}
else {
cv::MatND hist;
bool uniform = true; bool accumulate = false;
cv::calcHist(&image, 1, 0, label_mask, hist, 1,
&hist_size, &hist_range, uniform, accumulate);
cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2, -1,
cv::Mat());
cv::Mat hist_mat = cv::Mat::zeros(1, hist_size, CV_32FC1);
for (size_t j = 0; j < hist_size; j++) {
hist_mat.at<float>(0, j) = hist.at<float>(0, j);
}
//cv::Mat hist_mat = hist;
coef = coefficients(hist_mat, histogram_);
if (min_coef > coef) {
min_coef = coef;
}
if (max_coef < coef) {
max_coef = coef;
}
}
std_msgs::ColorRGBA coef_color = jsk_topic_tools::heatColor(coef);
for (size_t j = 0; j < coefficients_heat_image.rows; j++) {
for (size_t i = 0; i < coefficients_heat_image.cols; i++) {
if (label_mask.at<uchar>(j, i) == 255) {
coefficients_heat_image.at<cv::Vec3b>(j, i)
= cv::Vec3b(int(coef_color.b * 255),
int(coef_color.g * 255),
int(coef_color.r * 255));
coefficient_image.at<float>(j, i) = coef;
}
}
}
}
JSK_NODELET_INFO("coef: %f - %f", min_coef, max_coef);
pub_debug_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::BGR8,
coefficients_heat_image).toImageMsg());
pub_coefficient_image_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::TYPE_32FC1,
coefficient_image).toImageMsg());
// apply threshold operation
cv::Mat threshold_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_32FC1);
if (threshold_method_ == 0) { // smaller than
cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
cv::THRESH_BINARY_INV);
}
else if (threshold_method_ == 1) { // greater than
cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
cv::THRESH_BINARY);
}
else if (threshold_method_ == 2 || threshold_method_ == 3) {
// convert image into 8UC to apply otsu' method
cv::Mat otsu_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_8UC1);
cv::Mat otsu_result_image = cv::Mat::zeros(
coefficient_image.rows, coefficient_image.cols, CV_8UC1);
coefficient_image.convertTo(otsu_image, 8, 255.0);
cv::threshold(otsu_image, otsu_result_image, coef_threshold_, 255,
cv::THRESH_OTSU);
// convert it into float image again
if (threshold_method_ == 2) {
otsu_result_image.convertTo(threshold_image, 32, 1 / 255.0);
}
else if (threshold_method_ == 3) {
otsu_result_image.convertTo(threshold_image, 32, - 1 / 255.0, 1.0);
}
}
cv::Mat threshold_uchar_image = cv::Mat(threshold_image.rows,
threshold_image.cols,
CV_8UC1);
threshold_image.convertTo(threshold_uchar_image, 8, 255.0);
// convert image from float to uchar
pub_result_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::MONO8,
threshold_uchar_image).toImageMsg());
}
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;
}
}
}
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
JSK_NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_.publish(torus_array_msg);
// publish pose stamped
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = torus_msg.header;
pose_stamped.pose = torus_msg.pose;
pub_pose_stamped_.publish(pose_stamped);
pub_torus_array_with_failure_.publish(torus_array_msg);
pub_torus_with_failure_.publish(torus_msg);
}
else {
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
torus_msg.failure = true;
pub_torus_with_failure_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_with_failure_.publish(torus_array_msg);
JSK_NODELET_INFO("failed to find torus");
}
}
template<class T> void filter (const sensor_msgs::PointCloud2::ConstPtr &input,
const PCLIndicesMsg::ConstPtr &indices) {
pcl::PointCloud<T> pcl_input_cloud, output;
fromROSMsg(*input, pcl_input_cloud);
boost::mutex::scoped_lock lock (mutex_);
std::vector<int> ex_indices;
ex_indices.resize(0);
int width = input->width;
int height = input->height;
int ox, oy, sx, sy;
sx = step_x_;
ox = sx/2;
if(height == 1) {
sy = 1;
oy = 0;
} else {
sy = step_y_;
oy = sy/2;
}
if (indices) {
std::vector<int> flags;
flags.resize(width*height);
//std::vector<int>::iterator it;
//for(it = indices->begin(); it != indices->end(); it++)
//flags[*it] = 1;
for(unsigned int i = 0; i < indices->indices.size(); i++) {
flags[indices->indices.at(i)] = 1;
}
for(int y = oy; y < height; y += sy) {
for(int x = ox; x < width; x += sx) {
if (flags[y*width + x] == 1) {
ex_indices.push_back(y*width + x); // just use points in indices
}
}
}
} else {
for(int y = oy; y < height; y += sy) {
for(int x = ox; x < width; x += sx) {
ex_indices.push_back(y*width + x);
}
}
}
pcl::ExtractIndices<T> extract;
extract.setInputCloud (pcl_input_cloud.makeShared());
extract.setIndices (boost::make_shared <std::vector<int> > (ex_indices));
extract.setNegative (false);
extract.filter (output);
if (output.points.size() > 0) {
sensor_msgs::PointCloud2 ros_out;
toROSMsg(output, ros_out);
ros_out.header = input->header;
ros_out.width = (width - ox)/sx;
if((width - ox)%sx) ros_out.width += 1;
ros_out.height = (height - oy)/sy;
if((height - oy)%sy) ros_out.height += 1;
ros_out.row_step = ros_out.point_step * ros_out.width;
ros_out.is_dense = input->is_dense;
#if DEBUG
JSK_NODELET_INFO("%dx%d (%d %d)(%d %d) -> %dx%d %d", width,height, ox, oy, sx, sy,
ros_out.width, ros_out.height, ex_indices.size());
#endif
pub_.publish(ros_out);
JSK_NODELET_INFO("%s:: input header stamp is [%f]", getName().c_str(),
input->header.stamp.toSec());
JSK_NODELET_INFO("%s:: output header stamp is [%f]", getName().c_str(),
ros_out.header.stamp.toSec());
}
}
void FindObjectOnPlane::find(
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::CameraInfo::ConstPtr& info_msg,
const pcl_msgs::ModelCoefficients::ConstPtr& polygon_3d_coefficient_msg)
{
cv::Mat object_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
image_geometry::PinholeCameraModel model;
pcl::ModelCoefficients::Ptr polygon_coefficients
(new pcl::ModelCoefficients);
pcl_conversions::toPCL(*polygon_3d_coefficient_msg, *polygon_coefficients);
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(polygon_coefficients->values));
model.fromCameraInfo(info_msg);
std::vector<cv::Point> all_points;
for (int j = 0; j < object_image.rows; j++) {
for (int i = 0; i < object_image.cols; i++) {
if (object_image.at<uchar>(j, i) == 255) {
all_points.push_back(cv::Point(i, j));
}
}
}
cv::RotatedRect obb = cv::minAreaRect(all_points);
cv::Mat min_area_rect_image;
cv::cvtColor(object_image, min_area_rect_image, CV_GRAY2BGR);
cv::Point2f vertices2f[4];
obb.points(vertices2f);
cv::line(min_area_rect_image, vertices2f[0], vertices2f[1],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[1], vertices2f[2],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[2], vertices2f[3],
cv::Scalar(0, 0, 255), 4);
cv::line(min_area_rect_image, vertices2f[3], vertices2f[0],
cv::Scalar(0, 0, 255), 4);
cv::Rect bb = obb.boundingRect();
std::vector<cv::Point3f> search_points_3d;
std::vector<cv::Point2f> search_points_2d;
generateStartPoints(cv::Point2f(bb.x, bb.y),
model, polygon_coefficients,
search_points_3d, search_points_2d);
for (size_t i = 0; i < search_points_2d.size(); i++) {
cv::circle(min_area_rect_image, search_points_2d[i], 5, cv::Scalar(0, 255, 0), 1);
}
//for (size_t i = 0; i < search_points_3d.size(); i++) {
double min_area = DBL_MAX;
double min_angle;
cv::Point2f min_search_point;
double min_x;
double min_y;
for (size_t i = 0; i < search_points_2d.size(); i++) {
std::vector<double> angles;
std::vector<double> max_x;
std::vector<double> max_y;
generateAngles(object_image, search_points_2d[i], angles,
max_x, max_y,
model, plane);
// draw angles
for (size_t j = 0; j < angles.size(); j++) {
double area = drawAngle(min_area_rect_image, search_points_2d[i], angles[j],
max_x[j], max_y[j], model, plane, cv::Scalar(0, 255, 0));
if (area < min_area) {
min_area = area;
min_x = max_x[j];
min_y = max_y[j];
min_angle = angles[j];
min_search_point = search_points_2d[i];
}
}
}
drawAngle(min_area_rect_image, min_search_point, min_angle,
min_x, min_y, model, plane, cv::Scalar(0, 255, 255));
// convert the points into 3-D
pub_min_area_rect_image_.publish(
cv_bridge::CvImage(image_msg->header,
sensor_msgs::image_encodings::BGR8,
min_area_rect_image).toImageMsg());
JSK_NODELET_INFO("published");
}
