Beispiel #1
0
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;

	}
Beispiel #4
0
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;
}
LQRControler::LQRControler() {
	trajectory=new ReferenceTrajectory();
	Ke=Gain(4);
	deltaxsiant.setZero();
	xsiant.setZero();
	ts=0.012;
}
Beispiel #6
0
/**
 * 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();  
    }
}
Beispiel #7
0
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;
}
Beispiel #9
0
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;
	}
}
Beispiel #10
0
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;
}
Beispiel #11
0
    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
    }
Beispiel #12
0
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;
}
Beispiel #13
0
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
}
Beispiel #14
0
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;
}
Beispiel #16
0
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);
}
Beispiel #17
0
extern "C" float find_trailing_edge(float * gradient_imgv, int gradient_rows, int gradient_cols,
                                   int startcol, int endrow, int endcol,
                                   int n_neighbors, int * outpathv, float * cost_mat) {
    ExternNDArrayf gradient_img(gradient_imgv, gradient_rows, gradient_cols);
    ExternNDArrayi outpath(outpathv, endcol - startcol, 2);
    /* Assume the gradient image is all setup, initialize cost and back */
    printf("End point gradient: %0.2f\n", gradient_img(endrow, endcol));

    VectorXi neighbor_range(n_neighbors);
    //printf("Building neighbor range\n");
    for (struct {int ind; int neighbor;} N = {0, (-1 * n_neighbors / 2)};
         N.neighbor<(n_neighbors / 2) + 1;
         N.neighbor++, N.ind++) {
        neighbor_range(N.ind) = N.neighbor;
    }
    MatrixXf cost = MatrixXf::Zero(gradient_rows, gradient_cols);
    //ExternNDArrayf cost(cost_mat, gradient_rows, gradient_cols);
    MatrixXi back = MatrixXi::Zero(gradient_rows, gradient_cols);
    
    //printf("Looping over image\n");
    for (int col = startcol; col <= endcol; col++) {
        for (int row = 0; row < gradient_rows; row++) {
            // argmin over candidates
            int best_candidate = 0; // no travel in y-axis
            float best_cand_cost = INFINITY;
            for (int i=0; i < neighbor_range.rows(); i++) {
                float cand_cost = get_te_cost(row, col, neighbor_range(i), cost, gradient_img);
                if (cand_cost < best_cand_cost) {
                    best_candidate = neighbor_range(i);
                    best_cand_cost = cand_cost;
                }
            }

            back(row, col) = best_candidate;
            cost(row, col) = best_cand_cost;
        }
    }
    // Now determine the optimal path from the endrow, endcol position
    // We'll store the result in outpath -- since we know how that the path is constructed 
    // One column at a time, we know how big the path will be ahead of time, which is very helpful
    int curr_row = endrow;
    float total_cost = 0;
    //printf("Reconstructing the optimal path\n");
    for (struct {int ind; int col;} P = {0, endcol}; 
         P.col > startcol; P.col--, P.ind++) {
        //printf("%d\n", curr_row);
        total_cost += cost(curr_row, P.col);
        // x, y
        outpath(P.ind, 0) = P.col;
        outpath(P.ind, 1) = curr_row;

        curr_row = MIN(MAX(0, curr_row + back(curr_row, P.col)),cost.rows()-1);
    }
    //printf("Cost incurred on optimal path: %0.2f\n", total_cost);

    return total_cost;

}
int main(int, char**)
{
  cout.precision(3);
  MatrixXf m;
m.setRandom(3, 3);
cout << m << endl;

  return 0;
}
int main()
{
    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
         << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
}
int main(int, char**)
{
  cout.precision(3);
  MatrixXf m;
m.setConstant(3, 3, 5);
cout << m << endl;

  return 0;
}
Beispiel #21
0
void matrixMultiply(const RealDescriptor& x, const MatrixXf& matrix,
                    RealDescriptor& result) {

  Q_ASSERT(x.size() == matrix.cols());
  int targetDimension = matrix.rows();
  result.resize(targetDimension);

  VectorXf::Map(result.data(), targetDimension) = matrix * VectorXf::Map(x.data(), x.size());
}
Beispiel #22
0
bool Surface::read(const QString &p_sFileName, Surface &p_Surface)
{
    p_Surface.clear();

    printf("Reading surface...\n");
    QFile t_File(p_sFileName);

    if (!t_File.open(QIODevice::ReadOnly))
    {
        printf("\tError: Couldn't open the file\n");
        return false;
    }

    QDataStream t_DataStream(&t_File);
    t_DataStream.setByteOrder(QDataStream::BigEndian);

    //
    //   Magic numbers to identify QUAD and TRIANGLE files
    //
    //   QUAD_FILE_MAGIC_NUMBER =  (-1 & 0x00ffffff) ;
    //   NEW_QUAD_FILE_MAGIC_NUMBER =  (-3 & 0x00ffffff) ;
    //
    qint32 NEW_QUAD_FILE_MAGIC_NUMBER =  16777213;
    qint32 TRIANGLE_FILE_MAGIC_NUMBER =  16777214;
    qint32 QUAD_FILE_MAGIC_NUMBER     =  16777215;

    qint32 magic = IOUtils::fread3(t_DataStream);

    qint32 nvert, nface;
    MatrixXf verts;
    MatrixXi faces;

    if(magic == QUAD_FILE_MAGIC_NUMBER || magic == NEW_QUAD_FILE_MAGIC_NUMBER)
    {
        qint32 nvert = IOUtils::fread3(t_DataStream);
        qint32 nquad = IOUtils::fread3(t_DataStream);
        if(magic == QUAD_FILE_MAGIC_NUMBER)
            printf("\t%s is a quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);
        else
            printf("\t%s is a new quad file (nvert = %d nquad = %d)\n", p_sFileName.toLatin1().constData(),nvert,nquad);

        //vertices
        verts.resize(nvert, 3);
        if(magic == QUAD_FILE_MAGIC_NUMBER)
        {
            qint16 iVal;
            for(qint32 i = 0; i < nvert; ++i)
            {
                for(qint32 j = 0; j < 3; ++j)
                {
                    t_DataStream >> iVal;
                    IOUtils::swap_short(iVal);
                    verts(i,j) = ((float)iVal) / 100;
                }
            }
        }
Beispiel #23
0
VectorXs DenseCRF::currentMap( const 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;
}
Beispiel #24
0
void CDifodo::getDepthDerivatives(MatrixXf &cur_du, MatrixXf &cur_dv, MatrixXf &cur_dt)
{
	cur_du.resize(rows,cols);
	cur_dv.resize(rows,cols);
	cur_dt.resize(rows,cols);

	cur_du = du;
	cur_dv = dv;
	cur_dt = dt;
}
Beispiel #25
0
QPainterPath Layouter::mat2Path( const MatrixXf& pntMat )
{
	QPainterPath path;
	if (pntMat.rows() <= 0 || pntMat.cols() != 2)
		return path;
	path.moveTo(pntMat(0,0), pntMat(0,1));
	for (int i = 1; i < pntMat.rows(); ++i)
		path.lineTo(pntMat(i,0), pntMat(i,1));
	return path;
}
Beispiel #26
0
MatrixXf RealtimeMF::mfAxes()
{
  MatrixXf mfAx = MatrixXf::Zero(3,6*cRmfs_.size());
  for(uint32_t k=0; k<6*cRmfs_.size(); ++k){
    int j = (k%6)/2; // which of the rotation columns does this belong to
    float sign = (- float((k%6)%2) +0.5f)*2.0f; // sign of the axis
    mfAx.col(k) = sign*cRmfs_[k/6].col(j);
  }
  return mfAx;
};
Beispiel #27
0
void CDifodo::getPointsCoord(MatrixXf &x, MatrixXf &y, MatrixXf &z)
{
	x.resize(rows,cols);
	y.resize(rows,cols);
	z.resize(rows,cols);

	z = depth_inter[0];
	x = xx_inter[0];
	y = yy_inter[0];
}
Beispiel #28
0
FeatureModel::FeatureModel(	const CamModel & _camera,
								const MotionModel & _motion,
								const FeaturesState & _featuresState,
								const Mat & frame,
								const Point2f & pf,
								int patchSize,
								float rho_0,
								float sigma_rho):
								camera(_camera),
								motion(_motion),
								features(_featuresState)
	{
    Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));
    this->imageLocation << pf.x, pf.y;

    this->Patch = newPatch.clone();


    Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);

    // TODO: convert using motionmodel
    Vector3f r = motion->get_r();
    Quatenionf q = motion->get_q();


    Vector3f hC = this->camera.unproject(hd, true);
    Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();

    Matrix3f Rot = quat2rot(q);

    Vector3f hW = Rot*hC;

    float hx = hW(0);
    float hy = hW(1);
    float hz = hW(2);



    float theta = atan2(hx,hz);
    float phi = atan2(-hy, sqrt(hx*hx+hz*hz));

	// Updating state and Sigma
    MatrixXf Jx = this->computeJx();
	MatrixXf Jm = this->computeJm();
	Matrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);

	this->f = (VectorXf(6) << r, theta, phi, rho_0);
	this->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();

	this->motion->updateVariaceBlocks(Jx);
	this->features->updateVariaceBlocks(Jx);
	this->features.push_back((*this));
    return 1;

}
void NeighbourJoining::calcNewD(MatrixXf& currentD, MatrixXi& rowsID, const Pair& p) {
	//calculates distances to new node
	int j = 0;
	for (int i = 0; i < numCurrentNodes - 1; ++i) {
		if (i == p.i)
			j++;
		currentD(numCurrentNodes, i) = (currentD(p.i, i + j) + currentD(p.j, i + j) - currentD(p.i, p.j)) / 2;
		currentD(i, numCurrentNodes) = currentD(numCurrentNodes, i);
	}
	//cout << "distances to new node: " << currentD.row(numCurrentNodes).head(numCurrentNodes-1) <<endl;

	//swaps rows and columns so that the closest pair nodes go right and at the bottom of the matrix
	currentD.row(p.i).head(numCurrentNodes - 1).swap(
			currentD.row(numCurrentNodes - 1).head(numCurrentNodes - 1));

	currentD.col(p.i).head(numCurrentNodes - 1).swap(
			currentD.col(numCurrentNodes - 1).head(numCurrentNodes - 1));

	currentD.row(p.j).head(numCurrentNodes - 1).swap(
			currentD.row(numCurrentNodes).head(numCurrentNodes - 1));

	currentD.col(p.j).head(numCurrentNodes - 1).swap(
			currentD.col(numCurrentNodes).head(numCurrentNodes - 1));

	currentD.diagonal().setZero();
	//cout << "new Matrix:" << endl;  	printMatrix(currentD);

	//adjusts node IDs to new matrix indices
	int newNode = 2 * numObservableNodes - numCurrentNodes;
	rowsID.row(p.i).swap(rowsID.row(numCurrentNodes - 1));
	rowsID.row(p.j).swap(rowsID.row(newNode));

	//cout << "rowsID:   " << rowsID.transpose(); cout << endl;
}
Beispiel #30
0
	virtual VectorXf parameters() const {
		if (ktype_ == CONST_KERNEL)
			return VectorXf();
		else if (ktype_ == DIAG_KERNEL)
			return parameters_;
		else {
			MatrixXf p = parameters_;
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}