C++ Matrix::z方法代码示例

void StaticCylinderRenderer::draw( const StaticCylinder& cylinder ) const
{
  glPushAttrib( GL_COLOR );
  glPushAttrib( GL_LIGHTING );
  glPushAttrib( GL_LINE_WIDTH );

  glDisable( GL_LIGHTING );
  glColor3d( 0.0, 0.0, 0.0 );
  glLineWidth( 1.0 );

  const Eigen::Matrix<GLfloat,3,1> center{ cylinder.x().cast<GLfloat>() };
  const Eigen::Matrix<GLfloat,3,1> translation{ 0.5 * m_L * cylinder.axis().cast<GLfloat>() };
  const Eigen::AngleAxis<GLfloat> rotation{ cylinder.R().cast<GLfloat>() };
  const GLfloat r{ static_cast<GLfloat>( cylinder.r() ) };

  // Draw the top circle
  glPushMatrix();
  glTranslatef( center.x() + translation.x(), center.y() + translation.y(), center.z() + translation.z() );
  glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
  glScalef( r, 1.0, r );

  glEnableClientState( GL_VERTEX_ARRAY );
  glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
  glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
  glDisableClientState( GL_VERTEX_ARRAY );
  glPopMatrix();
  
  // Draw the bottom circle
  glPushMatrix();
  glTranslatef( center.x() - translation.x(), center.y() - translation.y(), center.z() - translation.z() );
  glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );
  glScalef( r, 1.0, r );
  
  glEnableClientState( GL_VERTEX_ARRAY );
  glVertexPointer( 3, GL_FLOAT, 0, m_crds.data() );
  glDrawArrays( GL_LINE_LOOP, 0, m_crds.size() / 3 );
  glDisableClientState( GL_VERTEX_ARRAY );
  glPopMatrix();

  // Draw the 'supports'
  glPushMatrix();
  glTranslatef( center.x(), center.y(), center.z() );
  glRotatef( ( 180.0 / MathDefines::PI<GLfloat>() ) * rotation.angle(), rotation.axis().x(), rotation.axis().y(), rotation.axis().z() );

  glBegin( GL_LINES );
  glVertex3d( 0.0, 0.5 * m_L, r );
  glVertex3d( 0.0, -0.5 * m_L, r );
  glVertex3d( 0.0, 0.5 * m_L, -r );
  glVertex3d( 0.0, -0.5 * m_L, -r );
  glVertex3d( r, 0.5 * m_L, 0.0 );
  glVertex3d( r, -0.5 * m_L, 0.0 );
  glVertex3d( -r, 0.5 * m_L, 0.0 );
  glVertex3d( -r, -0.5 * m_L, 0.0 );
  glEnd();
  glPopMatrix();

  glPopAttrib();
  glPopAttrib();
  glPopAttrib();
}

	bool getImagePoint( const Eigen::Matrix<Scalar_,3,1>& point, size_t &x, size_t &y ) const 
	{
	    if( point.z() <= 0 )
		return false;

	    x = ((point.x() / point.z()) - center_x) / scale_x;
	    y = ((point.y() / point.z()) - center_y) / scale_y;

	    // check bounds. x and y are always positive
	    if( (x >= width) || (y >= height) ) 
            return false;

	    return true;
	}

 static void projectWithCam( Eigen::Matrix<double, 2, 1> & sp,
                             const Eigen::Matrix<double, 3, 1> & p3d,
                             const Eigen::Matrix<double, 3, 3> & K )
 {
     sp[ 0 ] = K( 0, 0 ) * p3d.x() / p3d.z() + K( 0, 2 );
     sp[ 1 ] = K( 1, 1 ) * p3d.y() / p3d.z() + K( 1, 2 );
 }

void OpenGL3DSphereRenderer::drawVertexArray( const Eigen::Matrix<GLfloat,3,1>& primary_color, const Eigen::Matrix<GLfloat,3,1>& secondary_color )
{
  const int num_mesh_verts{ static_cast<int>( m_sphere_verts.cols() ) };

  glEnableClientState( GL_VERTEX_ARRAY );
  glEnableClientState( GL_NORMAL_ARRAY );
  assert( m_sphere_verts.cols() == m_sphere_normals.cols() );
  glVertexPointer( 3, GL_FLOAT, 0, m_sphere_verts.data() );
  glNormalPointer( GL_FLOAT, 0, m_sphere_normals.data() );

  // Quad 1
  GLfloat mcolorambient[] = { GLfloat(0.3) * primary_color.x(), GLfloat(0.3) * primary_color.y(), GLfloat(0.3) * primary_color.z(), 1.0 };
  glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, mcolorambient );
  GLfloat mcolordiffuse[] = { primary_color.x(), primary_color.y(), primary_color.z(), (GLfloat) 1.0 };
  glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, mcolordiffuse );
  glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );

  // Quad 2
  glPushMatrix();
  glRotatef( 180.0, 0.0, 0.0, 1.0 );
  glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
  glPopMatrix();

  // Quad 3
  GLfloat mcolorambient2[] = { GLfloat(0.3) * secondary_color.x(), GLfloat(0.3) * secondary_color.y(), GLfloat(0.3) * secondary_color.z(), 1.0 };
  glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, mcolorambient2 );
  GLfloat mcolordiffuse2[] = { secondary_color.x(), secondary_color.y(), secondary_color.z(), 1.0 };
  glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, mcolordiffuse2 );
  glPushMatrix();
  glRotatef( 90.0, 0.0, 0.0, 1.0 );
  glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
  glPopMatrix();

  // Quad 4
  glPushMatrix();
  glRotatef( 270.0, 0.0, 0.0, 1.0 );
  glDrawArrays( GL_TRIANGLES, 0, num_mesh_verts );
  glPopMatrix();

  glDisableClientState( GL_NORMAL_ARRAY );
  glDisableClientState( GL_VERTEX_ARRAY );
}

void PerspectiveCameraController::positionCamera()
{
  {
    Eigen::Matrix<GLdouble,4,4> rotation{ Eigen::Matrix<GLdouble,4,4>::Identity() };
    rotation.block<1,3>(0,0) = m_cam_y;
    rotation.block<1,3>(1,0) = m_cam_x;
    rotation.block<1,3>(2,0) = -m_cam_z;

    assert( ( rotation.block<3,3>(0,0) * rotation.block<3,3>(0,0).transpose() - Eigen::Matrix<GLdouble,3,3>::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );

    glMultMatrixd( rotation.data() );
  }

  {
    const Eigen::Matrix<GLdouble,3,1> camera_psn{ m_cam_lookat + m_cam_psn };
    glTranslated( -camera_psn.x(), -camera_psn.y(), -camera_psn.z() );
  }
}

void publishNode(unsigned int index, Eigen::Matrix<double,4,1> trans, 
                Eigen::Quaternion<double> fq,
                const frame_common::CamParams &cp,
                bool isFixed, sba::CameraNode &msg)
{ 
    msg.index = index;
    
    msg.transform.translation.x = trans.x();
    msg.transform.translation.y = trans.y();
    msg.transform.translation.z = trans.z();
    msg.transform.rotation.x = fq.x();
    msg.transform.rotation.y = fq.y();
    msg.transform.rotation.z = fq.z();
    msg.transform.rotation.w = fq.w();
    msg.fx = cp.fx;
    msg.fy = cp.fy;
    msg.cx = cp.cx;
    msg.cy = cp.cy;
    msg.baseline = cp.tx;
    msg.fixed = isFixed;
}

template <typename PointT, typename Scalar> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              const std::vector<int> &indices,
                              const Eigen::Matrix<Scalar, 4, 1> &centroid,
                              Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
  if (indices.empty ())
    return (0);

  // Initialize to 0
  covariance_matrix.setZero ();

  size_t point_count;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    point_count = indices.size ();
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      Eigen::Matrix<Scalar, 4, 1> pt;
      pt[0] = cloud[indices[i]].x - centroid[0];
      pt[1] = cloud[indices[i]].y - centroid[1];
      pt[2] = cloud[indices[i]].z - centroid[2];

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    point_count = 0;
    // For each point in the cloud
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud[indices[i]]))
        continue;

      Eigen::Matrix<Scalar, 4, 1> pt;
      pt[0] = cloud[indices[i]].x - centroid[0];
      pt[1] = cloud[indices[i]].y - centroid[1];
      pt[2] = cloud[indices[i]].z - centroid[2];

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
      ++point_count;
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);
  return (static_cast<unsigned int> (point_count));
}

//+----------------------------------------------------------------------------+
//| main()                                                                     |
//-----------------------------------------------------------------------------+
int
main(int argc, char** argv)
{
  common::PathFileString pfs(argv[0]);
  //////////////////////////////////////////////////////////////////////////////
  // User command line parser
  AnyOption *opt = new AnyOption();
  opt->autoUsagePrint(true);

  opt->addUsage( "" );
  opt->addUsage( "Usage: " );
  char buff[256];
  sprintf(buff, "       Example: %s -r example1_noisy.cfg", pfs.getFileName().c_str());
  opt->addUsage( buff );
  opt->addUsage( " -h  --help                 Prints this help" );
  opt->addUsage( " -r  --readfile <filename>  Reads the polyhedra description file" );
  opt->addUsage( " -t  --gltopic <topic name> (opt) ROS Topic to send OpenGL commands " );
  opt->addUsage( "" );

  opt->setFlag(  "help", 'h' );
  opt->setOption(  "readfile", 'r' );

  opt->processCommandArgs( argc, argv );

  if( opt->getFlag( "help" ) || opt->getFlag( 'h' ) ) {opt->printUsage(); delete opt; return(1);}

  std::string gltopic("OpenGLRosComTopic");
  if( opt->getValue( 't' ) != NULL  || opt->getValue( "gltopic" ) != NULL  ) gltopic = opt->getValue( 't' );
	std::cerr << "[OpenGL communication topic is set to : \"" << gltopic << "\"]\n";

  std::string readfile;
	if( opt->getValue( 'r' ) != NULL  || opt->getValue( "readfile" ) != NULL  )
	{
    readfile = opt->getValue( 'r' );
    std::cerr << "[ World description is read from \"" << readfile << "\"]\n";
	}
	else{opt->printUsage(); delete opt; return(-1);}

  delete opt;
  //
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Initialize ROS and OpenGLRosCom node (visualization)
  std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]"<< std::endl;
  ros::init(argc, argv, pfs.getFileName().c_str());
  if(ros::isInitialized())
    std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[OK]\n"<< std::endl;
  else{
    std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[FAILED]\n"<< std::endl;
    return(1);
  }
  COpenGLRosCom glNode;
  glNode.CreateNode(gltopic);
  //
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Reading the input file
  std::cerr << "Reading the input file..." << std::endl;
  ShapeVector shapes;
  PoseVector  poses;
  IDVector    ID;
  if(ReadPolyhedraConfigFile(readfile, shapes, poses, ID)==false)
  {
    std::cerr << "["<<pfs.getFileName()<<"]:" << "Error reading file \"" << readfile << "\"\n";
    return(-1);
  }
  std::cerr << "Read " << poses.size() << " poses of " << shapes.size() << " shapes with " << ID.size() << " IDs.\n";
  //
  //////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////
  // Visualizing the configuration of objects
  std::cerr << "Visualizing the configuration of objects..." << std::endl;
  for(unsigned int i=0; i<shapes.size(); i++)
  {
    for(size_t j=0; j<shapes[i].F.size(); j++)
    {
      glNode.LineWidth(1.0f);
      glNode.AddColor3(0.3f, 0.6f, 0.5f);
      glNode.AddBegin("LINE_LOOP");
      for(size_t k=0; k<shapes[i].F[j].Idx.size(); k++)
      {
        int idx = shapes[i].F[j].Idx[k];
        Eigen::Matrix<Real,3,1> Vt = poses[i]*shapes[i].V[ idx ];
        glNode.AddVertex(Vt.x(), Vt.y(), Vt.z());
      }
      glNode.AddEnd();
    }
  }
  glNode.SendCMDbuffer();
  ros::spinOnce();
  common::PressEnterToContinue();
  //
  //////////////////////////////////////////////////////////////////////////////

//.........这里部分代码省略.........