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;
}
LQRControler::LQRControler() {
trajectory=new ReferenceTrajectory();
Ke=Gain(4);
deltaxsiant.setZero();
xsiant.setZero();
ts=0.012;
}
/**
* 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);
}
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;
}
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());
}
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;
}
}
}
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;
}
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;
}
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;
}
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;
};
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];
}
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;
}
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;
}
}