void
pcl::visualization::getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24])
{
// Set up the normals
Eigen::Vector4d normals[6];
for (int i=0; i < 6; i++)
{
normals[i] = Eigen::Vector4d (0.0, 0.0, 0.0, 1.0);
// if i is even set to -1, if odd set to +1
normals[i] (i/2) = 1 - (i%2)*2;
}
// Transpose the matrix for use with normals
Eigen::Matrix4d view_matrix = view_projection_matrix.transpose ();
// Transform the normals to world coordinates
for (int i=0; i < 6; i++)
{
normals[i] = view_matrix * normals[i];
double f = 1.0/sqrt (normals[i].x () * normals[i].x () +
normals[i].y () * normals[i].y () +
normals[i].z () * normals[i].z ());
planes[4*i + 0] = normals[i].x ()*f;
planes[4*i + 1] = normals[i].y ()*f;
planes[4*i + 2] = normals[i].z ()*f;
planes[4*i + 3] = normals[i].w ()*f;
}
}
Box(Eigen::Vector4d ¢er, Eigen::Vector4d& u, Eigen::Vector4d &v,
Eigen::Vector4d &w, unsigned int id, unsigned int matId,
double uWidth, double vWidth, double wWidth)
: QuadricCollection(center, id, matId), u(u), v(v), w(w) {
BOOST_ASSERT_MSG(u[3] == 0 && v[3] == 0 && w[3] == 0, "u,v,w must have"
" fourth coordinate equal to zero!");// Got u:\n"
// << u << "v:\n" << v << "w:\n" << w)
// Prepare rotation matrix
Eigen::Matrix4d R;
R.row(0) = u;
R.row(1) = v;
R.row(2) = w;
R.row(3) = Eigen::Vector4d(0, 0, 0, 1);
/* Make all the planes forming the box (centered at origin).
Plane normals will be unit vectors pointing in positive/negative
x, y, z directions. The points x on the plane with normal
n = (a,b,c), distance d to origin satisfy n.p -d = 0,
or x^T Q x = 0 where
Q = |0 0 0 a/2|
|0 0 0 b/2|
|0 0 0 c/2|
|a/2 b/2 c/2 -d|
We define planes w.r.t. x, y, z axes, then rotate to u,v,w
*/
Eigen::Matrix4d posWPlane, negWPlane, posUPlane, negUPlane,
posVPlane, negVPlane;
posWPlane = negWPlane = posUPlane = negUPlane = posVPlane = negVPlane
= Eigen::Matrix4d::Zero();
posUPlane.row(3) = posUPlane.col(3) = Eigen::Vector4d(0.5, 0, 0, -uWidth/2.0);
negUPlane.row(3) = negUPlane.col(3) = Eigen::Vector4d(-0.5, 0, 0, -uWidth/2.0);
posVPlane.row(3) = posVPlane.col(3) = Eigen::Vector4d(0, 0.5, 0, -vWidth/2.0);
negVPlane.row(3) = negVPlane.col(3) = Eigen::Vector4d(0, -0.5, 0, -vWidth/2.0);
posWPlane.row(3) = posWPlane.col(3) = Eigen::Vector4d(0, 0, 0.5, -wWidth/2.0);
negWPlane.row(3) = negWPlane.col(3) = Eigen::Vector4d(0, 0, -0.5, -wWidth/2.0);
addQuadric(R.transpose() * posWPlane * R);
addQuadric(R.transpose() * negWPlane * R);
addQuadric(R.transpose() * posUPlane * R);
addQuadric(R.transpose() * negUPlane * R);
addQuadric(R.transpose() * posVPlane * R);
addQuadric(R.transpose() * negVPlane * R);
}
Plane Plane::transform(const Eigen::Affine3d& transform)
{
Eigen::Vector4d n;
n[0] = normal_[0];
n[1] = normal_[1];
n[2] = normal_[2];
n[3] = d_;
Eigen::Matrix4d m = transform.matrix();
Eigen::Vector4d n_d = m.transpose() * n;
Eigen::Vector4d n_dd = n_d.normalized();
return Plane(Eigen::Vector3d(n_dd[0], n_dd[1], n_dd[2]), n_dd[3]);
}
Eigen::MatrixXd transform_point3d(const Eigen::MatrixXd& verts, const Eigen::Matrix4d& trans)
{
int NP = verts.rows();
Eigen::MatrixXd res(NP,4);
res.col(0) = verts.col(0);
res.col(1) = verts.col(1);
res.col(2) = verts.col(2);
res.col(3) = Eigen::VectorXd::Ones(NP,1);
//std::cout << res << std::endl;
//std::cout << trans * res.row(0).transpose() << std::endl;
res = res * trans.transpose();
//std::cout << res << std::endl;
res.conservativeResize(NP, 3);
return res;
}
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) {
GLdouble modelmat[16];
glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
Eigen::Matrix4d InvModelMat;
InvModelMat.setZero();
for(int i=0; i<4; i++){
for(int j=0; j<4; j++){
InvModelMat(i,j)=modelmat[4*i+j];
}
}
InvModelMat=(InvModelMat.transpose());
InvModelMat=(InvModelMat.inverse());
Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1);
cc=InvModelMat*cc;
if(cc.norm()!=0) cc=cc/cc[3];
return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
//\fn Eigen::Vector4d triangulate(Point2f p1, Point2f p2, ExtrinsicParam e1, ExtrinsicParam e2);
///\brief This function triangulate a point in 3D.
///\param p1 The first point detected on the first image (cam1).
///\param p2 The second point detected on the second image (cam 2).
///\param e1 The first extrinsic object (cam1).
///\param e2 The second extrinsic object (cam2).
///\return The value of the triangulated point.
Eigen::Vector4d triangulate(cv::Point2f p1, cv::Point2f p2, ExtrinsicParam e1, ExtrinsicParam e2)
{
std::cout << "p1 = " << p1 << "\tp2 = " << p2 << std::endl;
Eigen::Matrix3d K = e1.getCameraMatrix(), K1 = e2.getCameraMatrix();
Eigen::MatrixXd Rt, Rt1;
Rt.resize(3,4);
Rt << e1.getRotationMatrix(), e1.getTranslationVector();
Rt1.resize(3,4);
Rt1 << e2.getRotationMatrix(), e2.getTranslationVector();
Eigen::MatrixXd KRt, KRt1;
KRt.noalias() = K*Rt;
KRt1.noalias() = K1*Rt1;
double p1x = (p1.x-K(0,2))/K(0,0);
double p2x = (p2.x-K1(0,2))/K1(0,0);
double p1y = (p1.y-K(1,2))/K(1,1);
double p2y = (p2.y-K1(1,2))/K1(1,1);
Eigen::VectorXd dist, dist1;
dist.resize(5);
dist1.resize(5);
dist << -0.133553,
0.078593,
0.001123,
0.001457,
-0.043746;
dist1 << -0.113323,
-0.023496,
-0.013891,
-0.003838,
0.267853;
double r2 = (p1x)*(p1x) + (p1y)*(p1y);
double xu1 = (p1x)*(1+dist(0)*r2+dist(1)*r2*r2+dist(4)*r2*r2*r2) + dist(2)*2*(p1x)*(p1y) + dist(3)*(r2+2*(p1x)*(p1x));
double yu1 = (p1y)*(1+dist(0)*r2+dist(1)*r2*r2+dist(4)*r2*r2*r2) + dist(3)*2*(p1x)*(p1y) + dist(2)*(r2+2*(p1y)*(p1y));
r2 = (p2x)*(p2x) + (p2y)*(p2y);
double xu2 = (p2x)*(1+dist(0)*r2+dist(1)*r2*r2+dist(4)*r2*r2*r2) + dist(2)*2*(p2x)*(p2y) + dist(3)*(r2+2*(p2x)*(p2x));
double yu2 = (p2y)*(1+dist(0)*r2+dist(1)*r2*r2+dist(4)*r2*r2*r2) + dist(3)*2*(p2x)*(p2y) + dist(2)*(r2+2*(p2y)*(p2y));
//std::cout << "xu1=" << xu1 << "\tyu1=" << yu1 << std::endl;
Eigen::Vector3d P1, P2;
P1(0) = p1.x;
P1(1) = p1.y;
P1(2) = 1.;
P2(0) = p2.x;
P2(1) = p2.y;
P2(2) = 1.;
P1.noalias() = e1.undistortPoint(P1);
P2.noalias() = e2.undistortPoint(P2);
Eigen::Matrix4d A;
/*A << (xu1)*Rt.row(2) - Rt.row(0),
(yu1)*Rt.row(2) - Rt.row(1),
(xu2)*Rt1.row(2) - Rt1.row(0),
(yu2)*Rt1.row(2) - Rt1.row(1);*/
A << (P1(0))*Rt.row(2) - Rt.row(0),
(P1(1))*Rt.row(2) - Rt.row(1),
(P2(0))*Rt1.row(2) - Rt1.row(0),
(P2(1))*Rt1.row(2) - Rt1.row(1);
Eigen::Matrix4d At, AtA;
At.noalias() = A.transpose();
AtA.noalias() = At*A;
Eigen::MatrixXd AInv;
pseudoInverse(A,AInv);
Eigen::EigenSolver<Eigen::Matrix4d> es(AtA);
double init = false;
Eigen::Vector4d min;
double indexOfMin;
for (int i=0; i < es.eigenvectors().cols(); i++)
{
if (es.eigenvectors().col(i).imag().isApprox(Eigen::Vector4d::Zero()))
{
Eigen::Vector4d real = es.eigenvectors().col(i).real();
double one = real(3);
real = (1./one)*real;
if (!init)
{
min.noalias() = real;
indexOfMin = i;
init = true;
}
else if (es.eigenvalues()[i].real() < es.eigenvalues()[indexOfMin].real())
{
min.noalias() = real;
indexOfMin = i;
}
}
}
std::cout << "triangulated point =\n" << min << std::endl;
return min;
}