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(); } }