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