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