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

void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<in.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		Eigen::VectorXf q = Q.col(i);
		out.col(i) = b.array().sum()*q - b;
	}
}

///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

std::vector<int> fillSubset(const Eigen::MatrixXf &data, Eigen::MatrixXf &subset, int num_columns)
{
    std::vector<int> column_indices;
    for(int i = 0; i < num_columns; i++)
    {
        int column_number = rand() % data.cols();
        column_indices.push_back(column_number);
        subset.col(i) = data.col(column_number);        
        //subset.col(i) = data.col(100 + i);        
    }
    return column_indices;
}

double RMSEcalc(std::vector< std::vector<int> > traindata, Eigen::MatrixXf U, Eigen::MatrixXf M) {
	Eigen::VectorXf sqvals(traindata[1].size());
#pragma omp parallel for
	for(int p=0;p<traindata[1].size();p++) {
		sqvals(p)=(U.col(traindata[0][p]-1).transpose()*M.col(traindata[1][p]-1) - traindata[2][p])*(U.col(traindata[0][p]-1).transpose()*M.col(traindata[1][p]-1) - traindata[2][p]);	
	}	
	double total;
       	total = sqvals.sum();
	return total;


}

	void calcMeanAndCovarWeighedVectorized(const Eigen::MatrixXf &input, const Eigen::VectorXd &inputWeights, Eigen::MatrixXf &out_covMat, Eigen::VectorXf &out_mean,Eigen::MatrixXf &temp)
	{
		out_mean=input.col(0); //to resize
		out_mean.setZero();
		double wSumInv=1.0/inputWeights.sum();
		for (int k=0;k<inputWeights.size();k++){
			double w=inputWeights[k];
			out_mean+=input.col(k)*(float)(w*wSumInv);
		}
		out_mean = input.rowwise().mean();
		temp = (input.colwise() - out_mean);
		for (int k=0;k<inputWeights.size();k++){
			temp.col(k) *= (float)(sqrt(inputWeights(k)*wSumInv));	//using square roots, as we only want the normalized weights to be included once for each result element in the multiplication below
		}
		out_covMat = temp*temp.transpose();
	}

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

VectorXs DenseCRF::currentMap( const Eigen::MatrixXf & Q ) const{
	VectorXs r(Q.cols());
	// Find the map
	for( int i=0; i<N_; i++ ){
		int m;
		Q.col(i).maxCoeff( &m );
		r[i] = m;
	}
	return r;
}

float
computeHistogramIntersection (const Eigen::VectorXf &histA, const Eigen::VectorXf &histB)
{
    Eigen::MatrixXf histAB (histA.rows(), 2);
    histAB.col(0) = histA;
    histAB.col(1) = histB;

    Eigen::VectorXf minv = histAB.rowwise().minCoeff();
    return minv.sum();
}

void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
    // Update the position of the first vertex if the keys 1,2, or 3 are pressed
    switch (key)
    {
        case  GLFW_KEY_1:
            V.col(0) << -0.5,  0.5;
            break;
        case GLFW_KEY_2:
            V.col(0) << 0,  0.5;
            break;
        case  GLFW_KEY_3:
            V.col(0) << 0.5,  0.5;
            break;
        default:
            break;
    }

    // Upload the change to the GPU
    VBO.update(V);
}

Eigen::Vector4f PhotoCamera::center(const Eigen::MatrixXf &camera)
{
    Eigen::Matrix3f C1, C2, C3, C4;
    C1 << camera.col(1), camera.col(2), camera.col(3);
    C2 << camera.col(0), camera.col(2), camera.col(3);
    C3 << camera.col(0), camera.col(1), camera.col(3);
    C4 << camera.col(0), camera.col(1), camera.col(2);

    Eigen::Vector4f C;
    C << C1.determinant(), -C2.determinant(), C3.determinant(), -C4.determinant();
    return C;
}

void HierarchicalWalkingIK::computeWalkingTrajectory(const Eigen::Matrix3Xf& comTrajectory,
                                                     const Eigen::Matrix6Xf& rightFootTrajectory,
                                                     const Eigen::Matrix6Xf& leftFootTrajectory,
                                                     std::vector<Eigen::Matrix3f>& rootOrientation,
                                                     Eigen::MatrixXf& trajectory)
{
    int rows = outputNodeSet->getSize();

    trajectory.resize(rows, rightFootTrajectory.cols());
    rootOrientation.resize(rightFootTrajectory.cols());

    Eigen::Vector3f com = colModelNodeSet->getCoM();

    Eigen::VectorXf configuration;
    int N = trajectory.cols();

    Eigen::Matrix4f leftFootPose  = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f rightFootPose = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f chestPose     = chest->getGlobalPose();
    Eigen::Matrix4f pelvisPose    = pelvis->getGlobalPose();

    for (int i = 0; i < N; i++)
    {
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),  leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * rightFootTrajectory.block(0, i, 3, 1), rightFootTrajectory.block(3, i, 3, 1), rightFootPose);

        // FIXME the orientation of the chest and chest is specific to armar 4
        // since the x-Axsis points in walking direction
        Eigen::Vector3f xAxisChest = (leftFootPose.block(0, 1, 3, 1) + rightFootPose.block(0, 1, 3, 1))/2;
        xAxisChest.normalize();
        chestPose.block(0, 0, 3, 3) = Bipedal::poseFromXAxis(xAxisChest);
        pelvisPose.block(0, 0, 3, 3) = Bipedal::poseFromYAxis(-xAxisChest);

        std::cout << "Frame #" << i << ", ";
        robot->setGlobalPose(leftFootPose);
        computeStepConfiguration(1000 * comTrajectory.col(i),
                                 rightFootPose,
                                 chestPose,
                                 pelvisPose,
                                 configuration);

        trajectory.col(i) = configuration;
        rootOrientation[i] = leftFootPose.block(0, 0, 3, 3);
    }
}

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

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
{
    // Get the position of the mouse in the window
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);

    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    double xworld = ((xpos/double(width))*2)-1;
    double yworld = (((height-1-ypos)/double(height))*2)-1; // NOTE: y axis is flipped in glfw

    // Update the position of the first vertex if the left button is pressed
    if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS)
        V.col(0) << xworld, yworld;

    // Upload the change to the GPU
    VBO.update(V);
}

void extractControlFrames(VirtualRobot::RobotPtr robot,
                          const Eigen::Matrix6Xf& leftFootTrajectory,
                          const Eigen::MatrixXf&  bodyTrajectory,
                          VirtualRobot::RobotNodeSetPtr bodyJoints,
                          VirtualRobot::RobotNodePtr node,
                          std::vector<Eigen::Matrix4f>& controlFrames)
{
    int N = leftFootTrajectory.cols();

    for (int i = 0; i < N; i++)
    {
        // Move basis along with the left foot
        Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),
                                                leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        robot->setGlobalPose(leftFootPose);
        bodyJoints->setJointValues(bodyTrajectory.col(i));

        controlFrames.push_back(node->getGlobalPose());
    }
}

  int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
  {
    // This problem is R^2 -> R^(fvec.rows()), so the dimension of the Jacobian is fvec.rows() x 2
    
    float epsilon = 1e-5;
    Eigen::VectorXf epsilonVector(2);

    Eigen::VectorXf f(this->Points.size());
    operator()(epsilonVector, f);
    
    for(unsigned int parameter = 0; parameter < 2; parameter++)
      {
      epsilonVector = x;
      epsilonVector(parameter) += epsilon;

      Eigen::VectorXf fForward(2);
      operator()(epsilonVector, fForward);

      fjac.col(parameter) = (fForward - f)/epsilon;
      }

    return 0;
  }