C++ Vector4d::y方法代码示例

void KinematicUtil::keep_robot_stable(KRobot& robot, const Matrix2d& foot_size, double security_offset, int max_it) {
    assert(security_offset > 0);
    SupportLeg sup = determine_support_leg(robot);
    Kin::JointIds foot_id = sup == left? Kin::JointIds::LFootEndpoint : Kin::JointIds::RFootEndpoint;
    Kin::ChainIds chain_id = sup == left? Kin::ChainIds::LLegChain : Kin::ChainIds::RLegChain;
    Matrix4d sup_foot_inv = robot.get_joint_by_id(foot_id).get_chain_matrix_inverse().matrix();
    Vector4d cog = robot.get_centre_of_gravity();
    cog = (Vector4d() << (sup_foot_inv * (Vector4d()<<cog.head<3>(), 1).finished()).head<3>(), cog(3)).finished();

    double cog_is_in_front = cog.x() + security_offset - foot_size(0,0);
    double cog_is_behind = -cog.x() + security_offset + foot_size(1,0);

    double cog_is_left;
    double cog_is_right;
    if(sup == left) {
        cog_is_left = cog.y() + security_offset - foot_size(0,1);
        cog_is_right = -cog.y() + security_offset + foot_size(1,1);
    } else {
        cog_is_left = cog.y() + security_offset + foot_size(1,1);
        cog_is_right = -cog.y() + security_offset - foot_size(0,1);
    }

    if(sup == unknown && cog_is_left > 0) {
        cog_is_left = 0;
    }

    Vector4d cog_target_offset(0, 0, 0, 1);
    assert(!(cog_is_behind > 0 && cog_is_in_front > 0));
    assert(!(cog_is_right > 0 && cog_is_left > 0));

    if(cog_is_behind > 0)
        cog_target_offset.x() = cog_is_behind;
    if(cog_is_in_front > 0)
        cog_target_offset.x() = -cog_is_in_front;
    if(cog_is_left > 0)
        cog_target_offset.y() = cog_is_left;
    if(cog_is_right > 0)
        cog_target_offset.y() = -cog_is_right;
    L_DEBUG(std::cout<<"FootSize\n"<<foot_size<<std::endl);
    L_DEBUG(std::cout<<"Front: "<< cog_is_in_front<<" Behing: "<<cog_is_behind<<" Left: "<<cog_is_left<<" Right: "<<cog_is_right<<std::endl);
    L_DEBUG(std::cout<<"Cog:  "<<cog.head<3>().transpose()<<" Proposed offset: "<<cog_target_offset.head<3>().transpose()<<std::endl);
    if(cog_target_offset.head<2>() == Vector2d::Zero())
        return;

    Vector4d cog_target = cog - cog_target_offset;
    L_DEBUG(std::cout<<"CogTarget:  "<<cog_target.head<3>().transpose()<<std::endl);

    Kin::KinematicTask task(robot);
    task.perform_centre_of_gravity_task(chain_id, cog_target.head<3>(), std::set<int>(), 1e0, max_it, Kin::KinematicTask::DisableType::direct_chain);
}

void NearPlaneClipper::setup( const Matrix4f& projectionMV )
{
    // get normalized near plane
    Vector4d plane = projectionMV.get_row( 2 ) +
                     projectionMV.get_row( 3 );
    plane *= 1.0 / sqrt( plane.x()*plane.x() +
                         plane.y()*plane.y() +
                         plane.z()*plane.z() );

    _nearPlane = Plane_d( plane.x(), plane.y(), plane.z(), plane.w() );

    // move near plane slightly ahead, so that when we will draw it, it will not be clipped
    _nearPlane.w -= 0.001;
//    _nearPlane.w -= 2.0;
}

int addPointAndProjection(SysSBA& sba, vector<Point, Eigen::aligned_allocator<Point> >& points, int ndi)
{
    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Project points into nodes.
    for (int i = 0; i < points.size(); i++)
    {
      double pointnoise = 0.1;
  
      // Add points into the system, and add noise.
      // Add up to .5 pixels of noise.
      Vector4d temppoint = points[i];
      temppoint.x() += pointnoise*(drand48() - 0.5);
      temppoint.y() += pointnoise*(drand48() - 0.5);
      temppoint.z() += pointnoise*(drand48() - 0.5);
      int index = sba.addPoint(temppoint);
    
      Vector3d proj;
      calculateProj(sba, points[i], ndi, proj);
      
      // If valid (within the range of the image size), add the stereo 
      // projection to SBA.
      //if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
      //{
        sba.addStereoProj(ndi, index, proj);
      //}
    }
    
    
    return sba.tracks.size() - points.size();
}

void Model::RotateObject(Shape* shape, TreeObject* object, Vector4d rotate)
{
  if (!shape)
    return;
  Vector3d rot(rotate.x(), rotate.y(), rotate.z());
  shape->Rotate(rot, rotate.w());
  ModelChanged();
}

void setupSBA(SysSBA &sys)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = 0;   // Baseline (no baseline since this is monocular)

    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Create a plane containing a wall of points.
    int npts_x = 10; // Number of points on the plane in x
    int npts_y = 5;  // Number of points on the plane in y
    
    double plane_width = 5;     // Width of the plane on which points are positioned (x)
    double plane_height = 2.5;    // Height of the plane on which points are positioned (y)
    double plane_distance = 5; // Distance of the plane from the cameras (z)

    // Vector containing the true point positions.
    vector<Point, Eigen::aligned_allocator<Point> > points;

    for (int ix = 0; ix < npts_x ; ix++)
    {
      for (int iy = 0; iy < npts_y ; iy++)
      {
        // Create a point on the plane in a grid.
        points.push_back(Point(plane_width/npts_x*(ix+.5), -plane_height/npts_y*(iy+.5), plane_distance, 1.0));
      }
    }
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 5; // Number of nodes.
    double path_length = 3; // Length of the path the nodes traverse.
    
    unsigned int i = 0, j = 0;
    
    for (i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
      rot.normalize();
      
      // Add a new node to the system.
      sys.addNode(trans, rot, cam_params, false);
    }
        
    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    double ptscale = 1.0;
        
    // Add points into the system, and add noise.
    for (i = 0; i < points.size(); i++)
    {
      // Add up to .5 points of noise.
      Vector4d temppoint = points[i];
      temppoint.x() += ptscale*(drand48() - 0.5);
      temppoint.y() += ptscale*(drand48() - 0.5);
      temppoint.z() += ptscale*(drand48() - 0.5);
      sys.addPoint(temppoint);
    }
    
    Vector2d proj;
    
    // Project points into nodes.
    for (i = 0; i < points.size(); i++)
    {
      for (j = 0; j < sys.nodes.size(); j++)
      {
        // Project the point into the node's image coordinate system.
        sys.nodes[j].setProjection();
        sys.nodes[j].project2im(proj, points[i]);
        
        // If valid (within the range of the image size), add the monocular 
        // projection to SBA.
        if (proj.x() > 0 && proj.x() < maxx-1 && proj.y() > 0 && proj.y() < maxy-1)
        {
          sys.addMonoProj(j, i, proj);
          //printf("Adding projection: Node: %d Point: %d Proj: %f %f\n", j, i, proj.x(), proj.y());
        }
      }
    }
    
    // Add noise to node position.
    
    double transscale = 1.0;
    double rotscale = 0.2;
    
    // Don't actually add noise to the first node, since it's fixed.
    for (i = 1; i < sys.nodes.size(); i++)
    {
      Vector4d temptrans = sys.nodes[i].trans;
      Quaterniond tempqrot = sys.nodes[i].qrot;
//.........这里部分代码省略.........

void setupSBA(SysSBA &sba)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = -30; // Baseline (no baseline since this is monocular)

    // Create a plane containing a wall of points.
    Plane plane0, plane1;
    plane0.resize(3, 2, 10, 5);
    
    plane1 = plane0;
    plane1.translate(0.1, 0.05, 0.0);
    
    plane1.rotate(PI/4.0, 1, 0, 0);
    plane1.translate(0.0, 0.0, 7.0);
    
    
    plane0.rotate(PI/4.0, 1, 0, 0);
    plane0.translate(0.0, 0.0, 7.0);
    
    //plane1.translate(0.05, 0.0, 0.05);
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 2; // Number of nodes.
    double path_length = 2; // Length of the path the nodes traverse.

    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    
    for (int i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
#if 0
      if (i >= 0)
	    {
	      // perturb a little
	      double tnoise = 0.5;	// meters
	      trans.x() += tnoise*(drand48()-0.5);
	      trans.y() += tnoise*(drand48()-0.5);
	      trans.z() += tnoise*(drand48()-0.5);
	    }
#endif

      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
#if 0
      if (i > 0)
	    {
	      // perturb a little
	      double qnoise = 0.1;	// meters
	      rot.x() += qnoise*(drand48()-0.5);
	      rot.y() += qnoise*(drand48()-0.5);
	      rot.z() += qnoise*(drand48()-0.5);
	    }
#endif
      rot.normalize();
      
      // Add a new node to the system.
      sba.addNode(trans, rot, cam_params, false);
    }
    
    Vector3d imagenormal(0, 0, 1);
    
    Matrix3d covar0;
    covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
    Matrix3d covar;
    
    Quaterniond rotation;
    Matrix3d rotmat;
    
    // Project points into nodes.
    addPointAndProjection(sba, plane0.points, 0);
    addPointAndProjection(sba, plane1.points, 1);
    
    int offset = plane0.points.size();
    
    Vector3d normal0 = sba.nodes[0].qrot.toRotationMatrix().transpose()*plane0.normal; 
    Vector3d normal1 = sba.nodes[1].qrot.toRotationMatrix().transpose()*plane1.normal; 
    
    printf("Normal: %f %f %f -> %f %f %f\n", plane0.normal.x(), plane0.normal.y(), plane0.normal.z(), normal0.x(), normal0.y(), normal0.z());
    printf("Normal: %f %f %f -> %f %f %f\n", plane1.normal.x(), plane1.normal.y(), plane1.normal.z(), normal1.x(), normal1.y(), normal1.z());
    
    for (int i = 0; i < plane0.points.size(); i++)
    {
      sba.addPointPlaneMatch(0, i, normal0, 1, i+offset, normal1);

      Matrix3d covar;
      covar << 0.1, 0, 0,
                0, 0.1, 0, 
          	0, 0, 0.1;
      sba.setProjCovariance(0, i+offset, covar);
      sba.setProjCovariance(1, i, covar);
    }
//.........这里部分代码省略.........

void setupSBA(SysSBA &sys)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = -30; // Baseline (no baseline since this is monocular)

    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Create a plane containing a wall of points.
    Plane middleplane;
    middleplane.resize(3, 2, 10, 5);
    middleplane.translate(0.0, 0.0, 7.0);
    
    Plane leftplane;
    leftplane.resize(1, 2, 6, 12);
    //    leftplane.rotate(-PI/4.0, 0, 1, 0);
    leftplane.translate(0, 0, 7.0);
    
    Plane rightplane;
    rightplane.resize(1, 2, 6, 12);
    //    rightplane.rotate(PI/4.0, 0, 1, 0);
    rightplane.translate(2, 0, 7.0);
    
    Plane topplane;
    topplane.resize(1, 1.5, 6, 12);
    //    topplane.rotate(PI/4.0, 1, 0, 0);
    topplane.translate(2, 0, 7.0);

    // Vector containing the true point positions.
    rightplane.normal = rightplane.normal; 
  
    vector<Point, Eigen::aligned_allocator<Point> > points;
    vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
    
    points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
    normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
    
    points.insert(points.end(), leftplane.points.begin(), leftplane.points.end());
    normals.insert(normals.end(), leftplane.points.size(), leftplane.normal);
    
    points.insert(points.end(), rightplane.points.begin(), rightplane.points.end());
    normals.insert(normals.end(), rightplane.points.size(), rightplane.normal);
    
    points.insert(points.end(), topplane.points.begin(), topplane.points.end());
    normals.insert(normals.end(), topplane.points.size(), topplane.normal);
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 2; // Number of nodes.
    double path_length = 0.5; // Length of the path the nodes traverse.

    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    
    unsigned int i = 0, j = 0;
    
    for (i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
#if 1
      if (i >= 0)
	{
	  // perturb a little
	  double tnoise = 0.5;	// meters
	  trans.x() += tnoise*(drand48()-0.5);
	  trans.y() += tnoise*(drand48()-0.5);
	  trans.z() += tnoise*(drand48()-0.5);
	}
#endif

      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
#if 1
      if (i >= 0)
	{
	  // perturb a little
	  double qnoise = 0.1;	// meters
	  rot.x() += qnoise*(drand48()-0.5);
	  rot.y() += qnoise*(drand48()-0.5);
	  rot.z() += qnoise*(drand48()-0.5);
	}
#endif
      rot.normalize();
      
      // Add a new node to the system.
      sys.addNode(trans, rot, cam_params, false);
    }
        
    double pointnoise = 1.0;
    
    // Add points into the system, and add noise.
    for (i = 0; i < points.size(); i++)
//.........这里部分代码省略.........

void setupSBA(SysSBA &sys)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = 0.3; // Baseline 

    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Create a plane containing a wall of points.
    Plane middleplane;
    middleplane.resize(3, 2, 10, 5);
    //middleplane.rotate(PI/4.0, PI/6.0, 1, 0);
    middleplane.rotate(PI/4.0, 1, 0, 1);
    middleplane.translate(0.0, 0.0, 5.0);
    
    // Create another plane containing a wall of points.
    Plane mp2;
    mp2.resize(3, 2, 10, 5);
    mp2.rotate(0, 0, 0, 1);
    mp2.translate(0.0, 0.0, 4.0);


    // Create another plane containing a wall of points.
    Plane mp3;
    mp3.resize(3, 2, 10, 5);
    mp3.rotate(-PI/4.0, 1, 0, 1);
    mp3.translate(0.0, 0.0, 4.5);



    // Vector containing the true point positions.
    vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
    
    points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
    normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
    points.insert(points.end(), mp2.points.begin(), mp2.points.end());
    normals.insert(normals.end(), mp2.points.size(), mp2.normal);
    points.insert(points.end(), mp3.points.begin(), mp3.points.end());
    normals.insert(normals.end(), mp3.points.size(), mp3.normal);
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 2; // Number of nodes.
    double path_length = 1.0; // Length of the path the nodes traverse.

    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    
    unsigned int i = 0;
    
    Vector3d inormal0 = middleplane.normal;
    Vector3d inormal1 = middleplane.normal;
    Vector3d inormal20 = mp2.normal;
    Vector3d inormal21 = mp2.normal;
    Vector3d inormal30 = mp3.normal;
    Vector3d inormal31 = mp3.normal;
    
    for (i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
#if 1
      if (i >= 2)
	    {
	      // perturb a little
	      double tnoise = 0.5;	// meters
	      trans.x() += tnoise*(drand48()-0.5);
	      trans.y() += tnoise*(drand48()-0.5);
	      trans.z() += tnoise*(drand48()-0.5);
	    }
#endif

      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
#if 1
      if (i >= 2)
	    {
	      // perturb a little
	      double qnoise = 0.1;	// meters
	      rot.x() += qnoise*(drand48()-0.5);
	      rot.y() += qnoise*(drand48()-0.5);
	      rot.z() += qnoise*(drand48()-0.5);
	    }
#endif
      rot.normalize();
      
      // Add a new node to the system.
      sys.addNode(trans, rot, cam_params, false);
      
      // set normal
      if (i == 0)
	{
	  inormal0 = rot.toRotationMatrix().transpose() * inormal0;
//.........这里部分代码省略.........