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; }
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; }
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; } } }