コード例 #1
0
ファイル: main.cpp プロジェクト: roijo/touch_plus_source_code
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);
}
コード例 #2
0
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;
}
コード例 #3
0
void compute()
{
	updated = false;

	ipc->update();

	if (wait_for_device_bool)
		wait_for_device();

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

	if ((mode == "surface" && imu.pitch > 60) || (mode == "tool" && imu.pitch < 60))
	{
		COUT << "exit 1" << endl;

		if (child_module_name != "")
			ipc->send_message(child_module_name, "exit", "");

		exit(0);
	}

	//----------------------------------------core algorithm----------------------------------------

	Mat image_flipped;
	if (mode == "surface")
		flip(image_current_frame, image_flipped, 0);
	else
		flip(image_current_frame, image_flipped, 0);
		// image_flipped = image_current_frame.clone();

	Mat image0 = image_flipped(Rect(0, 0, 640, 480));
	Mat image1 = image_flipped(Rect(640, 0, 640, 480));

	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;
	compute_channel_diff_image(image_small0, image_preprocessed0, exposure_adjusted, "image_preprocessed0");
	compute_channel_diff_image(image_small1, image_preprocessed1, exposure_adjusted, "image_preprocessed1");

	Mat image_preprocessed_smoothed0;
	Mat image_preprocessed_smoothed1;
	GaussianBlur(image_preprocessed0, image_preprocessed_smoothed0, Size(1, 19), 0, 0);
	GaussianBlur(image_preprocessed1, image_preprocessed_smoothed1, Size(1, 19), 0, 0);

	if (!CameraInitializerNew::adjust_exposure(camera, image_preprocessed0))
		return;

	exposure_adjusted = true;

	// imshow("image_small0", image_small0);
	// imshow("image_preprocessed0", image_preprocessed0);

	bool proceed0 = motion_processor0.compute(image_preprocessed_smoothed0, "0", false);
	bool proceed1 = motion_processor1.compute(image_preprocessed_smoothed1, "1", false);
	bool proceed = proceed0 && proceed1;

	if (proceed)
	{
		proceed0 = foreground_extractor0.compute(image_preprocessed0, image_preprocessed_smoothed0, motion_processor0, "0", true);
		proceed1 = foreground_extractor1.compute(image_preprocessed1, image_preprocessed_smoothed1, motion_processor1, "1", true);
		proceed = proceed0 && proceed1;
	}
	else
		hide_cursors();

	if (proceed)
	{
		proceed0 = hand_splitter0.compute(foreground_extractor0, motion_processor0, "0");
		proceed1 = hand_splitter1.compute(foreground_extractor1, motion_processor1, "1");
		proceed = proceed0 && proceed1;
	}
	else
		hide_cursors();

	if (mode == "surface" && proceed)
	{
		proceed0 = mono_processor0.compute(hand_splitter0, "0", false);
		proceed1 = mono_processor1.compute(hand_splitter1, "1", false);
		proceed = false;
		proceed = proceed0 && proceed1;

		if (proceed)
		{
			// stereo_processor.compute(mono_processor0, mono_processor1, motion_processor0, motion_processor1);

			points_pool[points_pool_count] = mono_processor0.points_unwrapped_result;
			points_ptr = &(points_pool[points_pool_count]);

			++points_pool_count;
			if (points_pool_count == points_pool_count_max)
				points_pool_count = 0;

			initialized = true;

			if (!show_point_sent)
			{
				ipc->send_message("menu_plus", "show point", "");
				show_point_sent = true;
			}

			if (pose_name == "point")
			{
				if (!show_calibration_sent)
				{
					ipc->send_message("menu_plus", "show calibration", "");
					show_calibration_sent = true;
				}

				hand_resolver.compute(mono_processor0, mono_processor1, motion_processor0, motion_processor1, image0, image1, reprojector);
				pointer_mapper.compute(hand_resolver, reprojector);

				if (pointer_mapper.calibrated)
				{
					if (enable_imshow)
					{
						enable_imshow = false;
						destroyAllWindows();
					}
					show_cursor_index = true;
				}

				if (show_cursor_index && pointer_mapper.thumb_down && pointer_mapper.index_down)
					show_cursor_thumb = true;
				else
					show_cursor_thumb = false;
			}
			else
			{
				pointer_mapper.reset();

				if (pose_name != "point")
				{
					show_cursor_index = false;
					show_cursor_thumb = false;
				}
			}
		}
		else
			hide_cursors();

		if (!pinch_to_zoom)
		{
			if (show_cursor_index)
			{
				ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_cursor_index.x) + "!" +
														 to_string(pointer_mapper.pt_cursor_index.y) + "!" +
														 to_string(pointer_mapper.dist_cursor_index_plane) + "!" +
														 to_string(pointer_mapper.index_down) + "!index");
			}
			else
				ipc->send_udp_message("win_cursor_plus", "hide_cursor_index");

			ipc->send_udp_message("win_cursor_plus", "hide_cursor_thumb");
		}
		else
		{
			ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_pinch_to_zoom_index.x) + "!" +
													 to_string(pointer_mapper.pt_pinch_to_zoom_index.y) + "!0!1!index");

			ipc->send_udp_message("win_cursor_plus", to_string(pointer_mapper.pt_pinch_to_zoom_thumb.x) + "!" +
													 to_string(pointer_mapper.pt_pinch_to_zoom_thumb.y) + "!0!1!thumb");
		}

		ipc->send_udp_message("win_cursor_plus", "update!" + to_string(frame_num));
	}
	else if (mode == "tool" && proceed)
	{
		enable_imshow = true;

		Mat image_active_light0;
		Mat image_active_light1;
		compute_active_light_image(image_small0, image_preprocessed0, image_active_light0);
		compute_active_light_image(image_small1, image_preprocessed1, image_active_light1);

		proceed0 = tool_mono_processor0.compute(image_active_light0, image_preprocessed0, "0");
		proceed1 = tool_mono_processor1.compute(image_active_light1, image_preprocessed1, "1");
		proceed = proceed0 && proceed1;

		if (proceed)
		{
			proceed0 = tool_stereo_processor.compute(tool_mono_processor0, tool_mono_processor1);
			proceed1 = tool_stereo_processor.matches.size() >= 4;
			proceed = proceed0 && proceed1;

			if (proceed)
			{
				tool_pointer_mapper.compute(reprojector, tool_stereo_processor, image0, image1);

				string data = "";
				data += to_string(tool_pointer_mapper.pt0.x) + "!" +
						to_string(tool_pointer_mapper.pt0.y) + "!" +
						to_string(tool_pointer_mapper.pt0.z) + "!";
				data += to_string(tool_pointer_mapper.pt1.x) + "!" +
						to_string(tool_pointer_mapper.pt1.y) + "!" +
						to_string(tool_pointer_mapper.pt1.z) + "!";
				data += to_string(tool_pointer_mapper.pt2.x) + "!" +
						to_string(tool_pointer_mapper.pt2.y) + "!" +
						to_string(tool_pointer_mapper.pt2.z) + "!";
				data += to_string(tool_pointer_mapper.pt3.x) + "!" +
						to_string(tool_pointer_mapper.pt3.y) + "!" +
						to_string(tool_pointer_mapper.pt3.z) + "!";
				data += to_string(tool_pointer_mapper.pt_center.x) + "!" +
						to_string(tool_pointer_mapper.pt_center.y) + "!" +
						to_string(tool_pointer_mapper.pt_center.z);

				ipc->send_udp_message("unity_demo", data);
			}
		}
	}

	++frame_num;

	if (increment_keypress_count)
	{
		if (keypress_count == calibration_points_count)
		{
			ipc->send_message("menu_plus", "show calibration next", "");

			COUT << "step " << calibration_step << " complete" << endl; 

			++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)
		{
			ipc->send_message("menu_plus", "show stage", "");

			pointer_mapper.compute_calibration_points();
			calibrating = false;
			increment_keypress_count = false;

			COUT << "calibration finished" << endl;
		}

		++keypress_count;
	}

	if (enable_imshow)
		waitKey(1);
}