void PointerMapper::compute(HandResolver& hand_resolver, Reprojector& reprojector)
{
active = false;
if (calibrated)
{
float dist_max = value_store.get_float("dist_max", 0);
float dist = get_distance(pt_palm, pt_index);
float ratio = dist / dist_max;
// COUT << ratio << endl;
}
else
{
float dist_max = value_store.get_float("dist_max", 0);
float dist = get_distance(pt_palm, pt_index);
if (dist > dist_max)
dist_max = dist;
value_store.set_float("dist_max", dist_max);
}
pt_palm = reprojector.reproject_to_3d(hand_resolver.pt_precise_palm0.x, hand_resolver.pt_precise_palm0.y,
hand_resolver.pt_precise_palm1.x, hand_resolver.pt_precise_palm1.y);
compute_cursor_point(index_down, hand_resolver.pt_precise_index0, hand_resolver.pt_precise_index1,
pt_index, reprojector, pt_cursor_index, dist_cursor_index_plane, actuate_dist, "compute_index");
compute_cursor_point(thumb_down, hand_resolver.pt_precise_thumb0, hand_resolver.pt_precise_thumb1,
pt_thumb, reprojector, pt_cursor_thumb, dist_cursor_thumb_plane, actuate_dist + 10, "compute_thumb");
if (pt_cursor_index.y > 1500)
{
pose_name = "";
index_down = false;
}
if (calibrated)
{
if (!index_down)
thumb_down = false;
compute_pinch_to_zoom(hand_resolver);
}
}
bool compute()
{
bool waiting_for_image_set = false;
updated = false;
++frame_count;
if (image_current_frame.cols == 0)
{
console_log("bad frame");
return false;
}
Mat image_flipped;
flip(image_current_frame, image_flipped, 0);
Mat image0 = image_flipped(Rect(0, 0, 640, 480));
Mat image1 = image_flipped(Rect(640, 0, 640, 480));
if (frame_count == 1)
setup_on_first_frame();
if (!play || settings.touch_control != "1")
{
if (enable_imshow)
waitKey(1);
return false;
}
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
imu.compute(x_accel, y_accel, z_accel);
//----------------------------------------core algorithm----------------------------------------
Mat image_small0;
Mat image_small1;
resize(image0, image_small0, Size(160, 120), 0, 0, INTER_LINEAR);
resize(image1, image_small1, Size(160, 120), 0, 0, INTER_LINEAR);
Mat image_preprocessed0;
Mat image_preprocessed1;
static bool exposure_set = false;
bool normalized = compute_channel_diff_image(image_small0, image_preprocessed0, exposure_set, "image_preprocessed0", true, exposure_set);
compute_channel_diff_image(image_small1, image_preprocessed1, exposure_set, "image_preprocessed1");
GaussianBlur(image_preprocessed0, image_preprocessed0, Size(3, 9), 0, 0);
GaussianBlur(image_preprocessed1, image_preprocessed1, Size(3, 9), 0, 0);
if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
{
static int step_count = 0;
if (step_count == 3)
surface_computer.init(image0);
++step_count;
return false;
}
exposure_set = true;
#if 0
{
Mat image_remapped0 = reprojector.remap(&image_small0, 0, true);
Mat image_remapped1 = reprojector.remap(&image_small1, 1, true);
reprojector.y_align(image_remapped0, image_remapped1, false);
GaussianBlur(image_remapped0, image_remapped0, Size(9, 9), 0, 0);
GaussianBlur(image_remapped1, image_remapped1, Size(9, 9), 0, 0);
Mat image_disparity;
static StereoSGBM stereo_sgbm(0, 32, 21, 0, 0, 0, 0, 20, 0, 0, false);
stereo_sgbm(image_remapped0, image_remapped1, image_disparity);
Mat image_disparity_8u;
double minVal, maxVal;
minMaxLoc(image_disparity, &minVal, &maxVal);
image_disparity.convertTo(image_disparity_8u, CV_8UC1, 255 / (maxVal - minVal));
imshow("image_remapped0", image_remapped0);
imshow("image_remapped1", image_remapped1);
imshow("image_disparity_8u", image_disparity_8u);
}
#endif
/*static bool show_wiggle_sent = false;
if (!show_wiggle_sent)
{
if (child_module_name != "")
ipc->open_udp_channel(child_module_name);
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");//todo
}
show_wiggle_sent = true;*/
if (enable_imshow)
{
imshow("image_small1", image_small1);
// imshow("image_preprocessed1", image_preprocessed1);
// setMouseCallback("image_preprocessed1", left_mouse_cb, NULL);
// waitKey(1);
// return false;
}
algo_name_vec_old = algo_name_vec;
algo_name_vec.clear();
static bool motion_processor_proceed = false;
static bool construct_background = false;
static bool first_pass = true;
bool proceed0;
bool proceed1;
if (normalized)
{
proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, imu.pitch,
construct_background, "0", true);
proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, imu.pitch,
construct_background, "1", false);
}
if (first_pass && motion_processor0.both_moving && motion_processor1.both_moving)
{
console_log("readjusting exposure");
first_pass = false;
exposure_set = false;
mat_functions_low_pass_filter.reset();
CameraInitializerNew::adjust_exposure(camera, image_preprocessed0, true);
}
else if (!first_pass)
construct_background = true;
if (!construct_background)
{
proceed0 = false;
proceed1 = false;
}
if (proceed0 && proceed1)
motion_processor_proceed = true;
bool proceed = motion_processor_proceed;
static bool menu_plus_signal0 = false;
if (!menu_plus_signal0)
{
menu_plus_signal0 = true;
// ipc->send_message("menu_plus", "hide window", "");
}
if (proceed)
{
proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", true);
proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0", false);
proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
waiting_for_image = true;
waiting_for_image_set = true;
proceed1 = scopa1.compute_mono0(hand_splitter1, pose_estimator, "1", false);
proceed0 = scopa0.compute_mono0(hand_splitter0, pose_estimator, "0", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
motion_processor0.target_frame = 10;
motion_processor1.target_frame = 10;
SCOPA::compute_stereo();
scopa1.compute_mono1("1");
scopa0.compute_mono1("0");
}
if (enable_imshow)
waitKey(1);
return waiting_for_image_set;
}
void setup_on_first_frame()
{
console_log("on first frame");
ipc->send_message("menu_plus", "set status", "verifying serial number");
serial_number = camera->getSerialNumber();
if (serial_number.size() == 10)
{
string char_string = "";
int char_count = 0;
for (char& c : serial_number)
{
char_string += c;
++char_count;
if (char_count == 4)
break;
}
if (char_string == "0101")
serial_verified = true;
}
if (!serial_verified)
wait_for_device();
ipc->send_message("menu_plus", "set loading progress", "10");
console_log("serial number: " + serial_number);
data_path_current_module = data_path + slash + serial_number;
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
Point3f heading = imu.compute_azimuth(x_accel, y_accel, z_accel);
ipc->send_message("menu_plus", "set status", "loading calibration data");
reprojector.load(*ipc, true);
ipc->send_message("menu_plus", "set loading progress", "40");
ipc->send_message("menu_plus", "set status", "initializing camera");
CameraInitializerNew::init(camera);
ipc->send_message("menu_plus", "set status", "loading pose data");
pose_estimator.init();
ipc->send_message("menu_plus", "set loading progress", "70");
ipc->send_message("menu_plus", "set status", "starting cursor subprocess");
#ifdef _WIN32
child_module_name = "win_cursor_plus";
if (IsWindows8OrGreater())
child_module_path = executable_path + slash + "win_cursor_plus" + slash + "win_cursor_plus" + extension0;
else
child_module_path = executable_path + slash + "win_cursor_plus_fallback" + slash + "win_cursor_plus" + extension0;
#elif __APPLE__
//todo: port to OSX
#endif
ipc->map_function("toggle imshow", [](const string message_body)
{
if (enable_imshow)
enable_imshow = false;
else
enable_imshow = true;
});
ipc->map_function("load settings", [](const string message_body)
{
load_settings();
});
ipc->send_message("menu_plus", "set loading progress", "100");
}
void compute()
{
updated = false;
if (image_current_frame.cols == 0)
{
COUT << "bad frame" << endl;
return;
}
Mat image_flipped;
flip(image_current_frame, image_flipped, 0);
Mat image0 = image_flipped(Rect(0, 0, 640, 480));
Mat image1 = image_flipped(Rect(640, 0, 640, 480));
if (first_frame)
{
on_first_frame();
first_frame = false;
}
if (!play || settings.touch_control != "1")
{
hide_cursors();
if (enable_imshow)
waitKey(1);
return;
}
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
imu.compute(x_accel, y_accel, z_accel);
//----------------------------------------core algorithm----------------------------------------
Mat image_small0;
Mat image_small1;
resize(image0, image_small0, Size(160, 120), 0, 0, INTER_LINEAR);
resize(image1, image_small1, Size(160, 120), 0, 0, INTER_LINEAR);
Mat image_preprocessed0;
Mat image_preprocessed1;
bool normalized = compute_channel_diff_image(image_small0, image_preprocessed0, exposure_adjusted, "image_preprocessed0", true);
compute_channel_diff_image(image_small1, image_preprocessed1, exposure_adjusted, "image_preprocessed1", false);
GaussianBlur(image_preprocessed0, image_preprocessed0, Size(3, 9), 0, 0);
GaussianBlur(image_preprocessed1, image_preprocessed1, Size(3, 9), 0, 0);
if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
{
static int step_count = 0;
if (step_count == 3)
{
surface_computer.init(image0);
}
++step_count;
return;
}
exposure_adjusted = true;
#ifdef false
{
Mat image_remapped0 = reprojector.remap(&image_small0, 0, true);
Mat image_remapped1 = reprojector.remap(&image_small1, 1, true);
reprojector.y_align(image_remapped0, image_remapped1, false);
GaussianBlur(image_remapped0, image_remapped0, Size(9, 9), 0, 0);
GaussianBlur(image_remapped1, image_remapped1, Size(9, 9), 0, 0);
Mat image_disparity;
static StereoSGBM stereo_sgbm(0, 32, 21, 0, 0, 0, 0, 20, 0, 0, false);
stereo_sgbm(image_remapped0, image_remapped1, image_disparity);
Mat image_disparity_8u;
double minVal, maxVal;
minMaxLoc(image_disparity, &minVal, &maxVal);
image_disparity.convertTo(image_disparity_8u, CV_8UC1, 255 / (maxVal - minVal));
imshow("image_remapped0", image_remapped0);
imshow("image_remapped1", image_remapped1);
imshow("image_disparity_8u", image_disparity_8u);
}
#endif
/*static bool show_wiggle_sent = false;
if (!show_wiggle_sent)
{
if (child_module_name != "")
ipc->open_udp_channel(child_module_name);
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");//todo
}
show_wiggle_sent = true;*/
if (enable_imshow)
{
imshow("image_small1", image_small1);
imshow("image_preprocessed1", image_preprocessed1);
// setMouseCallback("image_preprocessed1", left_mouse_cb, NULL);
// waitKey(1);
// return;
}
algo_name_vec_old = algo_name_vec;
algo_name_vec.clear();
static bool motion_processor_proceed = false;
static bool construct_background = false;
static bool first_pass = true;
bool proceed0;
bool proceed1;
if (normalized)
{
proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, imu.pitch,
construct_background, "0", true);
proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, imu.pitch,
construct_background, "1", true);
}
if (first_pass && motion_processor0.both_moving)
{
COUT << "readjusting exposure" << endl;
first_pass = false;
exposure_adjusted = false;
CameraInitializerNew::adjust_exposure(camera, image_preprocessed0, true);
}
else if (!first_pass)
construct_background = true;
if (!construct_background)
{
proceed0 = false;
proceed1 = false;
}
if (proceed0 && proceed1)
motion_processor_proceed = true;
bool proceed = motion_processor_proceed;
if (proceed)
{
proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", false);
proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0");
proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1");
proceed = proceed0 && proceed1;
}
proceed = false;
if (proceed)
{
proceed0 = mono_processor0.compute(hand_splitter0, "0", false);
proceed1 = mono_processor1.compute(hand_splitter1, "1", false);
proceed = proceed0 && proceed1;
}
if (proceed)
{
stereo_processor.compute(mono_processor0, mono_processor1, point_resolver, pointer_mapper, image0, image1);
}
++frame_num;
if (increment_keypress_count)
{
if (keypress_count == calibration_points_count)
{
COUT << "step " << calibration_step << " complete" << endl;
// ipc->send_message("menu_plus", "show calibration next", "");//todo
++calibration_step;
}
else if (keypress_count < calibration_points_count)
{
float percentage = (keypress_count * 100.0 / calibration_points_count);
COUT << percentage << endl;
pointer_mapper.add_calibration_point(calibration_step);
}
if (calibration_step == 4)
{
pointer_mapper.compute_calibration_points();
calibrating = false;
increment_keypress_count = false;
COUT << "calibration finished" << endl;
}
++keypress_count;
}
if (enable_imshow)
waitKey(1);
}
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();
}
void on_first_frame()
{
bool serial_verfied = false;
serial_number = camera->getSerialNumber();
if (serial_number.size() == 10)
{
string char_string = "";
int char_count = 0;
for (char& c : serial_number)
{
char_string += c;
++char_count;
if (char_count == 4)
break;
}
if (char_string == "0101")
serial_verfied = true;
}
if (!serial_verfied)
{
hide_cursors();
COUT << "please reconnect your Touch+ sensor" << endl;
COUT << "restarting" << endl;
wait_for_device();
}
data_path_current_module = data_path + "\\" + serial_number;
int x_accel;
int y_accel;
int z_accel;
camera->getAccelerometerValues(&x_accel, &y_accel, &z_accel);
Point3d heading = imu.compute_azimuth(x_accel, y_accel, z_accel);
// if (heading.y > 60)
// mode = "tool";
// else
mode = "surface";
ipc->send_message("menu_plus", "show notification", "Please wait:Initializing Touch+ Software");
reprojector.load(*ipc);
CameraInitializerNew::init(camera);
pose_estimator.init();
if (mode == "surface")
{
child_module_name = "win_cursor_plus";
if (IsWindows8OrGreater())
child_module_path = executable_path + "\\win_cursor_plus\\win_cursor_plus.exe";
else
child_module_path = executable_path + "\\win_cursor_plus_fallback\\win_cursor_plus.exe";
ipc->open_udp_channel("win_cursor_plus");
ipc->send_message("menu_plus", "show window", "");
ipc->send_message("menu_plus", "show wiggle", "");
}
else
{
ipc->open_udp_channel("unity_demo", 3333);
ipc->send_message("menu_plus", "hide window", "");
}
ipc->map_function("toggle imshow", [](const string message_body)
{
if (enable_imshow)
{
enable_imshow = false;
destroyAllWindows();
}
else
enable_imshow = true;
});
ipc->map_function("load settings", [](const string message_body)
{
load_settings();
});
increment_wait_count = true;
}
