Example #1
0
bool FactorGraph::add_virtual_pose_point(virtual_pose3_point3_t virtual_pose_point){
  int id = virtual_pose_point.id;
  if (DEBUG) cout << "in FactorGraph trying to add virtual pose point at time: " << endl << "t = " << virtual_pose_point.t << endl ;

  Pose3dTS_Node* pose = find_pose_from_time_and_id(virtual_pose_point.t, id);
  if (pose == NULL){
    cout << "[warning] {add_virtual_pose_point}: no pose for virtual_pose_point measurement at time " << virtual_pose_point.t << " for id " << id << "skipping.. " <<  endl;
    return false;
  }
  // TODO there is also data association going on here (currently done with ids):
  Point3dID_Node* point  = find_point_from_feature_id(virtual_pose_point.feature_id);
  if(point == NULL)
    point = build_point(virtual_pose_point.feature_id);
  MatrixXd mu,omega;
  conversions::lcm2eigen(virtual_pose_point.point,mu,omega);
  Point3d measure(mu);
  Noise inf = Information(omega);
  Virtual_Pose3d_Point3d_Factor* virtual_factor = 
    new Virtual_Pose3d_Point3d_Factor(dynamic_cast<Pose3d_Node*>(pose),
			      dynamic_cast<Point3d_Node*>(point),
			      measure,
			      inf);
  if (DEBUG) cout << "pose point factors: " << *virtual_factor << endl;
  _slam.add_factor(virtual_factor);
  if(_lcm_viewer)
    _virtual_collection->add(_virtual_link_count++,6,pose->unique_id(),3,point->unique_id());
  return true;
}
Example #2
0
void add_virtual_obstacles(){

    int dis_Y = Y1 - Y0;
    int dis_X = X1 - X0;
    int interval_Y = dis_Y / (VIRTUAL_NUMBER - 1);
    int interval_X = dis_X / (VIRTUAL_NUMBER - 1);
    printf("-------------------print the virtual obstacles---------------\n");
    int i=1;
    for (; i<VIRTUAL_NUMBER-1 ; i++){
        int p_x = X0 + i*interval_X;
        point_t p_with_Y0 = build_point(p_x, Y0);
        point_t p_with_Y1 = build_point(p_x, Y1);

        print_point(&p_with_Y0);
        print_point(&p_with_Y1);
        push_point(p_with_Y0);
        push_point(p_with_Y1);
    }
    i = 1;
    for(; i<VIRTUAL_NUMBER-1; i++){
        int p_y = Y0 + i*interval_Y;
        point_t p_with_X0 = build_point(X0, p_y);
        point_t p_with_X1 = build_point(X1, p_y);

        print_point(&p_with_X0);
        print_point(&p_with_X1);
        push_point(p_with_X0);
        push_point(p_with_X1);
    }

    point_t X0Y0 = build_point(X0, Y0);
    point_t X0Y1 = build_point(X0, Y1);
    point_t X1Y0 = build_point(X1, Y0);
    point_t X1Y1 = build_point(X1, Y1);
    push_point(X0Y0);
    push_point(X0Y1);
    push_point(X1Y0);
    push_point(X1Y1);

    print_point(&X0Y0);
    print_point(&X0Y1);
    print_point(&X1Y0);
    print_point(&X1Y1);
    printf("----------------virtual obstacles---------------\n");

 }
Example #3
0
bool FactorGraph::add_sidescan_detection(sidescan_detection_t detection){
  if (_poses[detection.id].size()==0){
    cout << "Rejecting odometry since no prior" << endl;
    return false;
  }

  Pose3dTS_Node* pose = find_pose_from_time_and_id(detection.t,detection.id); 
  
  if (pose == NULL){ // times might not be exact pick the closest one.
    cout << "No exact time match find closest" << endl;
    pose = find_closest_pose_from_time_and_id(detection.t,detection.id);
  }

  if (pose == NULL) cout << "Error unable to find pose for time = " << detection.t << " and id " << detection.id << endl;

  // TODO: decide if it's a detection of previously tracked target. EG do something like nearest neighbor or JCBB. Start with using feature id

  Point3dID_Node* point = find_point_from_feature_id(detection.feature_id);
  if (point == NULL)
    point = build_point(detection.feature_id);

  // here's the unique thing for a sidescan detection. It is always orthogonal to direction of motion. Therefore take the measurement and put it into "y" direction
  MatrixXd omega;
  conversions::lcm2eigen(detection,omega);
  Point3d measure(0.0,detection.mu,0.0); // TODO deal with depth/z, this sidescan detection should be
                                         // its own factor not use Pose3d_Point3d_Factor
  Noise inf = Information(omega);
  Pose3d_Point3d_Factor* detection_factor = 
    new Pose3d_Point3d_Factor(dynamic_cast<Pose3d_Node*>(pose),
			      dynamic_cast<Point3d_Node*>(point),
			      measure,
			      inf);
  if (DEBUG){ 
    cout << "Adding sidescan detection factor: " << *detection_factor << endl;
    cout << "Between pose with id = " << detection.id << " and time t = " << detection.t << "and point with feature id " << detection.feature_id << endl;
  }
  _slam.add_factor(detection_factor);
  if(_lcm_viewer)
    _feature_collection->add(_feature_link_count++,2,pose->unique_id(),3,point->unique_id());
  return true;
}