C++ MatrixXf::row方法代码示例

	bool ZMeshBilateralFilter::apply(const Eigen::MatrixXf& input, const Eigen::VectorXf& weights, const std::vector<bool>& tags)
	{
		if (pAnnSearch_==NULL)
			return false;
		if (getRangeDim()+getSpatialDim()!=input.cols())
			return false;

		int nSize = input.rows();
		output_ = input;
		pAnnSearch_->setFlags(tags);

		float searchRad = filterPara_.spatial_sigma*sqrt(3.0);
		for (int i=0; i<nSize; i++)
		{
			if (!tags[i]) continue;

			Eigen::VectorXf v(input.row(i));
			Eigen::VectorXi nnIdx;
			Eigen::VectorXf nnDists;

			// query
			Eigen::VectorXf queryV(v.head(spatialDim_));
			Eigen::VectorXf rangeV(v.tail(rangeDim_));
			int queryNum = pAnnSearch_->queryFRPoint(queryV, searchRad, 0, nnIdx, nnDists);
			//int queryNum = queryMeshTool_->query(i, 20, nnIdx, nnDists);

			// convolute
			Eigen::VectorXf sumRange(rangeDim_);
			sumRange.fill(0);
			float sumWeight = 0;
			for (int j=1; j<queryNum; j++)
			{
				int idx = nnIdx[j];
				if (!tags[idx]) continue;
				Eigen::VectorXf rangeU(input.row(idx).tail(rangeDim_));
				float distWeight = ZKernelFuncs::GaussianKernelFunc(sqrt(nnDists(j)), filterPara_.spatial_sigma);
				// if kernelFuncA_==NULL, then only using spatial filter
				float rangeWeidht = kernelFuncA_ ? kernelFuncA_(rangeV, rangeU, filterPara_.range_sigma) : 1.f;
				float weight = rangeWeidht*distWeight*weights(idx);
				//if (i==1)
				//	std::cout << rangeU << " * " << distWeight << "*" << rangeWeidht << "*" << weights(idx) << "\n";
				sumWeight += weight;
				sumRange += rangeU*weight;
			}
			if (!g_isZero(sumWeight))
				output_.row(i).tail(rangeDim_) = sumRange*(1.f/sumWeight);
		}

		return true;
	}

void normalize_each_row(Eigen::MatrixXf &matrix)
{
    //const float row_max_value = 255.0f;
    const float row_max_value = 1.0f;

    for(int row=0; row < matrix.rows(); row += 1)
    {
        const float t_min = matrix.row(row).minCoeff();
        const float t_max = matrix.row(row).maxCoeff();
        matrix.row(row).array() -= t_min;
        matrix.row(row) *= row_max_value/ (t_max - t_min);
    } // end of "for each row"

    return;
}

	void gaussNewtonFromSamplesWeighed(const Eigen::VectorXf &xb, const Eigen::VectorXf &rb, const Eigen::MatrixXf &X, const Eigen::VectorXf &weights, const Eigen::VectorXf &residuals, float regularization, Eigen::VectorXf &out_result)
	{
		//Summary:
		//out_result=xb - G rb
		//xb is the best sample, rb is the best sample residual vector
		//G=AB'inv(BB'+kI)
		//A.col(i)=weights[i]*(X.row(i)-best sample)'
		//B.col(i)=weights[i]*(residuals - rb)'
		//k=regularization

		//Get xb, r(xb)
		//cv::Mat xb=X.row(bestIndex);
		//cv::Mat rb=residuals.row(bestIndex);

		//Compute A and B
		MatrixXf A=X.transpose();
		MatrixXf B=residuals.transpose();
		for (int i=0; i<A.cols(); i++)
		{
			A.col(i)=weights[i]*(X.row(i).transpose()-xb);
			B.col(i)=weights[i]*(residuals.row(i).transpose()-rb);
		}
		MatrixXf I=MatrixXf::Identity(B.rows(),B.rows());
		I=I*regularization;
		MatrixXf G=(A*B.transpose())*(B*B.transpose()+I).inverse();
		out_result=xb - G * rb;
	}

void detectSiftMatchWithVLFeat(const char* img1_path, const char* img2_path, Eigen::MatrixXf &match) {

    int *m = 0;
    double *kp1 = 0, *kp2 = 0;
    vl_uint8 *desc1 = 0, *desc2 = 0;

    int nkp1 = detectSiftAndCalculateDescriptor(img1_path, kp1, desc1);
    int nkp2 = detectSiftAndCalculateDescriptor(img2_path, kp2, desc2);
    cout << "num kp1: " << nkp1 << endl;
    cout << "num kp2: " << nkp2 << endl;
    int nmatch = matchDescriptorWithRatioTest(desc1, desc2, nkp1, nkp2, m);
    cout << "num match: " << nmatch << endl;
    match.resize(nmatch, 6);
    for (int i = 0; i < nmatch; i++) {
        int index1 = m[i*2+0];
        int index2 = m[i*2+1];
        match.row(i) << kp1[index1*4+1], kp1[index1*4+0], 1, kp2[index2*4+1], kp2[index2*4+0], 1;
    }

    free(kp1);
    free(kp2);
    free(desc1);
    free(desc2);
    free(m);
}

void coordinateDescentLasso(
        const Eigen::MatrixXf &data,
        const Eigen::VectorXf &output,
        Eigen::VectorXf &weights, 
        const float lambda,
        const int nIters,
        const bool verbose)
{
    const int nExamples = data.rows();
    const int nFeatures = data.cols();
    for(int iter = 0; iter < nIters; ++iter){
        const int featureInd = iter % nFeatures;
        float rho = 0;
        for(int i = 0; i < nExamples; ++i)
            rho += residualWithoutKWeight(
                    weights,
                    data.row(i).transpose(),
                    output[i],
                    featureInd) * data(i, featureInd);
        auto column = data.col(featureInd);
        float sumOfColumn = (column.transpose() * column).sum();
        weights[featureInd] = coordinateDescentStepLasso(weights[featureInd], sumOfColumn, rho, lambda);
        if(verbose){
            const float err = rss(weights, data, output);
            std::cout << "iter " << iter << " err " << err << std::endl;
            std::cout << weights << std::endl;
        }
    }
}

LIMA_TENSORFLOWSPECIFICENTITIES_EXPORT Eigen::MatrixXi viterbiDecode(
  const Eigen::MatrixXf& score,
  const Eigen::MatrixXf& transitionParams){
  if(score.size()==0){
    std::cerr<<"The output is empty. Check the inputs.";
    return Eigen::MatrixXi();
  }
  if(transitionParams.size()==0){
    std::cerr<<"The transition matrix is empty. Check it.";
    return Eigen::MatrixXi();
  }
  //1. Initialization of matrices
  Eigen::MatrixXf trellis=  Eigen::MatrixXf::Zero(score.rows(),score.cols());
  Eigen::MatrixXi backpointers= Eigen::MatrixXi::Zero(score.rows(),score.cols());
  trellis.row(0)=score.row(0);
  Eigen::MatrixXf v(transitionParams.rows(),transitionParams.cols());

  //2.Viterbi algorithm
  for(auto k=1;k<score.rows();++k)
  {
    for(auto i=0;i<transitionParams.rows();++i){
      for(auto j=0;j<transitionParams.cols();++j){
        v(i,j)=trellis(k-1,i)+transitionParams(i,j);
      }
    }
    trellis.row(k)=score.row(k)+v.colwise().maxCoeff();//equivalent to np.max() function
    for(auto i=0;i<backpointers.cols();++i)
    {
      v.col(i).maxCoeff(&backpointers(k,i));//equivalent to np.argmax() function
    }
  }
  //In Eigen, vector is just a particular matrix with one row or one column
  Eigen::VectorXi viterbi(score.rows());
  trellis.row(trellis.rows()-1).maxCoeff(&viterbi(0));
  Eigen::MatrixXi bp = backpointers.colwise().reverse();
  for(auto i=0;i<backpointers.rows()-1;++i)
  {
    viterbi(i+1)=bp(i,viterbi(i));
  }

  float viterbi_score=trellis.row(trellis.rows()-1).maxCoeff();
  LIMA_UNUSED(viterbi_score);
  //std::cout<<"Viterbi score of the sentence: "<<viterbi_score<<std::endl;

  return viterbi.colwise().reverse();
}

/* return word nearest neighbors in the embedding space */
void getnn(FILE* fout, Eigen::MatrixXf m, const int idx){
    // find nearest neighbour
    Eigen::VectorXf dist = (m.rowwise() - m.row(idx)).rowwise().squaredNorm();
    std::vector<int> sortidx = REDSVD::Util::ascending_order(dist);
    for (int i=1;i<top;i++){
        fprintf(fout, "%s, ", tokename[sortidx[i]]);
    }
    fprintf(fout, "%s\n", tokename[sortidx[top]]);
}

void meanSubtract(Eigen::MatrixXf &data)
{
    double x_sum = data.row(0).sum();
    double y_sum = data.row(1).sum();
    x_sum /= data.cols();
    y_sum /= data.cols();
    Eigen::MatrixXf xsum = Eigen::MatrixXf::Constant(1, data.cols(), x_sum); 
    Eigen::MatrixXf ysum = Eigen::MatrixXf::Constant(1, data.cols(), y_sum); 
    for (int i = 0; i < data.rows(); i++)
    {
        if (i % 2 == 0)
        {
            data.row(i) = data.row(i) - xsum;
        }
        else
        {
            data.row(i) = ysum - data.row(i);
        }
    }
}

void
writeLDRFile(const std::string& file, const Eigen::MatrixXf& data)
{
    FILE *ldrFile = fopen(file.c_str(), "wb");

    for (int r = 0; r < data.rows(); r++)
    {
        Eigen::VectorXf v = data.row(r);
        float pBuffer[] = {v[0], v[1], v[2], v[3], v[4], v[5]};
        fwrite(pBuffer, 1, sizeof(pBuffer), ldrFile);
    }

    fclose(ldrFile);
};

void saveModel(std::string modelname, std::map<std::string, size_t>& word2id, Eigen::MatrixXf& inner, Eigen::MatrixXf& outer) {

    std::ofstream sink(modelname.c_str());

    if (!sink.good()) {
        std::cerr << "Error opening file " << modelname << std::endl;
        std::exit(-1);
    }

    sink << word2id.size() << std::endl;
    for (std::pair<const std::string, size_t>& kv : word2id) {
        sink << kv.first << "\t" << kv.second << std::endl;
    }

    sink << "inner vector" << std::endl;
    for (size_t i = 0; i < inner.rows(); ++i) {
        sink << inner.row(i) << std::endl;
    }

    sink << "outer vector" << std::endl;
    for (size_t i = 0; i < outer.rows(); ++i) {
        sink << outer.row(i) << std::endl;
    }
}

	Eigen::VectorXf ZMeshFilterManifold::computeMaxEigenVector(const Eigen::MatrixXf& inputMat, const std::vector<bool>& clusterK)
	{
		//Eigen::VectorXf ret(rangeDim_);
		Eigen::VectorXf ret(3);
		ret.fill(0);
		int count = 0;
		for (int i=0; i<clusterK.size(); i++) if (clusterK[i]) count++;
		Eigen::MatrixXf realInput(count, rangeDim_);
		//Eigen::MatrixXf realInput(count, 3);
		count = 0;
		for (int i=0; i<clusterK.size(); i++)
		{
			if (clusterK[i])
			{
				realInput.row(count) = inputMat.row(i);
				//realInput.row(count) = MATH::spherical2Cartesian(inputMat.row(i));
				count++;
			}
		}
		Eigen::MatrixXf mat = realInput.transpose()*realInput;
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenSolver(mat);
		if (eigenSolver.info()!=Eigen::Success) {
			std::cerr << "Error in SVD decomposition!\n";
			return ret;
		}
		Eigen::VectorXf eigenValues = eigenSolver.eigenvalues();
		Eigen::MatrixXf eigenVectors = eigenSolver.eigenvectors();
		int maxIdx = 0;
		float maxValue = eigenValues(maxIdx);
		for (int i=1; i<eigenValues.size(); i++)
		{
			if (eigenValues(i)>maxValue)
			{
				maxIdx = i;
				maxValue = eigenValues(maxIdx);
			}
		}
		ret = eigenVectors.col(maxIdx);
		return ret;
// 		Eigen::VectorXf retSph = MATH::cartesian2Spherical(ret, 1);
// 		return retSph.head(rangeDim_);
	}

int main(int argc, char** argv)
{
    std::string path;
    po::options_description desc("Calculates one point cloud for classification\n======================================\n**Allowed options");

    desc.add_options()
            ("help,h", "produce help message")
            ("path,p", po::value<std::string>(&path)->required(), "Path to folders with pics")

            ;

    po::variables_map vm;
    po::parsed_options parsed = po::command_line_parser(argc,argv).options(desc).allow_unregistered().run();
    po::store(parsed, vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return false;
    }

    try
    {
        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
        return false;
    }

//    std::string pretrained_binary_proto = "/home/martin/github/caffe/models/bvlc_alexnet/bvlc_alexnet.caffemodel";
//    std::string feature_extraction_proto = "/home/martin/github/caffe/models/bvlc_alexnet/deploy.prototxt";
    std::string pretrained_binary_proto = "/home/martin/github/caffe/models/bvlc_reference_caffenet/bvlc_reference_caffenet.caffemodel";
    std::string feature_extraction_proto = "/home/martin/github/caffe/models/bvlc_reference_caffenet/deploy.prototxt";
    std::string mean_file = "/home/martin/github/caffe/data/ilsvrc12/imagenet_mean.binaryproto";
    //    std::vector<std::string> extract_feature_blob_names;
    //    extract_feature_blob_names.push_back("fc7");



    Eigen::MatrixXf all_model_signatures_, test;

    v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float>::Parameter estimator_param;
    //estimator_param.init(argc, argv);
    v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float>::Ptr estimator;
    //v4r::CNN_Feat_Extractor<pcl::PointXYZRGB,float> estimator;
    estimator.reset(new v4r::CNN_Feat_Extractor<pcl::PointXYZRGB, float>(estimator_param));
    //estimator.reset();
    //estimator->setExtractFeatureBlobNames(extract_feature_blob_names);
    estimator->setFeatureExtractionProto(feature_extraction_proto);
    estimator->setPretrainedBinaryProto(pretrained_binary_proto);
    estimator->setMeanFile(mean_file);
    //estimator->init();
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    //pcl::PointCloud<pcl::PointXYZRGB> test(new pcl::PointCloud<pcl::PointXYZRGB>);
    char end = path.back();
    if(end!='/')
        path.append("/");
    std::vector<std::string> objects(v4r::io::getFoldersInDirectory(path));

    objects.erase(find(objects.begin(),objects.end(),"svm"));
    std::vector<std::string> paths,Files;
    std::vector<std::string> Temp;
    std::string fs, fp, fo;
    Eigen::VectorXi models;
    std::vector<std::string> modelnames;
    std::vector<int> indices;


    v4r::svmClassifier::Parameter paramSVM;
    paramSVM.knn_=1;
    paramSVM.do_cross_validation_=0;
    v4r::svmClassifier classifier(paramSVM);

    if(!(v4r::io::existsFile(path+"svm/Signatures.txt")&&v4r::io::existsFile(path+"svm/Labels.txt"))){
        for(size_t o=0;o<objects.size();o++){
        //for(size_t o=0;o<1;o++){
            fo = path;
            fo.append(objects[o]);
            fo.append("/");
            paths.clear();
            paths = v4r::io::getFilesInDirectory(fo,".*.JPEG",false);
            modelnames.push_back(objects[o]);
            int old_rows = all_model_signatures_.rows();
            all_model_signatures_.conservativeResize(all_model_signatures_.rows()+paths.size(),4096);
            for(size_t i=0;i<paths.size();i++){
//          for(size_t i=0;i<3;i++){
                fp = fo;
                fp.append(paths[i]);
                std::cout << "Teaching File: " << fp << std::endl;


                //            int rows = image.rows;
                //            int cols = image.cols;

                //            int a,b;
                //            if(rows>256)
                //                a = floor(rows/2)-128;
                //            else
                //                a=0;
//.........这里部分代码省略.........

void ModelRachel::SimulateModel(DF_OUTPUT df[], MAT_FLOAT T_ext_mpc, MAT_FLOAT T_ext_spot, MAT_FLOAT O_mpc, MAT_FLOAT O_spot, PARAMS& ParamsIn,
		const int& time_step_mpc, const int& time_step_spot, const int& total_rooms, int time_instances_mpc, int time_instances_spot,
		const int& control_type, const int& horizon) {

	int k = 0;
	float response = 0;

	int n_mpc = time_instances_mpc;
	int n_spot = time_instances_spot;

	PARAMS ParamsErr;
	ParamsErr = ErrorInParams(ParamsIn, ParamsIn.CommonErrors.err_bparams);

	ComputeCoefficients(time_step_mpc, total_rooms, ParamsIn);

	/* Output of the Program */

	// Temperature Matrices
	Eigen::MatrixXf T = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf TR1 = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf TR2 = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// Delta Matrices
	Eigen::MatrixXf DeltaTR1 = Eigen::MatrixXf::Zero(n_spot, total_rooms);
	Eigen::MatrixXf DeltaTR2 = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// PPV
	Eigen::MatrixXf PPV = Eigen::MatrixXf::Zero(n_spot, total_rooms);

	// AHU Parameters
	Eigen::MatrixXf r = Eigen::MatrixXf::Zero(n_spot, 1);
	Eigen::MatrixXf MixedAirTemperature = Eigen::MatrixXf::Zero(n_spot, 1);
	Eigen::MatrixXf PowerAHU = Eigen::MatrixXf::Zero(n_spot, 1);

	// Initialization
	size_t step_size_mpc = (horizon * 60 * 60) / time_step_mpc;
	size_t step_size_spot = (horizon * 60 * 60) / time_step_spot;

	std::cout << "Step Size MPC: " << step_size_mpc << ", Step Size SPOT: " << step_size_spot << "\n";

	// std::cout << duration << "\n" << horizon << "\n" << n << "\n" << step_size << "\n";

	MAT_FLOAT T_ext_blk = MAT_FLOAT::Ones(step_size_mpc, 1);
	MAT_FLOAT T_ext_eblk = MAT_FLOAT::Ones(step_size_mpc, 1);

	MAT_FLOAT O_blk = MAT_FLOAT::Ones(step_size_mpc, total_rooms);

	time_t start_time = df[k].t;
	struct tm *date = gmtime(&start_time);
	int Time_IH = (date->tm_min)/10;

	ControlBox cb;
	ControlVariables CV = cb.DefaultControl(total_rooms, ParamsIn);

	MAT_FLOAT SPOT_State = MAT_FLOAT::Zero(n_spot, total_rooms);
	SPOT_State.row(k) = CV.SPOT_CurrentState;

	T.row(k) = Eigen::VectorXf::Ones(total_rooms) * 21;
	TR1.row(k) = T.row(k) + DeltaTR1.row(k);
	TR2.row(k) = T.row(k) + DeltaTR2.row(k);

	PPV.row(k) = O_spot.row(k).array() * ((ParamsIn.PMV_Params.P1 * TR1.row(k))
					- (ParamsIn.PMV_Params.P2 * MAT_FLOAT::Zero(1, total_rooms))
					+ (ParamsIn.PMV_Params.P3 * MAT_FLOAT::Zero(1, total_rooms))
					- (ParamsIn.PMV_Params.P4 * MAT_FLOAT::Ones(1, total_rooms))).array();

	/* Update Output Frame */
	df[k].response = response;
	for (size_t room = 0; room < (size_t) total_rooms; room++) {
		df[k].ppv[room] = PPV(k, room);
		df[k].tspot[room] = TR1(k, room);
		df[k].tnospot[room] = TR2(k, room);
	}
	std::cout << "Room: \n";

	int time_step_ratio = time_step_mpc / time_step_spot;
	int k_spot_prev = k * time_step_ratio;	// Converting MPC previous index to SPOT index
	int k_spot = k * time_step_ratio;		// Converting MPC current index to SPOT index

	std::cout << TR2.row(k_spot) << "\n";
	for(size_t j = 1; j < time_step_ratio; j = j + 1) {
		/* Update Output Frame */
		df[k_spot_prev+j].weather_err = T_ext_blk(k_spot_prev);		// External Temperature
		df[k_spot_prev+j].power = PowerAHU(k_spot_prev);
		df[k_spot_prev+j].r = r(k_spot_prev);
		df[k_spot_prev+j].tmix = MixedAirTemperature(k_spot_prev);
		df[k_spot+j].response = response;

		for (size_t room = 0; room < (size_t) total_rooms; room++) {
			df[k_spot+j].ppv[room] = PPV(k_spot, room);
			df[k_spot+j].tspot[room] = TR1(k_spot, room);
			df[k_spot+j].tnospot[room] = TR2(k_spot, room);
			df[k_spot_prev+j].spot_status[room] = SPOT_State(k_spot_prev, room);
		}

		PowerAHU(k_spot_prev+j) = PowerAHU(k_spot_prev);
		r(k_spot_prev+j) = r(k_spot_prev);
		MixedAirTemperature(k_spot_prev+j) = MixedAirTemperature(k_spot_prev);

		for (size_t room = 0; room < (size_t) total_rooms; room++) {
//.........这里部分代码省略.........

    Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
    {
        // Get number of degrees of freedom
        size_t nDoF = rns->getAllRobotNodes().size();

        // Create matrices for the position and the orientation part of the jacobian.
        Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);

        const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];

        // Iterate over all degrees of freedom
        for (size_t i = 0; i < nDoF; i++)
        {
            RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];

            // Check if the tcp is affected by this DOF
            if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
            {
                // Calculus for rotational joints is different as for prismatic joints.
                if (dof->isRotationalJoint())
                {
                    // get axis
                    boost::shared_ptr<RobotNodeRevolute> revolute
                        = boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
                    THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);

                    // For CoM-Jacobians only the positional part is necessary
                    Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
                                            - dof->getGlobalPose().block(0, 3, 3, 1);
                    position.block(0, i, 3, 1) = axis.cross(toTCP);
                }
                else if (dof->isTranslationalJoint())
                {
                    // -> prismatic joint
                    boost::shared_ptr<RobotNodePrismatic> prismatic
                        = boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
                    THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);

                    position.block(0, i, 3, 1) = axis;
                }
            }
        }

        if (target.rows() == 2)
        {
            Eigen::MatrixXf result(2, nDoF);
            result.row(0) = position.row(0);
            result.row(1) = position.row(1);
            return result;
        }
        else if (target.rows() == 1)
        {
            VR_INFO << "One dimensional CoMs not supported." << endl;
        }

        return position;
    }

bool LogisticRegressionVT::LogisticVTImpl::TestCovariate(const Matrix& Xnull,
                                                         const Vector& yVec,
                                                         const Matrix& Xcol) {
  // const int n = Xnull.rows;
  const int d = Xnull.cols;
  const int k = Xcol.cols;
  copy(Xnull, &cov);  // Z = n by d = [z_1^T; z_2^T; ...]
  copy(Xcol, &geno);  // S = n by k = [S_1^T, S_2^T, ...]
  copy(yVec, &y);
  copy(this->null.GetPredicted(), &res);  // n by 1
  v = res.array() * (1. - res.array());
  res = y - res;
  Eigen::MatrixXf vsz(d, k);  // \sum_i v_i S_ki Z_i = n by d matrix
  Eigen::MatrixXf tmp;

  // calculate U and V
  const Eigen::MatrixXf& S = geno;
  const Eigen::MatrixXf& Z = cov;

  // U = (S.transpose() * (res.asDiagonal())).rowsum();
  U = res.transpose() * S;  // 1 by k matrix
                            // for (int i = 0; i < d; ++i) {
  //   vsz.col(i) = (Z * (v.array() *
  //   S.col().array()).matrix().asDiagonal()).rowsum();
  // }
  vsz = cov.transpose() * v.asDiagonal() * S;  // vsz: d by k matrix
  // const double zz = (v.array() *
  // (Z.array().square().rowise().sum()).array()).sum();
  Eigen::MatrixXf zz =
      cov.transpose() * v.asDiagonal() * cov;  // zz: d by d matrix

  // V.size(k, 1);
  //   V(i, 1) = (v.array() * (S.col(i).array().square())).sum() -
  //   vsz.row(i).transpose() * vsz.row(i) /   zz;
  // }
  V = (v.asDiagonal() * (S.array().square().matrix()))
          .colwise()
          .sum();  // - // 1 by k
  tmp = ((vsz).array() * (zz.ldlt().solve(vsz)).array()).colwise().sum();
  V -= tmp;

  // V = (v.asDiagonal() * (S.array().square().matrix())).colwise().sum() -
  // ((vsz).array() * (zz.ldlt().solve(vsz)).array()).colwise().sum();

  // Uk is n by k matrix
  // Uk.size(n, k);
  // for (int i = 0; i < k; ++i) {
  //   Uk.col(i) = res * (S.col(i) - vsz.col(i).transpose()) * Z.col(i) / zz;
  // }
  Uk = res.asDiagonal() * (S - Z * zz.ldlt().solve(vsz));
  // Vkk.size(k, k);
  // for (int i = 0; i < k; ++i) {
  //   for (int j = 0; j <= 1; ++j) {
  //     Vkk(i, j) = Uk.col(i) .transpose() * Uk.col(j);
  //   }
  //   if (i != j) {
  //     Vkk(j, i) = Vkk(i, j);
  //   } else {
  //     if (Vkk(i,i) == 0.0) {
  //       return false; // variance term should be larger than zero.
  //     }
  //   }
  //  }
  Vkk = Uk.transpose() * Uk;

  Eigen::MatrixXf t = U.array() / V.array().sqrt();
  Eigen::RowVectorXf tmp2 = t.row(0).cwiseAbs();
  tmp2.maxCoeff(&maxIndex);

  rep(-tmp2(maxIndex), k, &lower);
  rep(tmp2(maxIndex), k, &upper);
  makeCov(Vkk);
  if (mvnorm.getBandProbFromCor(k, (double*)lower.data(), (double*)upper.data(),
                                (double*)cor.data(), &pvalue)) {
    fprintf(stderr, "Cannot get MVN pvalue.\n");
    return false;
  }

  copy(U, &retU);
  copy(V, &retV);
  copy(t, &retT);
  copy(Vkk, &retCov);
  pvalue = 1.0 - pvalue;
  return true;
};