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