예제 #1
0
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;
      }
   }
}
예제 #2
0
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;
}
예제 #3
0
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;
}
예제 #4
0
/** 
 * 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();
}
예제 #5
0
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();
}
예제 #6
0
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;
}
예제 #7
0
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"); 
    }
  }
}
예제 #8
0
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++;
		}
	}
예제 #10
0
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;
}
예제 #11
0
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;
}
예제 #12
0
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;
}
예제 #13
0
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];
    };
}
예제 #14
0
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;
      }
   }
}
예제 #15
0
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]);
}
예제 #16
0
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;
	}