simulations/isam/isam/isam.cpp
/**
 * Visualize data during optimization in internal viewer
 * or send data via LCM (to an external viewer).
 */
Visualize data during optimization in internal viewer
or send data via LCM (to an external viewer).
Visualize data during optimization in internal viewer or send data via LCM ( to an external viewer ) .
void visualize(unsigned int step) {
  list<MatrixXd> point_marginals;
  list<MatrixXd> pose_marginals;
  if (calculate_covariances && (use_gui || use_lcm) && (step % mod_draw == 0)) {
    covariances(step, point_marginals, pose_marginals);
  }
#ifdef USE_LCM
  {
    const int id_trajectory = 0;
    const int id_landmarks = 1;
    const int id_constraints = 2;
    const int id_measurements = 3;
    const int id_pose_covs = 4;
    const int id_point_covs = 5;
    if (use_lcm && lcm_first) {
      lcm.send_reset();
      lcm_first = false;
    }
    if (use_lcm && step%mod_draw==0) {
      lcm.send_nodes(loader->poses(step), id_trajectory, (char*)"Trajectory", 1);
      lcm.send_nodes(loader->points(step), id_landmarks, (char*)"Landmarks", loader->is_3d() ? 3 : 2);
      lcm.send_links(loader->constraints(step), id_constraints,
          (char*)"Odometry", id_trajectory, id_trajectory);
      lcm.send_links(loader->measurements(step), id_measurements,
          (char*)"Measurements", id_trajectory, id_landmarks);
      if (calculate_covariances) {
        lcm.send_covariances(pose_marginals, id_pose_covs,
            (char*)"Pose Covs", id_trajectory, loader->is_3d());
        lcm.send_covariances(point_marginals, id_point_covs,
            (char*)"Point Covs", id_landmarks, loader->is_3d());
      }
    }
  }
#endif
#ifdef USE_GUI
  {
    const int id_trajectory = 0;
    const int id_landmarks = 1;
    const int id_constraints = 2;
    const int id_measurements = 3;
    const int id_pose_covs = 4;
    const int id_point_covs = 5;
    if (use_gui && step%mod_draw==0) {
      viewer.set_nodes(loader->poses(step), id_trajectory,
          (char*)"Trajectory", VIEWER_OBJ_POSE3D);
      viewer.set_nodes(loader->points(step), id_landmarks,
          (char*)"Landmarks", loader->is_3d() ? VIEWER_OBJ_POINT3D : VIEWER_OBJ_TREE);
      viewer.set_links(loader->constraints(step), id_constraints,
          "Odometry", id_trajectory, id_trajectory);
      viewer.set_links(loader->measurements(step), id_measurements,
          "Measurements", id_trajectory, id_landmarks);
      if (calculate_covariances) {
        viewer.set_covariances(pose_marginals, id_pose_covs,
            (char*)"Pose Covs", id_trajectory, loader->is_3d());
        viewer.set_covariances(point_marginals, id_point_covs,
            (char*)"Point Covs", id_landmarks, loader->is_3d());
      }
    }
  }
#endif
}
void visualize ( unsigned int step ) { list < MatrixXd > point_marginals ; list < MatrixXd > pose_marginals ; if ( calculate_covariances && ( use_gui || use_lcm ) && ( step % mod_draw == 0 ) ) { covariances ( step , point_marginals , pose_marginals ) ; } #ifdef USE_LCM { const int id_trajectory = 0 ; const int id_landmarks = 1 ; const int id_constraints = 2 ; const int id_measurements = 3 ; const int id_pose_covs = 4 ; const int id_point_covs = 5 ; if ( use_lcm && lcm_first ) { lcm . send_reset ( ) ; lcm_first = false ; } if ( use_lcm && step % mod_draw == 0 ) { lcm . send_nodes ( loader -> poses ( step ) , id_trajectory , ( char * ) " " , 1 ) ; lcm . send_nodes ( loader -> points ( step ) , id_landmarks , ( char * ) " " , loader -> is_3d ( ) ? 3 : 2 ) ; lcm . send_links ( loader -> constraints ( step ) , id_constraints , ( char * ) " " , id_trajectory , id_trajectory ) ; lcm . send_links ( loader -> measurements ( step ) , id_measurements , ( char * ) " " , id_trajectory , id_landmarks ) ; if ( calculate_covariances ) { lcm . send_covariances ( pose_marginals , id_pose_covs , ( char * ) " " , id_trajectory , loader -> is_3d ( ) ) ; lcm . send_covariances ( point_marginals , id_point_covs , ( char * ) " " , id_landmarks , loader -> is_3d ( ) ) ; } } } #endif #ifdef USE_GUI { const int id_trajectory = 0 ; const int id_landmarks = 1 ; const int id_constraints = 2 ; const int id_measurements = 3 ; const int id_pose_covs = 4 ; const int id_point_covs = 5 ; if ( use_gui && step % mod_draw == 0 ) { viewer . set_nodes ( loader -> poses ( step ) , id_trajectory , ( char * ) " " , VIEWER_OBJ_POSE3D ) ; viewer . set_nodes ( loader -> points ( step ) , id_landmarks , ( char * ) " " , loader -> is_3d ( ) ? VIEWER_OBJ_POINT3D : VIEWER_OBJ_TREE ) ; viewer . set_links ( loader -> constraints ( step ) , id_constraints , " " , id_trajectory , id_trajectory ) ; viewer . set_links ( loader -> measurements ( step ) , id_measurements , " " , id_trajectory , id_landmarks ) ; if ( calculate_covariances ) { viewer . set_covariances ( pose_marginals , id_pose_covs , ( char * ) " " , id_trajectory , loader -> is_3d ( ) ) ; viewer . set_covariances ( point_marginals , id_point_covs , ( char * ) " " , id_landmarks , loader -> is_3d ( ) ) ; } } } #endif }
Visualize data during optimization in internal viewer
 or send data via LCM (to an external viewer).
Visualize data during optimization in internal viewer or send data via LCM ( to an external viewer ) .
// ids also determine color in viewer // send to LCM viewer // display in internal 3D viewer