Beispiel #1
0
bool Geometry::GetMVBB(const Geometry::VertexSet &vertices, Box &box) {
    ApproxMVBB::Vector3List v;
    for (Geometry::VertexSet::const_iterator it = vertices.begin(); it!=vertices.end();++it){
        pcl::PointXYZ p;
        Vertex2Point(*it,p);
        v.emplace_back(p.x,p.y,p.z);
    }
    ApproxMVBB::Matrix3Dyn points(3,v.size());
    for(int i=0;i<v.size();i++)
        points.col(i)=v[i];
    //NOTE parameters see:https://github.com/gabyx/ApproxMVBB
    ApproxMVBB::OOBB oobb = ApproxMVBB::approximateMVBB(points,0.001,8,5,0,5);
    Eigen::Vector3f centroid(
            (const float &) ((oobb.m_minPoint.x()+oobb.m_maxPoint.x())/2),
            (const float &) ((oobb.m_minPoint.y()+oobb.m_maxPoint.y())/2),
            (const float &) ((oobb.m_minPoint.z()+oobb.m_maxPoint.z())/2));
    //upper/lower point in OOBB frame
    double width = oobb.m_maxPoint.x()-oobb.m_minPoint.x();
    double height = oobb.m_maxPoint.y()-oobb.m_minPoint.y();
    double depth = oobb.m_maxPoint.z()-oobb.m_minPoint.z();
    if (width<=0 || height<=0 ||depth<=0)
        return false;
//    coordinate transformation A_IK matrix from OOBB frame K to world frame I
    Eigen::Quaternionf q;
    q.x()= (float) oobb.m_q_KI.x();
    q.y()= (float) oobb.m_q_KI.y();
    q.z()= (float) oobb.m_q_KI.z();
    q.w()= (float) oobb.m_q_KI.w();
    centroid=q.matrix()*centroid;
//    a translation to apply to the cube from 0,0,0
    box.centroid=centroid;
//    a quaternion-based rotation to apply to the cube
    box.quanternion=q.inverse();
    box.depth=depth;
    box.height=height;
    box.width=width;
    return true;
}
Beispiel #2
0
 double computeAngleFromHorizontal(const Eigen::Quaternionf& iQuat) {
   Eigen::Matrix3f rot = iQuat.matrix();
   double angle = atan2(rot(2,1), rot(2,2));
   if (angle < 0) angle += 2*kPi;
   return angle;
 }
Beispiel #3
0
void draw(const Eigen::Quaternionf &q, const Eigen::Vector3f &translation, const cv::Scalar &color, std::list<cv::Point2f> &tail, bool draw_tail = true)
{

  // Project z axis to ground plane
  Eigen::Vector3f z(0.0, 0.0, 1.0);
  z = q.matrix() * z + translation;  

  // Metric dimension in 3D coordinate frame
  float d_x = 15.24;        // In meter
  float d_y = 28.6512;      // In meter
  
  // Convert to image coordinate  
  float s_x = img_draw.cols;
  float s_y = img_draw.rows;

  float o_x = s_x - translation(1) / d_y * s_x;
  float o_y = s_y - translation(0) / d_x * s_y;

  float p_x = s_x - z(1) / d_y * s_x;
  float p_y = s_y - z(0) / d_x * s_y;

  float s = sqrt((p_x - o_x) * (p_x - o_x) + (p_y - o_y) * (p_y - o_y));

  float u = (p_x - o_x) / s;
  float v = (p_y - o_y) / s;

  float theta = atan2(v, u);

  // 2D rotation matrix and translation
  Eigen::Rotation2Df rot(theta);
  Eigen::Vector2f t(o_x, o_y);

  // Camera cone
  std::vector<Eigen::Vector2f> camera;
  float l = 5;
  Eigen::Vector2f o = rot * Eigen::Vector2f(0.0, 0.0) + t;
  Eigen::Vector2f a = rot * Eigen::Vector2f(3 * l, -3 * l) + t;
  Eigen::Vector2f b = rot * Eigen::Vector2f(3 * l, 3 * l) + t;

  if (tail.size() == 250)
    tail.pop_front();

  tail.push_back(cv::Point2f(o(0), o(1)));

  cv::circle(img_draw, cv::Point2f(o_x, o_y), 3, color);
  cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(a(0), a(1)), color, 2, CV_AA);
  cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA);
  cv::line(img_draw, cv::Point2f(a(0), a(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA);

  if (true == draw_tail)
  {
   cv::Point2f p1, p2;
   for (std::list<cv::Point2f>::iterator iter = tail.begin(); iter != tail.end(); ++iter)
   {
      if (iter == tail.begin())
      {
        p1 = *iter;
       continue;
     }
      p2 = *iter;  
      cv::line(img_draw, p1, p2, color, 2, CV_AA);
     p1 = p2;
  }
  }
}