Exemple #1
0
  int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
  {
    // number of nodes
    int nnodes = nodes.size();
    if (nnodes < 2) return 0;

    int nlow = nnodes - window;
    if (nlow < 1) nlow = 1;     // always have one fixed node

    if (verbose)
      cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;

    // number of constraints
    int ncons = p2cons.size();

    // set up SPA
    SysSPA2d spa;
    spa.verbose = verbose;

    // node, constraint vectors and index mapping
    std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
    std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
    std::map<int,int> inds;
    std::vector<int> rinds;     // reverse indices

    // loop through all constraints and set up fixed nodes and constraints
    for (int i=0; i<ncons; i++)
      {
        Con2dP2 &con = p2cons[i];
        if (con.ndr >= nlow || con.nd1 >= nlow)
            wp2cons.push_back(con);

        if (con.ndr >= nlow && con.nd1 < nlow) // have a winner
          {
            int j = getind(inds,con.nd1); // corresponding index
            if (j < 0)      // not present, add it in
              {
                inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
                wnodes.push_back(nodes[con.nd1]);
              }
            rinds.push_back(con.nd1);
          }
        else if (con.nd1 >= nlow && con.ndr < nlow)
          {
            int j = getind(inds,con.ndr); // corresponding index
            if (j < 0)      // not present, add it in
              {
                inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
                wnodes.push_back(nodes[con.ndr]);
              }
            rinds.push_back(con.ndr);
          }
      }

    spa.nFixed = wnodes.size();
    if (verbose)
      cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;

    // add in variable nodes
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
          {
            int n0 = getind(inds,con.ndr);
            if (n0 < 0)
              {
                inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
                wnodes.push_back(nodes[con.ndr]);
                rinds.push_back(con.ndr);
              }
            int n1 = getind(inds,con.nd1);
            if (n1 < 0)
              {
                inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
                wnodes.push_back(nodes[con.nd1]);
                rinds.push_back(con.nd1);
              }
          }
      }

    if (verbose)
      {
        cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
        cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
      }

    // new constraint indices
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        con.ndr = getind(inds,con.ndr);
        con.nd1 = getind(inds,con.nd1);
      }

    // run spa
    niter = spa.doSPA(niter,sLambda,useCSparse);
    
    // reset constraint indices
    for (int i=0; i<(int)wp2cons.size(); i++)
      {
        Con2dP2 &con = wp2cons[i];
        con.ndr = rinds[con.ndr];
        con.nd1 = rinds[con.nd1];
      }
    return niter;
  }
int main(int argc, char **argv)
{
  char *fin;

  if (argc < 2)
    {
      cout << "Arguments are:  <input filename> [<number of nodes to use>]" << endl;
      return -1;
    }

  // number of nodes to use
  int nnodes = 0;

  if (argc > 2)
    nnodes = atoi(argv[2]);

  int doiters = 10;
  if (argc > 3)
    doiters = atoi(argv[3]);

  fin = argv[1];

  // node translation
  std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ntrans;
  // node rotation
  std::vector< double > arots;
  // constraint indices
  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind;
  // constraint local translation 
  std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ctrans;
  // constraint local rotation as quaternion
  std::vector< double > carot;
  // constraint precision
  std::vector< Eigen::Matrix<double,3,3>, Eigen::aligned_allocator<Eigen::Matrix<double,3,3> > > cvar;
  // scans
  std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;

  ReadSPA2dFile(fin,ntrans,arots,cind,ctrans,carot,cvar,scans);

  cout << "# [ReadSPA2dFile] Found " << (int)ntrans.size() << " nodes and " 
       << (int)cind.size() << " constraints" << endl;


  // system
  SysSPA2d spa;
  spa.verbose=false;
  spa.print_iros_stats=false;
  //  spa.useCholmod(false);
  spa.useCholmod(true);


  // use max nodes if we haven't specified it
  if (nnodes == 0) nnodes = ntrans.size();
  if (nnodes > (int)ntrans.size()) nnodes = ntrans.size();

  // add first node
  Node2d nd;

  // rotation
  nd.arot = arots[0];
  // translation
  Vector3d v;
  v.head(2) = ntrans[0];
  v(2) = 1.0;
  nd.trans = v;

  double cumtime = 0.0;
  //cout << nd.trans.transpose() << endl << endl;

  // add to system
  nd.setTransform();            // set up world2node transform
  nd.setDr();
  spa.nodes.push_back(nd);
  // add in nodes
  for (int i=0; i<nnodes-1; i+=doiters)
    {
      for (int j=0; j<doiters; j++)
        addnode(spa, i+j+1, ntrans, arots, cind, ctrans, carot, cvar);

      // cout << "[SysSPA2d] Using " << (int)spa.nodes.size() << " nodes and " 
      //      << (int)spa.p2cons.size() << " constraints" << endl;

      long long t0, t1;

      spa.nFixed = 1;           // one fixed frame

      t0 = utime();
      //      spa.doSPA(1,1.0e-4,SBA_SPARSE_CHOLESKY);
      spa.doSPA(1,1.0e-4,SBA_BLOCK_JACOBIAN_PCG,1.0e-8,15);
      t1 = utime();
      cumtime += t1 - t0;
      if (i%100 == 0) 
        {
          cout << "[SPA2D] iteration: " << i << " squared cost " << spa.errcost << endl;
          cerr << i << " " << cumtime*.001 << " " << spa.errcost << endl;
        }
    }



  printf("[TestSPA2D] Compute took %0.2f ms/node, total time %0.2f ms; error %0.2f\n", 0.001*(double)cumtime/(double)nnodes, cumtime*0.001, spa.errcost);
  // printf("[TestSPA] Accepted iterations: %d\n", niters);
  // printf("[TestSPA] Distance cost: %0.3f m rms\n", sqrt(spa.calcCost(true)/(double)spa.p2cons.size()));

    // if (verbose()){
    //   cerr << "iteration= " << niters 
    // 	   << "\t chi2= " << spa.calcCost();
    //     << "\t time= " << 0.0
    //     << "\t cumTime= " << 0.0
    //     << "\t kurtChi2= " << this->kurtChi2()
    //     << endl;
    // }



#if 0
  ofstream ofs("opt2d-ground.txt");
  for (int i=0; i<(int)ntrans.size(); i++)
    ofs << ntrans[i].transpose() << endl;
  ofs.close();

  ofstream ofs2("opt2d-opt.txt");
  for (int i=0; i<(int)spa.nodes.size(); i++)
    ofs2 << spa.nodes[i].trans.transpose().head(2) << endl;
  ofs2.close();
#endif

  //  spa.writeSparseA("sphere-sparse.txt",true);

  return 0;
}