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

 /**
  * Convert covariance matrix @param in to correlation matrix, and
  * put lower triangle to @param out
  */
 int cov2cor(const Eigen::MatrixXf& in, std::vector<double>* out) {
   int n = in.rows();
   if ( n != in.cols()) return -1;
   if (n == 0) return -1;
   (*out).resize(n * (n-1)/2);
   int k = 0;
   for (int i = 0 ; i < n; ++i) {
     for (int j = 0; j < i; ++j){
       (*out)[k++] = in(i, j ) / sqrt(in(i,i) * in(j, j));
     }
   }
   return 0;
 }

  /**
   * rescale this->upper, this->lower, and set this->cor
   */
  int makeCov(const Eigen::MatrixXf cov) {
    if (cov.rows() != cov.cols()) return -1;
    if (cov.rows() != (int) upper.size()) return -1;
    if (cov.rows() != (int) lower.size()) return -1;

    for (size_t i = 0; i < upper.size(); ++i) {
      upper[i] /= sqrt(cov(i, i));
      lower[i] /= sqrt(cov(i, i)); 
    }

    cov2cor(cov, &this->cor);
    return 0;
  }

// Compute the KL-divergence of a set of marginals
double DenseCRF::klDivergence( const Eigen::MatrixXf & Q ) const {
	double kl = 0;
	// Add the entropy term
	for( int i=0; i<Q.cols(); i++ )
		for( int l=0; l<Q.rows(); l++ )
			kl += Q(l,i)*log(std::max( Q(l,i), 1e-20f) );
	// Add the unary term
	if( unary_ ) {
		Eigen::MatrixXf unary = unary_->get();
		for( int i=0; i<Q.cols(); i++ )
			for( int l=0; l<Q.rows(); l++ )
				kl += unary(l,i)*Q(l,i);
	}
	
	// Add all pairwise terms
	Eigen::MatrixXf tmp;
	for( unsigned int k=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->apply( tmp, Q );
		kl += (Q.array()*tmp.array()).sum();
	}
	return kl;
}

int Conversion::convert(const Eigen::MatrixXf & depthMat, vpImage<float>&dmap)
{
      int height = depthMat.rows();
      int width  = depthMat.cols();
      dmap.resize(height, width);
      for(int i = 0 ; i< height ; i++){
       for(int j=0 ; j< width ; j++){
              dmap[i][j]=depthMat(i,j);
        }
      }
      
      return 1;
}

		Eigen::MatrixXf safeResize(Eigen::MatrixXf A, int nRows, int nCols) {
			Eigen::MatrixXf B(nRows,nCols);
			for (int i = 0; i < nRows; i++) {
				for (int j = 0; j < nCols; j++) {
					if (i < A.rows() && j < A.cols()) {
						B(i,j) = A(i,j);
					} else {
						B(i,j) = std::sqrt(-1);
					}
				}
			}
			return B;
		}

void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud)
{
	sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
	output->header = pcl_conversions::fromPCL(merged_cloud->header);
	output->header.frame_id = destination_frame.c_str();
	output->header.stamp = ros::Time::now();  //fixes #265
	output->angle_min = this->angle_min;
	output->angle_max = this->angle_max;
	output->angle_increment = this->angle_increment;
	output->time_increment = this->time_increment;
	output->scan_time = this->scan_time;
	output->range_min = this->range_min;
	output->range_max = this->range_max;

	uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
	output->ranges.assign(ranges_size, output->range_max + 1.0);

	for(int i=0; i<points.cols(); i++)
	{
		const float &x = points(0,i);
		const float &y = points(1,i);
		const float &z = points(2,i);

		if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
		{
			ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
			continue;
		}

		double range_sq = y*y+x*x;
		double range_min_sq_ = output->range_min * output->range_min;
		if (range_sq < range_min_sq_) {
			ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
			continue;
		}

		double angle = atan2(y, x);
		if (angle < output->angle_min || angle > output->angle_max)
		{
			ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
			continue;
		}
		int index = (angle - output->angle_min) / output->angle_increment;


		if (output->ranges[index] * output->ranges[index] > range_sq)
			output->ranges[index] = sqrt(range_sq);
	}

	laser_scan_publisher_.publish(output);
}

TEST(LeastSquaresTransform, Transform) {
    Eigen::MatrixXf A(7, 3), B(7,3);
    /*
    A:

   4:
    |\      A
    |'\
    | '\,
    |   \,
....|____\.............
    :    2
    :
    :
                   _,-, 2
            _,--'-'   |    B
    :  ,-/~'          |
....:/'...............|.......
    :                 4



    */
    A << 0, 4, 1,
        0, 3, 1,
        0, 2, 1,
        0, 1, 1,
        0, 0, 1,
        1, 0, 1,
        2, 0, 1;

    B << 0, 0, 1,
        1, 0, 1,
        2, 0, 1,
        3, 0, 1,
        4, 0, 1,
        4, 1, 1,
        4, 2, 1;

    Eigen::MatrixXf T;
    LeastSquaresTransform<cv::Point2f> lst;

    lst(A, B, T);
    EXPECT_EQ(T.cols(), 3);
    EXPECT_EQ(T.rows(), 3);
    Eigen::MatrixXf TExpected(3,3);
    TExpected << 0, 1, 0,
                -1, 0, 0,
                4, 0, 1;
    EXPECT_EQ(TExpected.isApprox(T, 0.0001f), true);
}

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

/**
 * @function fillEq
 */
void trifocalTensor::fillEq() {

  mPointer = 0;
  mNumTotalEq = 9*mPPP.size() + 3*mLLL.size() + 3*mPLP.size() + 1*mPLL.size();

  printf("Total number of equations (some of them redundant): %d \n", mNumTotalEq );
  mEq = Eigen::MatrixXf::Zero( mNumTotalEq, 27 );
  mB = Eigen::VectorXf::Zero( mNumTotalEq );

  Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
  printf("* [Beginning] Rank of mEq is: %d \n", lu.rank() );

  // Fill Point-Point-Point ( 9 Equations - 4 DOF )
  for( int i = 0; i < mPPP.size();  i++ ) {
    fillEq_PPP( mPPP[i][0], mPPP[i][1], mPPP[i][2] );
    //Eigen::MatrixXf yam = mEq.block(mPointer - 9, 0,  9, mEq.cols() );
	  Eigen::MatrixXf yam = mEq;
    Eigen::FullPivLU<Eigen::MatrixXf> lu(yam);
    printf("* [PPP][%d] Rank of mEq(%d x %d) is: %d \n", i, yam.rows(), yam.cols(), lu.rank() );
  }

  Eigen::FullPivLU<Eigen::MatrixXf> luA(mEq);
  printf("* After [PPP] Rank of mEq is: %d \n",luA.rank() );

  // Fill Line -Line - Line ( 3 Equations - 2 DOF )
  for( int j = 0; j < mLLL.size(); j++ ) {
    fillEq_LLL( mLLL[j][0], mLLL[j][1], mLLL[j][2] );
    Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
    printf("* [LLL][%d] Rank of mEq is: %d \n", j, lu.rank() );
  }

  // Fill Point - Line - Line ( 1 Equations - 1 DOF )
  for( int i = 0; i < mPLL.size(); ++i ) {
    fillEq_PLL( mPLL[i][0], mPLL[i][1], mPLL[i][2] );
    Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
    printf("* [PLL][%d] Rank of mEq is: %d \n", i, lu.rank() );
  }

  // Fill Point - Line - Point ( 3 Equations - 2 DOF )
  for( int i = 0; i < mPLP.size(); ++i ) {
    fillEq_PLP( mPLP[i][0], mPLP[i][1], mPLP[i][2] );
    Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
    printf("* [PLP][%d] Rank of mEq is: %d \n", i, lu.rank() );
  }




}

void PointProjector::unProject(HomogeneousPoint3fVector &points,
			       Eigen::MatrixXi &indexImage,
			       const Eigen::MatrixXf &depthImage) const {
  points.resize(depthImage.rows()*depthImage.cols());
  int count = 0;
  indexImage.resize(depthImage.rows(), depthImage.cols());
  HomogeneousPoint3f* point = &points[0];
  int cpix=0;
  for (int c=0; c<depthImage.cols(); c++){
    const float* f = &depthImage(0,c);
    int* i =&indexImage(0,c);
    for (int r=0; r<depthImage.rows(); r++, f++, i++){
      if (!unProject(*point, r,c,*f)){
	*i=-1;
	continue;
      }
      point++;
      cpix++;
      *i=count;
      count++;
    }
  }
  points.resize(count);
}

void DenseCRF::stepInference( Eigen::MatrixXf & Q, Eigen::MatrixXf & tmp1, Eigen::MatrixXf & tmp2 ) const{
	tmp1.resize( Q.rows(), Q.cols() );
	tmp1.fill(0);
	if( unary_ )
		tmp1 -= unary_->get();
	
	// Add up all pairwise potentials
	for( unsigned int k=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->apply( tmp2, Q );
		tmp1 -= tmp2;
	}
	
	// Exponentiate and normalize
	expAndNormalize( Q, tmp1 );
}

std::vector<Eigen::Vector2f> Random(const Eigen::MatrixXf& density)
{
	boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > die(
		impl::Rnd(),
		boost::uniform_real<float>(0.0f, 1.0f));

	std::vector<Eigen::Vector2f> seeds;
	for(unsigned int iy=0; iy<density.cols(); iy++) {
		for(unsigned int ix=0; ix<density.rows(); ix++) {
			if(die() < density(ix,iy))
				seeds.push_back(Eigen::Vector2f(ix, iy));
		}
	}

	return seeds;
}

// draw all the lines between aPts and Pts that have a corr>threshold.
// if aPtInd is in the range of aPts, then draw only the lines that comes from aPts[aPtInd]
void drawCorrLines(PlotLines::Ptr lines, const vector<btVector3>& aPts, const vector<btVector3>& bPts, const Eigen::MatrixXf& corr, float threshold, int aPtInd) {
  vector<btVector3> linePoints;
  vector<btVector4> lineColors;
  float max_corr = corr.maxCoeff(); // color lines by corr, where corr has been mapped [threshold,max_corr] -> [0,1]
  for (int i=0; i<corr.rows(); ++i)
    for (int j=0; j<corr.cols(); ++j)
    	if (corr(i,j) > threshold) {
    		if (aPtInd<0 || aPtInd>=corr.rows() || aPtInd==i) {
					linePoints.push_back(aPts[i]);
					linePoints.push_back(bPts[j]);
					float color_factor = (corr(i,j)-threshold)/(max_corr-threshold); //basically, it ranges from 0 to 1
					lineColors.push_back(btVector4(color_factor, color_factor,0,1));
				}
    	}
  lines->setPoints(linePoints, lineColors);
}

int ComputationGraph::matrix(Eigen::MatrixXf matrix) {
    std::vector<int> output;
    for (int i = 0; i < matrix.rows(); i++) {
        for (int j = 0; j < matrix.cols(); j++) {
            output.push_back(*((int *)&matrix(i, j)));
        }
    }

    nodes.push_back({
        -1,
        {},
        output
    });

    return nodes.size() - 1;
}

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