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