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

void RigidObject::computeBoundingBox() {
  _bounding_box.resize(8 * 3);
  Eigen::Map<const Eigen::MatrixXf> vertices_f(getPositions().data(), 3,
                                               getNPositions());
  Eigen::MatrixXd vertices;
  vertices = vertices_f.cast<double>();

  // subtract vertices mean
  Eigen::Vector3d mean_vertices = vertices.rowwise().mean();
  vertices = vertices - mean_vertices.replicate(1, getNPositions());

  // compute eigenvector covariance matrix
  Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(vertices *
                                                   vertices.transpose());
  Eigen::MatrixXd real_eigenvectors = eigen_solver.eigenvectors().real();

  // rotate centered vertices with inverse eigenvector matrix
  vertices = real_eigenvectors.transpose() * vertices;

  // compute simple bounding box
  auto mn = vertices.rowwise().minCoeff();
  auto mx = vertices.rowwise().maxCoeff();
  Eigen::Matrix<double, 3, 8> bounding_box;
  bounding_box << mn(0), mn(0), mn(0), mn(0), mx(0), mx(0), mx(0), mx(0), mn(1),
      mn(1), mx(1), mx(1), mn(1), mn(1), mx(1), mx(1), mn(2), mx(2), mn(2),
      mx(2), mn(2), mx(2), mn(2), mx(2);

  // rotate and translate bounding box back to original position
  Eigen::Matrix3d rot_back = real_eigenvectors;
  Eigen::Translation<double, 3> tra_back(mean_vertices);
  Eigen::Transform<double, 3, Eigen::Affine> t = tra_back * rot_back;
  bounding_box = t * bounding_box;

  // convert to float
  Eigen::Map<Eigen::MatrixXf> bounding_box_f(_bounding_box.data(), 3, 8);

  bounding_box_f = bounding_box.cast<float>();
}

Eigen::VectorXd ExperimentalTrajectory::getVariance(double time)
{
    if (varianceStartTrigger) {
        t0_variance = time;
        varianceStartTrigger = false;
    }


    double t = time - t0_variance;


    Eigen::VectorXd variance;

    if (t<=pointToPointDurationVector.sum()) {
        Eigen::MatrixXd Ks = kernelFunction(t);
        variance = maxCovariance - (Ks * designMatrixInv * Ks.transpose()).diagonal();
    }
    else{
        variance = Eigen::VectorXd::Zero(maxCovariance.rows());
    }

    return variance;
}

void
NurbsTools::pca (const vector_vec3d &data, Eigen::Vector3d &mean, Eigen::Matrix3d &eigenvectors,
                 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
  {
    printf ("[NurbsTools::pca] Error, data is empty\n");
    abort ();
  }

  mean = computeMean (data);

  unsigned s = unsigned (data.size ());

  Eigen::MatrixXd Q (3, s);

  for (unsigned i = 0; i < s; i++)
    Q.col (i) << (data[i] - mean);

  Eigen::Matrix3d C = Q * Q.transpose ();

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (C);
  if (eigensolver.info () != Success)
  {
    printf ("[NurbsTools::pca] Can not find eigenvalues.\n");
    abort ();
  }

  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

void ExperimentalTrajectory::calculateRbfnWeights()
{
    int nC = kernelCenters.rows();
    Eigen::MatrixXd rbfnDesignMatrix = rbfnKernelFunction(kernelCenters);
    // rbfnWeights = Eigen::MatrixXd::Zero(nDoF, nC);
    Eigen::MatrixXd wayTrans = waypoints.transpose();

    // std::cout << "phi = " << rbfnDesignMatrix.rows() << " x " << rbfnDesignMatrix.cols() << std::endl;
    // std::cout << "way = " << wayTrans.rows()<< " x " << wayTrans.cols() << std::endl;
    Eigen::MatrixXd A = rbfnDesignMatrix * rbfnDesignMatrix.transpose();
    // std::cout << "A = " << A.rows()<< " x " << A.cols() << std::endl;

    Eigen::MatrixXd b = rbfnDesignMatrix * wayTrans;

    // std::cout << "b = " << b.rows()<< " x " << b.cols() << std::endl;


    // rbfnWeights = (A.inverse() * b).transpose(); // the transpose makes weights = nDoF x nCenters which is better for the output function.

    // rbfnWeights = A.fullPivLu().solve(b).transpose();
    rbfnWeights = rbfnDesignMatrix.fullPivLu().solve(wayTrans).transpose();
    // std::cout << "rbfn weights:\n" << rbfnWeights << std::endl;

}

	void object::test<6>()
	{
		for (int i = 0;i<10;i++)
		{
			int dim = rand()%1000+2;
			int samplenum1 = rand()%1000+100;
			int samplenum2 = rand()%1000+100;

			Eigen::MatrixXd ha = Eigen::MatrixXd::Random(dim,samplenum1);
			Eigen::MatrixXd hb = Eigen::MatrixXd::Random(dim,samplenum2);



			Eigen::VectorXd haa = (ha.array()*ha.array()).colwise().sum();
			Eigen::VectorXd hbb = (hb.array()*hb.array()).colwise().sum();

			Eigen::MatrixXd hdist = -2*ha.transpose()*hb;

			hdist.colwise() += haa;

			hdist.rowwise() += hbb.transpose();

			
			Matrix<double> ga(ha),gb(hb);

			Matrix<double> gdist = -2*ga.transpose()*gb;

			Vector<double> gaa = (ga.array()*ga.array()).colwise().sum();
			Vector<double> gbb = (gb.array()*gb.array()).colwise().sum();

			gdist.colwise() += gaa;
			gdist.rowwise() += gbb;

			ensure(check_diff(hdist,gdist));
		}
	}

void cIKSolver::StepHybrid(const Eigen::MatrixXd& cons_desc, const tProblem& prob, Eigen::MatrixXd& joint_desc,
							Eigen::VectorXd& out_pose)
{
	const int num_dof = cKinTree::GetNumDof(joint_desc);
	const int num_joints = static_cast<int>(joint_desc.rows());
	const int num_cons = static_cast<int>(cons_desc.rows());

	Eigen::VectorXd err;
	Eigen::MatrixXd J;
	Eigen::MatrixXd J_weighted_buff = Eigen::MatrixXd::Zero(num_dof, num_dof);
	Eigen::VectorXd Jt_err_weighted_buff = Eigen::VectorXd::Zero(num_dof);
	Eigen::MatrixXd N = Eigen::MatrixXd::Identity(num_dof, num_dof);
	Eigen::VectorXi chain_joints(num_joints); // keeps track of joints in Ik chain

	double clamp_dist = prob.mClampDist;
	double damp = prob.mDamp;

	int min_priority = std::numeric_limits<int>::max();
	int max_priority = std::numeric_limits<int>::min();

	for (int c = 0; c < num_cons; ++c)
	{
		int curr_priority = static_cast<int>(cons_desc(c, eConsDescPriority));
		min_priority = std::min(min_priority, curr_priority);
		max_priority = std::max(max_priority, curr_priority);
	}

	for (int p = min_priority; p <= max_priority; ++p)
	{
		int curr_num_dof = static_cast<int>(N.cols());
		auto J_weighted = J_weighted_buff.block(0, 0, curr_num_dof, curr_num_dof);
		auto Jt_err_weighted = Jt_err_weighted_buff.block(0, 0, curr_num_dof, 1);

		J_weighted.setZero();
		Jt_err_weighted.setZero();

		chain_joints.setZero();
		
		int num_valid_cons = 0;
		for (int c = 0; c < num_cons; ++c)
		{
			const tConsDesc& curr_cons = cons_desc.row(c);
			int curr_priority = static_cast<int>(curr_cons(eConsDescPriority));

			if (curr_priority == p)
			{
				++num_valid_cons;
				err = BuildErr(joint_desc, out_pose, curr_cons, clamp_dist);
				J = BuildJacob(joint_desc, out_pose, curr_cons);

#if !defined(DISABLE_LINK_SCALE)
				for (int i = 0; i < num_joints; ++i)
				{
					// use entries in the jacobian to figure out if a joint is on the
					// link chain from the root to the constrained end effectors
					// this ignores the root which should not have any scaling
					int scale_idx = gPosDims + num_joints + i;
					int theta_idx = gPosDims + i;
					double scaling = J.col(scale_idx).squaredNorm();
					if (scaling > 0)
					{
						chain_joints(i) = 1;
					}
				}
#endif
				J = J * N;

				double weight = curr_cons(eConsDescWeight);
				J_weighted += weight * J.transpose() * J;
				Jt_err_weighted += weight * J.transpose() * err;
			}
		}

		if (num_valid_cons > 0)
		{
			// apply damping
			// a little more tricky with the null space

			auto N_block = N.block(0, 0, gPosDims + num_joints, N.cols());
			J_weighted += damp * N.transpose() * N;

#if !defined(DISABLE_LINK_SCALE)
			// damp link scaling according to stiffness
			for (int i = 0; i < num_joints; ++i)
			{
				// only scale links that are part of the IK chain
				if (chain_joints(i) == 1)
				{
					int idx = gPosDims + num_joints + i;
					const Eigen::VectorXd& N_row = N.row(idx);

					double d_scale = 1.f - joint_desc(i, cKinTree::eJointDescScale);
					double link_stiffness = joint_desc(i, cKinTree::eJointDescLinkStiffness);
					J_weighted += link_stiffness * N_row * N_row.transpose();
					Jt_err_weighted += link_stiffness * d_scale * N_row;
				}
			}
#endif

			Eigen::VectorXd y = J_weighted.lu().solve(Jt_err_weighted);
//.........这里部分代码省略.........

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

		  aslam::backend::DenseQrLinearSystemSolver qrSolver;
          qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false);

		  SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms");
		  SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase);

		  qrSolver.evaluateError(1, useMEstimator);
		  qrSolver.buildSystem(1, useMEstimator);


		  const Eigen::MatrixXd& jacobian = qrSolver.getJacobian();
		  const Eigen::VectorXd& b = qrSolver.e();

		  // check dimension of jacobian
		  int jrows = jacobian.rows();
		  int jcols = jacobian.cols();

		  int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove;
		  //int dimOfPriorErrorTerm = jrows;

		  // check the rank
		  Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian);
		  //lu_decomp.setThreshold(1e-20);
		  double threshold = lu_decomp.threshold();
		  int rank = lu_decomp.rank();
		  int fullRank = std::min(jacobian.rows(), jacobian.cols());
		  SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")");
		  bool rankDeficient = rank < fullRank;
		  if(rankDeficient)
		  {
			  SM_WARN("Marginalization jacobian is rank deficient!");
		  }
		  //SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever...");

		  Eigen::MatrixXd R_reduced;
		  Eigen::VectorXd d_reduced;
		  if (jrows < jcols)
		  {
			  SM_THROW(aslam::Exception, "underdetermined LSE!");
			  // underdetermined LSE, don't do QR
			  R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove);
			  d_reduced = b;

		  } else {
			  // PTF: Do we know what will happen when the jacobian matrix is rank deficient?
			  // MB: yes, bad things!

              // do QR decomposition
			  sm::timing::Timer myTimer("QR Decomposition");
              Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian);
			  Eigen::MatrixXd Q = qr.householderQ();
			  Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
			  Eigen::VectorXd d = Q.transpose()*b;
			  myTimer.stop();

			  if(numTopRowsInCov > 0)
			  {
				sm::timing::Timer myTimer("Covariance computation");
				Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R);
				Eigen::MatrixXd Rinv = lu_decomp.inverse();
				Eigen::MatrixXd covariance = Rinv * Rinv.transpose();
				outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov);
				myTimer.stop();
			  }

//			  size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove;
//			  SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!");

			  // get the top left block
			  SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows.");
			  SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols.");
			  //outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop);

              // cut off the zero rows at the bottom
              R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables);
			  //R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables);

              d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables);
              //d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep);
              //dimOfPriorErrorTerm = dimOfRemainingDesignVariables;
		  }

		  // now create the new error term
		  boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced));

		  outPriorErrorTermPtr.swap(err);

		  // restore initial block indices to prevent side effects
          for (size_t i = 0; i < inDesignVariables.size(); ++i) {
              inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]);
              inDesignVariables[i]->setColumnBase(originalColumnBase[i]);
          }
          int index = 0;
          for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
          {
              (*it)->setRowBase(originalRowBase[index++]);
          }
}

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

      // Handle negative (read: bad) covariances in the measurement. Rather
      // than exclude the measurement or make up a covariance, just take
      // the absolute value.
      if (measurementCovarianceSubset(i, i) < 0)
      {
        if (getDebug())
        {
          *debugStream_ << "WARNING: Negative covariance for index " << i << " of measurement (value is" << measurementCovarianceSubset(i, i)
            << "). Using absolute value...\n";
        }

        measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
      }

      // If the measurement variance for a given variable is very
      // near 0 (as in e-50 or so) and the variance for that
      // variable in the covariance matrix is also near zero, then
      // the Kalman gain computation will blow up. Really, no
      // measurement can be completely without error, so add a small
      // amount in that case.
      if (measurementCovarianceSubset(i, i) < 1e-6)
      {
        measurementCovarianceSubset(i, i) = 1e-6;

        if (getDebug())
        {
          *debugStream_ << "WARNING: measurement had very small error covariance for index " << i << ". Adding some noise to maintain filter stability.\n";
        }
      }
    }

    // The state-to-measurement function, h, will now be a measurement_size x full_state_size
    // matrix, with ones in the (i, i) locations of the values to be updated
    for (size_t i = 0; i < updateSize; ++i)
    {
      stateToMeasurementSubset(i, updateIndices[i]) = 1;
    }

    if (getDebug())
    {
      *debugStream_ << "Current state subset is:\n";
      *debugStream_ << stateSubset << "\n";
      *debugStream_ << "Measurement subset is:\n";
      *debugStream_ << measurementSubset << "\n";
      *debugStream_ << "Measurement covariance subset is:\n";
      *debugStream_ << measurementCovarianceSubset << "\n";
      *debugStream_ << "State-to-measurement subset is:\n";
      *debugStream_ << stateToMeasurementSubset << "\n";
    }

    // (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
    kalmanGainSubset = estimateErrorCovariance_ * stateToMeasurementSubset.transpose()
      * (stateToMeasurementSubset * estimateErrorCovariance_ * stateToMeasurementSubset.transpose() + measurementCovarianceSubset).inverse();

    // (2) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
    innovationSubset = (measurementSubset - stateSubset);

    // Wrap angles in the innovation
    for (size_t i = 0; i < updateSize; ++i)
    {
      if (updateIndices[i] == StateMemberRoll || updateIndices[i] == StateMemberPitch || updateIndices[i] == StateMemberYaw)
      {
        if (innovationSubset(i) < -pi_)
        {
          innovationSubset(i) += tau_;
        }
        else if (innovationSubset(i) > pi_)
        {
          innovationSubset(i) -= tau_;
        }
      }
    }

    state_ = state_ + kalmanGainSubset * innovationSubset;

    // (3) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
    Eigen::MatrixXd gainResidual = identity_ - kalmanGainSubset * stateToMeasurementSubset;
    estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose() +
        kalmanGainSubset * measurementCovarianceSubset * kalmanGainSubset.transpose();

    // Handle wrapping of angles
    wrapStateAngles();

    if (getDebug())
    {
      *debugStream_ << "Kalman gain subset is:\n";
      *debugStream_ << kalmanGainSubset << "\n";
      *debugStream_ << "Innovation:\n";
      *debugStream_ << innovationSubset << "\n\n";
      *debugStream_ << "Corrected full state is:\n";
      *debugStream_ << state_ << "\n";
      *debugStream_ << "Corrected full estimate error covariance is:\n";
      *debugStream_ << estimateErrorCovariance_ << "\n";
      *debugStream_ << "Last measurement time is:\n";
      *debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
      *debugStream_ << "\n---------------------- /Ekf::correct ----------------------\n";
    }
  }

/// Find the plan parameters by computing the Lagrangian coefficients by solving for the 
/// quadratic program
Eigen::Vector3d lagrangian () {

	bool dbg = 0;

	// Create the cost function
	int N = data.size();
	double H_ [N * N], f_ [N];
	for(int i = 0; i < N; i++) {
		for(int j = 0; j < N; j++) 
			H_[i * N + j] = data[i].second * data[j].second * data[i].first.dot(data[j].first);
		f_[i] = -1.0;
		H_[i * (N+1)] += 1e-8;
	}
	QuadProgPP::Matrix <double> H (&(H_[0]), N, N);
	QuadProgPP::Vector <double> f (&(f_[0]), N);
	if(dbg) cout << "H: " << H << endl;
	if(dbg) cout << "f: " << f << endl;

	// Create the inequality constraints (alpha_i >= 0)
	double I_ [N * N], i0_ [N];
	for(int i = 0; i < N; i++) {
		I_[i * (N+1)] = 1.0;
		i0_[i] = 0.0;
	}
	QuadProgPP::Matrix <double> I (&(I_[0]), N, N);
	QuadProgPP::Vector <double> i0 (&(i0_[0]), N);
	if(dbg) cout << "I: " << I << endl;
	if(dbg) cout << "i0: " << i0 << endl;

	// Create the equality constraint ((sum_i alpha_i * y_i) = 0)
	double E_ [N], e0_ [1];
	for(int i = 0; i < N; i++) E_[i] = data[i].second;
	e0_[0] = 0.0;
	QuadProgPP::Matrix <double> E (&(E_[0]), N, 1);
	QuadProgPP::Vector <double> e0 (&(e0_[0]), 1);
	if(dbg) cout << "E: " << E << endl;
	if(dbg) cout << "e0: " << e0 << endl;

	// Solve the problem
	QuadProgPP::Vector <double> x;
	solve_quadprog(H, f, E, e0, I, i0, x);
	if(dbg) cout << "x: " << x << endl;

	// Compute the line direction 
	Eigen::Vector2d w (0, 0);
	Eigen::VectorXd x_ (N), y(N);
	for(int i = 0; i < N; i++) {
		w += x[i] * data[i].second * data[i].first;
		x_(i) = x[i];
		y(i) = data[i].second;
	}
	if(dbg) cout << "w: " << w.transpose() << endl;

	// Compute the line intersection
	int minPos;
	for(int i = 0; i < N; i++) {
		if((x[i] > 0.1) && (data[i].second > 0)) {
			minPos = i;
			break;
		}
	}
	Eigen::MatrixXd X (N,2), G(N,N);
	for(int i = 0; i < N; i++) X.row(i) = data[i].first;
	G = X * X.transpose();
	double b = 1 - (G.row(minPos) * (x_.cwiseProduct(y)));
	if(dbg) printf("b: %lf\n", b);

	return Eigen::Vector3d(w(0), w(1), b);
}

int main() {
	
	/* Set up module test environment */
	
	std::ofstream  ofs ("./output/mt_results.txt", std::ofstream::out);
	
	/* TC_1: Open two random MIT-BIH measurements */
	ofs<<"**************** MT TC 1 ****************"<<std::endl;
	ofs<<"testcase 1: open two random MIT-BIH measurements"<<std::endl;
	
	EcgSigPrep esp_1(MT_MIT_BIH_MEASUREMENT_NAME_1_STR, MT_NUM_OF_LEADS_INT, MT_NUM_OF_SAMPLES_1_INT);
	EcgSigPrep esp_2(MT_MIT_BIH_MEASUREMENT_NAME_2_STR, MT_NUM_OF_LEADS_INT, MT_NUM_OF_SAMPLES_2_INT);
	
	ofs<<"testcase 1: DONE"<<std::endl<<std::endl;
	
	/* TC_2: Print out the first loaded beat from the first recording. Then load the next beat and print that out as well. */
	
	ofs<<"**************** MT TC 2 ****************"<<std::endl;
	ofs<<"testcase 2: Print out the first loaded beat from the first recording"<<std::endl;
	
	Eigen::MatrixXd signal = *(esp_1.getSignal());
	
	ofs<<signal.transpose()<<std::endl;
	
	ofs<<"testcase 2: Load next beat and print it"<<std::endl;
	
	esp_1.getNextSegment();
	signal = *(esp_1.getSignal());
	ofs<<signal.transpose()<<std::endl;
	
	ofs<<"testcase 2: DONE"<<std::endl<<std::endl;	
	
	/* TC_3: Set the dilatation and translation of the second beat. Print out results. */
	
	ofs<<"**************** MT TC 3 ****************"<<std::endl;
	ofs<<"testcase 3: Apply dilatation and translation to signal. Print results."<<std::endl;
	
	Hermite H(signal.cols());
	Eigen::MatrixXd dilatTransSig = signal;
	dilatTransSig = esp_1.setDilatTrans(MT_SAMPLE_DILATATION_DOUBLE, MT_SAMPLE_TRANSLATION_DOUBLE, H.get_ort_fun_roots(), dilatTransSig);
	
	ofs<<dilatTransSig.transpose()<<std::endl;
	
	ofs<<"testcase 3: DONE"<<std::endl<<std::endl;	
	
	/* TC_4: Test getter functions. Print results. */
	
	ofs<<"**************** MT TC 4 ****************"<<std::endl;
	ofs<<"testcase 4: Test getter functions."<<std::endl;
	
	ofs<<"sigPrep::getSignal()"<<std::endl;
	ofs<<esp_1.getSignal()->transpose()<<std::endl<<std::endl;
	
	ofs<<"sigPrep::getEntireSignal()"<<std::endl;
	ofs<<esp_1.getEntireSignal()->transpose()<<std::endl<<std::endl;
	
	ofs<<"sigPrep::getSigFirstVal()"<<std::endl;
	ofs<<esp_1.getSigFirstVal()<<std::endl<<std::endl;
	
	ofs<<"sigPrep::getSigLastVal()"<<std::endl;
	ofs<<esp_1.getSigLastVal()<<std::endl<<std::endl;
	
	ofs<<"sigPrep::getSigMaxVal()"<<std::endl;
	ofs<<esp_1.getSigMaxVal()<<std::endl<<std::endl;
	
	ofs<<"EcgsigPrep::getDilat()"<<std::endl;
	ofs<<esp_1.getDilat()<<std::endl<<std::endl;
	
	ofs<<"EcgsigPrep::getTrans()"<<std::endl;
	ofs<<esp_1.getTrans()<<std::endl<<std::endl;
	
	ofs<<"testcase 4: DONE"<<std::endl<<std::endl;	
	
	/* Clear up module test environment */
	ofs.close();
	
	return 0;
}

Eigen::MatrixXd pseudoInverse( Eigen::MatrixXd &A)
{
    Eigen::MatrixXd B = (A.transpose()*A).inverse()*A.transpose();
    return B;
}

		void KPLSModel::train()
		{	
			if (descriptor_matrix_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!"); 
			}
		// 	if (descriptor_matrix_.cols() < no_components_)
		// 	{
		// 		throw Exception::TooManyPLSComponents(__FILE__, __LINE__, no_components_, descriptor_matrix_.cols());
		// 	}
			
			kernel->calculateKernelMatrix(descriptor_matrix_, K_);

			Eigen::MatrixXd P;  // Matrix P saves all vectors p

			int cols = K_.cols();
			
			// determine the number of components that are to be created.
			// no_components_ contains the number of components desired by the user, 
			// but obviously we cannot calculate more PLS components than features
			int features = descriptor_matrix_.cols();
			unsigned int components_to_create = no_components_;
			if (features < no_components_) components_to_create = features; 

			U_.resize(K_.rows(), components_to_create);
			loadings_.resize(cols, components_to_create);
			weights_.resize(Y_.cols(), components_to_create);
			latent_variables_.resize(K_.rows(), components_to_create);
			P.resize(cols, components_to_create);
			
			Eigen::VectorXd w;
			Eigen::VectorXd t;
			Eigen::VectorXd c;
			Eigen::VectorXd u = Y_.col(0);

			Eigen::VectorXd u_old;
			
			for (unsigned int j = 0; j < components_to_create; j++)
			{
				for (int i = 0; i < 10000 ; i++)
				{	
					w = K_.transpose()*u / Statistics::scalarProduct(u);
					w = w / Statistics::euclNorm(w);
					t = K_*w;
					c = Y_.transpose()*t / Statistics::scalarProduct(t);
					u_old = u;
					u = Y_*c / Statistics::scalarProduct(c); 
				
					if (Statistics::euclDistance(u, u_old)/Statistics::euclNorm(u) > 0.0000001) 
					{ 
						continue;				
					}
					else  // if u has converged
					{
						break;
					}
				}

				Eigen::VectorXd p = K_.transpose() * t / Statistics::scalarProduct(t);
				K_ -= t * p.transpose();

				U_.col(j) = u;
				loadings_.col(j) = w;
				weights_.col(j) = c;
				P.col(j) = p;
				latent_variables_.col(j) = t;
			}

		// 	try // p's are not orthogonal to each other, so that in rare cases P.t()*loadings_ is not invertible
		// 	{
		// 		loadings_ = loadings_*(P.t()*loadings_).i();
		// 	}
		// 	catch(BALL::Exception::MatrixIsSingular e)
		// 	{
		// 		Eigen::MatrixXd I; I.setToIdentity(P.cols());
		// 		I *= 0.001;
		// 		loadings_ = loadings_*(P.t()*loadings_+I).i();
		// 	}

			Eigen::MatrixXd m = P.transpose()*loadings_;
			training_result_ = loadings_*m.colPivHouseholderQr().solve(weights_.transpose());

			calculateOffsets();
		}

// Get the total hamiltonian matrix element between phi1 and phi2
double hamiltonianElement(int N, BasisFunction& phi1, BasisFunction& phi2,
						  Eigen::MatrixXd& R, Eigen::MatrixXd& L2, Eigen::MatrixXd& M,
						  std::vector<double> q, Eigen::MatrixXd& Smat, int s_i, int s_j)
{

	// Construct the double-primed matrices
	Eigen::MatrixXd A_dp, B_dp, C_dp, k_dp;
	A_dp = phi1.getA() + phi2.getA();
	B_dp = phi1.getB() + phi2.getB();
	C_dp = phi1.getC() + phi2.getC();
	k_dp = phi1.getk() + phi2.getk();

	// Diagonalise A''
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(A_dp);
	Eigen::MatrixXd U = solver.eigenvectors(); // Eigenvectors
	Eigen::MatrixXd D = solver.eigenvalues().asDiagonal(); // Eigenvalues

	// Compute determinant and inverse of A''
	Eigen::MatrixXd D_inv = D;
	double detA_dp = 1.0;
	for (int i = 0; i < N; i++){
		D_inv(i, i) = 1.0/D(i, i);
		detA_dp *= D(i, i);
	}
	Eigen::MatrixXd A_dp_inv = U * D_inv * U.transpose();

	// Calculate overlap integral
	
	double S = overlap(N, A_dp_inv, B_dp, C_dp, R, detA_dp, phi1.getNorm(), phi2.getNorm(),
					   phi1.getJR(), phi2.getJR());
	Smat(s_i, s_j) = S;
	Smat(s_j, s_i) = S;
	
	// Kinetic energy integral
	double T = kinetic(N, phi1.getA(), phi2.getA(), A_dp, A_dp_inv, phi1.getk(), phi2.getk(),  k_dp, S, L2);

	// Drude potential energy integral
	double VD = drudePotential(A_dp_inv, B_dp, R, S, M, N);

	// Now calculate the coulombic potential integrals
	double VC = 0.0;
	double Rij = 0.0;
	std::vector<double> Rij_vec(3);
	
	for (int i = 0; i < N-1; i++){
		Rij_vec[0] = R(i, 0); Rij_vec[1] = R(i, 1); Rij_vec[2] = R(i, 2);
		Rij = R(i, 0)*R(i, 0) + R(i, 1)*R(i, 1) + R(i, 2)*R(i, 2);
		Rij = std::sqrt(Rij);
		
		// Get gi
		double gi =1.0/A_dp_inv(i, i);
		
		for (int j = i+1; j < N; j++){
			// Add the Rij term
			double VC_temp = S/Rij;

			std::vector<double> Pa_vec(3), Pb_vec(3), Pc_vec(3);
			double Pa = 0, Pb = 0, Pc = 0;
			for (int m = 0; m < 3; m++){
				Pa_vec[m] = Rij_vec[m];
				Pb_vec[m] = Rij_vec[m];
				Pc_vec[m] = Rij_vec[m];
				for (int n = 0; n < N; n++){
					Pa_vec[m] += 0.5*A_dp_inv(i, n) * k_dp(n, m);
					Pb_vec[m] -= 0.5*A_dp_inv(j, n) * k_dp(n, m);
					Pc_vec[m] -= 0.5*(A_dp_inv(j, n) - A_dp_inv(i, n))*k_dp(n, m);
				}
				Pa += Pa_vec[m]*Pa_vec[m];
				Pb += Pb_vec[m]*Pb_vec[m];
				Pc += Pc_vec[m]*Pc_vec[m];
			}
			Pa = std::sqrt(Pa);
			Pb = std::sqrt(Pb);
			Pc = std::sqrt(Pc);

			// Get  gj, and gij
			double gj = 1.0/A_dp_inv(j, j);
			double gij = A_dp_inv(i, i) + A_dp_inv(j, j) - 2.0*A_dp_inv(i, j);
			gij = 1.0/gij;

			// Add the aij, bji, cij terms
			VC_temp += coulombPotential(Pc, gij, S);
			VC_temp -= coulombPotential(Pa, gi, S);
			VC_temp -= coulombPotential(Pb, gj, S);

			VC += q[i]*q[j]*VC_temp;
			// Update Rij
			if (j != N-1) { 
				Rij_vec[0] += R(j, 0); Rij_vec[1] += R(j, 1); Rij_vec[2] += R(j, 2);
				double temp = R(j, 0)*R(j, 0) +R(j, 1)*R(j, 1) + R(j, 2)*R(j, 2);
				Rij += std::sqrt(temp);
			}

		}
	}

	double h_el = T + VD + VC;
	
	if (std::isnan(h_el) || std::isinf(h_el)) {  h_el = 0.0; std::cout << "Dodgy hamiltonian element.\n"; }
//.........这里部分代码省略.........

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();
  }
}

void propa_2d()
{
    trace.beginBlock("2d propagation");

    typedef DiscreteExteriorCalculus<2, 2, EigenLinearAlgebraBackend> Calculus;
		typedef DiscreteExteriorCalculusFactory<EigenLinearAlgebraBackend> CalculusFactory;

    {
        trace.beginBlock("solving time dependent equation");

        const Calculus::Scalar cc = 8; // px/s
        trace.info() << "cc = " << cc << endl;

        const Z2i::Domain domain(Z2i::Point(0,0), Z2i::Point(29,29));
        const Calculus calculus = CalculusFactory::createFromDigitalSet(generateDiskSet(domain), false);

        //! [time_laplace]
        const Calculus::DualIdentity0 laplace = calculus.laplace<DUAL>() + 1e-8 * calculus.identity<0, DUAL>();
        //! [time_laplace]
        trace.info() << "laplace = " << laplace << endl;

        trace.beginBlock("finding eigen pairs");
        //! [time_eigen]
        typedef Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> EigenSolverMatrix;
        const EigenSolverMatrix eigen_solver(laplace.myContainer);

        const Eigen::VectorXd eigen_values = eigen_solver.eigenvalues();
        const Eigen::MatrixXd eigen_vectors = eigen_solver.eigenvectors();
        //! [time_eigen]
        trace.info() << "eigen_values_range = " << eigen_values.minCoeff() << "/" << eigen_values.maxCoeff() << endl;
        trace.endBlock();

        //! [time_omega]
        const Eigen::VectorXd angular_frequencies = cc * eigen_values.array().sqrt();
        //! [time_omega]

        Eigen::VectorXcd initial_wave = Eigen::VectorXcd::Zero(calculus.kFormLength(0, DUAL));
        //set_initial_phase_null(calculus, initial_wave);
        set_initial_phase_dir_yy(calculus, initial_wave);

        {
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, initial_wave.real());
            board.saveSVG("propagation_time_wave_initial_coarse.svg");
        }

        //! [time_init_proj]
        Eigen::VectorXcd initial_projections = eigen_vectors.transpose() * initial_wave;
        //! [time_init_proj]

        // low pass
        //! [time_low_pass]
        const Calculus::Scalar lambda_cutoff = 4.5;
        const Calculus::Scalar angular_frequency_cutoff = 2*M_PI * cc / lambda_cutoff;
        int cutted = 0;
        for (int kk=0; kk<initial_projections.rows(); kk++)
        {
            const Calculus::Scalar angular_frequency = angular_frequencies(kk);
            if (angular_frequency < angular_frequency_cutoff) continue;
            initial_projections(kk) = 0;
            cutted ++;
        }
        //! [time_low_pass]
        trace.info() << "cutted = " << cutted << "/" << initial_projections.rows() << endl;

        {
            const Eigen::VectorXcd wave = eigen_vectors * initial_projections;
            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, wave.real());
            board.saveSVG("propagation_time_wave_initial_smoothed.svg");
        }

        const int kk_max = 60;
        trace.progressBar(0,kk_max);
        for (int kk=0; kk<kk_max; kk++)
        {
            //! [time_solve_time]
            const Calculus::Scalar time = kk/20.;
            const Eigen::VectorXcd current_projections = (angular_frequencies * std::complex<double>(0,time)).array().exp() * initial_projections.array();
            const Eigen::VectorXcd current_wave = eigen_vectors * current_projections;
            //! [time_solve_time]

            std::stringstream ss;
            ss << "propagation_time_wave_solution_" << std::setfill('0') << std::setw(3) << kk << ".svg";

            Board2D board;
            board << domain;
            board << CustomStyle("KForm", new KFormStyle2D(-1, 1));
            board << Calculus::DualForm0(calculus, current_wave.real());
            board.saveSVG(ss.str().c_str());

            trace.progressBar(kk+1,kk_max);
        }
        trace.info() << endl;

        trace.endBlock();
//.........这里部分代码省略.........