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

/* static */ bool ocraWbiConversions::wbiToOcraSegJacobian(const Eigen::MatrixXd &jac, Eigen::MatrixXd &J)
    {
        int dof = DIM_T + DIM_R;
        if(dof != jac.rows() || dof != J.rows()||jac.cols() != J.cols())
        {
            std::cout<<"ERROR: Input and output matrices dimensions should be the same" <<std::endl;
            return false;
        }

        // FOR FULL n+6 Jacobian ONLY
        Eigen::MatrixXd jac5,jac6;
        Eigen::Matrix3d jac1,jac2,jac3,jac4;
        jac5.resize(3,jac.cols()-6);
        jac6.resize(3,jac.cols()-6);


        jac1 = jac.topLeftCorner(3,3);
        jac2 = jac.block<3,3>(0,3);
        jac3 = jac.bottomLeftCorner(3,3);
        jac4 = jac.block<3,3>(3,3);
        jac5 = jac.topRightCorner(3,jac.cols()-6);
        jac6 = jac.bottomRightCorner(3,jac.cols()-6);

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

        return true;
    }

bool KinematicsMetrics::getManipulability(const robot_state::RobotState &state,
                                          const robot_model::JointModelGroup *joint_model_group,
                                          double &manipulability,
                                          bool translation) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }
  // Get joint limits penalty
  double penalty = getJointLimitsPenalty(state, joint_model_group);
  if (translation)
  {
    Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
    Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3,jacobian.cols()));
    Eigen::MatrixXd singular_values = svdsolver.singularValues();
    for (int i = 0; i < singular_values.rows(); ++i)
      logDebug("moveit.kin_metrics: Singular value: %d %f",i,singular_values(i,0));
    manipulability = penalty * singular_values.minCoeff()/singular_values.maxCoeff();
  }
  else
  {
    Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
    Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
    Eigen::MatrixXd singular_values = svdsolver.singularValues();
    for(int i=0; i < singular_values.rows(); ++i)
      logDebug("moveit.kin_metrics: Singular value: %d %f",i,singular_values(i,0));
    manipulability = penalty * singular_values.minCoeff()/singular_values.maxCoeff();
  }
  return true;
}

bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState &state,
                                               const robot_model::JointModelGroup *joint_model_group,
                                               double &manipulability_index,
                                               bool translation) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }

  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
  // Get joint limits penalty
  double penalty = getJointLimitsPenalty(state, joint_model_group);
  if (translation)
  {
    Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3,jacobian.cols());
    Eigen::MatrixXd matrix = jacobian_2*jacobian_2.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  else
  {
    Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  return true;
}

int main(int argc, char* argv[]) {
  shared_ptr<lcm::LCM> lcm(new lcm::LCM);

  if (!lcm->good()) return 1;

  auto quad = std::make_shared<Quadrotor>();

  const int num_states = getNumStates(*quad);
  const int num_positions = num_states / 2;
  const int num_inputs = getNumInputs(*quad);
  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(num_states, num_states);
  Q.topLeftCorner(num_positions, num_positions) =
      10.0 * Eigen::MatrixXd::Identity(num_positions, num_positions);
  Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(num_inputs, num_inputs);
  QuadrotorState<double> xG;
  xG.z = 1;
  QuadrotorInput<double> uG;
  uG.w1 = quad->m * quad->g * 0.25;
  uG.w2 = uG.w1;
  uG.w3 = uG.w1;
  uG.w4 = uG.w1;
  auto c = MakeTimeInvariantLqrSystem(*quad, xG, uG, Q, R);
  auto v = std::make_shared<BotVisualizer<QuadrotorState> >(
      lcm, GetDrakePath() + "/examples/Quadrotor/quadrotor.urdf",
      drake::multibody::joints::kRollPitchYaw);

  auto sys = cascade(feedback(quad, c), v);

  SimulationOptions options;
  options.realtime_factor = 1.0;
  options.initial_step_size = 0.005;
  if (commandLineOptionExists(argv, argv + argc, "--non-realtime")) {
    options.warn_real_time_violation = true;
  }

  for (int i = 0; i < 5; i++) {
    Eigen::Matrix<double, 12, 1> x0 = toEigen(xG);
    x0 += toEigen(getRandomVector<QuadrotorState>());
    simulate(*sys, 0, 10, x0, options);
  }

  // todo: change this back to runLCM instead of just simulate
}

void MagCal::calcMagComp()
{
    /*
     * Inspired by
     * http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
     *
     * Ellipsoid fit from:
     * http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
     *
     * To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
     */

    if (mMagSamples.size() < 9) {
        QMessageBox::warning(this, "Magnetometer compensation",
                             "Too few points.");
        return;
    }

    int samples = mMagSamples.size();
    Eigen::VectorXd ex(samples);
    Eigen::VectorXd ey(samples);
    Eigen::VectorXd ez(samples);

    for (int i = 0;i < samples;i++) {
        ex(i) = mMagSamples.at(i).at(0);
        ey(i) = mMagSamples.at(i).at(1);
        ez(i) = mMagSamples.at(i).at(2);
    }

    Eigen::MatrixXd eD(samples, 9);

    for (int i = 0;i < samples;i++) {
        eD(i, 0) = ex(i) * ex(i);
        eD(i, 1) = ey(i) * ey(i);
        eD(i, 2) = ez(i) * ez(i);
        eD(i, 3) = 2.0 * ex(i) * ey(i);
        eD(i, 4) = 2.0 * ex(i) * ez(i);
        eD(i, 5) = 2.0 * ey(i) * ez(i);
        eD(i, 6) = 2.0 * ex(i);
        eD(i, 7) = 2.0 * ey(i);
        eD(i, 8) = 2.0 * ez(i);
    }

    Eigen::MatrixXd etmp1 = eD.transpose() * eD;
    Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
    Eigen::VectorXd eV = etmp1.lu().solve(etmp2);

    Eigen::MatrixXd eA(4, 4);
    eA(0,0)=eV(0);   eA(0,1)=eV(3);   eA(0,2)=eV(4);   eA(0,3)=eV(6);
    eA(1,0)=eV(3);   eA(1,1)=eV(1);   eA(1,2)=eV(5);   eA(1,3)=eV(7);
    eA(2,0)=eV(4);   eA(2,1)=eV(5);   eA(2,2)=eV(2);   eA(2,3)=eV(8);
    eA(3,0)=eV(6);   eA(3,1)=eV(7);   eA(3,2)=eV(8);   eA(3,3)=-1.0;

    Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
    Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
    eT(3, 0) = eCenter(0);
    eT(3, 1) = eCenter(1);
    eT(3, 2) = eCenter(2);

    Eigen::MatrixXd eR = eT * eA * eT.transpose();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
    Eigen::MatrixXd eVecs = eEv.eigenvectors();
    Eigen::MatrixXd eVals = eEv.eigenvalues();

    Eigen::MatrixXd eRadii(3, 1);
    eRadii(0) = sqrt(1.0 / eVals(0));
    eRadii(1) = sqrt(1.0 / eVals(1));
    eRadii(2) = sqrt(1.0 / eVals(2));

    Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
    Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();

    mMagComp.resize(9);
    mMagComp[0] = eComp(0, 0);
    mMagComp[1] = eComp(0, 1);
    mMagComp[2] = eComp(0, 2);

    mMagComp[3] = eComp(1, 0);
    mMagComp[4] = eComp(1, 1);
    mMagComp[5] = eComp(1, 2);

    mMagComp[6] = eComp(2, 0);
    mMagComp[7] = eComp(2, 1);
    mMagComp[8] = eComp(2, 2);

    mMagCompCenter.resize(3);
    mMagCompCenter[0] = eCenter(0, 0);
    mMagCompCenter[1] = eCenter(1, 0);
    mMagCompCenter[2] = eCenter(2, 0);

    QVector<double> magX, magY, magZ;

    for (int i = 0;i < mMagSamples.size();i++) {
        double mx = mMagSamples.at(i).at(0);
        double my = mMagSamples.at(i).at(1);
        double mz = mMagSamples.at(i).at(2);

        mx -= mMagCompCenter.at(0);
        my -= mMagCompCenter.at(1);
//.........这里部分代码省略.........

static void gradCov(omxFitFunction *oo, FitContext *fc)
{
	const double Scale = Global->llScale;
	omxExpectation *expectation = oo->expectation;
	BA81FitState *state = (BA81FitState*) oo->argStruct;
	BA81Expect *estate = (BA81Expect*) expectation->argStruct;
	if (estate->verbose >= 1) mxLog("%s: cross product approximation", oo->name());

	estate->grp.ba81OutcomeProb(estate->itemParam->data, FALSE);

	const int numThreads = Global->numThreads;
	const int numUnique = estate->getNumUnique();
	ba81NormalQuad &quad = estate->getQuad();
	const int numSpecific = quad.numSpecific;
	const int maxDims = quad.maxDims;
	const int pDims = numSpecific? maxDims-1 : maxDims;
	const int maxAbilities = quad.maxAbilities;
	Eigen::MatrixXd icovMat(pDims, pDims);
	if (maxAbilities) {
		Eigen::VectorXd mean;
		Eigen::MatrixXd srcMat;
		estate->getLatentDistribution(fc, mean, srcMat);
		icovMat = srcMat.topLeftCorner(pDims, pDims);
		Matrix tmp(icovMat.data(), pDims, pDims);
		int info = InvertSymmetricPosDef(tmp, 'U');
		if (info) {
			omxRaiseErrorf("%s: latent covariance matrix is not positive definite", oo->name());
			return;
		}
		icovMat.triangularView<Eigen::Lower>() = icovMat.transpose().triangularView<Eigen::Lower>();
	}
	std::vector<int> &rowMap = estate->grp.rowMap;
	double *rowWeight = estate->grp.rowWeight;
	std::vector<bool> &rowSkip = estate->grp.rowSkip;
	const int totalQuadPoints = quad.totalQuadPoints;
	omxMatrix *itemParam = estate->itemParam;
	omxBuffer<double> patternLik(numUnique);

	const int priDerivCoef = pDims + triangleLoc1(pDims);
	const int numLatents = maxAbilities + triangleLoc1(maxAbilities);
	const int thrDerivSize = itemParam->cols * state->itemDerivPadSize;
	const int totalOutcomes = estate->totalOutcomes();
	const int numItems = state->freeItemParams? estate->numItems() : 0;
	const size_t numParam = fc->varGroup->vars.size();
	std::vector<double> thrGrad(numThreads * numParam);
	std::vector<double> thrMeat(numThreads * numParam * numParam);
	const double *wherePrep = quad.wherePrep.data();

	if (numSpecific == 0) {
		omxBuffer<double> thrLxk(totalQuadPoints * numThreads);
		omxBuffer<double> derivCoef(totalQuadPoints * priDerivCoef);

		if (state->freeLatents) {
#pragma omp parallel for num_threads(numThreads)
			for (int qx=0; qx < totalQuadPoints; qx++) {
				const double *where = wherePrep + qx * maxDims;
				calcDerivCoef(fc, state, estate, icovMat.data(), where,
					      derivCoef.data() + qx * priDerivCoef);
			}
		}

#pragma omp parallel for num_threads(numThreads)
		for (int px=0; px < numUnique; px++) {
			if (rowSkip[px]) continue;
			int thrId = omx_absolute_thread_num();
			double *lxk = thrLxk.data() + thrId * totalQuadPoints;
			omxBuffer<double> expected(totalOutcomes); // can use maxOutcomes instead TODO
			std::vector<double> deriv0(thrDerivSize);
			std::vector<double> latentGrad(numLatents);
			std::vector<double> patGrad(numParam);
			double *grad = thrGrad.data() + thrId * numParam;
			double *meat = thrMeat.data() + thrId * numParam * numParam;
			estate->grp.ba81LikelihoodSlow2(px, lxk);

			// If patternLik is already valid, maybe could avoid this loop TODO
			double patternLik1 = 0;
			for (int qx=0; qx < totalQuadPoints; qx++) {
				patternLik1 += lxk[qx];
			}
			patternLik[px] = patternLik1;

			// if (!validPatternLik(state, patternLik1))  complain, TODO

			for (int qx=0; qx < totalQuadPoints; qx++) {
				double tmp = lxk[qx];
				mapLatentDeriv(state, estate, tmp, derivCoef.data() + qx * priDerivCoef,
					       latentGrad.data());

				for (int ix=0; ix < numItems; ++ix) {
					int pick = estate->grp.dataColumns[ix][rowMap[px]];
					if (pick == NA_INTEGER) continue;
					OMXZERO(expected.data(), estate->itemOutcomes(ix));
					expected[pick-1] = tmp;
					const double *spec = estate->itemSpec(ix);
					double *iparam = omxMatrixColumn(itemParam, ix);
					const int id = spec[RPF_ISpecID];
					double *myDeriv = deriv0.data() + ix * state->itemDerivPadSize;
					(*Glibrpf_model[id].dLL1)(spec, iparam, wherePrep + qx * maxDims,
							      expected.data(), myDeriv);
				}
//.........这里部分代码省略.........

TEST(SparseMatrixFunctionTests, testSchurComplement1)
{
  try {
    using namespace aslam::backend;
    typedef sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> SparseBlockMatrix;
    // Create the sparse Hessian. Two dense blocks. Three sparse.
    int structure[5] = {2, 2, 3, 3, 3};
    std::partial_sum(structure, structure + 5, structure);
    int marginalizedStartingBlock = 2;
    int marginalizedStartingIndex = structure[ marginalizedStartingBlock - 1 ];
    double lambda = 1;
    SparseBlockMatrix H(structure, structure, 5, 5, true);
    Eigen::VectorXd e(H.rows());
    e.setRandom();
    Eigen::VectorXd b(H.rowBaseOfBlock(marginalizedStartingBlock));
    b.setZero();
    boost::shared_ptr<SparseBlockMatrix> A(H.slice(0, marginalizedStartingBlock, 0, marginalizedStartingBlock, true));
    ASSERT_EQ(marginalizedStartingBlock, A->bRows());
    ASSERT_EQ(marginalizedStartingBlock, A->bCols());
    A->clear(false);
    std::vector<Eigen::MatrixXd> invVi;
    invVi.resize(H.bRows() - marginalizedStartingBlock);
    // Fill in H.
    *H.block(0, 0, true) = sm::eigen::randomCovariance<2>() * 100;
    *H.block(1, 1, true) = sm::eigen::randomCovariance<2>() * 100;
    *H.block(2, 2, true) = sm::eigen::randomCovariance<3>() * 100;
    *H.block(3, 3, true) = sm::eigen::randomCovariance<3>() * 100;
    *H.block(4, 4, true) = sm::eigen::randomCovariance<3>() * 100;
    // Start with two off diagonals.
    H.block(0, 4, true)->setRandom();
    H.block(0, 4, true)->array() *= 100;
    H.block(1, 4, true)->setRandom();
    H.block(1, 4, true)->array() *= 100;
    //std::cout << "H:\n" << H << std::endl;
    applySchurComplement(H,
                         e,
                         lambda,
                         marginalizedStartingBlock,
                         true,
                         *A,
                         invVi,
                         b);
    Eigen::MatrixXd Hd = H.toDense();
    Eigen::MatrixXd U = Hd.topLeftCorner(marginalizedStartingIndex, marginalizedStartingIndex);
    Eigen::MatrixXd V = Hd.bottomRightCorner(H.rows() - marginalizedStartingIndex, H.rows() - marginalizedStartingIndex);
    Eigen::MatrixXd W = Hd.topRightCorner(marginalizedStartingIndex, H.rows() - marginalizedStartingIndex);
    V.diagonal().array() += lambda;
    Eigen::MatrixXd AA = U - W * V.inverse() * W.transpose();
    AA.diagonal().array() += lambda;
    Eigen::VectorXd epsSparse = e.tail(e.size() - marginalizedStartingIndex);
    Eigen::VectorXd epsDense = e.head(marginalizedStartingIndex);
    Eigen::VectorXd bb = epsDense - W * V.inverse() * epsSparse;
    {
      SCOPED_TRACE("");
      Eigen::MatrixXd Asa = A->toDense().selfadjointView<Eigen::Upper>();
      sm::eigen::assertNear(Asa, AA, 1e-12, SM_SOURCE_FILE_POS, "Testing the lhs schur complement");
    }
    {
      SCOPED_TRACE("");
      sm::eigen::assertNear(b, bb, 1e-12, SM_SOURCE_FILE_POS, "Testing the rhs schur complement");
    }
    // Let's try it again to make sure stuff gets initialized correctly.
    applySchurComplement(H,
                         e,
                         lambda,
                         marginalizedStartingBlock,
                         true,
                         *A,
                         invVi,
                         b);
    {
      SCOPED_TRACE("");
      Eigen::MatrixXd Asa = A->toDense().selfadjointView<Eigen::Upper>();
      sm::eigen::assertNear(Asa, AA, 1e-12, SM_SOURCE_FILE_POS, "Testing the lhs schur complement");
    }
    {
      SCOPED_TRACE("");
      sm::eigen::assertNear(b, bb, 1e-12, SM_SOURCE_FILE_POS, "Testing the rhs schur complement");
    }
    // Now we check the update function.
    Eigen::VectorXd dx(marginalizedStartingIndex);
    dx.setRandom();
    Eigen::VectorXd denseDs = V.inverse() * (epsSparse - W.transpose() * dx);
    for (int i = 0; i < H.bRows() - marginalizedStartingBlock; ++i) {
      Eigen::VectorXd outDsi;
      buildDsi(i, H, e, marginalizedStartingBlock, invVi[i], dx, outDsi);
      Eigen::VectorXd dsi = denseDs.segment(H.rowBaseOfBlock(i + marginalizedStartingBlock) - marginalizedStartingIndex, H.rowsOfBlock(i + marginalizedStartingBlock));
      sm::eigen::assertNear(outDsi, dsi, 1e-12, SM_SOURCE_FILE_POS, "Checking the update step calculation");
    }
  } catch (const std::exception& e) {
    FAIL() << "Exception: " << e.what();
  }
}