semantic_world.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #include <ros/ros.h>
00038 #include <visualization_msgs/MarkerArray.h>
00039 #include <geometry_msgs/Quaternion.h>
00040 
00041 // MoveIt!
00042 #include <moveit/semantic_world/semantic_world.h>
00043 #include <geometric_shapes/shape_operations.h>
00044 #include <moveit_msgs/PlanningScene.h>
00045 
00046 // OpenCV
00047 #include <opencv2/imgproc/imgproc.hpp>
00048 
00049 // Eigen
00050 #include <eigen_conversions/eigen_msg.h>
00051 #include <Eigen/Geometry>
00052 
00053 namespace moveit
00054 {
00055 namespace semantic_world
00056 {
00057 SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene)
00058   : planning_scene_(planning_scene)
00059 {
00060   table_subscriber_ = node_handle_.subscribe("table_array", 1, &SemanticWorld::tableCallback, this);
00061   visualization_publisher_ = node_handle_.advertise<visualization_msgs::MarkerArray>("visualize_place", 20, true);
00062   collision_object_publisher_ = node_handle_.advertise<moveit_msgs::CollisionObject>("/collision_object", 20);
00063   planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
00064 }
00065 
00066 visualization_msgs::MarkerArray
00067 SemanticWorld::getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped>& poses) const
00068 {
00069   ROS_DEBUG("Visualizing: %d place poses", (int)poses.size());
00070   visualization_msgs::MarkerArray marker;
00071   for (std::size_t i = 0; i < poses.size(); ++i)
00072   {
00073     visualization_msgs::Marker m;
00074     m.action = m.ADD;
00075     m.type = m.SPHERE;
00076     m.ns = "place_locations";
00077     m.id = i;
00078     m.pose = poses[i].pose;
00079     m.header = poses[i].header;
00080 
00081     m.scale.x = 0.02;
00082     m.scale.y = 0.02;
00083     m.scale.z = 0.02;
00084 
00085     m.color.r = 1.0;
00086     m.color.g = 0.0;
00087     m.color.b = 0.0;
00088     m.color.a = 1.0;
00089     marker.markers.push_back(m);
00090   }
00091   return marker;
00092 }
00093 
00094 bool SemanticWorld::addTablesToCollisionWorld()
00095 {
00096   moveit_msgs::PlanningScene planning_scene;
00097   planning_scene.is_diff = true;
00098 
00099   // Remove the existing tables
00100   std::map<std::string, object_recognition_msgs::Table>::iterator it;
00101   for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00102   {
00103     moveit_msgs::CollisionObject co;
00104     co.id = it->first;
00105     co.operation = moveit_msgs::CollisionObject::REMOVE;
00106     planning_scene.world.collision_objects.push_back(co);
00107     //    collision_object_publisher_.publish(co);
00108   }
00109 
00110   planning_scene_diff_publisher_.publish(planning_scene);
00111   planning_scene.world.collision_objects.clear();
00112   current_tables_in_collision_world_.clear();
00113   // Add the new tables
00114   for (std::size_t i = 0; i < table_array_.tables.size(); ++i)
00115   {
00116     moveit_msgs::CollisionObject co;
00117     std::stringstream ss;
00118     ss << "table_" << i;
00119     co.id = ss.str();
00120     current_tables_in_collision_world_[co.id] = table_array_.tables[i];
00121     co.operation = moveit_msgs::CollisionObject::ADD;
00122 
00123     const std::vector<geometry_msgs::Point>& convex_hull = table_array_.tables[i].convex_hull;
00124 
00125     EigenSTL::vector_Vector3d vertices(convex_hull.size());
00126     std::vector<unsigned int> triangles((vertices.size() - 2) * 3);
00127     for (unsigned int j = 0; j < convex_hull.size(); ++j)
00128       vertices[j] = Eigen::Vector3d(convex_hull[j].x, convex_hull[j].y, convex_hull[j].z);
00129     for (unsigned int j = 1; j < triangles.size() - 1; ++j)
00130     {
00131       unsigned int i3 = j * 3;
00132       triangles[i3++] = 0;
00133       triangles[i3++] = j;
00134       triangles[i3] = j + 1;
00135     }
00136 
00137     shapes::Shape* table_shape = shapes::createMeshFromVertices(vertices, triangles);
00138     if (!table_shape)
00139       continue;
00140 
00141     shapes::Mesh* table_mesh = static_cast<shapes::Mesh*>(table_shape);
00142     shapes::Mesh* table_mesh_solid = orientPlanarPolygon(*table_mesh);
00143     if (!table_mesh_solid)
00144     {
00145       delete table_shape;
00146       continue;
00147     }
00148 
00149     shapes::ShapeMsg table_shape_msg;
00150     if (!shapes::constructMsgFromShape(table_mesh_solid, table_shape_msg))
00151     {
00152       delete table_shape;
00153       delete table_mesh_solid;
00154       continue;
00155     }
00156 
00157     const shape_msgs::Mesh& table_shape_msg_mesh = boost::get<shape_msgs::Mesh>(table_shape_msg);
00158 
00159     co.meshes.push_back(table_shape_msg_mesh);
00160     co.mesh_poses.push_back(table_array_.tables[i].pose);
00161     co.header = table_array_.tables[i].header;
00162     planning_scene.world.collision_objects.push_back(co);
00163     //    collision_object_publisher_.publish(co);
00164     delete table_shape;
00165     delete table_mesh_solid;
00166   }
00167   planning_scene_diff_publisher_.publish(planning_scene);
00168   return true;
00169 }
00170 
00171 object_recognition_msgs::TableArray SemanticWorld::getTablesInROI(double minx, double miny, double minz, double maxx,
00172                                                                   double maxy, double maxz) const
00173 {
00174   object_recognition_msgs::TableArray tables_in_roi;
00175   std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00176   for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00177   {
00178     if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
00179         it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
00180         it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
00181     {
00182       tables_in_roi.tables.push_back(it->second);
00183     }
00184   }
00185   return tables_in_roi;
00186 }
00187 
00188 std::vector<std::string> SemanticWorld::getTableNamesInROI(double minx, double miny, double minz, double maxx,
00189                                                            double maxy, double maxz) const
00190 {
00191   std::vector<std::string> result;
00192   std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00193   for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00194   {
00195     if (it->second.pose.position.x >= minx && it->second.pose.position.x <= maxx &&
00196         it->second.pose.position.y >= miny && it->second.pose.position.y <= maxy &&
00197         it->second.pose.position.z >= minz && it->second.pose.position.z <= maxz)
00198     {
00199       result.push_back(it->first);
00200     }
00201   }
00202   return result;
00203 }
00204 
00205 void SemanticWorld::clear()
00206 {
00207   table_array_.tables.clear();
00208   current_tables_in_collision_world_.clear();
00209 }
00210 
00211 std::vector<geometry_msgs::PoseStamped>
00212 SemanticWorld::generatePlacePoses(const std::string& table_name, const shapes::ShapeConstPtr& object_shape,
00213                                   const geometry_msgs::Quaternion& object_orientation, double resolution,
00214                                   double delta_height, unsigned int num_heights) const
00215 {
00216   object_recognition_msgs::Table chosen_table;
00217   std::map<std::string, object_recognition_msgs::Table>::const_iterator it =
00218       current_tables_in_collision_world_.find(table_name);
00219 
00220   if (it != current_tables_in_collision_world_.end())
00221   {
00222     chosen_table = it->second;
00223     return generatePlacePoses(chosen_table, object_shape, object_orientation, resolution, delta_height, num_heights);
00224   }
00225 
00226   std::vector<geometry_msgs::PoseStamped> place_poses;
00227   ROS_ERROR("Did not find table %s to place on", table_name.c_str());
00228   return place_poses;
00229 }
00230 
00231 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(
00232     const object_recognition_msgs::Table& chosen_table, const shapes::ShapeConstPtr& object_shape,
00233     const geometry_msgs::Quaternion& object_orientation, double resolution, double delta_height,
00234     unsigned int num_heights) const
00235 {
00236   std::vector<geometry_msgs::PoseStamped> place_poses;
00237   if (object_shape->type != shapes::MESH && object_shape->type != shapes::SPHERE && object_shape->type != shapes::BOX &&
00238       object_shape->type != shapes::CONE)
00239   {
00240     return place_poses;
00241   }
00242 
00243   double x_min(std::numeric_limits<double>::max()), x_max(-std::numeric_limits<double>::max());
00244   double y_min(std::numeric_limits<double>::max()), y_max(-std::numeric_limits<double>::max());
00245   double z_min(std::numeric_limits<double>::max()), z_max(-std::numeric_limits<double>::max());
00246 
00247   Eigen::Quaterniond rotation(object_orientation.x, object_orientation.y, object_orientation.z, object_orientation.w);
00248   Eigen::Affine3d object_pose(rotation);
00249   double min_distance_from_edge;
00250   double height_above_table;
00251 
00252   if (object_shape->type == shapes::MESH)
00253   {
00254     const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(object_shape.get());
00255 
00256     for (std::size_t i = 0; i < mesh->vertex_count; ++i)
00257     {
00258       Eigen::Vector3d position(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
00259       position = object_pose * position;
00260 
00261       if (x_min > position.x())
00262         x_min = position.x();
00263       if (x_max < position.x())
00264         x_max = position.x();
00265       if (y_min > position.y())
00266         y_min = position.y();
00267       if (y_max < position.y())
00268         y_max = position.y();
00269       if (z_min > position.z())
00270         z_min = position.z();
00271       if (z_max < position.z())
00272         z_max = position.z();
00273     }
00274     min_distance_from_edge = 0.5 * std::max<double>(fabs(x_max - x_min), fabs(y_max - y_min));
00275     height_above_table = -z_min;
00276   }
00277   else if (object_shape->type == shapes::BOX)  // assuming box is being kept down upright
00278   {
00279     const shapes::Box* box = static_cast<const shapes::Box*>(object_shape.get());
00280     min_distance_from_edge = std::max<double>(fabs(box->size[0]), fabs(box->size[1])) / 2.0;
00281     height_above_table = fabs(box->size[2]) / 2.0;
00282   }
00283   else if (object_shape->type == shapes::SPHERE)
00284   {
00285     const shapes::Sphere* sphere = static_cast<const shapes::Sphere*>(object_shape.get());
00286     min_distance_from_edge = sphere->radius;
00287     height_above_table = -sphere->radius;
00288   }
00289   else if (object_shape->type == shapes::CYLINDER)  // assuming cylinder is being kept down upright
00290   {
00291     const shapes::Cylinder* cylinder = static_cast<const shapes::Cylinder*>(object_shape.get());
00292     min_distance_from_edge = cylinder->radius;
00293     height_above_table = cylinder->length / 2.0;
00294   }
00295   else if (object_shape->type == shapes::CONE)  // assuming cone is being kept down upright
00296   {
00297     const shapes::Cone* cone = static_cast<const shapes::Cone*>(object_shape.get());
00298     min_distance_from_edge = cone->radius;
00299     height_above_table = cone->length / 2.0;
00300   }
00301 
00302   return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights,
00303                             min_distance_from_edge);
00304 }
00305 
00306 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table& table,
00307                                                                           double resolution, double height_above_table,
00308                                                                           double delta_height, unsigned int num_heights,
00309                                                                           double min_distance_from_edge) const
00310 {
00311   std::vector<geometry_msgs::PoseStamped> place_poses;
00312   // Assumption that the table's normal is along the Z axis
00313   if (table.convex_hull.empty())
00314     return place_poses;
00315   const int scale_factor = 100;
00316   std::vector<cv::Point2f> table_contour;
00317   float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
00318   for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
00319   {
00320     if (table.convex_hull[j].x < x_min)
00321       x_min = table.convex_hull[j].x;
00322     else if (table.convex_hull[j].x > x_max)
00323       x_max = table.convex_hull[j].x;
00324     if (table.convex_hull[j].y < y_min)
00325       y_min = table.convex_hull[j].y;
00326     else if (table.convex_hull[j].y > y_max)
00327       y_max = table.convex_hull[j].y;
00328   }
00329   for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
00330     table_contour.push_back(
00331         cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
00332 
00333   double x_range = fabs(x_max - x_min);
00334   double y_range = fabs(y_max - y_min);
00335   int max_range = (int)x_range + 1;
00336   if (max_range < (int)y_range + 1)
00337     max_range = (int)y_range + 1;
00338 
00339   int image_scale = std::max<int>(max_range, 4);
00340   cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
00341 
00342   for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
00343   {
00344     cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
00345   }
00346 
00347   unsigned int num_x = fabs(x_max - x_min) / resolution + 1;
00348   unsigned int num_y = fabs(y_max - y_min) / resolution + 1;
00349 
00350   ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y);
00351 
00352   std::vector<std::vector<cv::Point> > contours;
00353   std::vector<cv::Vec4i> hierarchy;
00354   cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
00355 
00356   for (std::size_t j = 0; j < num_x; ++j)
00357   {
00358     int point_x = j * resolution * scale_factor;
00359     for (std::size_t k = 0; k < num_y; ++k)
00360     {
00361       for (std::size_t mm = 0; mm < num_heights; ++mm)
00362       {
00363         int point_y = k * resolution * scale_factor;
00364         cv::Point2f point2f(point_x, point_y);
00365         double result = cv::pointPolygonTest(contours[0], point2f, true);
00366         if ((int)result >= (int)(min_distance_from_edge * scale_factor))
00367         {
00368           Eigen::Vector3d point((double)(point_x) / scale_factor + x_min, (double)(point_y) / scale_factor + y_min,
00369                                 height_above_table + mm * delta_height);
00370           Eigen::Affine3d pose;
00371           tf::poseMsgToEigen(table.pose, pose);
00372           point = pose * point;
00373           geometry_msgs::PoseStamped place_pose;
00374           place_pose.pose.orientation.w = 1.0;
00375           place_pose.pose.position.x = point.x();
00376           place_pose.pose.position.y = point.y();
00377           place_pose.pose.position.z = point.z();
00378           place_pose.header = table.header;
00379           place_poses.push_back(place_pose);
00380         }
00381       }
00382     }
00383   }
00384   return place_poses;
00385 }
00386 
00387 bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose& pose, const object_recognition_msgs::Table& table,
00388                                          double min_distance_from_edge, double min_vertical_offset) const
00389 {
00390   // Assumption that the table's normal is along the Z axis
00391   if (table.convex_hull.empty())
00392     return false;
00393   float x_min = table.convex_hull[0].x, x_max = x_min, y_min = table.convex_hull[0].y, y_max = y_min;
00394   for (std::size_t j = 1; j < table.convex_hull.size(); ++j)
00395   {
00396     if (table.convex_hull[j].x < x_min)
00397       x_min = table.convex_hull[j].x;
00398     else if (table.convex_hull[j].x > x_max)
00399       x_max = table.convex_hull[j].x;
00400     if (table.convex_hull[j].y < y_min)
00401       y_min = table.convex_hull[j].y;
00402     else if (table.convex_hull[j].y > y_max)
00403       y_max = table.convex_hull[j].y;
00404   }
00405   const int scale_factor = 100;
00406   std::vector<cv::Point2f> table_contour;
00407   for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
00408     table_contour.push_back(
00409         cv::Point((table.convex_hull[j].x - x_min) * scale_factor, (table.convex_hull[j].y - y_min) * scale_factor));
00410 
00411   double x_range = fabs(x_max - x_min);
00412   double y_range = fabs(y_max - y_min);
00413   int max_range = (int)x_range + 1;
00414   if (max_range < (int)y_range + 1)
00415     max_range = (int)y_range + 1;
00416 
00417   int image_scale = std::max<int>(max_range, 4);
00418   cv::Mat src = cv::Mat::zeros(image_scale * scale_factor, image_scale * scale_factor, CV_8UC1);
00419 
00420   for (std::size_t j = 0; j < table.convex_hull.size(); ++j)
00421   {
00422     cv::line(src, table_contour[j], table_contour[(j + 1) % table.convex_hull.size()], cv::Scalar(255), 3, 8);
00423   }
00424 
00425   std::vector<std::vector<cv::Point> > contours;
00426   std::vector<cv::Vec4i> hierarchy;
00427   cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
00428 
00429   Eigen::Vector3d point(pose.position.x, pose.position.y, pose.position.z);
00430   Eigen::Affine3d pose_table;
00431   tf::poseMsgToEigen(table.pose, pose_table);
00432 
00433   // Point in table frame
00434   point = pose_table.inverse() * point;
00435   // Assuming Z axis points upwards for the table
00436   if (point.z() < -fabs(min_vertical_offset))
00437   {
00438     ROS_ERROR("Object is not above table");
00439     return false;
00440   }
00441 
00442   int point_x = (point.x() - x_min) * scale_factor;
00443   int point_y = (point.y() - y_min) * scale_factor;
00444   cv::Point2f point2f(point_x, point_y);
00445   double result = cv::pointPolygonTest(contours[0], point2f, true);
00446   ROS_DEBUG("table distance: %f", result);
00447 
00448   if ((int)result >= (int)(min_distance_from_edge * scale_factor))
00449     return true;
00450 
00451   return false;
00452 }
00453 
00454 std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose& pose, double min_distance_from_edge,
00455                                            double min_vertical_offset) const
00456 {
00457   std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00458   for (it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00459   {
00460     ROS_DEBUG("Testing table: %s", it->first.c_str());
00461     if (isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset))
00462       return it->first;
00463   }
00464   return std::string();
00465 }
00466 
00467 void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr& msg)
00468 {
00469   table_array_ = *msg;
00470   ROS_INFO("Table callback with %d tables", (int)table_array_.tables.size());
00471   transformTableArray(table_array_);
00472   // Callback on an update
00473   if (table_callback_)
00474   {
00475     ROS_INFO("Calling table callback");
00476     table_callback_();
00477   }
00478 }
00479 
00480 void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray& table_array) const
00481 {
00482   for (std::size_t i = 0; i < table_array.tables.size(); ++i)
00483   {
00484     std::string original_frame = table_array.tables[i].header.frame_id;
00485     if (table_array.tables[i].convex_hull.empty())
00486       continue;
00487     ROS_INFO_STREAM("Original pose: " << table_array.tables[i].pose.position.x << ","
00488                                       << table_array.tables[i].pose.position.y << ","
00489                                       << table_array.tables[i].pose.position.z);
00490     std::string error_text;
00491     const Eigen::Affine3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame);
00492     Eigen::Affine3d original_pose;
00493     tf::poseMsgToEigen(table_array.tables[i].pose, original_pose);
00494     original_pose = original_transform * original_pose;
00495     tf::poseEigenToMsg(original_pose, table_array.tables[i].pose);
00496     table_array.tables[i].header.frame_id = planning_scene_->getTransforms().getTargetFrame();
00497     ROS_INFO_STREAM("Successfully transformed table array from " << original_frame << "to "
00498                                                                  << table_array.tables[i].header.frame_id);
00499     ROS_INFO_STREAM("Transformed pose: " << table_array.tables[i].pose.position.x << ","
00500                                          << table_array.tables[i].pose.position.y << ","
00501                                          << table_array.tables[i].pose.position.z);
00502   }
00503 }
00504 
00505 shapes::Mesh* SemanticWorld::orientPlanarPolygon(const shapes::Mesh& polygon) const
00506 {
00507   if (polygon.vertex_count < 3 || polygon.triangle_count < 1)
00508     return 0;
00509   // first get the normal of the first triangle of the input polygon
00510   Eigen::Vector3d vec1, vec2, vec3, normal;
00511 
00512   int vIdx1 = polygon.triangles[0];
00513   int vIdx2 = polygon.triangles[1];
00514   int vIdx3 = polygon.triangles[2];
00515   vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
00516   vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
00517   vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
00518   vec2 -= vec1;
00519   vec3 -= vec1;
00520   normal = vec3.cross(vec2);
00521 
00522   if (normal[2] < 0.0)
00523     normal *= -1.0;
00524 
00525   normal.normalize();
00526 
00527   shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count);  // + polygon.vertex_count * 2);
00528   solid->type = shapes::MESH;
00529 
00530   // copy the first set of vertices
00531   memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
00532   // copy the first set of triangles
00533   memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
00534 
00535   for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
00536   {
00537     int vIdx1 = polygon.triangles[tIdx * 3];
00538     int vIdx2 = polygon.triangles[tIdx * 3 + 1];
00539     int vIdx3 = polygon.triangles[tIdx * 3 + 2];
00540 
00541     vec1 =
00542         Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
00543     vec2 =
00544         Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
00545     vec3 =
00546         Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
00547 
00548     vec2 -= vec1;
00549     vec3 -= vec1;
00550 
00551     Eigen::Vector3d triangle_normal = vec2.cross(vec1);
00552 
00553     if (triangle_normal.dot(normal) < 0.0)
00554       std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
00555   }
00556   return solid;
00557 }
00558 
00559 shapes::Mesh* SemanticWorld::createSolidMeshFromPlanarPolygon(const shapes::Mesh& polygon, double thickness) const
00560 {
00561   if (polygon.vertex_count < 3 || polygon.triangle_count < 1 || thickness <= 0)
00562     return 0;
00563   // first get the normal of the first triangle of the input polygon
00564   Eigen::Vector3d vec1, vec2, vec3, normal;
00565 
00566   int vIdx1 = polygon.triangles[0];
00567   int vIdx2 = polygon.triangles[1];
00568   int vIdx3 = polygon.triangles[2];
00569   vec1 = Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
00570   vec2 = Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
00571   vec3 = Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
00572   vec2 -= vec1;
00573   vec3 -= vec1;
00574   normal = vec3.cross(vec2);
00575 
00576   if (normal[2] < 0.0)
00577     normal *= -1.0;
00578 
00579   normal.normalize();
00580 
00581   // shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count);// + polygon.vertex_count *
00582   // 2);
00583 
00584   shapes::Mesh* solid =
00585       new shapes::Mesh(polygon.vertex_count * 2, polygon.triangle_count * 2);  // + polygon.vertex_count * 2);
00586   solid->type = shapes::MESH;
00587 
00588   // copy the first set of vertices
00589   memcpy(solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
00590   // copy the first set of triangles
00591   memcpy(solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
00592 
00593   for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
00594   {
00595     solid->triangles[(tIdx + polygon.triangle_count) * 3 + 0] = solid->triangles[tIdx * 3 + 0] + polygon.vertex_count;
00596     solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1] = solid->triangles[tIdx * 3 + 1] + polygon.vertex_count;
00597     solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2] = solid->triangles[tIdx * 3 + 2] + polygon.vertex_count;
00598 
00599     int vIdx1 = polygon.triangles[tIdx * 3];
00600     int vIdx2 = polygon.triangles[tIdx * 3 + 1];
00601     int vIdx3 = polygon.triangles[tIdx * 3 + 2];
00602 
00603     vec1 =
00604         Eigen::Vector3d(polygon.vertices[vIdx1 * 3], polygon.vertices[vIdx1 * 3 + 1], polygon.vertices[vIdx1 * 3 + 2]);
00605     vec2 =
00606         Eigen::Vector3d(polygon.vertices[vIdx2 * 3], polygon.vertices[vIdx2 * 3 + 1], polygon.vertices[vIdx2 * 3 + 2]);
00607     vec3 =
00608         Eigen::Vector3d(polygon.vertices[vIdx3 * 3], polygon.vertices[vIdx3 * 3 + 1], polygon.vertices[vIdx3 * 3 + 2]);
00609 
00610     vec2 -= vec1;
00611     vec3 -= vec1;
00612 
00613     Eigen::Vector3d triangle_normal = vec2.cross(vec1);
00614 
00615     if (triangle_normal.dot(normal) < 0.0)
00616       std::swap(solid->triangles[tIdx * 3 + 1], solid->triangles[tIdx * 3 + 2]);
00617     else
00618       std::swap(solid->triangles[(tIdx + polygon.triangle_count) * 3 + 1],
00619                 solid->triangles[(tIdx + polygon.triangle_count) * 3 + 2]);
00620   }
00621 
00622   for (unsigned vIdx = 0; vIdx < polygon.vertex_count; ++vIdx)
00623   {
00624     solid->vertices[(vIdx + polygon.vertex_count) * 3 + 0] = solid->vertices[vIdx * 3 + 0] - thickness * normal[0];
00625     solid->vertices[(vIdx + polygon.vertex_count) * 3 + 1] = solid->vertices[vIdx * 3 + 1] - thickness * normal[1];
00626     solid->vertices[(vIdx + polygon.vertex_count) * 3 + 2] = solid->vertices[vIdx * 3 + 2] - thickness * normal[2];
00627   }
00628 
00629   return solid;
00630 }
00631 }
00632 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10