コード例 #1
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::changeEta(int idx, SSVector& et)
{
   METHOD( "SLUFactor::changeEta()" );

   int es = et.size(); // see altValues()
   update(idx, et.altValues(), et.altIndexMem(), es);
   et.setSize(0);
   et.forceSetup();
}
コード例 #2
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::solveRight(SSVector& x, const SVector& b) //const
{
   METHOD( "SLUFactor::solveRight()" );

   solveTime.start();

   vec.assign(b);
   x.clear();
   CLUFactor::solveRight(x.altValues(), vec.get_ptr());

   solveCount++;
   solveTime.stop();
}
コード例 #3
0
DSVector::DSVector(const SSVector& old)
   : theelem( 0 )
{
   allocMem(old.size() + 1);
   SVector::operator= ( old );

   assert(DSVector::isConsistent());
}
コード例 #4
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::solveLeft(SSVector& x, const SVector& b) //const
{
   METHOD( "SLUFactor::solveLeft()" );

   solveTime.start();

   ssvec.assign(b);

   x.clear();
   int sz = ssvec.size(); // see .altValues()
   int n = vSolveLeft(x.getEpsilon(), x.altValues(), x.altIndexMem(),
      ssvec.altValues(), ssvec.altIndexMem(), sz);

   if (n > 0)
   {
      x.setSize(n);
      x.forceSetup();
   }
   else
      x.unSetup();

   ssvec.setSize(0);
   ssvec.forceSetup();

   solveCount++;
   solveTime.stop();
}
コード例 #5
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::solveRight4update(SSVector& x, const SVector& b)
{
   METHOD( "SLUFactor::solveRight4update()" );

   solveTime.start();

   int m;
   int n;
   int f;

   x.clear();
   ssvec = b;
   n = ssvec.size();
   if (l.updateType == ETA)
   {
      m = vSolveRight4update(x.getEpsilon(), x.altValues(), x.altIndexMem(),
         ssvec.altValues(), ssvec.altIndexMem(), n, 0, 0, 0);
      x.setSize(m);
      //x.forceSetup();
      x.unSetup();
      eta.setup_and_assign(x);
   }
   else
   {
      forest.clear();
      m = vSolveRight4update(x.getEpsilon(), x.altValues(), x.altIndexMem(),
         ssvec.altValues(), ssvec.altIndexMem(), n,
         forest.altValues(), &f, forest.altIndexMem());
      forest.setSize(f);
      forest.forceSetup();
      x.setSize(m);
      x.forceSetup();
   }
   usetup = true;

   solveCount++;
   solveTime.stop();
}
コード例 #6
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::solveLeft(
   SSVector&      x,
   Vector&        y,
   const SVector& rhs1,
   SSVector&      rhs2) //const
{
   METHOD( "SLUFactor::solveLeft()" );

   solveTime.start();

   int   n;
   Real* svec = ssvec.altValues();
   int*  sidx = ssvec.altIndexMem();
   int   rn   = rhs2.size();
   int*  ridx = rhs2.altIndexMem();

   x.clear();
   y.clear();
   ssvec.assign(rhs1);
   n = ssvec.size(); // see altValues();
   n = vSolveLeft2(x.getEpsilon(), x.altValues(), x.altIndexMem(), svec, sidx, n,
      y.get_ptr(), rhs2.altValues(), ridx, rn);

   x.setSize(n);

   if (n > 0)
      x.forceSetup();
   else
      x.unSetup();

   rhs2.setSize(0);
   rhs2.forceSetup();
   ssvec.setSize(0);
   ssvec.forceSetup();

   solveCount++;
   solveTime.stop();
}
コード例 #7
0
ファイル: slufactor.cpp プロジェクト: nicolaje/reliable-slam
void SLUFactor::solve3right4update(
   SSVector&      x,
   Vector&        y,
   Vector&        y2,
   const SVector& b,
   SSVector&      rhs,
   SSVector&      rhs2)
{
   METHOD( "SLUFactor::solve3right4update()" );

   solveTime.start();

   int  m;
   int  n;
   int  f;
   int* sidx = ssvec.altIndexMem();
   int  rsize = rhs.size();
   int* ridx = rhs.altIndexMem();
   int rsize2 = rhs2.size();
   int* ridx2 = rhs2.altIndexMem();

   x.clear();
   y.clear();
   y2.clear();
   usetup = true;
   ssvec = b;

   if (l.updateType == ETA)
   {
      n = ssvec.size();
      m = vSolveRight4update3(x.getEpsilon(),
         x.altValues(), x.altIndexMem(), ssvec.get_ptr(), sidx, n,
         y.get_ptr(), rhs.getEpsilon(), rhs.altValues(), ridx, rsize,
         y2.get_ptr(), rhs2.getEpsilon(),rhs2.altValues(), ridx2, rsize2,
         0, 0, 0);
      x.setSize(m);
      //      x.forceSetup();
      x.unSetup();
      eta.setup_and_assign(x);
   }
   else
   {
      forest.clear();
      n = ssvec.size();
      m = vSolveRight4update3(x.getEpsilon(),
         x.altValues(), x.altIndexMem(), ssvec.get_ptr(), sidx, n,
         y.get_ptr(), rhs.getEpsilon(), rhs.altValues(), ridx, rsize,
         y2.get_ptr(), rhs2.getEpsilon(),rhs2.altValues(), ridx2, rsize2,
         forest.altValues(), &f, forest.altIndexMem());
      x.setSize(m);
      x.forceSetup();
      forest.setSize(f);
      forest.forceSetup();
   }
   solveCount++;
   solveTime.stop();
}