grid_line.h
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 #ifndef JSK_RECOGNITION_UTILS_GRID_LINE_H_
00036 #define JSK_RECOGNITION_UTILS_GRID_LINE_H_
00037 #include <pcl/point_types.h>
00038 #include <boost/shared_ptr.hpp>
00039 #include "jsk_recognition_utils/grid_index.h"
00040 
00041 #include <Eigen/Core>
00042 #include <Eigen/Geometry>
00043 
00044 namespace jsk_recognition_utils
00045 {
00046   class GridLine
00047   {
00048   public:
00049     typedef boost::shared_ptr<GridLine> Ptr;
00050     GridLine(const pcl::PointXYZRGB a, const pcl::PointXYZRGB b);
00051     virtual ~GridLine();
00052     virtual bool penetrateGrid(const pcl::PointXYZRGB& A,
00053                                const pcl::PointXYZRGB& B,
00054                                const pcl::PointXYZRGB& C,
00055                                const pcl::PointXYZRGB& D);
00056     virtual bool penetrateGrid(const Eigen::Vector3f A,
00057                                const Eigen::Vector3f B,
00058                                const Eigen::Vector3f C,
00059                                const Eigen::Vector3f D)
00060     {
00061       // std::cout << "checking "
00062       //           << "[" << from[0] << ", " << from[1] << ", " << from[2] << "]"
00063       //           << "--"
00064       //           << "[" << to[0] << ", " << to[1] << ", " << to[2] << "]"
00065       //           << std::endl;
00066       // std::cout << " penetrates "
00067       //           << "[" << A[0] << ", " << A[1] << ", " << A[2] << "], "
00068       //           << "[" << B[0] << ", " << B[1] << ", " << B[2] << "], "
00069       //           << "[" << C[0] << ", " << C[1] << ", " << C[2] << "], "
00070       //           << "[" << D[0] << ", " << D[1] << ", " << D[2] << "]" << std::endl;
00071       Eigen::Vector3f Across = (A - from).cross(d_);
00072       Eigen::Vector3f Bcross = (B - from).cross(d_);
00073       Eigen::Vector3f Ccross = (C - from).cross(d_);
00074       Eigen::Vector3f Dcross = (D - from).cross(d_);
00075       bool ab_direction = Across.dot(Bcross) < 0;
00076       bool ac_direction = Across.dot(Ccross) < 0;
00077       bool ad_direction = Across.dot(Dcross) < 0;
00078       bool bc_direction = Bcross.dot(Ccross) < 0;
00079       if (Across.norm() == 0 || 
00080           Bcross.norm() == 0 || 
00081           Ccross.norm() == 0 || 
00082           Dcross.norm() == 0) {
00083         return true;
00084       }
00085       else if ((ab_direction == ac_direction) && 
00086                (ab_direction == ad_direction)
00087                && (ab_direction == bc_direction)) {
00088         return false;
00089       }
00090       else {
00091         return true;
00092       }
00093     }
00094     
00095     const Eigen::Vector3f from;
00096     const Eigen::Vector3f to;
00097   protected:
00098     const Eigen::Vector3f d_;
00099   };
00100 }
00101 
00102 #endif


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