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

void StateEstimatorKinematic::computeKss(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &C, int zDim )
{

    Eigen::Matrix<double,6,6> B = C.transpose();
    // Eigen::Matrix<double,6,6> P;
    dare(A.transpose(), B, _P, zDim); // A^T is used here
    _K.setZero();
    Eigen::Matrix<double,6,6> PB;
    Eigen::Matrix<double,6,6> BtPB_R;
    if (zDim == 6) {
        PB = _P * B;
        BtPB_R = B.transpose() * PB + _R;
        _K = PB* BtPB_R.inverse()
             ;
    }
    else
    {
        PB.topLeftCorner(6,zDim) = _P * B.topLeftCorner(6,zDim);
        BtPB_R.topLeftCorner(zDim,zDim) = B.topLeftCorner(6,zDim).transpose() * PB.topLeftCorner(6,zDim) + _R.topLeftCorner(zDim,zDim);
        _K.topLeftCorner(6,zDim) = PB.topLeftCorner(6,zDim)* BtPB_R.topLeftCorner(zDim,zDim).inverse();
    }
}

void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim)
{
    Eigen::Matrix<double,6,6> Ainv = A.inverse();
    Eigen::Matrix<double,6,6> ABRB;
    if (zDim == 6)
    {
        ABRB = Ainv * B * _R.llt().solve(B.transpose());
    }
    else {
        ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose());
    }
    Eigen::Matrix<double,2*6,2*6> Z;
    Z.block(0,0,6,6) = Ainv;
    Z.block(0,6,6,6) = ABRB;
    Z.block(6,0,6,6) = _Q * Ainv;
    Z.block(6,6,6,6) = A.transpose() + _Q * ABRB;

    Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces;
    ces.compute(Z);

    Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues();
    Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors();

    Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec;

    int ctr = 0;
    for (int i = 0; i < 2*6; i++) {
        if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
            unstableEigVec.col(ctr) = eigVec.col(i);
            ctr++;
            if (ctr > 6)
                break;
        }
    }

    Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse();
    Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv;

    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            P(i,j) = PP(i,j).real();
        }
    }
}

/* static */ bool ocraWbiConversions::wbiToOcraCoMJacobian(const Eigen::MatrixXd &jac, Eigen::Matrix<double,3,Eigen::Dynamic> &J)
    {

        if(DIM_T != jac.rows() || jac.cols() != J.cols())
        {
            std::cout<<"ERROR: Input and output matrices dimensions should be the same" <<std::endl;
            return false;
        }
        Eigen::MatrixXd jac3;
        Eigen::Matrix3d jac1,jac2;
        jac3.resize(3,jac.cols()-6);

        jac1 = jac.topLeftCorner(3,3);
        jac2 = jac.block<3,3>(0,3);
        jac3 = jac.topRightCorner(3,jac.cols()-6);
        J.topLeftCorner(3,3) = jac2;
        J.block<3,3>(0,3) = jac1;
        J.topRightCorner(3,jac.cols()-6) = jac3;


        return true;
    }

void run ()
{
  auto input_header = Header::open (argument[0]);
  Header output_header (input_header);
  output_header.datatype() = DataType::from_command_line (DataType::from<float> ());

  // Linear
  transform_type linear_transform;
  bool linear = false;
  auto opt = get_options ("linear");
  if (opt.size()) {
    linear = true;
    linear_transform = load_transform (opt[0][0]);
  } else {
    linear_transform.setIdentity();
  }

  // Replace
  const bool replace = get_options ("replace").size();
  if (replace && !linear) {
    INFO ("no linear is supplied so replace with the default (identity) transform");
    linear = true;
  }

  // Template
  opt = get_options ("template");
  Header template_header;
  if (opt.size()) {
    if (replace)
      throw Exception ("you cannot use the -replace option with the -template option");
    template_header = Header::open (opt[0][0]);
    for (size_t i = 0; i < 3; ++i) {
      output_header.size(i) = template_header.size(i);
      output_header.spacing(i) = template_header.spacing(i);
    }
    output_header.transform() = template_header.transform();
    add_line (output_header.keyval()["comments"], std::string ("regridded to template image \"" + template_header.name() + "\""));
  }

  // Warp 5D warp
  // TODO add reference to warp format documentation
  opt = get_options ("warp_full");
  Image<default_type> warp;
  if (opt.size()) {
    warp = Image<default_type>::open (opt[0][0]).with_direct_io();
    if (warp.ndim() != 5)
      throw Exception ("the input -warp_full image must be a 5D file.");
    if (warp.size(3) != 3)
      throw Exception ("the input -warp_full image must have 3 volumes (x,y,z) in the 4th dimension.");
    if (warp.size(4) != 4)
      throw Exception ("the input -warp_full image must have 4 volumes in the 5th dimension.");
    if (linear)
      throw Exception ("the -warp_full option cannot be applied in combination with -linear since the "
                       "linear transform is already included in the warp header");
  }

  // Warp from image1 or image2
  int from = 1;
  opt = get_options ("from");
  if (opt.size()) {
    from = opt[0][0];
    if (!warp.valid())
      WARN ("-from option ignored since no 5D warp was input");
  }

  // Warp deformation field
  opt = get_options ("warp");
  if (opt.size()) {
    if (warp.valid())
      throw Exception ("only one warp field can be input with either -warp or -warp_mid");
    warp = Image<default_type>::open (opt[0][0]).with_direct_io (Stride::contiguous_along_axis(3));
    if (warp.ndim() != 4)
      throw Exception ("the input -warp file must be a 4D deformation field");
    if (warp.size(3) != 3)
      throw Exception ("the input -warp file must have 3 volumes in the 4th dimension (x,y,z positions)");
  }

  // Inverse
  const bool inverse = get_options ("inverse").size();
  if (inverse) {
    if (!(linear || warp.valid()))
      throw Exception ("no linear or warp transformation provided for option '-inverse'");
    if (warp.valid())
      if (warp.ndim() == 4)
        throw Exception ("cannot apply -inverse with the input -warp_df deformation field.");
    linear_transform = linear_transform.inverse();
  }

  // Half
  const bool half = get_options ("half").size();
  if (half) {
    if (!(linear))
      throw Exception ("no linear transformation provided for option '-half'");
    {
      Eigen::Matrix<default_type, 4, 4> temp;
      temp.row(3) << 0, 0, 0, 1.0;
      temp.topLeftCorner(3,4) = linear_transform.matrix().topLeftCorner(3,4);
      linear_transform.matrix() = temp.sqrt().topLeftCorner(3,4);
    }
  }
//.........这里部分代码省略.........

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

    if(so3)
    {
        for(int x = 0; x < 3; x++)
        {
            for(int y = 0; y < 3; y++)
            {
                resultRt(x, y) = resultR(x, y);
            }
        }
    }

    for(int i = NUM_PYRS - 1; i >= 0; i--)
    {
        if(rgb)
        {
            projectToPointCloud(lastDepth[i], pointClouds[i], intr, i);
        }

        Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Zero();

        K(0, 0) = intr(i).fx;
        K(1, 1) = intr(i).fy;
        K(0, 2) = intr(i).cx;
        K(1, 2) = intr(i).cy;
        K(2, 2) = 1;

        lastRGBError = std::numeric_limits<float>::max();

        for(int j = 0; j < iterations[i]; j++)
        {
            Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Rt = resultRt.inverse();

            Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R = Rt.topLeftCorner(3, 3);

            Eigen::Matrix<double, 3, 3, Eigen::RowMajor> KRK_inv = K * R * K.inverse();
            mat33 krkInv;
            memcpy(&krkInv.data[0], KRK_inv.cast<float>().eval().data(), sizeof(mat33));

            Eigen::Vector3d Kt = Rt.topRightCorner(3, 1);
            Kt = K * Kt;
            float3 kt = {(float)Kt(0), (float)Kt(1), (float)Kt(2)};

            int sigma = 0;
            int rgbSize = 0;

            if(rgb)
            {
                TICK("computeRgbResidual");
                computeRgbResidual(pow(minimumGradientMagnitudes[i], 2.0) / pow(sobelScale, 2.0),
                                   nextdIdx[i],
                                   nextdIdy[i],
                                   lastDepth[i],
                                   nextDepth[i],
                                   lastImage[i],
                                   nextImage[i],
                                   corresImg[i],
                                   sumResidualRGB,
                                   maxDepthDeltaRGB,
                                   kt,
                                   krkInv,
                                   sigma,
                                   rgbSize,
                                   GPUConfig::getInstance().rgbResThreads,
                                   GPUConfig::getInstance().rgbResBlocks);
                TOCK("computeRgbResidual");