C++ Vector3::distance方法代码示例

 double DistanceFromLineSegment(
     const tf::Vector3& line_start,
     const tf::Vector3& line_end,
     const tf::Vector3& point)
 {    
   return point.distance(ProjectToLineSegment(line_start, line_end, point));
 }

bool Hand_filter::jumped(const tf::Vector3& p_current,const tf::Vector3& p_previous, const float threashold) const{
    if(p_current.distance(p_previous) < threashold){
        return false;
    }else{
        return true;
    }
}

int farthestPoint(const tf::Vector3& point, const std::vector<tf::Vector3>& hand_positions)
{
  double dist, max_distant = 0;
  size_t index = 0;
  for (size_t i = 0; i < hand_positions.size(); i++)
  {
    dist = point.distance(hand_positions[i]);
    if (dist > max_distant)
    {
      max_distant = dist;
      index = i;
    }
  }
  return index;
}

void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){

    if(bFirst){

        origin_buffer.push_back(origin);
        orientation_buffer.push_back(orientation);

        if(origin_buffer.size() == origin_buffer.capacity()){
            bFirst = false;
            ROS_INFO("====== jump filter full ======");
        }

    }else{

        origin_tmp      = origin_buffer[origin_buffer.size()-1];
        orientation_tmp = orientation_buffer[orientation_buffer.size()-1];

        if(bDebug){
            std::cout<< "=== jum debug === " << std::endl;
            std::cout<< "p    : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl;
            std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl;
            std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl;

            std::cout<< "q    : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() <<  "\t" << orientation.w() << std::endl;
            std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl;
            std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl;
        }

        /// Position jump
        if(jumped(origin,origin_tmp,origin_threashold)){
            ROS_INFO("position jumped !");
            origin = origin_tmp;
           // exit(0);
        }else{
            origin_buffer.push_back(origin);
        }

        /// Orientation jump
        if(jumped(orientation,orientation_tmp,orientation_threashold)){
            ROS_INFO("orientation jumped !");
            orientation = orientation_tmp;
            //exit(0);
        }else{
            orientation_buffer.push_back(orientation);
        }
    }
}

  void navCB(const ardrone_autonomy::NavdataConstPtr& nav_msg)
  {
    if (last_stamp > nav_msg->header.stamp)
      got_first_nav = false;

    if (!got_first_nav)
    {
      // clear markers
      markers.init();

      // clear ekf
      kalman_filter.initFilter();

      // clear rest
      lastMarkerPos = tf::Vector3(0, 0, 0);
      sumRelPos = tf::Vector3(0, 0, 0);
      numRelPos = 0;
      lastPos1t = -1;
      lastPos95t = 1;
      lastPublished = ros::Time::now();

      // init
      got_first_nav = true;
      last_stamp = nav_msg->header.stamp;
      last_yaw = nav_msg->rotZ;

      return;
    }

    // calc time diff
    double dt_s = (nav_msg->header.stamp - last_stamp).toNSec() / (1000.0 * 1000.0 * 1000.0);
    last_stamp = nav_msg->header.stamp;

    // calc dx, dy
    // transforme to "our" drone CO
    float dx = -dt_s * nav_msg->vy / 1000; // in m/s
    float dy = dt_s * nav_msg->vx / 1000; // in m/s

    // predict EKF
    Eigen::Vector3f odometry;
    odometry(0) = dx; // local position update
    odometry(1) = dy; // local position update
    odometry(2) = (nav_msg->rotZ - last_yaw) / 180 * M_PI; // treat absolute value as incremental update

    last_yaw = nav_msg->rotZ;

    // update pose of robot according to odometry measurement
    // this also increases the uncertainty of the filter
    kalman_filter.predictionStep(odometry);

    float vx_global = cos(kalman_filter.state(2)) * odometry(0) - sin(kalman_filter.state(2)) * odometry(1);
    float vy_global = sin(kalman_filter.state(2)) * odometry(0) + cos(kalman_filter.state(2)) * odometry(1);

    tf::Vector3 pos = tf::Vector3(kalman_filter.state[0], kalman_filter.state[1], nav_msg->altd / 1000.0);

    visnav_project::State s;
    s.header = nav_msg->header;
    s.x = pos.x();
    s.y = pos.y();
    s.z = pos.z();
    s.vx = vx_global;
    s.vy = vy_global;
    s.yaw = kalman_filter.state[2];
    pub_pose.publish(s);

    // only publish at 30hz....
    if ((ros::Time::now() - lastPublished).toSec() > 0.01)
    {

      // add to ekf_marker
      ekf_marker.addFilterState(kalman_filter.state, kalman_filter.sigma, nav_msg->altd / 1000.0);

      // make pos
      tf::Vector3 pos = tf::Vector3(kalman_filter.state[0], kalman_filter.state[1], nav_msg->altd / 1000.0);
      tf::Transform transform;
      transform.setOrigin(pos);
      transform.setRotation(tf::Quaternion(0, 0, M_PI / 2 + kalman_filter.state[2]));

      // add to marker array?
      if (lastMarkerPos.distance(pos) > 0.1)
      {
        //markers.addMarkerPose(tf::StampedTransform(transform, nav_msg->header.stamp, "/world", "/ardrone"));
        lastMarkerPos = pos;
      }

      // publish
      ekf_marker.publish_last_n_states(1);
      markers.publish_markers();

      lastPublished = ros::Time::now();
    }
  }