C++ Matrix3d::block方法代码示例

bool localizerGVG::processMeetpoint(localizer::UpdateFilter::Request  &req,
				    localizer::UpdateFilter::Response &res) {
              
  ROS_INFO("processMeetpoint received meetpoint %d [%lf,%lf,%lf]", req.id, req.x, req.y,req.yaw);
  ROS_INFO("processMeetpoint robot pose [%lf,%lf,%lf]",X(0),X(1),X(2));
  if(nL==0) {
    //Reset the odometry covariance.
    P=MatrixXd::Zero(3,3);
  }
  // Propagate the cross-corelation between robot and the 
  // existing landmarks up to now
  propagateRL();

  // For the new landmark
  
  Vector3d xl; //the coordinates of the landmark (meetpoint) in World Frame
  Vector3d z; // Vector representation of the measurment
  z<<req.x, req.y, req.yaw;
  xl.segment(0,2)=X.segment(0,2)+C(X(2))*z.segment(0,2);
  xl(2)=X(2)+z(2);
  // Create the measurment matrices
  MatrixXd Hr(3,3);
  Vector2d dx;
  dx=xl.segment(0,2)-X.segment(0,2);
  double _c=cos(X(2));
  double _s=sin(X(2));
  Hr<<-_c, -_s, + _c*dx(1)-_s*dx(0),
    _s, -_c, - _s*dx(1)-_c*dx(0),
    0,   0, -1;
  Matrix3d Hl; // Hl is a 2 by 2 Matrix
  Hl=MatrixXd::Zero(3,3);
  Hl.block(0,0,2,2)=CT(X(2));
  Hl(2,2)=1;
  if(newLandmark(req.id)) {
    
    // If pre-fix on loaded map, need to mark that this new node is in current frame
    if (!mapLoadLocalization) preFixIdList.push_back(req.id);
          
    // New Landmark
    idList.push_back(req.id);
    P.conservativeResize(P.rows()+3,P.cols()+3);
    X.conservativeResize(X.size()+3);
    X.segment(3+nL*3,3)=xl;
    // Create new Pllcv matrices describing the cross corelation 
    // between the old landmarks and the new Landmark. Page 79 eq. 5.73 
    for (int count = 0; count < nL; count++) {
      Matrix3d newPllcv;
      newPllcv = -P.block(3 + count*3, 0, 3, 3)*Hr.transpose()*Hl;
      P.block(3 + count*3, 3 + nL*3, 3, 3) = newPllcv;
      P.block(3 + nL*3, 3 + count*3, 3, 3) = newPllcv.transpose();    
    }
    // Create new Prl matrix describing the cross corelation 
    // between the Robot and the new Landmark. Page 79 eq. 5.73 
    MatrixXd newPrl(3,3);
    
    newPrl=-P.block(0,0,3,3)*Hr.transpose()*Hl;
    P.block(0,3+nL*3,3,3)=newPrl;
    P.block(3+nL*3,0,3,3)=newPrl.transpose();
    // Create the new Pll matrix describing the covariance of the landmark
    // page 79 eq. 5.74
    Matrix3d newPll;
    newPll=Hl.transpose()*(Hr*P.block(0,0,3,3)*Hr.transpose()+R)*Hl;
    P.block(3+nL*3,3+nL*3,3,3)=newPll;
    nL++;
  }
  else {
    
    // Find which landmark it is:
    int id=-1;
    for(unsigned int i=0; i<idList.size(); i++) {
      if(idList[i] == req.id) id=i;
    }
            
    //Sanity check:
    if(id < 0) {
      cerr<<"Not able to find id:"<<req.id<< "in the list of ids"<<endl;
      for(unsigned int i=0; i<idList.size(); i++){
	      cerr<<idList[i]<<" ";
      }
      cerr<<endl;
    }
    // Check if the revisited node was one from previous map or current map
    // If from current map, don't use this node to compute transformation between old/new map
    bool oldMapNode = true;
    for (vector<int>::iterator it = preFixIdList.begin(); it != preFixIdList.end(); ++it){
      if((*it)==id) {
        oldMapNode = false;
        break;
      }
    }
    
    if (!mapLoadLocalization && oldMapNode) {
      // Copy the covariance information to Prr and also Xr
      Vector3d offset;
      offset(2) = X(2)-bearing_angle;
      Vector2d v;
      v(0) = X(3*id+3);
      v(1) = X(3*id+4);
      Vector2d rotated_mp;
      rotated_mp = C(offset(2))*v;
//.........这里部分代码省略.........