C++ Matrix4d::transpose方法代码示例

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 &center, 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;
}