obj_to_pointcloud.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014-2017, the neonavigation authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <sensor_msgs/PointCloud2.h>
00032 #include <pcl_ros/point_cloud.h>
00033 #include <pcl_ros/transforms.h>
00034 
00035 #include <pcl/point_types.h>
00036 #include <pcl/conversions.h>
00037 #include <pcl_conversions/pcl_conversions.h>
00038 #include <pcl/io/vtk_lib_io.h>
00039 #include <pcl/filters/voxel_grid.h>
00040 
00041 #include <cmath>
00042 #include <random>
00043 #include <string>
00044 #include <iostream>
00045 #include <sstream>
00046 #include <vector>
00047 
00048 #include <neonavigation_common/compatibility.h>
00049 
00050 pcl::PointXYZ operator-(const pcl::PointXYZ& a, const pcl::PointXYZ& b)
00051 {
00052   auto c = a;
00053   c.x -= b.x;
00054   c.y -= b.y;
00055   c.z -= b.z;
00056   return c;
00057 }
00058 pcl::PointXYZ operator+(const pcl::PointXYZ& a, const pcl::PointXYZ& b)
00059 {
00060   auto c = a;
00061   c.x += b.x;
00062   c.y += b.y;
00063   c.z += b.z;
00064   return c;
00065 }
00066 pcl::PointXYZ operator*(const pcl::PointXYZ& a, const float& b)
00067 {
00068   auto c = a;
00069   c.x *= b;
00070   c.y *= b;
00071   c.z *= b;
00072   return c;
00073 }
00074 
00075 std::vector<std::string> split(const std::string& input, char delimiter)
00076 {
00077   std::istringstream stream(input);
00078 
00079   std::string field;
00080   std::vector<std::string> result;
00081   while (std::getline(stream, field, delimiter))
00082   {
00083     result.push_back(field);
00084   }
00085   return result;
00086 }
00087 
00088 class ObjToPointcloudNode
00089 {
00090 public:
00091   ObjToPointcloudNode()
00092     : nh_()
00093     , pnh_("~")
00094     , engine_(seed_gen_())
00095   {
00096     neonavigation_common::compat::checkCompatMode();
00097     pub_cloud_ = neonavigation_common::compat::advertise<sensor_msgs::PointCloud2>(
00098         nh_, "mapcloud",
00099         pnh_, "cloud", 1, true);
00100 
00101     pnh_.param("frame_id", frame_id_, std::string("map"));
00102     pnh_.param("objs", file_, std::string(""));
00103     if (file_.compare("") == 0)
00104     {
00105       ROS_ERROR("OBJ file not specified");
00106       ros::shutdown();
00107       return;
00108     }
00109     pnh_.param("points_per_meter_sq", ppmsq_, 600.0);
00110     pnh_.param("downsample_grid", downsample_grid_, 0.05);
00111     pnh_.param("offset_x", offset_x_, 0.0);
00112     pnh_.param("offset_y", offset_y_, 0.0);
00113     pnh_.param("offset_z", offset_z_, 0.0);
00114     pnh_.param("scale", scale_, 1.0);
00115 
00116     auto pc = convertObj(split(file_, ','));
00117     pub_cloud_.publish(pc);
00118   }
00119 
00120 private:
00121   ros::NodeHandle nh_;
00122   ros::NodeHandle pnh_;
00123   ros::Publisher pub_cloud_;
00124 
00125   std::string file_;
00126   std::string frame_id_;
00127   double ppmsq_;
00128   double downsample_grid_;
00129   double offset_x_;
00130   double offset_y_;
00131   double offset_z_;
00132   double scale_;
00133 
00134   std::random_device seed_gen_;
00135   std::default_random_engine engine_;
00136 
00137   sensor_msgs::PointCloud2 convertObj(const std::vector<std::string>& files)
00138   {
00139     sensor_msgs::PointCloud2 pc_msg;
00140     pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh());
00141     pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>());
00142     pcl::PointCloud<pcl::PointXYZ>::Ptr pc_rs(new pcl::PointCloud<pcl::PointXYZ>());
00143 
00144     pcl::PointXYZ offset(static_cast<float>(offset_x_), static_cast<float>(offset_y_), static_cast<float>(offset_z_));
00145 
00146     pc_rs->points.reserve(500000);
00147     for (auto& file : files)
00148     {
00149       auto ext = file.substr(file.find_last_of(".") + 1);
00150 
00151       if (ext == "pcd")
00152       {
00153         if (pcl::io::loadPCDFile(file, *pc) == -1)
00154         {
00155           ROS_ERROR("Failed to load PCD file");
00156           ros::shutdown();
00157           return pc_msg;
00158         }
00159         for (auto& p : pc->points)
00160         {
00161           p.x *= scale_;
00162           p.y *= scale_;
00163           p.z *= scale_;
00164           p = p + offset;
00165           pc_rs->points.push_back(p);
00166         }
00167       }
00168       else if (ext == "obj")
00169       {
00170         if (pcl::io::loadPolygonFileOBJ(file, *mesh) == -1)
00171         {
00172           ROS_ERROR("Failed to load OBJ file");
00173           ros::shutdown();
00174           return pc_msg;
00175         }
00176 
00177         pcl::fromPCLPointCloud2(mesh->cloud, *pc);
00178         pc_rs->header = pc->header;
00179         for (auto& p : pc->points)
00180         {
00181           p.x *= scale_;
00182           p.y *= scale_;
00183           p.z *= scale_;
00184         }
00185 
00186         std::uniform_real_distribution<float> ud(0.0, 1.0);
00187         for (auto& poly : mesh->polygons)
00188         {
00189           if (poly.vertices.size() != 3)
00190           {
00191             ROS_ERROR("Input mesh mush be triangle");
00192             ros::shutdown();
00193             return pc_msg;
00194           }
00195           auto& p0 = pc->points[poly.vertices[0]];
00196           auto& p1 = pc->points[poly.vertices[1]];
00197           auto& p2 = pc->points[poly.vertices[2]];
00198 
00199           auto a = p1 - p0;
00200           auto b = p2 - p0;
00201 
00202           float s = 0.5 * sqrtf(
00203                               powf(a.y * b.z - a.z * b.y, 2.0) +
00204                               powf(a.z * b.x - a.x * b.z, 2.0) +
00205                               powf(a.x * b.y - a.y * b.x, 2.0));
00206           float numf = ppmsq_ * s;
00207           int num = numf;
00208           if (numf - num > ud(engine_))
00209             num++;
00210           for (int i = 0; i < num; i++)
00211           {
00212             float r0 = ud(engine_);
00213             float r1 = ud(engine_);
00214             if (r0 + r1 > 1.0)
00215             {
00216               r0 = 1.0 - r0;
00217               r1 = 1.0 - r1;
00218             }
00219             pcl::PointXYZ p = p0 + (a * r0 + b * r1) + offset;
00220             pc_rs->points.push_back(p);
00221           }
00222         }
00223       }
00224     }
00225     pc_rs->width = pc_rs->points.size();
00226     pc_rs->height = 1;
00227     pc_rs->is_dense = true;
00228 
00229     // Down-sample
00230     pcl::PointCloud<pcl::PointXYZ>::Ptr pc_ds(new pcl::PointCloud<pcl::PointXYZ>);
00231     pcl::VoxelGrid<pcl::PointXYZ> ds;
00232     ds.setInputCloud(pc_rs);
00233     ds.setLeafSize(downsample_grid_, downsample_grid_, downsample_grid_);
00234     ds.filter(*pc_ds);
00235 
00236     pcl::toROSMsg(*pc_ds, pc_msg);
00237     pc_msg.header.frame_id = frame_id_;
00238     pc_msg.header.stamp = ros::Time::now();
00239     ROS_INFO("pointcloud (%d points) has been generated from %d verticles",
00240              (int)pc_ds->size(),
00241              (int)pc->size());
00242     return pc_msg;
00243   }
00244 };
00245 
00246 int main(int argc, char** argv)
00247 {
00248   ros::init(argc, argv, "obj_to_pointcloud");
00249 
00250   ObjToPointcloudNode m2p;
00251   ros::spin();
00252 
00253   return 0;
00254 }


obj_to_pointcloud
Author(s): Atsushi Watanabe
autogenerated on Sat Jun 22 2019 20:07:20