C++ MatrixXf类代码示例

void Neuromag::run()
{
    MatrixXf matValue;

    qint32 size = 0;

    while(m_bIsRunning) {
        if(m_pRawMatrixBuffer_In) {
            //pop matrix
            matValue = m_pRawMatrixBuffer_In->pop();

            //Write raw data to fif file
            if(m_bWriteToFile) {
                size += matValue.rows()*matValue.cols() * 4;

                if(size > MAX_DATA_LEN) {
                    size = 0;
                    this->splitRecordingFile();
                }

                m_mutex.lock();
                if(m_pOutfid) {
                    m_pOutfid->write_raw_buffer(matValue.cast<double>());
                }
                m_mutex.unlock();
            } else {
                size = 0;
            }

            if(m_pRTMSA_Neuromag) {
                m_pRTMSA_Neuromag->data()->setValue(this->calibrate(matValue));
            }
        }
    }
}

MatrixXf Arm::pseudo_inverse() {
    MatrixXf Jacovian = compute_Jacobian();
    MatrixXf jjtInv = (Jacovian * Jacovian.transpose());
    jjtInv = jjtInv.inverse();
    
    return (Jacovian.transpose() * jjtInv);
}

	MatrixXf transformPoints(Matrix3f X, MatrixXf P){

		MatrixXf Pfull(3, P.cols());

		for(int i=0; i<P.cols(); i++){

			Pfull(0, i) = P(0, i);
			Pfull(1, i) = P(1, i);
			Pfull(2, i) = 1;
		}

		Pfull = X*Pfull;

		MatrixXf Pt(2, P.cols());

		for(int i=0; i<P.cols(); i++){

			Pt(0, i) = Pfull(0, i);
			Pt(1, i) = Pfull(1, i);

		}

		return Pt;

	}

Matrix4f Arm::rodrigues(Vector3f rot) {
        Vector3f R; R << rot(0), rot(1), rot(2);

  		if(R.norm() > 0.0f) {
  			R.normalize();
  		}
        

        Matrix3f identity;
        identity << Matrix3f::Identity();

        Matrix3f crossProd(3,3);
        crossProd(0,0) = 0.0f; crossProd(0, 1) =  -(R(0)); crossProd(0,2) = R(1);
        crossProd(1,0) = R(2); crossProd(1, 1) = 0.0; crossProd(1,2) = -(R(0));
        crossProd(2,0) = -(R(1)); crossProd(2, 1) = R(0); crossProd(2,2) = 0.0;

        Matrix3f crossProd_squ(3,3);
        crossProd_squ = crossProd * crossProd;

        float theta = rot.norm();

        MatrixXf result = identity + sin(theta) * crossProd + (1-cos(theta))*crossProd_squ;

        result.conservativeResize(4,4);
        result(0, 3) = 0.0f; result(1, 3) = 0.0f; result(2,3) = 0.0f; result(3,3) = 1.0f;
        result(3, 0) = 0.0f; result(3, 1) = 0.0f; result(3,2) = 0.0f;

        return result;
}

/**
 * Normalizes each eigenface in a matrix.
 * 
 * @param eigenfaces  A matrix of eigen faces to normalize
 */
void normalizeEigenFaces(MatrixXf &eigenfaces)
{
    for(int i = 0; i < eigenfaces.cols(); i++)
    {
        eigenfaces.col(i).normalize();  
    }
}

void RealtimeMF_openni::projectDirections(cv::Mat& I, const MatrixXf& dirs,
    double f_d, const Matrix<uint8_t,Dynamic,Dynamic>& colors)
{
  double scale = 0.1;
  VectorXf p0(3); p0 << 0.35,0.25,1;
  double u0 = p0(0)/p0(2)*f_d + 320.;
  double v0 = p0(1)/p0(2)*f_d + 240.;
  for(uint32_t k=0; k < dirs.cols(); ++k)
  {
    VectorXf p1 = p0 + dirs.col(k)*scale;
    double u1 = p1(0)/p1(2)*f_d + 320.;
    double v1 = p1(1)/p1(2)*f_d + 240.;
    cv::line(I, cv::Point(u0,v0), cv::Point(u1,v1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);

    double arrowLen = 10.;
    double angle = atan2(v1-v0,u1-u0);

    double ru1 = u1 - arrowLen*cos(angle + M_PI*0.25);
    double rv1 = v1 - arrowLen*sin(angle + M_PI*0.25);
    cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
    ru1 = u1 - arrowLen*cos(angle - M_PI*0.25);
    rv1 = v1 - arrowLen*sin(angle - M_PI*0.25);
    cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
        CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
  }
  cv::circle(I, cv::Point(u0,v0), 2, CV_RGB(0,0,0), 2, CV_AA);
}

bool ROSCameraCalibration::load_calibration(std::string const & filename) {
  ifstream fin(filename.c_str());
  if(!fin) {
    cout << "ERROR: Could not open '" << filename << "' for parsing." << endl;
    return false;
  }
  YAML::Parser parser(fin);

  YAML::Node doc;
  while(parser.GetNextDocument(doc)) {
    doc["image_width"] >> image_width;
    doc["image_height"] >> image_height;
    doc["camera_name"] >> camera_name;
    
    load_matrix(doc["camera_matrix"], camera_matrix);
    load_matrix(doc["rectification_matrix"], rectification_matrix);
    load_matrix(doc["distortion_coefficients"], distortion_coefficients);
    load_matrix(doc["projection_matrix"], projection_matrix);
  }
  fin.close();

  // inverse_projection_matrix = pseudo_inverse(projection_matrix);
  // implemented through SVD
  SVD<Matrix<float, 4, 3> > svd(projection_matrix.transpose());
  MatrixXf diag = svd.singularValues().asDiagonal();
  for(int i = 0; i < diag.rows(); i++) {
    if(diag(i, i) <= 1e-6) diag(i, i) = 0;
    else diag(i, i) = 1./diag(i, i);
  }
  inverse_projection_matrix = svd.matrixU()*diag*svd.matrixV().transpose();

  return true;
}

void CutMask::protectCore(MatrixXf & overlap, BlockType type)
{
	switch(type)
	{
	case VERTICAL:
                overlap.block(0, BandSize, BlockSize, BlockSize - BandSize) = MatrixXf::Constant(BlockSize, BlockSize - BandSize, FLT_MAX);
		break;

	case V_BOTHSIDES:
		{
			int width = BlockSize - (2 * BandSize);
			overlap.block(0, BandSize, BlockSize, width) = MatrixXf::Constant(BlockSize, width, FLT_MAX);
		}break;

	case L_SHAPED:
		{
			int size = BlockSize - BandSize;
			overlap.block(BandSize, BandSize, size, size) = MatrixXf::Constant(size, size, FLT_MAX);
		}break;

	case N_SHAPED:
		{
			int width = BlockSize - (2 * BandSize);
			int height = BlockSize - BandSize;
			overlap.block(BandSize, BandSize, height, width) = MatrixXf::Constant(height, width, FLT_MAX);
		}break;

        case NONE_BLOCK:
        case HORIZONTAL:
                break;
	}
}

double IntersectionOverUnion::evaluate( MatrixXf & d_mul_Q, const MatrixXf & Q ) const {
    assert( gt_.rows() == Q.cols() );
    const int N = Q.cols(), M = Q.rows();
    d_mul_Q = 0*Q;

    VectorXd in(M), un(M);
    in.fill(0.f);
    un.fill(1e-20);
    for( int i=0; i<N; i++ ) {
        if( 0 <= gt_[i] && gt_[i] < M ) {
            in[ gt_[i] ] += Q(gt_[i],i);
            un[ gt_[i] ] += 1;
            for( int l=0; l<M; l++ )
                if( l!=gt_[i] )
                    un[ l ] += Q(l,i);
        }
    }
    for( int i=0; i<N; i++ )
        if( 0 <= gt_[i] && gt_[i] < M ) {
            for( int l=0; l<M; l++ )
                if( l==gt_[i] )
                    d_mul_Q(l,i) = Q(l,i) / (un[l]*M);
                else
                    d_mul_Q(l,i) = - Q(l,i) * in[l] / ( un[l] * un[l] * M);
        }
    return (in.array()/un.array()).sum()/M;
}

    void operator()() {
#ifdef FAST_AND_FAT
        //! Performing feature transformation on a block results
        //! is about a factor of two speedup. This suggests moving
        //! feature storage from nodes to images. However, the
        //! change will make introduction of different node
        //! types (with different sized feature vectors) difficult.
        //! It also means that the metric functors would need to
        //! be passed a copy of the graph in addition to the nodes.
        for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
            lock();
            map<unsigned, MatrixXf>::const_iterator ft = _imgFeatureData.find(*it);
            if (ft == _imgFeatureData.end()) {
                MatrixXf X(_Lt.cols(), _srcGraph[*it].numNodes());
                for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                    X.col(segId) = _srcGraph[*it][segId].features;
                }
                ft = _imgFeatureData.insert(_imgFeatureData.end(), make_pair(*it, X));
            }
            unlock();

            const MatrixXf X = _Lt.triangularView<Eigen::Upper>() * ft->second;
            for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                _negGraph[*it][segId].features = _posGraph[*it][segId].features = X.col(segId);
            }
        }
#else
        const TriangularView<MatrixXf, Eigen::Upper> Lt(_Lt);
        for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
            for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
                _negGraph[*it][segId].features = _posGraph[*it][segId].features = Lt * _srcGraph[*it][segId].features;
            }
        }
#endif
    }

VectorXf EMclustering::logsumexp(MatrixXf x, int dim)
{
	int r = x.rows();
	int c = x.cols();

	VectorXf y(r);
	MatrixXf tmp1(r,c);
	VectorXf tmp2(r);
	VectorXf s(r);

	y = x.rowwise().maxCoeff();//cerr<<"y"<<y<<endl<<endl;
	x = x.colwise() - y;	
	//cerr<<"x"<<x<<endl<<endl;
	tmp1 = x.array().exp();	
	//cerr<<"t"<<tmp1<<endl<<endl;
	tmp2 = tmp1.rowwise().sum();	
	//cerr<<"t"<<tmp2<<endl<<endl;
	s = y.array() + tmp2.array().log();

	for(int i=0;i<s.size();i++)
	{
		if(!isfinite(s(i)))
		{
			s(i) = y(i);
		}
	}

	y.resize(0);
	tmp1.resize(0,0);
	tmp2.resize(0);
	
	return s;
}

void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
    VectorXf PHt = P*H.transpose();
    MatrixXf S = H*PHt;
    S(0,0) += R;
    MatrixXf Si = S.inverse();
    Si = make_symmetric(Si);
    MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
    PSD_check.transpose();
    PSD_check.conjugate();

    VectorXf W = PHt*Si;
    x = x+W*v;
    
    //Joseph-form covariance update
    MatrixXf eye(P.rows(), P.cols());
    eye.setIdentity();
    MatrixXf C = eye - W*H;
    P = C*P*C.transpose() + W*R*W.transpose();  

    float eps = 2.2204*pow(10.0,-16); //numerical safety 
    P = P+eye*eps;

    PSD_check = P.llt().matrixL();
    PSD_check.transpose();
    PSD_check.conjugate(); //for upper tri
}

float OptSO3::linesearch(Matrix3f& R, Matrix3f& M_t_min, const Matrix3f& H, 
    float N, float t_max, float dt)
{
  Matrix3f A = R.transpose() * H;

  EigenSolver<MatrixXf> eig(A);
  MatrixXcf U = eig.eigenvectors();
  MatrixXcf invU = U.inverse();
  VectorXcf d = eig.eigenvalues();
#ifndef NDEBUG
  cout<<"A"<<endl<<A<<endl;
  cout<<"U"<<endl<<U<<endl;
  cout<<"d"<<endl<<d<<endl;
#endif

  Matrix3f R_t_min=R;
  float f_t_min = 999999.0f;
  float t_min = 0.0f;
  //for(int i_t =0; i_t<10; i_t++)
  for(float t =0.0f; t<t_max; t+=dt)
  {
    //float t= ts[i_t];
    VectorXcf expD = ((d*t).array().exp());
    MatrixXf MN = (U*expD.asDiagonal()*invU).real();
    Matrix3f R_t = R*MN.topLeftCorner(3,3);

    float detR = R_t.determinant();
    float maxDeviationFromI = ((R_t*R_t.transpose() 
          - Matrix3f::Identity()).cwiseAbs()).maxCoeff();
    if ((R_t(0,0)==R_t(0,0)) 
        && (abs(detR-1.0f)< 1e-2) 
        && (maxDeviationFromI <1e-1))
    {
      float f_t = evalCostFunction(R_t)/float(N);
#ifndef NDEBUG
      cout<< " f_t = "<<f_t<<endl;
#endif
      if (f_t_min > f_t && f_t != 0.0f)
      {
        R_t_min = R_t;
        M_t_min = MN.topLeftCorner(3,3);
        f_t_min = f_t;
        t_min = t;
      }
    }else{
      cout<<"R_t is corruputed detR="<<detR
        <<"; max deviation from I="<<maxDeviationFromI 
        <<"; nans? "<<R_t(0,0)<<" f_t_min="<<f_t_min<<endl;
    }
  }
  if(f_t_min == 999999.0f) return f_t_min;
  // case where the MN is nans
  R = R_t_min;
#ifndef NDEBUG
#endif
  cout<<"R: det(R) = "<<R.determinant()<<endl<<R<<endl;
  cout<< "t_min="<<t_min<<" f_t_min="<<f_t_min<<endl;
  return f_t_min; 
}

// normalizeMatch respect to "In defense of eight point algorithm"
void normalizeMatch(MatrixXf &mat, Matrix3f &T1, Matrix3f &T2) {
  MatrixXf pts1 = mat.leftCols<3>();
  MatrixXf pts2 = mat.block(0, 3, mat.rows(), 3);
  normalizePts(pts1, T1);
  normalizePts(pts2, T2);
  mat.leftCols<3>() = pts1;
  mat.block(0, 3, mat.rows(), 3) = pts2;
}

static inline MatrixXf toMatrix(const std::vector<Vector3f> &data) {
    MatrixXf result;
    result.resize(3, data.size());
    for (size_t i = 0; i < data.size(); ++i) {
        result.col(i) = data[i];
    }
    return std::move(result);
}