vfloat ang2projvec(const vec& r1, const vec& r2, const vec& normal) { pvecerror( "vfloat ang2projvec(const vec& r1, const vec& r2, const vec& normal)"); vec rt1 = project_to_plane(r1, normal); vec rt2 = project_to_plane(r2, normal); if (rt1 == dv0 || rt2 == dv0) { vecerror = 1; return 0; } vfloat tang = ang2vec(rt1, rt2); if (tang == 0) return tang; // projections are parallel vec at = rt1 || rt2; int i = check_par(at, normal, 0.0001); //mcout<<"r1="<<r1<<"r2="<<r2<<"normal="<<normal // <<"rt1="<<rt1<<"rt2="<<rt2<<"\ntang="<<tang // <<"\nat="<<at<<" i="<<i<<'\n'; if (i == -1) return 2.0 * M_PI - tang; return tang; // it works if angle <= PI }
void PointerMapper::compute_calibration_points() { vector<float> x_vec; vector<float> y_vec; vector<float> z_vec; float x_median; float y_median; float z_median; if (pt_calib_vec0.size() > 0 && pt_calib_vec0.size() > 1 && pt_calib_vec0.size() > 2 && pt_calib_vec0.size() > 3) { x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec0) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib0 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec1) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib1 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec2) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib2 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec3) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib3 = Point3f(x_median, y_median, z_median); const float x0_plane = pt_calib0.x; const float y0_plane = pt_calib0.y; const float z0_plane = pt_calib0.z + 100; const float x1_plane = pt_calib1.x; const float y1_plane = pt_calib1.y; const float z1_plane = pt_calib1.z + 100; const float x2_plane = pt_calib2.x; const float y2_plane = pt_calib2.y; const float z2_plane = pt_calib2.z + 100; const float x3_plane = pt_calib3.x; const float y3_plane = pt_calib3.y; const float z3_plane = pt_calib3.z + 100; plane = Plane(Point3f(x0_plane, y0_plane, z0_plane), Point3f(x1_plane, y1_plane, z1_plane), Point3f(x2_plane, y2_plane, z2_plane)); direction_plane = cross_product(Point3f(x0_plane - x1_plane, y0_plane - y1_plane, z0_plane - z1_plane), Point3f(x2_plane - x1_plane, y2_plane - y1_plane, z2_plane - z1_plane)); direction_plane.x = -direction_plane.x; direction_plane.y = -direction_plane.y; direction_plane.z = -direction_plane.z; project_to_plane(pt_calib0, pt_calib0_projected, dist_calib0_plane); project_to_plane(pt_calib1, pt_calib1_projected, dist_calib1_plane); project_to_plane(pt_calib2, pt_calib2_projected, dist_calib2_plane); project_to_plane(pt_calib3, pt_calib3_projected, dist_calib3_plane); const float d0 = get_distance(pt_calib0_projected.x, pt_calib0_projected.y, pt_calib2_projected.x, pt_calib2_projected.y); const float d1 = get_distance(pt_calib1_projected.x, pt_calib1_projected.y, pt_calib3_projected.x, pt_calib3_projected.y); d_max = std::max(d0, d1); rect_warper.setSource(pt_calib0_projected.x, pt_calib0_projected.y, pt_calib1_projected.x, pt_calib1_projected.y, pt_calib2_projected.x, pt_calib2_projected.y, pt_calib3_projected.x, pt_calib3_projected.y); rect_warper.setDestination(0, 0, 1000, 0, 1000, 1000, 0, 1000); calibrated = true; } else COUT << "not calibrated" << endl; }
void PointerMapper::compute_cursor_point(bool& target_down, Point2f& pt_target0, Point2f& pt_target1, Point3f& pt_target, Reprojector& reprojector, Point2f& pt_cursor, float& dist_cursor_target_plane, const float actuation_dist, string name) { LowPassFilter* low_pass_filter = value_store.get_low_pass_filter("low_pass_filter" + name); if (do_reset) { do_reset = false; low_pass_filter->reset(); } if (pt_target0.x != -1 && pt_target1.x != -1) { active = true; pt_target = reprojector.reproject_to_3d(pt_target0.x, pt_target0.y, pt_target1.x, pt_target1.y); if (calibrated) { Point3f pt_target_projected; float dist_target_plane; project_to_plane(pt_target, pt_target_projected, dist_target_plane); rect_warper.warp(pt_target_projected.x, pt_target_projected.y, pt_cursor.x, pt_cursor.y); if (value_store.has_point2f("pt_cursor" + name)) { Point2f temp = value_store.get_point2f("pt_cursor" + name); float alpha = get_distance(pt_cursor, temp) / 1000; if (alpha < 0.05) alpha = 0.05; else if (alpha > 1) alpha = 1; // low_pass_filter->compute(alpha, 0.5, "alpha" + name); low_pass_filter->compute(pt_cursor, alpha, "pt_cursor" + name); } value_store.set_point2f("pt_cursor" + name, pt_cursor); float hit_dist = compute_hit_dist(pt_target_projected); dist_cursor_target_plane = dist_target_plane - hit_dist; low_pass_filter->compute(dist_cursor_target_plane, 0.2, "dist_cursor_target_plane"); float hit_dist_processed = hit_dist; // float hit_dist_processed_old = value_store.get_float("hit_dist_processed_old" + name, hit_dist_processed); // hit_dist_processed += ((hit_dist_processed - hit_dist_processed_old) * 0.25); // value_store.set_float("hit_dist_processed_old" + name, hit_dist_processed); float dist_cursor_target_plane_no_lowpass = dist_target_plane - hit_dist_processed; if (dist_cursor_target_plane_no_lowpass <= actuation_dist + 2) value_store.set_bool("actuated" + name, true); if (value_store.get_bool("actuated" + name)) { target_down = true; if (dist_cursor_target_plane_no_lowpass > actuation_dist + 5) { target_down = false; value_store.set_bool("actuated" + name, false); } } } } else low_pass_filter->reset(); }