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