double inter_cone(t_mesh *mesh, t_ray *ray, t_ray *m_ray)
{
t_cone *con;
t_vector pos;
t_vector dir;
double abc[3];
double d;
pos = transform_pos(mesh->result, &ray->pos);
dir = transform_dir(mesh->result, &ray->dir);
con = &mesh->prim.cone;
abc[0] = pow(dir.x, 2) + pow(dir.y, 2)
- pow(dir.z, 2) * pow(tan(con->coeff), 2);
abc[1] = (2 * (dir.x * pos.x + dir.y * pos.y))
- (2 * dir.z * pos.z * pow(tan(con->coeff), 2));
abc[2] = pow(pos.x, 2) + pow(pos.y, 2)
- (pow(pos.z, 2) * pow(tan(con->coeff), 2));
d = determinant(abc[0], abc[1], abc[2]);
if (d >= 0.0001)
{
m_ray->pos = pos;
m_ray->dir = dir;
return (d);
}
return (-1);
}
void transform_pos(treebank_type& treebank, Iterator& first, Iterator last)
{
if (treebank.antecedents_.empty()) return;
// preterminal!
if (treebank.antecedents_.size() == 1 && treebank.antecedents_.front().antecedents_.empty()) {
if (treebank.cat_ != "-NONE-") {
if (first == last)
throw std::runtime_error("short POS sequence??");
treebank.cat_ = *first;
++ first;
}
} else
for (treebank_type::antecedents_type::iterator aiter = treebank.antecedents_.begin(); aiter != treebank.antecedents_.end(); ++ aiter)
transform_pos(*aiter, first, last);
}
double inter_plan(t_mesh *mesh, t_ray *ray, t_ray *m_ray)
{
t_plan *plan;
t_vector pos;
t_vector dir;
double d;
pos = transform_pos(mesh->result, &ray->pos);
dir = transform_dir(mesh->result, &ray->dir);
plan = &mesh->prim.plan;
d = -(dot(plan->normal, pos))
/ dot(plan->normal, dir);
if (d >= 0.0001)
{
m_ray->pos = pos;
m_ray->dir = dir;
return (d);
}
return (-1);
}
double inter_cylinder(t_mesh *mesh, t_ray *ray, t_ray *m_ray)
{
t_cylinder *cyl;
t_vector pos;
t_vector dir;
double abc[3];
double d;
pos = transform_pos(mesh->result, &ray->pos);
dir = transform_dir(mesh->result, &ray->dir);
cyl = &mesh->prim.cylinder;
abc[0] = pow(dir.x, 2) + pow(dir.y, 2);
abc[1] = 2 * (dir.x * pos.x + dir.y * pos.y);
abc[2] = pow(pos.x, 2) + pow(pos.y, 2) - pow(cyl->radius, 2);
d = determinant(abc[0], abc[1], abc[2]);
if (d >= 0.0001)
{
m_ray->pos = pos;
m_ray->dir = dir;
return (d);
}
return (-1);
}
double inter_sphere(t_mesh *mesh, t_ray *ray, t_ray *m_ray)
{
t_sphere *sphere;
t_vector pos;
t_vector dir;
double abc[3];
double d;
pos = transform_pos(mesh->result, &ray->pos);
dir = transform_dir(mesh->result, &ray->dir);
sphere = &mesh->prim.sphere;
abc[0] = square_length(dir);
abc[1] = 2 * dot(dir, pos);
abc[2] = square_length(pos) - pow(sphere->radius, 2);
d = determinant(abc[0], abc[1], abc[2]);
if (d >= 0.0001)
{
m_ray->pos = pos;
m_ray->dir = dir;
return (d);
}
return (-1);
}
int main(int argc, char** argv)
{
try {
options(argc, argv);
if (int(leaf_mode) + treebank_mode > 1)
throw std::runtime_error("multiple output options specified: leaf/treebank(default: treebank)");
if (int(leaf_mode) + treebank_mode == 0)
treebank_mode = true;
typedef boost::spirit::istream_iterator iter_type;
const bool flush_output = (output_file == "-"
|| (boost::filesystem::exists(output_file)
&& ! boost::filesystem::is_regular_file(output_file)));
penntreebank_grammar<iter_type> grammar;
treebank_type parsed;
sentence_type sent;
pos_set_type pos;
std::string line;
utils::compress_istream is(input_file, 1024 * 1024);
utils::compress_ostream os(output_file, 1024 * 1024);
std::auto_ptr<utils::compress_istream> ms;
if (! pos_file.empty()) {
if (! boost::filesystem::exists(pos_file))
throw std::runtime_error("no map file: " + pos_file.string());
ms.reset(new utils::compress_istream(pos_file, 1024 * 1024));
}
is.unsetf(std::ios::skipws);
iter_type iter(is);
iter_type iter_end;
while (iter != iter_end) {
parsed.clear();
if (! boost::spirit::qi::phrase_parse(iter, iter_end, grammar, boost::spirit::standard::space, parsed)) {
std::string buffer;
for (int i = 0; i != 64 && iter != iter_end; ++ i, ++iter)
buffer += *iter;
throw std::runtime_error("parsing failed: " + buffer);
}
if (! root_symbol.empty())
parsed.cat_ = root_symbol;
else if (parsed.cat_.empty())
parsed.cat_ = "ROOT";
if (validate)
if (! treebank_validate(parsed))
throw std::runtime_error("invalid tree");
if (ms.get()) {
typedef boost::tokenizer<utils::space_separator, utils::piece::const_iterator, utils::piece> tokenizer_type;
if (! utils::getline(*ms, line))
throw std::runtime_error("# of lines do not match with POS");
utils::piece line_piece(line);
tokenizer_type tokenizer(line_piece);
pos.clear();
pos.insert(pos.end(), tokenizer.begin(), tokenizer.end());
pos_set_type::iterator iter = pos.begin();
transform_pos(parsed, iter, pos.end());
if (iter != pos.end())
throw std::runtime_error("too long POS sequence?");
}
if (remove_none)
transform_remove_none(parsed);
if (normalize)
transform_normalize(parsed);
if (remove_cycle)
transform_cycle(parsed);
if (unescape_terminal)
transform_unescape(parsed);
if (leaf_mode) {
sent.clear();
transform_leaf(parsed, sent);
if (! sent.empty()) {
std::copy(sent.begin(), sent.end() - 1, std::ostream_iterator<std::string>(os, " "));
os << sent.back();
}
} else if (treebank_mode) {
if (parsed.antecedents_.empty())
os << "(())";
else
treebank_output(parsed, os);
}
os << '\n';
if (flush_output)
os << std::flush;
}
}
catch (const std::exception& err) {
std::cerr << "error: " << err.what() << std::endl;
return 1;
}
return 0;
}
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;
}
}
}
