cube.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 
00037 #include "jsk_recognition_utils/geo/cube.h"
00038 #include "jsk_recognition_utils/geo_util.h"
00039 
00040 namespace jsk_recognition_utils
00041 {
00042     Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot):
00043     pos_(pos), rot_(rot)
00044   {
00045     dimensions_.resize(3);
00046   }
00047 
00048   Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot,
00049              const std::vector<double>& dimensions):
00050     pos_(pos), rot_(rot), dimensions_(dimensions)
00051   {
00052     
00053   }
00054   Cube::Cube(const Eigen::Vector3f& pos, const Eigen::Quaternionf& rot,
00055              const Eigen::Vector3f& dimensions):
00056     pos_(pos), rot_(rot)
00057   {
00058     dimensions_.resize(3);
00059     dimensions_[0] = dimensions[0];
00060     dimensions_[1] = dimensions[1];
00061     dimensions_[2] = dimensions[2];
00062   }
00063 
00064   Cube::Cube(const Eigen::Vector3f& pos,
00065              const Line& line_a, const Line& line_b, const Line& line_c)
00066   {
00067     double distance_a_b = line_a.distance(line_b);
00068     double distance_a_c = line_a.distance(line_c);
00069     double distance_b_c = line_b.distance(line_c);
00070     Line::Ptr axis;
00071     dimensions_.resize(3);
00072     Eigen::Vector3f ex, ey, ez;
00073     if (distance_a_b >= distance_a_c &&
00074         distance_a_b >= distance_b_c) {
00075       axis = line_a.midLine(line_b);
00076       line_a.parallelLineNormal(line_c, ex);
00077       line_c.parallelLineNormal(line_b, ey);
00078       
00079     }
00080     else if (distance_a_c >= distance_a_b &&
00081              distance_a_c >= distance_b_c) {
00082       axis = line_a.midLine(line_c);
00083       line_a.parallelLineNormal(line_b, ex);
00084       line_b.parallelLineNormal(line_c, ey);
00085     }
00086     else {
00087       // else if (distance_b_c >= distance_a_b &&
00088       //          distance_b_c >= distance_a_c) {
00089       axis = line_b.midLine(line_c);
00090       line_b.parallelLineNormal(line_a, ex);
00091       line_a.parallelLineNormal(line_c, ey);
00092     }
00093     dimensions_[0] = ex.norm();
00094     dimensions_[1] = ey.norm();
00095     axis->getDirection(ez);
00096     ez.normalize();
00097     ex.normalize();
00098     ey.normalize();
00099     if (ex.cross(ey).dot(ez) < 0) {
00100       ez = - ez;
00101     }
00102     rot_ = rotFrom3Axis(ex, ey, ez);
00103     axis->foot(pos, pos_);       // project
00104   }
00105 
00106   Cube::Cube(const jsk_recognition_msgs::BoundingBox& box)
00107   {
00108     dimensions_.resize(3);
00109     dimensions_[0] = box.dimensions.x;
00110     dimensions_[1] = box.dimensions.y;
00111     dimensions_[2] = box.dimensions.z;
00112     Eigen::Affine3f pose;
00113     tf::poseMsgToEigen(box.pose, pose);
00114     pos_ = Eigen::Vector3f(pose.translation());
00115     rot_ = Eigen::Quaternionf(pose.rotation()); // slow
00116   }
00117   
00118   Cube::~Cube()
00119   {
00120 
00121   }
00122 
00123   std::vector<Segment::Ptr> Cube::edges()
00124   {
00125     std::vector<Segment::Ptr> ret;
00126     Eigen::Vector3f A = pos_
00127       + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
00128                 (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
00129                 (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00130     Eigen::Vector3f B = pos_
00131       + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
00132                 (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
00133                 (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00134     Eigen::Vector3f C = pos_
00135       + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
00136                 (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
00137                 (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00138     Eigen::Vector3f D = pos_
00139       + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
00140                 (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
00141                 (+ dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00142     Eigen::Vector3f E = pos_
00143       + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
00144                 (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
00145                 (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00146     Eigen::Vector3f F = pos_
00147       + rot_ * ((+ dimensions_[0] * Eigen::Vector3f::UnitX()) +
00148                 (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
00149                 (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00150     Eigen::Vector3f G = pos_
00151       + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
00152                 (+ dimensions_[1] * Eigen::Vector3f::UnitY()) +
00153                 (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00154     Eigen::Vector3f H = pos_
00155       + rot_ * ((- dimensions_[0] * Eigen::Vector3f::UnitX()) +
00156                 (- dimensions_[1] * Eigen::Vector3f::UnitY()) +
00157                 (- dimensions_[2] * Eigen::Vector3f::UnitZ())) / 2.0;
00158     
00159     ret.push_back(Segment::Ptr(new Segment(A, B)));
00160     ret.push_back(Segment::Ptr(new Segment(B, C)));
00161     ret.push_back(Segment::Ptr(new Segment(C, D)));
00162     ret.push_back(Segment::Ptr(new Segment(D, A)));
00163     ret.push_back(Segment::Ptr(new Segment(E, F)));
00164     ret.push_back(Segment::Ptr(new Segment(F, G)));
00165     ret.push_back(Segment::Ptr(new Segment(G, H)));
00166     ret.push_back(Segment::Ptr(new Segment(H, E)));
00167     ret.push_back(Segment::Ptr(new Segment(A, E)));
00168     ret.push_back(Segment::Ptr(new Segment(B, F)));
00169     ret.push_back(Segment::Ptr(new Segment(C, G)));
00170     ret.push_back(Segment::Ptr(new Segment(D, H)));
00171     return ret;
00172   } 
00173   
00174   ConvexPolygon::Ptr Cube::intersectConvexPolygon(Plane& plane)
00175   {
00176     std::vector<Segment::Ptr> candidate_edges = edges();
00177     Vertices intersects;
00178     for (size_t i = 0; i < candidate_edges.size(); i++) {
00179       Segment::Ptr edge = candidate_edges[i];
00180       Eigen::Vector3f p;
00181       if (edge->intersect(plane, p)) {
00182         intersects.push_back(p);
00183       }
00184     }
00185     //ROS_INFO("%lu intersects", intersects.size());
00186     // Compute convex hull
00187     pcl::ConvexHull<pcl::PointXYZ> chull;
00188     pcl::PointCloud<pcl::PointXYZ>::Ptr chull_input
00189       = verticesToPointCloud<pcl::PointXYZ>(intersects);
00190     pcl::PointCloud<pcl::PointXYZ>::Ptr chull_cloud
00191       (new pcl::PointCloud<pcl::PointXYZ>);
00192     chull.setDimension(2);
00193     chull.setInputCloud(chull_input);
00194     {
00195       boost::mutex::scoped_lock lock(global_chull_mutex);
00196       chull.reconstruct(*chull_cloud);
00197     }
00198     
00199     return ConvexPolygon::Ptr(
00200       new ConvexPolygon(pointCloudToVertices(*chull_cloud)));
00201   }
00202   
00203   jsk_recognition_msgs::BoundingBox Cube::toROSMsg()
00204   {
00205     jsk_recognition_msgs::BoundingBox ret;
00206     ret.pose.position.x = pos_[0];
00207     ret.pose.position.y = pos_[1];
00208     ret.pose.position.z = pos_[2];
00209     ret.pose.orientation.x = rot_.x();
00210     ret.pose.orientation.y = rot_.y();
00211     ret.pose.orientation.z = rot_.z();
00212     ret.pose.orientation.w = rot_.w();
00213     ret.dimensions.x = dimensions_[0];
00214     ret.dimensions.y = dimensions_[1];
00215     ret.dimensions.z = dimensions_[2];
00216     return ret;
00217   }
00218 
00219   Vertices Cube::vertices()
00220   {
00221     Vertices vs;
00222     vs.push_back(buildVertex(0.5, 0.5, 0.5));
00223     vs.push_back(buildVertex(-0.5, 0.5, 0.5));
00224     vs.push_back(buildVertex(-0.5, -0.5, 0.5));
00225     vs.push_back(buildVertex(0.5, -0.5, 0.5));
00226     vs.push_back(buildVertex(0.5, 0.5, -0.5));
00227     vs.push_back(buildVertex(-0.5, 0.5, -0.5));
00228     vs.push_back(buildVertex(-0.5, -0.5, -0.5));
00229     vs.push_back(buildVertex(0.5, -0.5, -0.5));
00230     return vs;
00231   }
00232 
00233   Vertices Cube::transformVertices(const Eigen::Affine3f& pose_offset)
00234   {
00235     Vertices vs = vertices();
00236     Vertices transformed_vertices;
00237     for (size_t i = 0; i < vs.size(); i++) {
00238       transformed_vertices.push_back(pose_offset * vs[i]);
00239     }
00240     return transformed_vertices;
00241   }
00242   
00243   Polygon::Ptr Cube::buildFace(const Eigen::Vector3f v0,
00244                                const Eigen::Vector3f v1,
00245                                const Eigen::Vector3f v2,
00246                                const Eigen::Vector3f v3)
00247   {
00248     Vertices vs;
00249     vs.push_back(v0);
00250     vs.push_back(v1);
00251     vs.push_back(v2);
00252     vs.push_back(v3);
00253     Polygon::Ptr(new Polygon(vs));
00254   }
00255   
00256   std::vector<Polygon::Ptr> Cube::faces()
00257   {
00258     std::vector<Polygon::Ptr> fs(6);
00259     Vertices vs = vertices();
00260     Eigen::Vector3f A = vs[0];
00261     Eigen::Vector3f B = vs[1];
00262     Eigen::Vector3f C = vs[2];
00263     Eigen::Vector3f D = vs[3];
00264     Eigen::Vector3f E = vs[4];
00265     Eigen::Vector3f F = vs[5];
00266     Eigen::Vector3f G = vs[6];
00267     Eigen::Vector3f H = vs[7];
00268     Vertices vs0, vs1, vs2, vs3, vs4, vs5, vs6;
00269     vs0.push_back(A); vs0.push_back(E); vs0.push_back(F); vs0.push_back(B);
00270     vs1.push_back(B); vs1.push_back(F); vs1.push_back(G); vs1.push_back(C);
00271     vs2.push_back(C); vs2.push_back(G); vs2.push_back(H); vs2.push_back(D);
00272     vs3.push_back(D); vs3.push_back(H); vs3.push_back(E); vs3.push_back(A);
00273     vs4.push_back(A); vs4.push_back(B); vs4.push_back(C); vs4.push_back(D);
00274     vs5.push_back(E); vs5.push_back(H); vs5.push_back(G); vs5.push_back(F);
00275     fs[0].reset(new Polygon(vs0));
00276     fs[1].reset(new Polygon(vs1));
00277     fs[2].reset(new Polygon(vs2));
00278     fs[3].reset(new Polygon(vs3));
00279     fs[4].reset(new Polygon(vs4));
00280     fs[5].reset(new Polygon(vs5));
00281     return fs;
00282   }
00283 
00284   Eigen::Vector3f Cube::buildVertex(double i, double j, double k)
00285   {
00286     Eigen::Vector3f local = (Eigen::Vector3f::UnitX() * i * dimensions_[0] +
00287                              Eigen::Vector3f::UnitY() * j * dimensions_[1] +
00288                              Eigen::Vector3f::UnitZ() * k * dimensions_[2]);
00289     return Eigen::Translation3f(pos_) * rot_ * local;
00290   }
00291   
00292   Eigen::Vector3f Cube::nearestPoint(const Eigen::Vector3f& p,
00293                                      double& distance)
00294   {
00295     std::vector<Polygon::Ptr> current_faces = faces();
00296     double min_distance = DBL_MAX;
00297     Eigen::Vector3f min_point;
00298     for (size_t i = 0; i < current_faces.size(); i++) {
00299       Polygon::Ptr f = current_faces[i];
00300       double d;
00301       Eigen::Vector3f q = f->nearestPoint(p, d);
00302       if (min_distance > d) {
00303         min_distance = d;
00304         min_point = q;
00305       }
00306     }
00307     distance = min_distance;
00308     return min_point;
00309   }
00310 }


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37