bundle.cc

this is software for visual SLAM

      Matrix<6> m6; // Temp working space
      Vector<6> v6; // Temp working space
      
      // Calculate on-diagonal blocks of S (i.e. only one camera at a time:)
      for(unsigned int j=0; j<mvCameras.size(); j++)
	{
	  Camera &cam_j = mvCameras[j];
	  if(cam_j.bFixed) continue;
	  int nCamJStartRow = cam_j.nStartRow;
	  
	  // First, do the diagonal elements.
	  //m6= cam_j.m6U;     // can't do this anymore because cam_j.m6U is LL!!
	  for(int r=0; r<6; r++)
	    {
	      for(int c=0; c<r; c++)
		m6[r][c] = m6[c][r] = cam_j.m6U[r][c];
	      m6[r][r] = cam_j.m6U[r][r];
	    };
	  
	  for(int nn = 0; nn< 6; nn++)
	    m6[nn][nn] *= (1.0 + mdLambda);
	  
	  v6 = cam_j.v6EpsilonA;
	  
	  vector<Meas*> &vMeasLUTj = mvMeasLUTs[j];
	  // Sum over measurements (points):
	  for(unsigned int i=0; i<mvPoints.size(); i++)
	    {
	      Meas* pMeas = vMeasLUTj[i];
	      if(pMeas == NULL || pMeas->bBad)
		continue;
	      m6 -= pMeas->m63W * mvPoints[i].m3VStarInv * pMeas->m63W.T();  // SLOW SLOW should by 6x6sy
	      v6 -= pMeas->m63W * (mvPoints[i].m3VStarInv * mvPoints[i].v3EpsilonB);
	    }
	  mS.slice(nCamJStartRow, nCamJStartRow, 6, 6) = m6;
	  vE.slice(nCamJStartRow,6) = v6;
	}
      
      // Now find off-diag elements of S. These are camera-point-camera combinations, of which there are lots.
      // New code which doesn't waste as much time finding i-jk pairs; these are pre-stored in a per-i list.
      for(unsigned int i=0; i<mvPoints.size(); i++)
	{
	  Point &p = mvPoints[i];
	  int nCurrentJ = -1;
	  int nJRow = -1;
	  Meas* pMeas_ij;
	  Meas* pMeas_ik;
	  Matrix<6,3> m63_MIJW_times_m3VStarInv;
	  
	  for(vector<OffDiagScriptEntry>::iterator it=p.vOffDiagonalScript.begin();
	      it!=p.vOffDiagonalScript.end();
	      it++)
	    {
	      OffDiagScriptEntry &e = *it;
	      pMeas_ik = mvMeasLUTs[e.k][i];
	      if(pMeas_ik == NULL || pMeas_ik->bBad)
		continue;
	      if(e.j != nCurrentJ)
		{
		  pMeas_ij = mvMeasLUTs[e.j][i];
		  if(pMeas_ij == NULL || pMeas_ij->bBad)
		    continue;
		  nCurrentJ = e.j;
		  nJRow = mvCameras[e.j].nStartRow;
		  m63_MIJW_times_m3VStarInv = pMeas_ij->m63W * p.m3VStarInv;
		}
	      int nKRow = mvCameras[pMeas_ik->c].nStartRow;
#ifndef WIN32
		  mS.slice(nJRow, nKRow, 6, 6) -= m63_MIJW_times_m3VStarInv * pMeas_ik->m63W.T();
#else
		  Matrix<6> m = mS.slice(nJRow, nKRow, 6, 6);
		  m -= m63_MIJW_times_m3VStarInv * pMeas_ik->m63W.T();
          mS.slice(nJRow, nKRow, 6, 6) = m;
#endif
	      assert(nKRow < nJRow);
	    }
	}
      
      // Did this purely LL triangle - now update the TR bit as well!
      // (This is actually unneccessary since the lapack cholesky solver
      // uses only one triangle, but I'm leaving it in here anyway.)
      for(int i=0; i<mS.num_rows(); i++)
	for(int j=0; j<i; j++)
	  mS[j][i] = mS[i][j];
      
      // Got fat matrix S and vector E from part(iii). Now Cholesky-decompose
      // the sucker, and find the camera update vector.
      Vector<> vCamerasUpdate(mS.num_rows());
      // This used to read as follows when I was using TooN's Cholesky:
      //        Cholesky<> S_chol(mS);
      //        vCamerasUpdate = S_chol.backsub(vE);
      vCamerasUpdate = vE;
      bool bSuccess  = LapackCholeskySolve_ReplaceArgs(mS, vCamerasUpdate);
      if(!bSuccess)
	return false;
      
      // Part (iv): Compute the map updates
      Vector<> vMapUpdates(mvPoints.size() * 3);
      for(unsigned int i=0; i<mvPoints.size(); i++)
	{
	  Vector<3> v3Sum;
	  Zero(v3Sum);
	  for(unsigned int j=0; j<mvCameras.size(); j++)
	    {
	      Camera &cam = mvCameras[j];
	      if(cam.bFixed)
		continue;
	      Meas *pMeas = mvMeasLUTs[j][i];
	      if(pMeas == NULL || pMeas->bBad)
		continue;
	      v3Sum+=pMeas->m63W.T() * vCamerasUpdate.slice(cam.nStartRow,6);
	    }
	  Vector<3> v3 = mvPoints[i].v3EpsilonB - v3Sum;
	  vMapUpdates.slice(i * 3, 3) = mvPoints[i].m3VStarInv * v3;
	  if(isnan(vMapUpdates.slice(i * 3, 3) * vMapUpdates.slice(i * 3, 3)))
	    {
	      cerr << "NANNERY! " << endl;
	      cerr << mvPoints[i].m3VStarInv << endl;
	    };
	}
      
      // OK, got the two update vectors.
      // First check for convergence..
      // (this is a very poor convergence test)
      double dSumSquaredUpdate = vCamerasUpdate * vCamerasUpdate + vMapUpdates * vMapUpdates;
      if(dSumSquaredUpdate< *mgvdUpdateConvergenceLimit)
	mbConverged = true;
      
      // Now re-project everything and measure the error;
      // NB we don't keep these projections, SLOW, bit of a waste.
      
      // Temp versions of updated pose and pos:
      for(unsigned int j=0; j<mvCameras.size(); j++)
	{
	  if(mvCameras[j].bFixed)
	    mvCameras[j].se3CfWNew = mvCameras[j].se3CfW;
	  else
	    mvCameras[j].se3CfWNew = SE3::exp(vCamerasUpdate.slice(mvCameras[j].nStartRow, 6)) * mvCameras[j].se3CfW;
	}
      for(unsigned int i=0; i<mvPoints.size(); i++)
	mvPoints[i].v3PosNew = mvPoints[i].v3Pos + vMapUpdates.slice(i*3, 3);
      // Calculate new error by re-projecting, doing tukey, etc etc:
      dNewError = FindNewError<MEstimator>();
      
      cout <<setprecision(1) << "L" << mdLambda << setprecision(3) <<  "\tOld " << dCurrentError << "  New " << dNewError << "  Diff " << dCurrentError - dNewError << "\t";
      
      // Was the step good? If not, modify lambda and try again!!
      // (if it was good, will break from this loop.)
      if(dNewError > dCurrentError)
	{
	  cout << " TRY AGAIN " << endl;
	  ModifyLambda_BadStep();
	};
      
      mnCounter++;
      if(mnCounter >= *mgvnMaxIterations)
	mbHitMaxIterations = true;
    }   // End of while error too big loop
  
  if(dNewError < dCurrentError) // Was the last step a good one?
    {
      cout << " WINNER            ------------ " << endl;
      // Woo! got somewhere. Update lambda and make changes permanent.
      ModifyLambda_GoodStep();
      for(unsigned int j=0; j<mvCameras.size(); j++)
	mvCameras[j].se3CfW = mvCameras[j].se3CfWNew;
      for(unsigned int i=0; i<mvPoints.size(); i++)
	mvPoints[i].v3Pos = mvPoints[i].v3PosNew; 
      mnAccepted++;
    }
  
  // Finally, ditch all the outliers.
  vector<list<Meas>::iterator> vit;
  for(list<Meas>::iterator itr = mMeasList.begin(); itr!=mMeasList.end(); itr++)
    if(itr->bBad)
      {
	vit.push_back(itr);
	mvOutlierMeasurementIdx.push_back(make_pair(itr->p, itr->c));
	mvPoints[itr->p].nOutliers++;
	mvMeasLUTs[itr->c][itr->p] = NULL;
      };
  
  for(unsigned int i=0; i<vit.size(); i++)
    mMeasList.erase(vit[i]);
  
  cout << "Nuked " << vit.size() << " measurements." << endl;
  return true;
}

// Find the new total error if cameras and points used their 
// new coordinates
template<class MEstimator>
double Bundle::FindNewError()
{
  ofstream ofs;
  double dNewError = 0;
  vector<double> vdErrorSquared;
  for(list<Meas>::iterator itr = mMeasList.begin(); itr!=mMeasList.end(); itr++)
    {
      Meas &meas = *itr;
      // Project the point.
      Vector<3> v3Cam = mvCameras[meas.c].se3CfWNew * mvPoints[meas.p].v3PosNew;
      if(v3Cam[2] <= 0)
	{
	  dNewError += 1.0;
	  cout << ".";
	  continue;
	};
      Vector<2> v2ImPlane = project(v3Cam);
      Vector<2> v2Image   = mCamera.Project(v2ImPlane);
      Vector<2> v2Error =   meas.dSqrtInvNoise * (meas.v2Found - v2Image);
      double dErrorSquared = v2Error * v2Error;
      dNewError += MEstimator::ObjectiveScore(dErrorSquared, mdSigmaSquared);
    }
  return dNewError;
}

// Optimisation: make a bunch of tables, one per camera
// which point to measurements (if any) for each point
// This is faster than a std::map lookup.
void Bundle::GenerateMeasLUTs()
{
  mvMeasLUTs.clear();
  for(unsigned int nCam = 0; nCam < mvCameras.size(); nCam++)
    {
      mvMeasLUTs.push_back(vector<Meas*>());
      mvMeasLUTs.back().resize(mvPoints.size(), NULL);
    };
  for(list<Meas>::iterator it = mMeasList.begin(); it!=mMeasList.end(); it++)
    mvMeasLUTs[it->c][it->p] =  &(*it);
}

// Optimisation: make a per-point list of all
// observation camera-camera pairs; this is then
// scanned to make the off-diagonal elements of matrix S
void Bundle::GenerateOffDiagScripts()
{
  for(unsigned int i=0; i<mvPoints.size(); i++)
    {
      Point &p = mvPoints[i];
      p.vOffDiagonalScript.clear();
      for(set<int>::iterator it_j = p.sCameras.begin(); it_j!=p.sCameras.end(); it_j++)
	{
	  int j = *it_j;
	  if(mvCameras[j].bFixed)
	    continue;
	  Meas *pMeas_j = mvMeasLUTs[j][i];
	  assert(pMeas_j != NULL);
	  
	  for(set<int>::iterator it_k = p.sCameras.begin(); it_k!=it_j; it_k++)
	    {
	      int k = *it_k;
	      if(mvCameras[k].bFixed)
		continue;
	      
	      Meas *pMeas_k = mvMeasLUTs[k][i];
	      assert(pMeas_k != NULL);
	      
	      OffDiagScriptEntry e;
	      e.j = j;
	      e.k = k;
	      p.vOffDiagonalScript.push_back(e);
	    }
	}
    }
}

void Bundle::ModifyLambda_GoodStep()
{
  mdLambdaFactor = 2.0;
  mdLambda *= 0.3;
};

void Bundle::ModifyLambda_BadStep()
{
  mdLambda = mdLambda * mdLambdaFactor;
  mdLambdaFactor = mdLambdaFactor * 2;
};


Vector<3> Bundle::GetPoint(int n)
{
  return mvPoints.at(n).v3Pos;
}

SE3 Bundle::GetCamera(int n)
{
  return mvCameras.at(n).se3CfW;
}

set<int> Bundle::GetOutliers()
{
  set<int> sOutliers;
  set<int>::iterator hint = sOutliers.begin();
  for(unsigned int i=0; i<mvPoints.size(); i++)
    {
      Point &p = mvPoints[i];
      if(p.nMeasurements > 0 && p.nMeasurements == p.nOutliers)
	hint = sOutliers.insert(hint, i);
    }
  return sOutliers;
};


vector<pair<int, int> > Bundle::GetOutlierMeasurements()
{
  return mvOutlierMeasurementIdx;
}