コード例 #1
0
ファイル: example.cpp プロジェクト: Gabs48/HaloLinux_Software
int main() {
  // instance of the main class that manages and optimizes the pose graph
  Slam slam;

  // locally remember poses
  vector<Pose2d_Node*> pose_nodes;

  Noise noise3 = Information(100. * eye(3));
  Noise noise2 = Information(100. * eye(2));

  // create a first pose (a node)
  Pose2d_Node* new_pose_node = new Pose2d_Node();
  // add it to the graph
  slam.add_node(new_pose_node);
  // also remember it locally
  pose_nodes.push_back(new_pose_node);

  // create a prior measurement (a factor)
  Pose2d origin(0., 0., 0.);
  Pose2d_Factor* prior = new Pose2d_Factor(pose_nodes[0], origin, noise3);
  // add it to the graph
  slam.add_factor(prior);

  for (int i=1; i<4; i++) {
    // next pose
    Pose2d_Node* new_pose_node = new Pose2d_Node();
    slam.add_node(new_pose_node);
    pose_nodes.push_back(new_pose_node);

    // connect to previous with odometry measurement
    Pose2d odometry(1., 0., 0.); // x,y,theta
    Pose2d_Pose2d_Factor* constraint = new Pose2d_Pose2d_Factor(pose_nodes[i-1], pose_nodes[i], odometry, noise3);
    slam.add_factor(constraint);
  }

  // create a landmark
  Point2d_Node* new_point_node = new Point2d_Node();
  slam.add_node(new_point_node);

  // create a pose and the landmark by a measurement
  Point2d measure(5., 3.); // x,y
  Pose2d_Point2d_Factor* measurement =
    new Pose2d_Point2d_Factor(pose_nodes[1], new_point_node, measure, noise2);
  slam.add_factor(measurement);

  // optimize the graph
  slam.batch_optimization();

  // accessing the current estimate of a specific pose
  cout << "Pose 2: " << pose_nodes[2]->value() << endl;

  // printing the complete graph
  cout << endl << "Full graph:" << endl;
  slam.write(cout);

  return 0;
}
コード例 #2
0
ファイル: isam.cpp プロジェクト: Gabs48/HaloLinux_Software
/**
 * Incrementally process factors.
 */
void incremental_slam() {
  unsigned int step = 0;


  unsigned int next_step = step;
  // step by step after reading log file
  for (; loader->more_data(&next_step); step = next_step) {
    check_quit();

    double t0 = tic();

    tic("setup");

    // add new variables and constraints for current step
    for (unsigned int s = step; s < next_step; s++) {
      for (list<Node*>::const_iterator it = loader->nodes(s).begin();
          it != loader->nodes(s).end(); it++) {
        if (prop.verbose)
          cout << **it << endl;
        slam.add_node(*it);
      }
      for (list<Factor*>::const_iterator it = loader->factors(s).begin();
          it != loader->factors(s).end(); it++) {
        if (prop.verbose)
          cout << **it << endl;
        slam.add_factor(*it);
      }
    }

    toc("setup");
    tic("incremental");

    if (!(batch_processing || no_optimization)) {
      slam.update();
    }

    toc("incremental");

    if (save_stats) {
      stats.resize(step + 1);
      stats[step].time = toc(t0);
      stats[step].chi2 = slam.normalized_chi2();
      stats[step].nnz = slam.get_R().nnz();
      stats[step].local_chi2 = slam.local_chi2(100); // last 100 constraints
      stats[step].nnodes = slam.get_nodes().size();
      stats[step].nconstraints = slam.get_factors().size();
    }

    // visualization is not counted in timing
    if (!(batch_processing || no_optimization)) {
      visualize(step);
    }
  }

  visualize(step - 1);

  if (!no_optimization) {
    if (batch_processing) {
      tic("batch");
      slam.batch_optimization();
      toc("batch");
    } else {
      // end with a batch step/relinearization
      prop.mod_batch = 1;
      slam.set_properties(prop);
      tic("final");
      slam.update();
      toc("final");
    }
  }

  visualize(step - 1);
}
コード例 #3
0
int main() {
  // locally defined, as nodes and factors below are allocated on the
  // stack, and therefore will go out of scope after the end of this
  // function; note that you can (and typically have to ) dynamically
  // allocate, see demo.cpp, but I used local variables here to make
  // the example simpler.
  Slam slam;

  Noise noise = SqrtInformation(10. * eye(3));

  // first pose graph
  Pose2d prior_origin(0., 0., 0.);
  Pose2d_Node a0;
  slam.add_node(&a0);
  Pose2d_Factor p_a0(&a0, prior_origin, noise);
  slam.add_factor(&p_a0);
  Pose2d odo(1., 0., 0.);
  Pose2d_Node a1;
  slam.add_node(&a1);
  Pose2d_Pose2d_Factor o_a01(&a0, &a1, odo, noise);
  slam.add_factor(&o_a01);

  // second pose graph
  Pose2d_Node b0;
  slam.add_node(&b0);
  Pose2d_Factor p_b0(&b0, prior_origin, noise);
  slam.add_factor(&p_b0);
  Pose2d_Node b1;
  slam.add_node(&b1);
  Pose2d_Pose2d_Factor o_b01(&b0, &b1, odo, noise);
  slam.add_factor(&o_b01);

  cout << "two independent pose graphs:" << endl;
  cout << "batch optimization... (nothing changes)" << endl;
  slam.batch_optimization();
  cout << "...done" << endl;
  cout << "a0: " << a0.value() << endl;
  cout << "a1: " << a1.value() << endl;
  cout << "b0: " << b0.value() << endl;
  cout << "b1: " << b1.value() << endl;

  // constraint between two pose graphs:

  // first pose graph now also has a anchor node, requires an arbitrary
  // prior (here origin); also initializes the anchor node
  Anchor2d_Node anchor0(&slam);
  slam.add_node(&anchor0);
  //Pose2d_Factor p_anchor0(&anchor0, prior_origin, noise);
  //slam.add_factor(&p_anchor0);

  // anchor node for second trajectory (as before)
  Anchor2d_Node anchor1(&slam);
  slam.add_node(&anchor1);

  Pose2d measure0(0., 1., 0.);
  // now we need 2 optional parameters to refer to both anchor nodes -
  // the order determines which trajectory the measurement originates
  // from; also, the first one already needs to be initialized (done
  // using prior above), the second one gets initialized if needed
  Pose2d_Pose2d_Factor d_a1_b1(&a1, &b1, measure0, noise, &anchor0, &anchor1);
  slam.add_factor(&d_a1_b1);

  cout << "\nfirst constraint added:" << endl;

  cout << "batch optimization... (nothing changes)" << endl;
  slam.batch_optimization();
  cout << "...done" << endl;

  cout << "a0: " << a0.value() << endl;
  cout << "a1: " << a1.value() << endl;
  cout << "b0: " << b0.value() << endl;
  cout << "b1: " << b1.value() << endl;
  cout << "anchor0: " << anchor0.value() << endl;
  cout << "anchor1: " << anchor1.value() << endl;

  Pose2d measure1(0., 0.5, 0.); // conflicting measurement, want least squares
  Pose2d_Pose2d_Factor d_a1_b1_2(&a1, &b1, measure1, noise, &anchor0, &anchor1);
  slam.add_factor(&d_a1_b1_2);

  cout << "\nsecond conflicting constraint added (least squares is 0.75):" << endl;

  cout << "batch optimization... (least squares solution)" << endl;
  slam.batch_optimization();
  cout << "...done" << endl;

  cout.precision(2);
  cout << fixed;
  cout << "a0: " << a0.value() << endl;
  cout << "a1: " << a1.value() << endl;
  cout << "b0: " << b0.value() << endl;
  cout << "b1: " << b1.value() << endl;
  cout << "anchor0: " << anchor0.value() << endl;
  cout << "anchor1: " << anchor1.value() << endl;

  slam.save("anchorNodes.graph");

  return 0;
}
コード例 #4
0
int main(int argc, const char* argv[]) {
  Pose2d origin;
  Noise noise = SqrtInformation(10. * eye(3));
  Pose2d_Node* pose_node_1 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_1); // add it to the Slam graph
  Pose2d_Factor* prior = new Pose2d_Factor(pose_node_1, origin, noise); // create prior measurement, an factor
  slam.add_factor(prior); // add it to the Slam graph

  Pose2d_Node* pose_node_2 = new Pose2d_Node(); // create node
  slam.add_node(pose_node_2); // add it to the Slam graph

  Pose2d delta(1., 0., 0.);
  Pose2d_Pose2d_Factor* odo = new Pose2d_Pose2d_Factor(pose_node_1, pose_node_2, delta, noise);
  slam.add_factor(odo);

  slam.batch_optimization();

#if 0
  const Covariances& covariances = slam.covariances();
#else
  // operate on a copy (just an example, cloning is useful for running
  // covariance recovery in a separate thread)
  const Covariances& covariances = slam.covariances().clone();
#endif

  // recovering the full covariance matrix
  cout << "Full covariance matrix:" << endl;
  MatrixXd cov_full = covariances.marginal(slam.get_nodes());
  cout << cov_full << endl << endl;

  // sanity checking by inverting the information matrix, not using R
  SparseSystem Js = slam.jacobian();
  MatrixXd J(Js.num_cols(), Js.num_cols());
  for (int r=0; r<Js.num_cols(); r++) {
    for (int c=0; c<Js.num_cols(); c++) {
      J(r,c) = Js(r,c);
    }
  }
  MatrixXd H = J.transpose() * J;
  MatrixXd cov_full2 = H.inverse();
  cout << cov_full2 << endl;

  // recovering the block-diagonals only of the full covariance matrix
  cout << "Block-diagonals only:" << endl;
  Covariances::node_lists_t node_lists;
  list<Node*> nodes;
  nodes.push_back(pose_node_1);
  node_lists.push_back(nodes);
  nodes.clear();
  nodes.push_back(pose_node_2);
  node_lists.push_back(nodes);
  list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
  int i = 1;
  for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
    cout << "block " << i << endl;
    cout << *it << endl;
  }

  // recovering individual entries, here the right block column
  cout << "Right block column:" << endl;
  Covariances::node_pair_list_t node_pair_list;
  node_pair_list.push_back(make_pair(pose_node_1, pose_node_2));
  node_pair_list.push_back(make_pair(pose_node_2, pose_node_2));
  list<MatrixXd> cov_entries = covariances.access(node_pair_list);
  for (list<MatrixXd>::iterator it = cov_entries.begin(); it!=cov_entries.end(); it++) {
    cout << *it << endl;
  }
}
コード例 #5
0
ファイル: glc.cpp プロジェクト: Duckietown-NCTU/Software
int main() {
  // setup a simple graph
  // known poses
  Pose2d x0 (0,  0,   M_PI/9.0);
  Pose2d x1 (10, 10,  0.0     );
  Pose2d x2 (20, 20,  M_PI/6.0);
  Pose2d x3 (30, 30, -M_PI/4.0);
  Pose2d x4 (40, 40, -M_PI/8.0);
  // known landmarks
  Point2d la (100, 100);
  
  // mesurements
  Pose2d z0 = x0;
  Pose2d z01 = x1.ominus(x0);
  Pose2d z12 = x2.ominus(x1);
  Pose2d z23 = x3.ominus(x2);
  Pose2d z34 = x4.ominus(x3);
  Pose2d z13 = x3.ominus(x1);
  // landmark measurement
  Point2d z1a = x1.transform_to(la);
    
  // add noise to measurements
  double sigma = 0.01;
  MatrixXd Q = sigma*sigma*eye(3);
  Noise Qsqinf = Information(Q.inverse());
  MatrixXd Q2 = sigma*sigma*eye(2);
  Noise Q2sqinf = Information(Q2.inverse());
  z0  = add_noise(z0,  sigma);
  z01 = add_noise(z01, sigma);
  z12 = add_noise(z12, sigma);
  z23 = add_noise(z23, sigma);
  z34 = add_noise(z34, sigma);
  z13 = add_noise(z13, sigma);
  z1a = add_noise(z1a, sigma);
  
  // nodes
  Pose2d_Node*  n0 = new Pose2d_Node();
  Pose2d_Node*  n1 = new Pose2d_Node();
  Pose2d_Node*  n2 = new Pose2d_Node();
  Pose2d_Node*  n3 = new Pose2d_Node();
  Pose2d_Node*  n4 = new Pose2d_Node();
  Point2d_Node* na = new Point2d_Node();
  
  // make graph
  Slam slam;
  Properties prop;
  prop.verbose = true; prop.quiet = false; prop.method = GAUSS_NEWTON;
  slam.set_properties(prop);
  
  // add nodes to graph
  slam.add_node(n0);
  slam.add_node(n1);
  slam.add_node(n2),
  slam.add_node(n3);
  slam.add_node(n4);
  slam.add_node(na);
    
  // create factors and add them to the graph
  Pose2d_Factor* z0f = new Pose2d_Factor(n0, z0, Qsqinf);
  slam.add_factor(z0f);
  Pose2d_Pose2d_Factor* z01f = new Pose2d_Pose2d_Factor(n0, n1, z01, Qsqinf);
  slam.add_factor(z01f);
  Pose2d_Pose2d_Factor* z12f = new Pose2d_Pose2d_Factor(n1, n2, z12, Qsqinf);
  slam.add_factor(z12f);
  Pose2d_Pose2d_Factor* z23f = new Pose2d_Pose2d_Factor(n2, n3, z23, Qsqinf);
  slam.add_factor(z23f);
  Pose2d_Pose2d_Factor* z34f = new Pose2d_Pose2d_Factor(n3, n4, z34, Qsqinf);
  slam.add_factor(z34f);
  Pose2d_Pose2d_Factor* z13f = new Pose2d_Pose2d_Factor(n1, n3, z13, Qsqinf);
  slam.add_factor(z13f);
  Pose2d_Point2d_Factor* z1af = new Pose2d_Point2d_Factor(n1, na, z1a, Q2sqinf);
  slam.add_factor(z1af);
  
  slam.batch_optimization();
  slam.print_stats();
  ofstream f; f.open ("before.graph"); slam.write(f); f.close();
  //slam.print_graph();
  //print_all(slam);
  
  // get true marginal distribution over p(x0, x2, x3, x4)
  list<Node*> nodes_subset;
  nodes_subset.push_back(n0);
  nodes_subset.push_back(n2);
  nodes_subset.push_back(n3);
  nodes_subset.push_back(n4);
  MatrixXd P_true = slam.covariances().marginal(nodes_subset);
  //MatrixXd L_true = get_info (&slam);
  VectorXd mu_true(12);
  mu_true.segment<3>(0) = n0->value().vector();
  mu_true.segment<3>(3) = n2->value().vector();
  mu_true.segment<3>(6) = n3->value().vector();
  mu_true.segment<3>(9) = n4->value().vector();  
  
  // remove node 1 using GLC ========================================
  bool sparse = true;  // sparse approximate or dense exact
  vector<Factor*> felim = glc_elim_factors (n1); //usefull for local managment of factors
  //vector<Factor*> fnew = glc_remove_node (slam, n1, sparse); // not root shifted
  GLC_RootShift rs;
  vector<Factor*> fnew = glc_remove_node (slam, n1, sparse, &rs); // root shifted
  // ================================================================
  cout << felim.size() << " factor(s) removed." << endl;
  cout << fnew.size() << " new GLC factor(s) added." << endl;
  slam.batch_optimization();
  slam.print_stats();
  f.open ("after.graph"); slam.write(f); f.close();

  // get glc marginal distribution over p(x0, x2, x3, x4)
  MatrixXd P_glc = slam.covariances().marginal(nodes_subset);
  //MatrixXd L_glc = get_info (&slam);
  VectorXd mu_glc(12);
  mu_glc.segment<3>(0) = n0->value().vector();
  mu_glc.segment<3>(3) = n2->value().vector();
  mu_glc.segment<3>(6) = n3->value().vector();
  mu_glc.segment<3>(9) = n4->value().vector();  

  // calcualte the KLD
  int n = mu_glc.size();
  double A = (P_glc.inverse() * P_true).trace();
  VectorXd du = mu_glc - mu_true;
  // deal with difference in angles
  for(int i=0; i<du.size(); i++) {
    if(i % 3 == 2)
      du(i) = standardRad(du(i));
  }   
  double B = du.transpose() * P_glc.inverse() * du;
  double C = log(P_true.determinant()) - log(P_glc.determinant());
  double kld = 0.5*(A + B - C - n);
  cout << "KLD: " << kld << endl;
  
  return 0;
}