void ossimFftFilter::fillMatrixForward(T *realPart, T nullPix, NEWMAT::Matrix& real, NEWMAT::Matrix& img)const { ossim_uint32 w = real.Ncols(); ossim_uint32 h = real.Nrows(); ossim_uint32 yIdx = 0; ossim_uint32 xIdx = 0; for(yIdx = 0; yIdx < h; ++yIdx) { for(xIdx = 0; xIdx < w; ++xIdx) { if((double)(*realPart) != nullPix) { real[yIdx][xIdx] = (double)(*realPart); } else { real[yIdx][xIdx] = 0.0; } img[yIdx][xIdx] = 0.0; ++realPart; } } }
void FullBFMatrix::VertConcatBelowMe(const NEWMAT::Matrix& B) { if (!B.Ncols()) return; if (int(Ncols()) != B.Ncols()) {throw BFMatrixException("FullBFMatrix::VertConcatBelowMe: Matrices must have same # of columns");} *mp &= B; }
NEWMAT::Matrix rspfMatrix3x3::create(const NEWMAT::Matrix& rhs) { NEWMAT::Matrix m(3, 3); if (rhs.Ncols() != 3 || rhs.Nrows() != 3) { rspfNotify(rspfNotifyLevel_FATAL) << "rspfMatrix3x3::create(const NEWMAT::Matrix& rhs) ERROR:" << "\nMatrix passed to function not a 3x3!" << "\nnumber of columns: " << rhs.Ncols() << "\nnumber of rows: " << rhs.Nrows() << "\nReturn blank 3x3 matrix...\n"; return m; } m[0][0] = rhs[0][0]; m[0][1] = rhs[0][1]; m[0][2] = rhs[0][2]; m[0][3] = rhs[0][3]; m[1][0] = rhs[1][0]; m[1][1] = rhs[1][1]; m[1][2] = rhs[1][2]; m[1][3] = rhs[1][3]; m[2][0] = rhs[2][0]; m[2][1] = rhs[2][1]; m[2][2] = rhs[2][2]; m[2][3] = rhs[2][3]; return m; }
/** * stable invert stolen from ossimRpcSolver */ NEWMAT::Matrix ossimRpcProjection::invert(const NEWMAT::Matrix& m)const { ossim_uint32 idx = 0; NEWMAT::DiagonalMatrix d; NEWMAT::Matrix u; NEWMAT::Matrix v; // decompose m.t*m which is stored in Temp into the singular values and vectors. // NEWMAT::SVD(m, d, u, v, true, true); // invert the diagonal // this is just doing the reciprical fo all diagonal components and store back int // d. ths compute d inverse. // for(idx=0; idx < (ossim_uint32)d.Ncols(); ++idx) { if(d[idx] > 1e-14) //TBC : use DBL_EPSILON ? { d[idx] = 1.0/d[idx]; } else { d[idx] = 0.0; //DEBUG TBR cout<<"warning: singular matrix in SVD"<<endl; } } //compute inverse of decomposed m; return v*d*u.t(); }
NEWMAT::Matrix rspfSensorModelTuple::invert(const NEWMAT::Matrix& m) const { rspf_uint32 idx = 0; NEWMAT::DiagonalMatrix d; NEWMAT::Matrix u; NEWMAT::Matrix v; NEWMAT::SVD(m, d, u, v, true, true); for(idx=0; idx < (rspf_uint32)d.Ncols(); ++idx) { if(d[idx] > 1e-14) //TBC : use DBL_EPSILON ? { d[idx] = 1.0/d[idx]; } else { d[idx] = 0.0; if (traceDebug()) { rspfNotify(rspfNotifyLevel_WARN) << "DEBUG: rspfSensorModelTuple::invert(): " << "\nsingular matrix in SVD..." << std::endl; } } } return v*d*u.t(); }
void FullBFMatrix::HorConcat2MyRight(const NEWMAT::Matrix& B) { if (!B.Nrows()) return; if (int(Nrows()) != B.Nrows()) {throw BFMatrixException("FullBFMatrix::HorConcat2MyRight: Matrices must have same # of rows");} *mp |= B; }
void FullBFMatrix::VertConcat(const NEWMAT::Matrix& B, BFMatrix& AB) const { if (B.Ncols() && int(Ncols()) != B.Ncols()) {throw BFMatrixException("FullBFMatrix::VertConcat: Matrices must have same # of columns");} FullBFMatrix *pAB = dynamic_cast<FullBFMatrix *>(&AB); if (pAB) { // This means output is a full matrix *pAB = *this; pAB->VertConcatBelowMe(B); } else { SparseBFMatrix<double> *psdAB = dynamic_cast<SparseBFMatrix<double> *>(&AB); if (psdAB) { *psdAB = SparseBFMatrix<double>(this->AsMatrix()); psdAB->VertConcatBelowMe(B); } else { SparseBFMatrix<float> *psfAB = dynamic_cast<SparseBFMatrix<float> *>(&AB); if (psfAB) { *psfAB = SparseBFMatrix<float>(this->AsMatrix()); psfAB->VertConcatBelowMe(B); } else throw BFMatrixException("FullBFMatrix::VertConcat: dynamic cast error"); } } }
ossimMatrix4x4::ossimMatrix4x4(const NEWMAT::Matrix& m) :theData(4,4) { if((m.Nrows() == 4) && (m.Ncols() == 4)) { theData = m; } else if((m.Nrows()==3)&& (m.Ncols()==3)) { theData[0][0] = m[0][0]; theData[0][1] = m[0][1]; theData[0][2] = m[0][2]; theData[0][3] = 0.0; theData[1][0] = m[1][0]; theData[1][1] = m[1][1]; theData[1][2] = m[1][2]; theData[1][3] = 0.0; theData[2][0] = m[2][0]; theData[2][1] = m[2][1]; theData[2][2] = m[2][2]; theData[2][3] = 0.0; theData[3][0] = 0.0; theData[3][1] = 0.0; theData[3][2] = 0.0; theData[3][3] = 1.0; } else { theData[0][0] = 1.0; theData[0][1] = 0.0; theData[0][2] = 0.0; theData[0][3] = 0.0; theData[1][0] = 0.0; theData[1][1] = 1.0; theData[1][2] = 0.0; theData[1][3] = 0.0; theData[2][0] = 0.0; theData[2][1] = 0.0; theData[2][2] = 1.0; theData[2][3] = 0.0; theData[3][0] = 0.0; theData[3][1] = 0.0; theData[3][2] = 0.0; theData[3][3] = 1.0; } }
void CUniversalKriging::Init_a(const CGridPoint& pt, CGridPointVector& va, NEWMAT::Matrix &a)const { ASSERT(a.Ncols() == a.Nrows()); size_t neq = va.size() + GetMDT(); // Initialize the main kriging matrix: a.ReSize(int(neq), int(neq)); a = 0; // Fill in the kriging matrix: for (int i = 0; i < (int)va.size(); i++) { for (int j = i; j<(int)va.size(); j++) { double cov = m_pVariogram->cova3(va[i], va[j]); a[i][j] = cov; a[j][i] = cov; } } // Fill in the OK unbiasedness portion of the matrix (if not doing SK): if (!m_p.m_bSK) { double unbias = m_pVariogram->GetUnbias(); ASSERT(neq > va.size()); for (int i = 0; i < (int)va.size(); ++i) { a[(int)va.size()][i] = unbias; a[i][(int)va.size()] = unbias; } } // Add the additional Unbiasedness constraints: int im = int(va.size()) + 1; //const CDetrending& detrending = m_pVariogram->GetDetrending(); for (int i = 0; i < (int)m_externalDrift.size(); i++) { double resce = m_pVariogram->GetMaxCov() / max(LIMIT, m_externalDrift[i].GetE(pt)); //compute for (int j = 0; j < (int)va.size(); j++) { a[im][j] = m_externalDrift[i].GetE(va[j])*resce; a[j][im] = a[im][j]; } im++; } }
bool ossimObservationSet::evaluate(NEWMAT::Matrix& measResiduals, NEWMAT::Matrix& objPartials, NEWMAT::Matrix& parPartials) { // Dimension output matrices measResiduals = NEWMAT::Matrix(numMeas(), 2); objPartials = NEWMAT::Matrix(numMeas()*3, 2); parPartials = NEWMAT::Matrix(theNumPartials, 2); int img = 1; int cParIndex = 1; int cObjIndex = 1; for (ossim_uint32 cObs=0; cObs<numObs(); ++cObs) { int numMeasPerObs = theObs[cObs]->numMeas(); if (traceDebug()) { ossimNotify(ossimNotifyLevel_DEBUG)<<"\n cObs= "<<cObs; } for (int cImg=0; cImg<numMeasPerObs; ++cImg) { NEWMAT::Matrix cResid(1, 2); theObs[cObs]->getResiduals(cImg, cResid); if (traceDebug()) { ossimNotify(ossimNotifyLevel_DEBUG) <<"\n cImg, img, cObjIndex, cParIndex, cResid: " <<cImg<<" "<<img<<" "<<cObjIndex<<" "<<cParIndex<<" "<<cResid; } measResiduals.Row(img) = cResid; img++; NEWMAT::Matrix cObjPar(3, 2); theObs[cObs]->getObjSpacePartials(cImg, cObjPar); objPartials.SubMatrix(cObjIndex,cObjIndex+2,1,2) << cObjPar; cObjIndex += 3; int numPar = theObs[cObs]->numPars(cImg); NEWMAT::Matrix cParamPar(numPar, 2); theObs[cObs]->getParameterPartials(cImg, cParamPar); parPartials.SubMatrix(cParIndex,cParIndex+numPar-1,1,2) << cParamPar; cParIndex += numPar; } } return true; }
bool ossim::matrixToHpr( ossim_float64 hpr[3], const NEWMAT::Matrix& lsrMatrix, const NEWMAT::Matrix& rotationalMatrix) { bool result = false; NEWMAT::Matrix invertLsr(lsrMatrix.i()); hpr[0] = 0.0; hpr[1] = 0.0; hpr[2] = 0.0; result = matrixToHpr(hpr, invertLsr*rotationalMatrix); if(std::abs(hpr[0]) < FLT_EPSILON) { hpr[0] = 0.0; } if(std::abs(hpr[1]) < FLT_EPSILON) { hpr[1] = 0.0; } if(std::abs(hpr[2]) < FLT_EPSILON) { hpr[2] = 0.0; } return result; }
TFEulerYPR TransformReference::transformEulerYPR(const std::string & target_frame, const TFEulerYPR & euler_in) { NEWMAT::Matrix local = Pose3D::matrixFromEuler(0,0,0,euler_in.yaw, euler_in.pitch, euler_in.roll); NEWMAT::Matrix Transform = getMatrix(target_frame, euler_in.frame, euler_in.time); NEWMAT::Matrix output = local.i() * Transform; Euler eulers = Pose3D::eulerFromMatrix(output,1); TFEulerYPR retEuler; retEuler.yaw = eulers.yaw; retEuler.pitch = eulers.pitch; retEuler.roll = eulers.roll; return retEuler; }
void TransformListener::transformPointCloud(const std::string & target_frame, const Transform& net_transform, const ros::Time& target_time, const std_msgs::PointCloud & cloudIn, std_msgs::PointCloud & cloudOut) { NEWMAT::Matrix transform = transformAsMatrix(net_transform); unsigned int length = cloudIn.get_pts_size(); NEWMAT::Matrix matIn(4, length); double * matrixPtr = matIn.Store(); for (unsigned int i = 0; i < length ; i++) { matrixPtr[i] = cloudIn.pts[i].x; matrixPtr[length +i] = cloudIn.pts[i].y; matrixPtr[2 * length + i] = cloudIn.pts[i].z; matrixPtr[3 * length + i] = 1; }; NEWMAT::Matrix matOut = transform * matIn; // Copy relevant data from cloudIn, if needed if (&cloudIn != &cloudOut) { cloudOut.header = cloudIn.header; cloudOut.set_pts_size(length); cloudOut.set_chan_size(cloudIn.get_chan_size()); for (unsigned int i = 0 ; i < cloudIn.get_chan_size() ; ++i) cloudOut.chan[i] = cloudIn.chan[i]; } matrixPtr = matOut.Store(); //Override the positions cloudOut.header.stamp = target_time; cloudOut.header.frame_id = target_frame; for (unsigned int i = 0; i < length ; i++) { cloudOut.pts[i].x = matrixPtr[i]; cloudOut.pts[i].y = matrixPtr[1*length + i]; cloudOut.pts[i].z = matrixPtr[2*length + i]; }; }
void ossimFftFilter::fillMatrixInverse(T *realPart, T *imgPart, NEWMAT::Matrix& real, NEWMAT::Matrix& img)const { ossim_uint32 w = real.Ncols(); ossim_uint32 h = real.Nrows(); ossim_uint32 yIdx = 0; ossim_uint32 xIdx = 0; for(yIdx = 0; yIdx < h; ++yIdx) { for(xIdx = 0; xIdx < w; ++xIdx) { real[yIdx][xIdx] = (double)(*realPart); img[yIdx][xIdx] = (double)(*imgPart); ++realPart; ++imgPart; } } }
rspfColumnVector3d rspfMatrix3x3::getEigenValues(const NEWMAT::Matrix& matrix) { if (matrix.Ncols() != 3 || matrix.Nrows() != 3) { rspfNotify(rspfNotifyLevel_FATAL) << "FATAL: rspfColumnVector3d operator*(const NEWMAT::Matrix& lhs," << "\nconst rspfColumnVector3d& rhs), " << "\nMatrix passed to function not a 3x3!" << "\nnumber of columns: " << matrix.Ncols() << "\nnumber of rows: " << matrix.Nrows() << "\nReturn blank rspfColumnVector3d...\n"; return rspfColumnVector3d(); } NEWMAT::DiagonalMatrix d; NEWMAT::SymmetricMatrix s; s << matrix; NEWMAT::EigenValues(s, d); return rspfColumnVector3d(d[0], d[1], d[2]); }
bool collision_space::EnvironmentModelOctree::isCollision(unsigned int model_id) { bool result = false; const std::vector<planning_models::KinematicModel::Link*> &links = m_modelLinks[model_id]; for (unsigned int i = 0 ; !result && i < links.size() ; ++i) { switch (links[i]->shape->type) { case planning_models::KinematicModel::Shape::BOX: { /** \todo math here is a bit clumsy.... should be fixed when bette libTF is available */ NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix(); float axes[3][3]; for (unsigned int j=0; j<3; j++) for (unsigned int k=0; k<3; k++) axes[j][k] = mat.element(j,k); libTF::Position libTFpos; links[i]->globalTrans.getPosition(libTFpos); float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z}; const double *size = static_cast<planning_models::KinematicModel::Box*>(links[i]->shape)->size; const float sizef[3] = {size[0], size[1], size[2]}; result = m_octree.intersectsBox(pos, sizef, axes); } break; case planning_models::KinematicModel::Shape::CYLINDER: { NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix(); float axes[3][3]; for (unsigned int j=0; j<3; j++) for (unsigned int k=0; k<3; k++) axes[j][k] = mat.element(j,k); libTF::Position libTFpos; links[i]->globalTrans.getPosition(libTFpos); float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z }; float radius = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->radius; float length = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->length; float L = radius * 2.0; const float sizef[3] = {length, L, L}; result = m_octree.intersectsBox(pos, sizef, axes); } break; case planning_models::KinematicModel::Shape::SPHERE: { libTF::Position libTFpos; links[i]->globalTrans.getPosition(libTFpos); float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z}; float radius = static_cast<planning_models::KinematicModel::Sphere*>(links[i]->shape)->radius; result = m_octree.intersectsSphere(pos, radius); } break; default: fprintf(stderr, "Geometry type not implemented: %d\n", links[i]->shape->type); break; } } return result; }
double CUniversalKriging::Evaluate(const CGridPoint& pt, int iXval)const { //if variogram is not initialised correckly, we retuern no data; if (!m_pVariogram->IsInit()) return m_param.m_noData; // Find the nearest samples: CGridPointVector va; Init_va(pt, iXval, va); int mdt = GetMDT(); // Test number of samples found: // Test if there are enough samples to estimate all drift terms: if ((int)va.size() < m_p.m_nbPoint || (int)va.size() <= mdt) return m_param.m_noData; if ( /*iXval<0 &&*/ va[0].GetDistance(pt) > m_param.m_maxDistance) return m_param.m_noData; // There are enough samples - proceed with estimation. // Go ahead and set up the OK portion of the kriging matrix: NEWMAT::ColumnVector r; NEWMAT::Matrix a; Init_a(pt, va, a); Init_r(pt, va, r); // If estimating the trend then reset all the right hand side terms=0.0: if (m_p.m_bTrend) r = 0; //copy r to compute variance NEWMAT::ColumnVector rr(r); // Solve the kriging system: int neq = mdt + int(va.size()); //reduce the matrix until the 2/3 of the number of neightbor bool bOK = false; while (!bOK && neq >= mdt + m_p.m_nbPoint * 2 / 3)//by RSA 25/10/2010 { Try { a = a.i(); bOK = true; } Catch(NEWMAT::Exception) { OutputDebugStringW(L"a=a.i() failled; Reduce number of point"); Init_a(pt, va, a); Init_r(pt, va, r); neq--; int firstRow = 1 + mdt + int(va.size()) - neq; a = a.SubMatrix(firstRow, a.Nrows(), firstRow, a.Ncols()); r = r.SubMatrix(firstRow, r.Nrows(), 1, 1); } if (m_pAgent && m_pAgent->TestConnection(m_hxGridSessionID) == S_FALSE) throw(CHxGridException()); } //if we don't solve system, return missing if (!bOK) return m_param.m_noData; NEWMAT::ColumnVector s = a*r; CDiscretisation m_discr; m_discr.Initialization(pt, *m_pVariogram, m_p, m_param.m_bFillNugget); // Compute the solution: double uuk = 0.0; double ukv = m_discr.cbb; for (int j = 0; j < neq; j++) { ukv -= s[j] * rr[j]; if (j < (int)va.size()) uuk += s[j] * (m_prePostTransfo.Transform(va[j].m_event) - m_p.m_SKmean); } uuk += m_p.m_SKmean; uuk = m_prePostTransfo.InvertTransform(uuk, m_param.m_noData); // if ( /*iXval<0 &&*/ m_param.m_bRegionalLimit && uuk > m_param.m_noData) { CStatistic stat; for (size_t i = 0; i < va.size(); i++) stat += va[i].m_event; bool bOutside = uuk<stat[LOWEST] - m_param.m_regionalLimitSD*stat[STD_DEV] || uuk>stat[HIGHEST] + m_param.m_regionalLimitSD*stat[STD_DEV]; if (bOutside) { if (m_param.m_bRegionalLimitToBound) uuk = min(stat[HIGHEST] + m_param.m_regionalLimitSD*stat[STD_DEV], max(stat[LOWEST] - m_param.m_regionalLimitSD*stat[STD_DEV], uuk)); else uuk = m_param.m_noData; } } if ( /*iXval<0 &&*/ m_param.m_bGlobalLimit && uuk > m_param.m_noData) { bool bOutside = uuk<m_stat[LOWEST] - m_param.m_globalLimitSD*m_stat[STD_DEV] || uuk>m_stat[HIGHEST] + m_param.m_globalLimitSD*m_stat[STD_DEV]; if (bOutside) { if (m_param.m_bGlobalLimitToBound) uuk = min(m_stat[HIGHEST] + m_param.m_globalLimitSD*m_stat[STD_DEV], max(m_stat[LOWEST] - m_param.m_globalLimitSD*m_stat[STD_DEV], uuk)); else uuk = m_param.m_noData; } } if ( /*iXval<0 &&*/ m_param.m_bGlobalMinMaxLimit && uuk > m_param.m_noData) { bool bOutside = uuk<m_param.m_globalMinLimit || uuk>m_param.m_globalMaxLimit; if (bOutside) { if (m_param.m_bGlobalMinMaxLimitToBound) uuk = min(m_param.m_globalMaxLimit, max(m_param.m_globalMinLimit, uuk)); else uuk = m_param.m_noData; } } return uuk; }