<commit_before>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;
	int led_red_status = 0;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// paint road area
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		//std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			// obtacle on the road: draw rectangle, adjust pwm for servo, blink led
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			pwm_servo_right(high_right + servo_offset + road_offset + obstacle_position(obstacle.y + (obstacle.height / 2)));

			led_red_status ++;

			if(led_red_status < 2){
				led_R.high();
			} else {
				led_R.low();
				if(led_red_status == 4)
					led_red_status = 0;
			}
		} else {
			led_R.low();
			led_red_status = 0;
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));

   			pwm_servo_left(high_left + servo_offset + road_offset + car_position(0));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 2){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 2){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return 1;
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<commit_msg>Led toggles on fps rate<commit_after>// arm-poky-linux-gnueabi-gcc -lopencv_video -lopencv_core -lopencv_highgui -lopencv_imgproc -lstdc++ -lpthread -shared-libgcc opencv.cpp -o opencv

#include "evision.h"
#include "tracer.h"
#include "gpio.h"

// Global variables
GPIO pwm_right(1, "out"), pwm_left(3, "out"); // led_right(2 , "out");
GPIO led_front(5, "out"), led_R(4, "out"), start(7 ,"out"); // 0,2,6 bulit
int high_right = 600, high_left = 600, period = 20000; //PWM high time in us
int top_edge = 50, servo_offset = 0, road_offset = 0;

// GUI globals
cv::Mat guiframe;
bool new_frame = false;
bool running = true;

// Window titles
char main_window_name[] = "Lightning Asystant :: Team eVision";
char settings_window_name[] = "Settings :: Team eVision";

// Settings variables
int settings_show_step = 0;
int settings_contrast = 0;
int settings_blur = 1;
int settings_threshold = 128;
int settings_servo_offset = 500;
int settings_road_approx = 5;
int settings_middle_line = 240;

// Graphics
std::vector<std::vector<cv::Point>> contours;

//Functions declaration
void pwm_servo_right(int);
void pwm_servo_left(int);
int car_position(int);
int obstacle_position(int);

int main(int argc, char** argv)
{
    pthread_t processing_thread;
    long time_old;
    int fps_counter = 0;
    int gui_key;

	// for cpu affinity
	cpu_set_t cpuset;
	int cpu = 0;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    // process priority
    setpriority(PRIO_PROCESS, 0, -20);

    // GUI setup
	cv::namedWindow(main_window_name);

	// Setup window
	cv::namedWindow(settings_window_name);
	cv::createTrackbar("Show Step   ", settings_window_name, &settings_show_step, LAST_STEP-1);
	cv::createTrackbar("Contrast    ", settings_window_name, &settings_contrast, 1);
	cv::createTrackbar("Mean Blur   ", settings_window_name, &settings_blur, 1);
	cv::createTrackbar("Threshold   ", settings_window_name, &settings_threshold, 255);
	cv::createTrackbar("Road Approx ", settings_window_name, &settings_road_approx, 30);
	cv::createTrackbar("Middle Line ", settings_window_name, &settings_middle_line, 320);
	cv::createTrackbar("Servo Offset", settings_window_name, &settings_servo_offset, 1000);
	cv::createTrackbar("Servo Right ", settings_window_name, &high_right, 1500);
	cv::createTrackbar("Servo Left  ", settings_window_name, &high_left, 1500);

	pthread_create(&processing_thread, NULL, processing_thread_function, NULL);
	//pthread_create(&pwm_thread, NULL, pwm_thread_function, NULL);

    while(running){

    	while(!new_frame){ } // loop

		cv::imshow(main_window_name, guiframe);

		// calculate fps
		if(time(NULL) != time_old){
			add_info(fps_counter);
			fps_counter = 1;
			time_old = time(NULL);
		} else {
			fps_counter++;
		}

		new_frame = 0;

		gui_key = cv::waitKey(5);
		if(gui_key >= 0) {
			gui_key %= 0xFF;

			// ESC
			if((gui_key == 43) || (gui_key == 27)){
				led_front.low();
				led_R.low();
				running = false;
			} else if((gui_key == 48) || (gui_key == 32)) {
				start.high(); // 1 = start/stop
				usleep(50000);
				start.low();
			} else if((gui_key == 50) || (gui_key == 66)) {
				led_front.toggle(); // 2 = faruri
			} else if((gui_key == 51) || (gui_key == 67)) {
				led_R.toggle(); // 3 = far pieton
			} else {
				std::cout << gui_key << std::endl;
			}
		}
    }

	pthread_join(processing_thread, NULL);

    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}

void send_frame_to_gui(cv::Mat &frame, int step){
	if((new_frame == 0) && (step == settings_show_step)){
		//cv::pyrUp(frame, guiframe);
		frame.copyTo(guiframe);
		new_frame = 1;
	}
}

bool contour_area(int a, int b){
	return cv::contourArea(contours[a]) > cv::contourArea(contours[b]);
}

bool get_obstacle(
	int parent,
	std::vector<std::vector<cv::Point>> contours,
	std::vector<cv::Vec4i> hierarchy,
	cv::Rect &obstacle){

	int start = hierarchy[parent][2];
	int closest = -1;
	bool found = false;

	while(start > 0){

		if(closest == -1){
			closest = start;
			found = true;
		} else if(cv::boundingRect(contours[start]).y > cv::boundingRect(contours[closest]).y){
			closest = start;
		}

		start = hierarchy[start][0];
	}

	if(found){
		obstacle = cv::boundingRect(contours[closest]);
	}

	return found;
}

void draw_obstacles(cv::Mat &threshold_frame, cv::Mat &cam_frame){
	std::vector<std::vector<cv::Point>> road(2);
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(threshold_frame, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
	if(contours.size() > 1){

		// create index vector
		std::vector<int> contour_indexes(contours.size());

		for(unsigned int i = 0; i < contours.size(); i++){
			contour_indexes[i] = i;
		}

		// sorting index vector acording to controur area
		std::sort(contour_indexes.begin(), contour_indexes.end(), contour_area);

		cv::approxPolyDP(cv::Mat(contours[contour_indexes[0]]), road[0], settings_road_approx, true);
		cv::approxPolyDP(cv::Mat(contours[contour_indexes[1]]), road[1], settings_road_approx, true);

		// validate road detection
		int road_edge = -1;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y > 235) && (road_edge == -1)){
				road_edge = road[0][i].x;

			} else if((road[0][i].y > 235) && (road[0][i].x < road_edge)){

				road_edge = road[0][i].x;
			}
		}

		if(road_edge < 0)
			return ;

		for(unsigned int i = 0; i < road[1].size(); ++i){
			if((road[1][i].y > 235) && (road[1][i].x > road_edge)){

				// wrong detection
				return;
			}
		}

		// paint road area
		cv::drawContours(cam_frame, road, 0, cv::Scalar(0, 255 ,0), 2);
		cv::drawContours(cam_frame, road, 1, cv::Scalar(255, 0, 0), 2);

		// get road offset
		// reference point is the left most point from the top edge of road contour
		int new_road_offset = 600;
		for(unsigned int i = 0; i < road[0].size(); ++i){
			if((road[0][i].y == top_edge) && (road[0][i].x < new_road_offset)){
				new_road_offset = road[0][i].x;
			}
		}

		if((new_road_offset > 50) && (new_road_offset < 250)){
			road_offset = new_road_offset;
		}

		//std::cout << road_offset << std::endl;

		cv::Rect obstacle;
		if(get_obstacle(contour_indexes[0], contours, hierarchy, obstacle)){
			// obtacle on the road: draw rectangle, adjust pwm for servo, blink led
			cv::rectangle(cam_frame, obstacle, cv::Scalar(0, 0, 255));
			pwm_servo_right(high_right + servo_offset + road_offset + obstacle_position(obstacle.y + (obstacle.height / 2)));

			led_R.toggle();
		} else {
			led_R.low();
		}

		if(get_obstacle(contour_indexes[1], contours, hierarchy, obstacle)){
			cv::rectangle(cam_frame, obstacle, cv::Scalar(255, 0, 255));

   			pwm_servo_left(high_left + servo_offset + road_offset + car_position(0));
		}

		cv::line(cam_frame, cv::Point(settings_middle_line, 320), cv::Point(settings_middle_line, top_edge), cv::Scalar(0, 255, 255));
	}
}

void *processing_thread_function(void* unsused)
{
	cv::VideoCapture 	cap(0); // camera interface
    cv::Mat 			frame, cam_frame, bw_frame, blur_frame, contrast_frame;
    cv::Mat				threshold_frame, canny_frame, contour_frame;
    Tracer				processing_tracer;

    // for cpu affinity
	cpu_set_t cpuset;
	int cpu = 1;

	CPU_ZERO(&cpuset);       //clears the cpuset
	CPU_SET( cpu , &cpuset); //set CPU 2 on cpuset
	sched_setaffinity(0, sizeof(cpuset), &cpuset);

    if(!cap.isOpened())  // check if we succeeded
    {
        std::cout << "Could not open default video device" << std::endl;
        pthread_exit(NULL);

    }

   	while(running) {
		if( !cap.read(frame) ){
			std::cout << "Camera was disconected";
			break;
		}

		cv::pyrDown(frame, cam_frame);
		send_frame_to_gui(cam_frame, SENSOR_IMAGE);
		processing_tracer.start();

		// All processing are done on gray image
		cv::cvtColor(cam_frame, bw_frame, CV_BGR2GRAY);
		processing_tracer.event("Convert to Gray");
		send_frame_to_gui(bw_frame, GRAY_IMAGE);

		// Increase contrast by distributing color histogram to contain all values
		if(settings_contrast){
			cv::equalizeHist(bw_frame, contrast_frame);
		} else {
			contrast_frame = bw_frame;
		}
		processing_tracer.event("Equalize histogram");
		send_frame_to_gui(contrast_frame, CONTRAST_IMAGE);

		// Apply a special blur filter which preserves edges
		if(settings_blur){
			cv::medianBlur(contrast_frame, blur_frame, 7);
		} else {
			blur_frame = contrast_frame;
		}
		processing_tracer.event("Median blur");
		send_frame_to_gui(blur_frame, BLUR_IMAGE);

		// detect edges using histeresys
		cv::Canny(contrast_frame, canny_frame, 30, 100);
		processing_tracer.event("Edge detection");
		send_frame_to_gui(canny_frame, CANNY_IMAGE);

		// Apply threshhold
		threshold(blur_frame, threshold_frame, settings_threshold, 255, CV_THRESH_BINARY);
		// Disable image top from detection to remove false edges
		cv::rectangle(threshold_frame, cv::Rect(0, 0, 320, top_edge), cv::Scalar(0), CV_FILLED);
		processing_tracer.event("Appling threshold");
		send_frame_to_gui(threshold_frame, THRESHOLD_IMAGE);

		// detect and paint contours
		draw_obstacles(threshold_frame, cam_frame);
		processing_tracer.event("Contour detection");
		send_frame_to_gui(cam_frame, CONTOUR_IMAGE);
	}

	processing_tracer.end();
	pthread_exit(NULL);
}

void pwm_servo_right(int h_r){
	int static local_hr = 0;
	if(abs(h_r - local_hr) > 2){
		local_hr = h_r;
		pwm_right.high();
		usleep(local_hr);
		pwm_right.low();
		usleep(period - local_hr);
	}
}

void pwm_servo_left(int h_l){
	int static local_hl = 0;
	if(abs(h_l - local_hl) > 2){
		local_hl = h_l;
		pwm_left.high();
		usleep(local_hl);
		pwm_left.low();
		usleep(period - local_hl);
	}
}

int obstacle_position(int y_position){
	if(y_position > top_edge){
		return int ( 0.9 * y_position + 15);
	} else {
		return 0;
	}
}

int car_position(int y_position){
	if(y_position > top_edge){
		return 1;
	} else {
		return 0;
	}
}

void add_info(int fps){
	// white canvas
	cv::Mat img_info (25, 320, CV_8UC3, cv::Scalar(255, 255, 255));

	cv::putText(img_info, std::string("FPS: ") + std::to_string(fps), cv::Point(5, 20),
			cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cv::Scalar(0,0,255), 1, CV_AA);

	cv::imshow(settings_window_name, img_info);
}
<|endoftext|>


<commit_before>/*
 * Quantum++
 *
 * Copyright (c) 2013 - 2014 Vlad Gheorghiu (vgheorgh@gmail.com)
 *
 * This file is part of Quantum++.
 *
 * Quantum++ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Quantum++ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Quantum++.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "qpp.h"

// #include "MATLAB/matlab.h" // support for MATLAB

using namespace qpp;

int main()
{
//	/*   comment this line with "//" to uncomment the whole example

	// Qudit Teleportation
	{
		std::size_t D = 3; // size of the system
		std::cout << std::endl << "**** Qudit Teleportation, D = " << D
				<< " ****" << std::endl;
		ket mes_AB = ket::Zero(D * D); // maximally entangled state resource
		for (std::size_t i = 0; i < D; i++)
			mes_AB += mket( { i, i }, { D, D });
		mes_AB /= std::sqrt((double) D);
		cmat Bell_aA = adjoint( // circuit that measures in the qudit Bell basis
				gt.CTRL(gt.Xd(D), { 0 }, { 1 }, 2, D)
						* kron(gt.Fd(D), gt.Id(D)));
		ket psi_a = randket(D); // random state as input on a
		std::cout << "** Initial state:" << std::endl;
		displn(psi_a);
		ket input_aAB = kron(psi_a, mes_AB); // joint input state aAB
		// output before measurement
		ket output_aAB = apply(input_aAB, Bell_aA, { 0, 1 }, 3, D);
		auto measured_aA = measure(ptrace2(prj(output_aAB), { D * D, D }),
				gt.Id(D * D)); // measure on aA
		std::discrete_distribution<std::size_t> dd(measured_aA.first.begin(),
				measured_aA.first.end());
		std::cout << "** Measurement probabilities: ";
		displn(measured_aA.first, ", ");
		std::size_t m = dd(rdevs._rng); // sample
		auto midx = n2multiidx(m, { D, D });
		std::cout << "** Measurement result: ";
		displn(midx, " ");
		// conditional result on B before correction
		ket output_m_aAB = apply(output_aAB, prj(mket(midx, { D, D })),
				{ 0, 1 }, 3, D) / std::sqrt(measured_aA.first[m]);
		cmat correction_B = powm(gt.Zd(D), midx[0])
				* powm(adjoint(gt.Xd(D)), midx[1]); // correction operator
				// apply correction on B
		output_aAB = apply(output_m_aAB, correction_B, { 2 }, 3, D);
		cmat rho_B = ptrace1(prj(output_aAB), { D * D, D });
		std::cout << "** Bob's density operator: " << std::endl;
		displn(rho_B);
		std::cout << "** Norm difference: " << norm(rho_B - prj(psi_a))
				<< std::endl; // verification
	}

	// Qudit Dense Coding
	{
		std::size_t D = 3; // size of the system
		std::cout << std::endl << "**** Qudit Dense Coding, D = " << D
				<< " ****" << std::endl;
		ket mes_AB = ket::Zero(D * D); // maximally entangled state resource
		for (std::size_t i = 0; i < D; i++)
			mes_AB += mket( { i, i }, { D, D });
		mes_AB /= std::sqrt((double) D);
		cmat Bell_AB = adjoint( // circuit that measures in the qudit Bell basis
				gt.CTRL(gt.Xd(D), { 0 }, { 1 }, 2, D)
						* kron(gt.Fd(D), gt.Id(D)));
		// equal probabilities of choosing a message
		std::uniform_int_distribution<std::size_t> udd(0, D * D - 1);
		std::size_t m_A = udd(rdevs._rng); // sample, obtain the message index
		auto midx = n2multiidx(m_A, { D, D });
		std::cout << "** Alice sent: ";
		displn(midx, " ");
		// Alice's operation
		cmat U_A = powm(gt.Zd(D), midx[0]) * powm(adjoint(gt.Xd(D)), midx[1]);
		// Alice encodes the message
		ket psi_AB = apply(mes_AB, U_A, { 0 }, 2, D);
		// Bob measures the joint system in the qudit Bell basis
		psi_AB = apply(psi_AB, Bell_AB, { 0, 1 }, 2, D);
		auto measured = measure(psi_AB, gt.Id(D * D));
		std::cout << "** Bob measurement probabilities: ";
		displn(measured.first, ", ");
		// Bob samples according to the measurement probabilities
		std::discrete_distribution<std::size_t> dd(measured.first.begin(),
				measured.first.end());
		std::size_t m_B = dd(rdevs._rng);
		std::cout << "** Bob received: ";
		displn(n2multiidx(m_B, { D, D }), " ") << std::endl;
	}

	// */
}
<commit_msg>commit<commit_after>/*
 * Quantum++
 *
 * Copyright (c) 2013 - 2014 Vlad Gheorghiu (vgheorgh@gmail.com)
 *
 * This file is part of Quantum++.
 *
 * Quantum++ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Quantum++ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Quantum++.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "qpp.h"

// #include "MATLAB/matlab.h" // support for MATLAB

using namespace qpp;

int main()
{
//	/*   comment this line with "//" to uncomment the whole example

	// Qudit Teleportation
	{
		std::size_t D = 3; // size of the system
		std::cout << std::endl << "**** Qudit Teleportation, D = " << D
				<< " ****" << std::endl;
		ket mes_AB = ket::Zero(D * D); // maximally entangled state resource
		for (std::size_t i = 0; i < D; i++)
			mes_AB += mket( { i, i }, { D, D });
		mes_AB /= std::sqrt((double) D);
		cmat Bell_aA = adjoint( // circuit that measures in the qudit Bell basis
				gt.CTRL(gt.Xd(D), { 0 }, { 1 }, 2, D)
						* kron(gt.Fd(D), gt.Id(D)));
		ket psi_a = randket(D); // random state as input on a
		std::cout << "** Initial state:" << std::endl;
		displn(psi_a);
		ket input_aAB = kron(psi_a, mes_AB); // joint input state aAB
		// output before measurement
		ket output_aAB = apply(input_aAB, Bell_aA, { 0, 1 }, 3, D);
		auto measured_aA = measure(ptrace2(prj(output_aAB), { D * D, D }),
				gt.Id(D * D)); // measure on aA
		std::discrete_distribution<std::size_t> dd(measured_aA.first.begin(),
				measured_aA.first.end());
		std::cout << "** Measurement probabilities: ";
		displn(measured_aA.first, ", ");
		std::size_t m = dd(rdevs._rng); // sample
		auto midx = n2multiidx(m, { D, D });
		std::cout << "** Measurement result: ";
		displn(midx, " ");
		// conditional result on B before correction
		ket output_m_aAB = apply(output_aAB, prj(mket(midx, { D, D })),
				{ 0, 1 }, 3, D) / std::sqrt(measured_aA.first[m]);
		cmat correction_B = powm(gt.Zd(D), midx[0])
				* powm(adjoint(gt.Xd(D)), midx[1]); // correction operator
				// apply correction on B
		output_aAB = apply(output_m_aAB, correction_B, { 2 }, 3, D);
		cmat rho_B = ptrace1(prj(output_aAB), { D * D, D });
		std::cout << "** Bob's density operator: " << std::endl;
		displn(rho_B);
		std::cout << "** Norm difference: " << norm(rho_B - prj(psi_a))
				<< std::endl; // verification
	}

	// Qudit Dense Coding
	{
		std::size_t D = 3; // size of the system
		std::cout << std::endl << "**** Qudit Dense Coding, D = " << D
				<< " ****" << std::endl;
		ket mes_AB = ket::Zero(D * D); // maximally entangled state resource
		for (std::size_t i = 0; i < D; i++)
			mes_AB += mket( { i, i }, { D, D });
		mes_AB /= std::sqrt((double) D);
		cmat Bell_AB = adjoint( // circuit that measures in the qudit Bell basis
				gt.CTRL(gt.Xd(D), { 0 }, { 1 }, 2, D)
						* kron(gt.Fd(D), gt.Id(D)));
		// equal probabilities of choosing a message
		std::uniform_int_distribution<std::size_t> uid(0, D * D - 1);
		std::size_t m_A = uid(rdevs._rng); // sample, obtain the message index
		auto midx = n2multiidx(m_A, { D, D });
		std::cout << "** Alice sent: ";
		displn(midx, " ");
		// Alice's operation
		cmat U_A = powm(gt.Zd(D), midx[0]) * powm(adjoint(gt.Xd(D)), midx[1]);
		// Alice encodes the message
		ket psi_AB = apply(mes_AB, U_A, { 0 }, 2, D);
		// Bob measures the joint system in the qudit Bell basis
		psi_AB = apply(psi_AB, Bell_AB, { 0, 1 }, 2, D);
		auto measured = measure(psi_AB, gt.Id(D * D));
		std::cout << "** Bob measurement probabilities: ";
		displn(measured.first, ", ");
		// Bob samples according to the measurement probabilities
		std::discrete_distribution<std::size_t> dd(measured.first.begin(),
				measured.first.end());
		std::size_t m_B = dd(rdevs._rng);
		std::cout << "** Bob received: ";
		displn(n2multiidx(m_B, { D, D }), " ") << std::endl;
	}

	// */
}
<|endoftext|>


<commit_before>/*
 * Eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/fitting/linear_shape_fitting.hpp
 *
 * Copyright 2014, 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef LINEARSHAPEFITTING_HPP_
#define LINEARSHAPEFITTING_HPP_

#include "eos/morphablemodel/MorphableModel.hpp"

#include "Eigen/LU"

#include "opencv2/core/core.hpp"

#include "boost/optional.hpp"

#include <vector>
#include <cassert>

namespace eos {
	namespace fitting {

/**
 * Fits the shape of a Morphable Model to given 2D landmarks (i.e. estimates the maximum likelihood solution of the shape coefficients) as proposed in [1].
 * It's a linear, closed-form solution fitting of the shape, with regularisation (prior towards the mean).
 *
 * [1] O. Aldrian & W. Smith, Inverse Rendering of Faces with a 3D Morphable Model, PAMI 2013.
 *
 * Note: Using less than the maximum number of coefficients to fit is not thoroughly tested yet and may contain an error.
 * Note: Returns coefficients following standard normal distribution (i.e. all have similar magnitude). Why? Because we fit using the normalised basis?
 * Note: The standard deviations given should be a vector, i.e. different for each landmark. This is not implemented yet.
 *
 * @param[in] morphable_model The Morphable Model whose shape (coefficients) are estimated.
 * @param[in] affine_camera_matrix A 3x4 affine camera matrix from world to clip-space (should probably be of type CV_32FC1 as all our calculations are done with float).
 * @param[in] landmarks 2D landmarks from an image, given in clip-coordinates.
 * @param[in] vertex_ids The vertex ids in the model that correspond to the 2D points.
 * @param[in] lambda The regularisation parameter (weight of the prior towards the mean).
 * @param[in] num_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Not tested thoroughly.
 * @param[in] detector_standard_deviation The standard deviation of the 2D landmarks given (e.g. of the detector used).
 * @param[in] model_standard_deviation The standard deviation of the 3D vertex points in the 3D model.
 * @return The estimated shape-coefficients (alphas).
 */
inline std::vector<float> fit_shape_to_landmarks_linear(morphablemodel::MorphableModel morphable_model, cv::Mat affine_camera_matrix, std::vector<cv::Vec2f> landmarks, std::vector<int> vertex_ids, float lambda=20.0f, boost::optional<int> num_coefficients_to_fit=boost::optional<int>(), boost::optional<float> detector_standard_deviation=boost::optional<float>(), boost::optional<float> model_standard_deviation=boost::optional<float>())
{
	using cv::Mat;
	assert(landmarks.size() == vertex_ids.size());

	int num_coeffs_to_fit = num_coefficients_to_fit.get_value_or(morphable_model.get_shape_model().get_num_principal_components());

	// $\hat{V} \in R^{3N\times m-1}$, subselect the rows of the eigenvector matrix $V$ associated with the $N$ feature points
	// And we insert a row of zeros after every third row, resulting in matrix $\hat{V}_h \in R^{4N\times m-1}$:
	Mat V_hat_h = Mat::zeros(4 * landmarks.size(), num_coeffs_to_fit, CV_32FC1);
	int row_index = 0;
	for (int i = 0; i < landmarks.size(); ++i) {
		Mat basis_rows = morphable_model.get_shape_model().get_normalised_pca_basis(vertex_ids[i]); // In the paper, the not-normalised basis might be used? I'm not sure, check it. It's even a mess in the paper. PH 26.5.2014: I think the normalised basis is fine/better.
																							//basisRows.copyTo(V_hat_h.rowRange(rowIndex, rowIndex + 3));
		basis_rows.colRange(0, num_coeffs_to_fit).copyTo(V_hat_h.rowRange(row_index, row_index + 3));
		row_index += 4; // replace 3 rows and skip the 4th one, it has all zeros
	}
	// Form a block diagonal matrix $P \in R^{3N\times 4N}$ in which the camera matrix C (P_Affine, affineCam) is placed on the diagonal:
	Mat P = Mat::zeros(3 * landmarks.size(), 4 * landmarks.size(), CV_32FC1);
	for (int i = 0; i < landmarks.size(); ++i) {
		Mat submatrix_to_replace = P.colRange(4 * i, (4 * i) + 4).rowRange(3 * i, (3 * i) + 3);
		affine_camera_matrix.copyTo(submatrix_to_replace);
	}
	// The variances: Add the 2D and 3D standard deviations.
	// If the user doesn't provide them, we choose the following:
	// 2D (detector) variance: Assuming the detector has a standard deviation of 3 pixels, and the face size (IED) is around 80px. That's 3.75% of the IED. Assume that an image is on average 512x512px so 80/512 = 0.16 is the size the IED occupies inside an image.
	//                         Now we're in clip-coords ([-1, 1]) and take 0.16 of the range [-1, 1], 0.16/2 = 0.08, and then the standard deviation of the detector is 3.75% of 0.08, i.e. 0.0375*0.08 = 0.003.
	// 3D (model) variance: 0.0f. It only makes sense to set it to something when we have a different variance for different vertices.
	float sigma_2D_3D = detector_standard_deviation.get_value_or(0.003f) + model_standard_deviation.get_value_or(0.0f);
	// Note: Isn't it a bit strange to add these as they have different units/normalisations? Check the paper.
	Mat Sigma = Mat::zeros(3 * landmarks.size(), 3 * landmarks.size(), CV_32FC1);
	for (int i = 0; i < 3 * landmarks.size(); ++i) {
		Sigma.at<float>(i, i) = 1.0f / sigma_2D_3D; // the higher the sigma_2D_3D, the smaller the diagonal entries of Sigma will be
	}
	Mat Omega = Sigma.t() * Sigma; // just squares the diagonal
	// The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$
	Mat y = Mat::ones(3 * landmarks.size(), 1, CV_32FC1);
	for (int i = 0; i < landmarks.size(); ++i) {
		y.at<float>(3 * i, 0) = landmarks[i][0];
		y.at<float>((3 * i) + 1, 0) = landmarks[i][1];
		//y.at<float>((3 * i) + 2, 0) = 1; // already 1, stays (homogeneous coordinate)
	}
	// The mean, with an added homogeneous coordinate (x_1, y_1, z_1, 1, x_2, ...)^t
	Mat v_bar = Mat::ones(4 * landmarks.size(), 1, CV_32FC1);
	for (int i = 0; i < landmarks.size(); ++i) {
		cv::Vec4f model_mean = morphable_model.get_shape_model().get_mean_at_point(vertex_ids[i]);
		v_bar.at<float>(4 * i, 0) = model_mean[0];
		v_bar.at<float>((4 * i) + 1, 0) = model_mean[1];
		v_bar.at<float>((4 * i) + 2, 0) = model_mean[2];
		//v_bar.at<float>((4 * i) + 3, 0) = 1; // already 1, stays (homogeneous coordinate)
		// note: now that a Vec4f is returned, we could use copyTo?
	}

	// Bring into standard regularised quadratic form with diagonal distance matrix Omega
	Mat A = P * V_hat_h; // camera matrix times the basis
	Mat b = P * v_bar - y; // camera matrix times the mean, minus the landmarks.
	//Mat c_s; // The x, we solve for this! (the variance-normalised shape parameter vector, $c_s = [a_1/sigma_{s,1} , ..., a_m-1/sigma_{s,m-1}]^t$
	//int numShapePc = morphableModel.getShapeModel().getNumberOfPrincipalComponents();
	int num_shape_pc = num_coeffs_to_fit;
	Mat AtOmegaA = A.t() * Omega * A;
	Mat AtOmegaAReg = AtOmegaA + lambda * Mat::eye(num_shape_pc, num_shape_pc, CV_32FC1);
	// Invert using OpenCV:
	Mat AtOmegaARegInv = AtOmegaAReg.inv(cv::DECOMP_SVD); // DECOMP_SVD calculates the pseudo-inverse if the matrix is not invertible.
														  // Invert (and perform some sanity checks) using Eigen:
	using RowMajorMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
	Eigen::Map<RowMajorMatrixXf> AtOmegaAReg_Eigen(AtOmegaAReg.ptr<float>(), AtOmegaAReg.rows, AtOmegaAReg.cols);
	Eigen::FullPivLU<RowMajorMatrixXf> luOfAtOmegaAReg(AtOmegaAReg_Eigen); // Calculate the full-pivoting LU decomposition of the regularized AtA. Note: We could also try FullPivHouseholderQR if our system is non-minimal (i.e. there are more constraints than unknowns).
	auto rankOfAtOmegaAReg = luOfAtOmegaAReg.rank();
	bool isAtOmegaARegInvertible = luOfAtOmegaAReg.isInvertible();
	float threshold = /*2 * */ std::abs(luOfAtOmegaAReg.maxPivot()) * luOfAtOmegaAReg.threshold();
	RowMajorMatrixXf AtARegInv_EigenFullLU = luOfAtOmegaAReg.inverse(); // Note: We should use ::solve() instead
	Mat AtOmegaARegInvFullLU(AtARegInv_EigenFullLU.rows(), AtARegInv_EigenFullLU.cols(), CV_32FC1, AtARegInv_EigenFullLU.data()); // create an OpenCV Mat header for the Eigen data

	Mat AtOmegatb = A.t() * Omega.t() * b;
	Mat c_s = -AtOmegaARegInv * AtOmegatb; // Note/Todo: We get coefficients ~ N(0, sigma) I think. They are not multiplied with the eigenvalues.

	return std::vector<float>(c_s);
};

	} /* namespace fitting */
} /* namespace eos */

#endif /* LINEARSHAPEFITTING_HPP_ */
<commit_msg>Removed conversion warnings by explicit casting<commit_after>/*
 * Eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/fitting/linear_shape_fitting.hpp
 *
 * Copyright 2014, 2015 Patrik Huber
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#ifndef LINEARSHAPEFITTING_HPP_
#define LINEARSHAPEFITTING_HPP_

#include "eos/morphablemodel/MorphableModel.hpp"

#include "Eigen/LU"

#include "opencv2/core/core.hpp"

#include "boost/optional.hpp"

#include <vector>
#include <cassert>

namespace eos {
	namespace fitting {

/**
 * Fits the shape of a Morphable Model to given 2D landmarks (i.e. estimates the maximum likelihood solution of the shape coefficients) as proposed in [1].
 * It's a linear, closed-form solution fitting of the shape, with regularisation (prior towards the mean).
 *
 * [1] O. Aldrian & W. Smith, Inverse Rendering of Faces with a 3D Morphable Model, PAMI 2013.
 *
 * Note: Using less than the maximum number of coefficients to fit is not thoroughly tested yet and may contain an error.
 * Note: Returns coefficients following standard normal distribution (i.e. all have similar magnitude). Why? Because we fit using the normalised basis?
 * Note: The standard deviations given should be a vector, i.e. different for each landmark. This is not implemented yet.
 *
 * @param[in] morphable_model The Morphable Model whose shape (coefficients) are estimated.
 * @param[in] affine_camera_matrix A 3x4 affine camera matrix from world to clip-space (should probably be of type CV_32FC1 as all our calculations are done with float).
 * @param[in] landmarks 2D landmarks from an image, given in clip-coordinates.
 * @param[in] vertex_ids The vertex ids in the model that correspond to the 2D points.
 * @param[in] lambda The regularisation parameter (weight of the prior towards the mean).
 * @param[in] num_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Not tested thoroughly.
 * @param[in] detector_standard_deviation The standard deviation of the 2D landmarks given (e.g. of the detector used).
 * @param[in] model_standard_deviation The standard deviation of the 3D vertex points in the 3D model.
 * @return The estimated shape-coefficients (alphas).
 */
inline std::vector<float> fit_shape_to_landmarks_linear(morphablemodel::MorphableModel morphable_model, cv::Mat affine_camera_matrix, std::vector<cv::Vec2f> landmarks, std::vector<int> vertex_ids, float lambda=20.0f, boost::optional<int> num_coefficients_to_fit=boost::optional<int>(), boost::optional<float> detector_standard_deviation=boost::optional<float>(), boost::optional<float> model_standard_deviation=boost::optional<float>())
{
	using cv::Mat;
	assert(landmarks.size() == vertex_ids.size());

	int num_coeffs_to_fit = num_coefficients_to_fit.get_value_or(morphable_model.get_shape_model().get_num_principal_components());
	int num_landmarks = static_cast<int>(landmarks.size());

	// $\hat{V} \in R^{3N\times m-1}$, subselect the rows of the eigenvector matrix $V$ associated with the $N$ feature points
	// And we insert a row of zeros after every third row, resulting in matrix $\hat{V}_h \in R^{4N\times m-1}$:
	Mat V_hat_h = Mat::zeros(4 * num_landmarks, num_coeffs_to_fit, CV_32FC1);
	int row_index = 0;
	for (int i = 0; i < num_landmarks; ++i) {
		Mat basis_rows = morphable_model.get_shape_model().get_normalised_pca_basis(vertex_ids[i]); // In the paper, the not-normalised basis might be used? I'm not sure, check it. It's even a mess in the paper. PH 26.5.2014: I think the normalised basis is fine/better.
																							//basisRows.copyTo(V_hat_h.rowRange(rowIndex, rowIndex + 3));
		basis_rows.colRange(0, num_coeffs_to_fit).copyTo(V_hat_h.rowRange(row_index, row_index + 3));
		row_index += 4; // replace 3 rows and skip the 4th one, it has all zeros
	}
	// Form a block diagonal matrix $P \in R^{3N\times 4N}$ in which the camera matrix C (P_Affine, affineCam) is placed on the diagonal:
	Mat P = Mat::zeros(3 * num_landmarks, 4 * num_landmarks, CV_32FC1);
	for (int i = 0; i < num_landmarks; ++i) {
		Mat submatrix_to_replace = P.colRange(4 * i, (4 * i) + 4).rowRange(3 * i, (3 * i) + 3);
		affine_camera_matrix.copyTo(submatrix_to_replace);
	}
	// The variances: Add the 2D and 3D standard deviations.
	// If the user doesn't provide them, we choose the following:
	// 2D (detector) variance: Assuming the detector has a standard deviation of 3 pixels, and the face size (IED) is around 80px. That's 3.75% of the IED. Assume that an image is on average 512x512px so 80/512 = 0.16 is the size the IED occupies inside an image.
	//                         Now we're in clip-coords ([-1, 1]) and take 0.16 of the range [-1, 1], 0.16/2 = 0.08, and then the standard deviation of the detector is 3.75% of 0.08, i.e. 0.0375*0.08 = 0.003.
	// 3D (model) variance: 0.0f. It only makes sense to set it to something when we have a different variance for different vertices.
	float sigma_2D_3D = detector_standard_deviation.get_value_or(0.003f) + model_standard_deviation.get_value_or(0.0f);
	// Note: Isn't it a bit strange to add these as they have different units/normalisations? Check the paper.
	Mat Sigma = Mat::zeros(3 * num_landmarks, 3 * num_landmarks, CV_32FC1);
	for (int i = 0; i < 3 * num_landmarks; ++i) {
		Sigma.at<float>(i, i) = 1.0f / sigma_2D_3D; // the higher the sigma_2D_3D, the smaller the diagonal entries of Sigma will be
	}
	Mat Omega = Sigma.t() * Sigma; // just squares the diagonal
	// The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$
	Mat y = Mat::ones(3 * num_landmarks, 1, CV_32FC1);
	for (int i = 0; i < num_landmarks; ++i) {
		y.at<float>(3 * i, 0) = landmarks[i][0];
		y.at<float>((3 * i) + 1, 0) = landmarks[i][1];
		//y.at<float>((3 * i) + 2, 0) = 1; // already 1, stays (homogeneous coordinate)
	}
	// The mean, with an added homogeneous coordinate (x_1, y_1, z_1, 1, x_2, ...)^t
	Mat v_bar = Mat::ones(4 * num_landmarks, 1, CV_32FC1);
	for (int i = 0; i < num_landmarks; ++i) {
		cv::Vec4f model_mean = morphable_model.get_shape_model().get_mean_at_point(vertex_ids[i]);
		v_bar.at<float>(4 * i, 0) = model_mean[0];
		v_bar.at<float>((4 * i) + 1, 0) = model_mean[1];
		v_bar.at<float>((4 * i) + 2, 0) = model_mean[2];
		//v_bar.at<float>((4 * i) + 3, 0) = 1; // already 1, stays (homogeneous coordinate)
		// note: now that a Vec4f is returned, we could use copyTo?
	}

	// Bring into standard regularised quadratic form with diagonal distance matrix Omega
	Mat A = P * V_hat_h; // camera matrix times the basis
	Mat b = P * v_bar - y; // camera matrix times the mean, minus the landmarks.
	//Mat c_s; // The x, we solve for this! (the variance-normalised shape parameter vector, $c_s = [a_1/sigma_{s,1} , ..., a_m-1/sigma_{s,m-1}]^t$
	//int numShapePc = morphableModel.getShapeModel().getNumberOfPrincipalComponents();
	int num_shape_pc = num_coeffs_to_fit;
	Mat AtOmegaA = A.t() * Omega * A;
	Mat AtOmegaAReg = AtOmegaA + lambda * Mat::eye(num_shape_pc, num_shape_pc, CV_32FC1);
	// Invert using OpenCV:
	Mat AtOmegaARegInv = AtOmegaAReg.inv(cv::DECOMP_SVD); // DECOMP_SVD calculates the pseudo-inverse if the matrix is not invertible.
														  // Invert (and perform some sanity checks) using Eigen:
	using RowMajorMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
	Eigen::Map<RowMajorMatrixXf> AtOmegaAReg_Eigen(AtOmegaAReg.ptr<float>(), AtOmegaAReg.rows, AtOmegaAReg.cols);
	Eigen::FullPivLU<RowMajorMatrixXf> luOfAtOmegaAReg(AtOmegaAReg_Eigen); // Calculate the full-pivoting LU decomposition of the regularized AtA. Note: We could also try FullPivHouseholderQR if our system is non-minimal (i.e. there are more constraints than unknowns).
	auto rankOfAtOmegaAReg = luOfAtOmegaAReg.rank();
	bool isAtOmegaARegInvertible = luOfAtOmegaAReg.isInvertible();
	float threshold = /*2 * */ std::abs(luOfAtOmegaAReg.maxPivot()) * luOfAtOmegaAReg.threshold();
	RowMajorMatrixXf AtARegInv_EigenFullLU = luOfAtOmegaAReg.inverse(); // Note: We should use ::solve() instead
	Mat AtOmegaARegInvFullLU(AtARegInv_EigenFullLU.rows(), AtARegInv_EigenFullLU.cols(), CV_32FC1, AtARegInv_EigenFullLU.data()); // create an OpenCV Mat header for the Eigen data

	Mat AtOmegatb = A.t() * Omega.t() * b;
	Mat c_s = -AtOmegaARegInv * AtOmegatb; // Note/Todo: We get coefficients ~ N(0, sigma) I think. They are not multiplied with the eigenvalues.

	return std::vector<float>(c_s);
};

	} /* namespace fitting */
} /* namespace eos */

#endif /* LINEARSHAPEFITTING_HPP_ */
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2014 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP
#define MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP

#include <mapnik/symbolizer_keys.hpp>
#include <mapnik/color.hpp>
#include <type_traits>

namespace mapnik {

template <typename T, keys key>
struct symbolizer_default {};

// specializations for properties
// gamma
template <>
struct symbolizer_default<value_double, keys::gamma>
{
    static value_double value() { return 1.0; }
};

// gamma-method
template <>
struct symbolizer_default<gamma_method_enum, keys::gamma_method>
{
    static gamma_method_enum value() { return GAMMA_POWER; }
};

// opacity
template <>
struct symbolizer_default<value_double, keys::opacity>
{
    static value_double value() { return 1.0; }
};

// alignment
template <>
struct symbolizer_default<pattern_alignment_enum, keys::alignment>
{
    static pattern_alignment_enum value() { return LOCAL_ALIGNMENT; }
};

// offset
template <>
struct symbolizer_default<value_double, keys::offset>
{
    static value_double value() { return 0.0; }
};

// comp-op
template <>
struct symbolizer_default<composite_mode_e, keys::comp_op>
{
    static composite_mode_e value() { return src_over; }
};

// clip
template <>
struct symbolizer_default<value_bool, keys::clip>
{
    static value_bool value() { return false; }
};

// fill
template <>
struct symbolizer_default<color, keys::fill>
{
    static color value() { return color(128,128,128); }
};

// fill-opacity
template <>
struct symbolizer_default<value_double, keys::fill_opacity>
{
    static value_double value() { return 1.0; }
};

// stroke
template <>
struct symbolizer_default<color, keys::stroke>
{
    static color value() { return color(0,0,0); }
};

// stroke-width
template <>
struct symbolizer_default<value_double, keys::stroke_width>
{
    static value_double value() { return 1.0; }
};

// stroke-opacity
template <>
struct symbolizer_default<value_double, keys::stroke_opacity>
{
    static value_double value() { return 1.0; }
};

// stroke-linejoin
template <>
struct symbolizer_default<line_join_enum, keys::stroke_linejoin>
{
    static line_join_enum value() { return MITER_JOIN; }
};

// stroke-linecap
template <>
struct symbolizer_default<line_cap_enum, keys::stroke_linecap>
{
    static line_cap_enum value() { return BUTT_CAP; }
};

// stroke-gamma
template <>
struct symbolizer_default<value_double, keys::stroke_gamma>
{
    static value_double value() { return 1.0; }
};

// stroke-gamma-method
template <>
struct symbolizer_default<gamma_method_enum, keys::stroke_gamma_method>
{
    static gamma_method_enum value() { return GAMMA_POWER; }
};

// stroke-dashoffset
template <>
struct symbolizer_default<value_integer, keys::stroke_dashoffset>
{
    static value_integer value() { return 0; }
};

// stroke-dasharray

// stroke-miterlimit
template <>
struct symbolizer_default<value_double, keys::stroke_miterlimit>
{
    static value_double value() { return 4.0; }
};

// geometry-transform

// rasterizer
template <>
struct symbolizer_default<line_rasterizer_enum, keys::line_rasterizer>
{
    static line_rasterizer_enum value() { return RASTERIZER_FULL; }
};

// transform

// spacing
template <>
struct symbolizer_default<value_double, keys::spacing>
{
    static value_double value() { return 100.0; }
};

// max-error
template <>
struct symbolizer_default<value_double, keys::max_error>
{
    static value_double value() { return 0.2; }
};

// allow-overlap
template <>
struct symbolizer_default<value_bool, keys::allow_overlap>
{
    static value_bool value() { return false; }
};

// ignore-placement
template <>
struct symbolizer_default<value_bool, keys::ignore_placement>
{
    static value_bool value() { return false; }
};

// width
template <>
struct symbolizer_default<value_double, keys::width>
{
    static value_double value() { return 0.0; }
};

// height
template <>
struct symbolizer_default<value_double, keys::height>
{
    static value_double value() { return 0.0; }
};

// file ??
template <>
struct symbolizer_default<std::string, keys::file>
{
    static std::string value() { return ""; }
};

// shield-dx
template <>
struct symbolizer_default<value_double, keys::shield_dx>
{
    static value_double value() { return 0.0; }
};

// shield-dy
template <>
struct symbolizer_default<value_double, keys::shield_dy>
{
    static value_double value() { return 0.0; }
};

// unlock-image
template <>
struct symbolizer_default<value_bool, keys::unlock_image>
{
    static value_bool value() { return false; }
};

// mode
template <>
struct symbolizer_default<value_bool, keys::mode>
{
    static value_bool value() { return false; }
};

// scaling
template <>
struct symbolizer_default<value_double, keys::scaling>
{
    static value_double value() { return 1.0; }
};

// filter-factor
template <>
struct symbolizer_default<value_double, keys::filter_factor>
{
    static value_double value() { return 1.0; }
};

// mesh-size
template <>
struct symbolizer_default<value_integer, keys::mesh_size>
{
    static value_integer value() { return 0; }
};

// premultiplied
template <>
struct symbolizer_default<value_bool, keys::premultiplied>
{
    static value_bool value() { return false; }
};

// smooth
template <>
struct symbolizer_default<value_double, keys::smooth>
{
    static value_double value() { return 0.0; }
};

// simplify-algorithm
template <>
struct symbolizer_default<simplify_algorithm_e, keys::simplify_algorithm>
{
    static simplify_algorithm_e value() { return radial_distance; }
};

// simplify
template <>
struct symbolizer_default<value_double, keys::simplify_tolerance>
{
    static value_double value() { return 0.0; }
};

// halo-rasterizer
template <>
struct symbolizer_default<halo_rasterizer_enum, keys::halo_rasterizer>
{
    static halo_rasterizer_enum value() { return HALO_RASTERIZER_FULL; }
};

// text-placements

// placement (point-placement-type FIXME)
template <>
struct symbolizer_default<point_placement_enum, keys::point_placement_type>
{
    static point_placement_enum value() { return CENTROID_POINT_PLACEMENT; }
};

// marker placement
template <>
struct symbolizer_default<marker_placement_enum, keys::markers_placement_type>
{
    static marker_placement_enum value() { return MARKER_POINT_PLACEMENT; }
};

// multi-policy
template <>
struct symbolizer_default<marker_multi_policy_enum, keys::markers_multipolicy>
{
    static marker_multi_policy_enum value() { return  MARKER_EACH_MULTI; }
};

// placement

// colorizer

// halo-transform

// num-columns
// start-column
// repeat-key
// symbolizer-properties ??
// largest-box-only
// minimum-path-length
// halo-comp-op
// text-transform
// horizontal-alignment
// justify-alignment
// vertical-alignment
// upright

// avoid-edges
template <>
struct symbolizer_default<value_bool, keys::avoid_edges>
{
    static value_bool value() { return false; }
};

// font-feature-settings

} // namespace mapnik

#endif // MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP
<commit_msg>default to GLOBAL alignment<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2014 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP
#define MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP

#include <mapnik/symbolizer_keys.hpp>
#include <mapnik/color.hpp>
#include <type_traits>

namespace mapnik {

template <typename T, keys key>
struct symbolizer_default {};

// specializations for properties
// gamma
template <>
struct symbolizer_default<value_double, keys::gamma>
{
    static value_double value() { return 1.0; }
};

// gamma-method
template <>
struct symbolizer_default<gamma_method_enum, keys::gamma_method>
{
    static gamma_method_enum value() { return GAMMA_POWER; }
};

// opacity
template <>
struct symbolizer_default<value_double, keys::opacity>
{
    static value_double value() { return 1.0; }
};

// alignment
template <>
struct symbolizer_default<pattern_alignment_enum, keys::alignment>
{
    static pattern_alignment_enum value() { return GLOBAL_ALIGNMENT; }
};

// offset
template <>
struct symbolizer_default<value_double, keys::offset>
{
    static value_double value() { return 0.0; }
};

// comp-op
template <>
struct symbolizer_default<composite_mode_e, keys::comp_op>
{
    static composite_mode_e value() { return src_over; }
};

// clip
template <>
struct symbolizer_default<value_bool, keys::clip>
{
    static value_bool value() { return false; }
};

// fill
template <>
struct symbolizer_default<color, keys::fill>
{
    static color value() { return color(128,128,128); }
};

// fill-opacity
template <>
struct symbolizer_default<value_double, keys::fill_opacity>
{
    static value_double value() { return 1.0; }
};

// stroke
template <>
struct symbolizer_default<color, keys::stroke>
{
    static color value() { return color(0,0,0); }
};

// stroke-width
template <>
struct symbolizer_default<value_double, keys::stroke_width>
{
    static value_double value() { return 1.0; }
};

// stroke-opacity
template <>
struct symbolizer_default<value_double, keys::stroke_opacity>
{
    static value_double value() { return 1.0; }
};

// stroke-linejoin
template <>
struct symbolizer_default<line_join_enum, keys::stroke_linejoin>
{
    static line_join_enum value() { return MITER_JOIN; }
};

// stroke-linecap
template <>
struct symbolizer_default<line_cap_enum, keys::stroke_linecap>
{
    static line_cap_enum value() { return BUTT_CAP; }
};

// stroke-gamma
template <>
struct symbolizer_default<value_double, keys::stroke_gamma>
{
    static value_double value() { return 1.0; }
};

// stroke-gamma-method
template <>
struct symbolizer_default<gamma_method_enum, keys::stroke_gamma_method>
{
    static gamma_method_enum value() { return GAMMA_POWER; }
};

// stroke-dashoffset
template <>
struct symbolizer_default<value_integer, keys::stroke_dashoffset>
{
    static value_integer value() { return 0; }
};

// stroke-dasharray

// stroke-miterlimit
template <>
struct symbolizer_default<value_double, keys::stroke_miterlimit>
{
    static value_double value() { return 4.0; }
};

// geometry-transform

// rasterizer
template <>
struct symbolizer_default<line_rasterizer_enum, keys::line_rasterizer>
{
    static line_rasterizer_enum value() { return RASTERIZER_FULL; }
};

// transform

// spacing
template <>
struct symbolizer_default<value_double, keys::spacing>
{
    static value_double value() { return 100.0; }
};

// max-error
template <>
struct symbolizer_default<value_double, keys::max_error>
{
    static value_double value() { return 0.2; }
};

// allow-overlap
template <>
struct symbolizer_default<value_bool, keys::allow_overlap>
{
    static value_bool value() { return false; }
};

// ignore-placement
template <>
struct symbolizer_default<value_bool, keys::ignore_placement>
{
    static value_bool value() { return false; }
};

// width
template <>
struct symbolizer_default<value_double, keys::width>
{
    static value_double value() { return 0.0; }
};

// height
template <>
struct symbolizer_default<value_double, keys::height>
{
    static value_double value() { return 0.0; }
};

// file ??
template <>
struct symbolizer_default<std::string, keys::file>
{
    static std::string value() { return ""; }
};

// shield-dx
template <>
struct symbolizer_default<value_double, keys::shield_dx>
{
    static value_double value() { return 0.0; }
};

// shield-dy
template <>
struct symbolizer_default<value_double, keys::shield_dy>
{
    static value_double value() { return 0.0; }
};

// unlock-image
template <>
struct symbolizer_default<value_bool, keys::unlock_image>
{
    static value_bool value() { return false; }
};

// mode
template <>
struct symbolizer_default<value_bool, keys::mode>
{
    static value_bool value() { return false; }
};

// scaling
template <>
struct symbolizer_default<value_double, keys::scaling>
{
    static value_double value() { return 1.0; }
};

// filter-factor
template <>
struct symbolizer_default<value_double, keys::filter_factor>
{
    static value_double value() { return 1.0; }
};

// mesh-size
template <>
struct symbolizer_default<value_integer, keys::mesh_size>
{
    static value_integer value() { return 0; }
};

// premultiplied
template <>
struct symbolizer_default<value_bool, keys::premultiplied>
{
    static value_bool value() { return false; }
};

// smooth
template <>
struct symbolizer_default<value_double, keys::smooth>
{
    static value_double value() { return 0.0; }
};

// simplify-algorithm
template <>
struct symbolizer_default<simplify_algorithm_e, keys::simplify_algorithm>
{
    static simplify_algorithm_e value() { return radial_distance; }
};

// simplify
template <>
struct symbolizer_default<value_double, keys::simplify_tolerance>
{
    static value_double value() { return 0.0; }
};

// halo-rasterizer
template <>
struct symbolizer_default<halo_rasterizer_enum, keys::halo_rasterizer>
{
    static halo_rasterizer_enum value() { return HALO_RASTERIZER_FULL; }
};

// text-placements

// placement (point-placement-type FIXME)
template <>
struct symbolizer_default<point_placement_enum, keys::point_placement_type>
{
    static point_placement_enum value() { return CENTROID_POINT_PLACEMENT; }
};

// marker placement
template <>
struct symbolizer_default<marker_placement_enum, keys::markers_placement_type>
{
    static marker_placement_enum value() { return MARKER_POINT_PLACEMENT; }
};

// multi-policy
template <>
struct symbolizer_default<marker_multi_policy_enum, keys::markers_multipolicy>
{
    static marker_multi_policy_enum value() { return  MARKER_EACH_MULTI; }
};

// placement

// colorizer

// halo-transform

// num-columns
// start-column
// repeat-key
// symbolizer-properties ??
// largest-box-only
// minimum-path-length
// halo-comp-op
// text-transform
// horizontal-alignment
// justify-alignment
// vertical-alignment
// upright

// avoid-edges
template <>
struct symbolizer_default<value_bool, keys::avoid_edges>
{
    static value_bool value() { return false; }
};

// font-feature-settings

} // namespace mapnik

#endif // MAPNIK_SYMBOLIZER_DEFAULT_VALUES_HPP
<|endoftext|>


<commit_before>/************************************************************************/
/*                                                                      */
/*    Copyright 2008-2011 by Michael Hanselmann and Ullrich Koethe      */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://hci.iwr.uni-heidelberg.de/vigra/                       */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#ifndef VIGRA_UNSUPERVISED_DECOMPOSITION_HXX
#define VIGRA_UNSUPERVISED_DECOMPOSITION_HXX

#include <numeric>
#include "mathutil.hxx"
#include "matrix.hxx"
#include "singular_value_decomposition.hxx"
#include "random.hxx"

namespace vigra
{

/** \addtogroup Unsupervised_Decomposition Unsupervised Decomposition

    Unsupervised matrix decomposition methods.
**/
//@{

/*****************************************************************/
/*                                                               */
/*              principle component analysis (PCA)               */
/*                                                               */
/*****************************************************************/

   /** \brief Decompose a matrix according to the PCA algorithm.

        This function implements the PCA algorithm (principle component analysis).

        \arg features must be a matrix with shape <tt>(numFeatures * numSamples)</tt>, which is
        decomposed into the matrices
        \arg fz with shape <tt>(numFeatures * numComponents)</tt> and
        \arg zv with shape <tt>(numComponents * numSamples)</tt>

        such that
        \f[
            \mathrm{features} \approx \mathrm{fz} * \mathrm{zv}
        \f]
        (this formula requires that the features have been center around the mean by
        \ref <tt>prepareRows(features, features, ZeroMean)</tt>).

        The shape parameter <tt>numComponents</tt> determines the complexity of
        the decomposition model and therefore the approximation quality (if
        <tt>numComponents == numFeatures</tt>, the representation becomes exact).
        Intuitively, <tt>fz</tt> is a projection matrix from the reduced space
        into the original space, and <tt>zv</tt> is  the reduced representation
        of the data, using just <tt>numComponents</tt> features.

        Declaration:

        \#include \<vigra/unsupervised_decomposition.hxx\>

        \code
        namespace vigra {

            template <class U, class C1, class C2, class C3>
            void
            principleComponents(MultiArrayView<2, U, C1> const & features,
                                MultiArrayView<2, U, C2> fz,
                                MultiArrayView<2, U, C3> zv);
        }
        \endcode

        Usage:
        \code
        Matrix<double> data(numFeatures, numSamples);
        ... // fill the imput matrix

        int numComponents = 3;
        Matrix<double> fz(numFeatures, numComponents),
                       zv(numComponents, numSamples);

        // center the data
        prepareRows(data, data, ZeroMean);

        // compute the reduced representation
        principleComponents(data, fz, zv);

        Matrix<double> model = fz*zv;
        double meanSquaredError = squaredNorm(data - model) / numSamples;
        \endcode
   */
template <class U, class C1, class C2, class C3>
void
principleComponents(MultiArrayView<2, U, C1> const & features,
                    MultiArrayView<2, U, C2> fz,
                    MultiArrayView<2, U, C3> zv)
{
    using namespace linalg; // activate matrix multiplication and arithmetic functions

    int numFeatures = rowCount(features);
    int numSamples = columnCount(features);
    int numComponents = columnCount(fz);
    vigra_precondition(numSamples >= numFeatures,
      "principleComponents(): The number of samples has to be larger than the number of features.");
    vigra_precondition(numFeatures >= numComponents && numComponents >= 1,
      "principleComponents(): The number of features has to be larger or equal to the number of components in which the feature matrix is decomposed.");
    vigra_precondition(rowCount(fz) == numFeatures,
      "principleComponents(): The output matrix fz has to be of dimension numFeatures*numComponents.");
    vigra_precondition(columnCount(zv) == numSamples && rowCount(zv) == numComponents,
      "principleComponents(): The output matrix zv has to be of dimension numComponents*numSamples.");

    Matrix U(numSamples, numFeatures), S(numFeatures, 1), V(numFeatures, numFeatures);
    singularValueDecomposition(features.transpose(), U, S, V);

    for(int k=0; k<numComponents; ++k)
    {
        rowVector(zv, k) = columnVector(U, k).transpose() * S(k, 0);
        columnVector(fz, k) = columnVector(V, k);
    }
}

/*****************************************************************/
/*                                                               */
/*         probabilistic latent semantic analysis (pLSA)         */
/*         see T Hofmann, Probabilistic Latent Semantic          */
/*         Indexing for details                                  */
/*                                                               */
/*****************************************************************/

   /** \brief Option object for the \ref pLSA algorithm.
   */
class PLSAOptions
{
  public:
        /** Initialize all options with default values.
        */
    PLSAOptions()
    : min_rel_gain(1e-4),
      max_iterations(50),
      normalized_component_weights(true)
    {}

        /** Maximum number of iterations which is performed by the pLSA algorithm.

            default: 50
        */
    PLSAOptions & maximumNumberOfIterations(unsigned int n)
    {
        vigra_precondition(n >= 1,
            "PLSAOptions::maximumNumberOfIterations(): number must be a positive integer.");
        max_iterations = n;
        return *this;
    }

        /** Minimum relative gain which is required for the algorithm to continue the iterations.

            default: 1e-4
        */
    PLSAOptions & minimumRelativeGain(double g)
    {
        vigra_precondition(g >= 0.0,
            "PLSAOptions::minimumRelativeGain(): number must be positive or zero.");
        min_rel_gain = g;
        return *this;
    }

        /** Normalize the entries of the zv result array.

            If true, the columns of zv sum to one. Otherwise, they have the same
            column sum as the original feature matrix.

            default: true
        */
    PLSAOptions & normalizedComponentWeights(bool v = true)
    {
        normalized_component_weights = v;
        return *this;
    }

    double min_rel_gain;
    int max_iterations;
    bool normalized_component_weights;
};

   /** \brief Decompose a matrix according to the pLSA algorithm.

        This function implements the pLSA algorithm (probabilistic latent semantic analysis)
        proposed in

        T. Hofmann:
        "Probabilistic Latent Semantic Analysis",
        in: UAI'99, Proc. 15th Conf. on Uncertainty in Artificial Intelligence,
        pp. 289-296, Morgan Kaufmann, 1999

        \arg features must be a matrix with shape <tt>(numFeatures * numSamples)</tt>, which is
        decomposed into the matrices
        \arg fz with shape <tt>(numFeatures * numComponents)</tt> and
        \arg zv with shape <tt>(numComponents * numSamples)</tt>

        such that
        \f[
            \mathrm{features} \approx \mathrm{fz} * \mathrm{zv}
        \f]
        (this formula applies when pLSA is called with
        <tt>PLSAOptions.normalizedComponentWeights(false)</tt>. Otherwise, you must
        normalize the features by calling \ref <tt>prepareColumns(features, features, UnitSum)</tt> to make the formula hold).

        The shape parameter <tt>numComponents</tt> determines the complexity of
        the decomposition model and therefore the approximation quality.
        Intuitively, features are a set of words, and the samples a set of
        documents. The entries of the <tt>features</tt> matrix denote the relative
        frequency of the words in each document. The components represents a
        (presumably small) set of topics. The matrix <tt>fz</tt> encodes the
        relative frequency of words in the different topics, and the matrix
        <tt>zv</tt> encodes to what extend each topic explains the content of each
        document.

        The option object determines the iteration termination conditions and the ouput
        normalization. In addition, you may pass a random number generator to pLSA()
        which is used to create the initial solution.

        Declarations:

        \#include \<vigra/unsupervised_decomposition.hxx\>

        \code
        namespace vigra {

            template <class U, class C1, class C2, class C3, class Random>
            void
            pLSA(MultiArrayView<2, U, C1> const & features,
                 MultiArrayView<2, U, C2> & fz,
                 MultiArrayView<2, U, C3> & zv,
                 Random const& random,
                 PLSAOptions const & options = PLSAOptions());

            template <class U, class C1, class C2, class C3>
            void
            pLSA(MultiArrayView<2, U, C1> const & features,
                 MultiArrayView<2, U, C2> & fz,
                 MultiArrayView<2, U, C3> & zv,
                 PLSAOptions const & options = PLSAOptions());
        }
        \endcode

        Usage:
        \code
        Matrix<double> words(numWords, numDocuments);
        ... // fill the imput matrix

        int numTopics = 3;
        Matrix<double> fz(numWords, numTopics),
                       zv(numTopics, numDocuments);

        pLSA(words, fz, zv, PLSAOptions().normalizedComponentWeights(false));

        Matrix<double> model = fz*zv;
        double meanSquaredError = (words - model).squaredNorm() / numDocuments;
        \endcode
   */
doxygen_overloaded_function(template <...> void pLSA)

template <class U, class C1, class C2, class C3, class Random>
void
pLSA(MultiArrayView<2, U, C1> const & features,
     MultiArrayView<2, U, C2> fz,
     MultiArrayView<2, U, C3> zv,
     Random const& random,
     PLSAOptions const & options = PLSAOptions())
{
    using namespace linalg; // activate matrix multiplication and arithmetic functions

    int numFeatures = rowCount(features);
    int numSamples = columnCount(features);
    int numComponents = columnCount(fz);
    vigra_precondition(numFeatures >= numComponents && numComponents >= 1,
      "pLSA(): The number of features has to be larger or equal to the number of components in which the feature matrix is decomposed.");
    vigra_precondition(rowCount(fz) == numFeatures,
      "pLSA(): The output matrix fz has to be of dimension numFeatures*numComponents.");
    vigra_precondition(columnCount(zv) == numSamples && rowCount(zv) == numComponents,
      "pLSA(): The output matrix zv has to be of dimension numComponents*numSamples.");

    // random initialization of result matrices, subsequent normalization
    UniformRandomFunctor<Random> randf(random);
    initMultiArray(destMultiArrayRange(fz), randf);
    initMultiArray(destMultiArrayRange(zv), randf);
    prepareColumns(fz, fz, UnitSum);
    prepareColumns(zv, zv, UnitSum);

    // init vars
    double eps = 1.0/NumericTraits::max(); // epsilon > 0
    double lastChange = NumericTraits::max(); // infinity
    double err = 0;
    double err_old;
    int iteration = 0;

    // expectation maximization (EM) algorithm
    Matrix columnSums(1, numSamples);
    features.sum(columnSums);
    Matrix expandedSums = ones(numFeatures, 1) * columnSums;

    while(iteration < options.max_iterations && (lastChange > options.min_rel_gain))
    {
        Matrix fzv = fz*zv;

        //if(iteration%25 == 0)
        //{
            //std::cout << "iteration: " << iteration << std::endl;
            //std::cout << "last relative change: " << lastChange << std::endl;
        //}

        Matrix factor = features / pointWise(fzv + (U)eps);
        zv *= (fz.transpose() * factor);
        fz *= (factor * zv.transpose());
        prepareColumns(fz, fz, UnitSum);
        prepareColumns(zv, zv, UnitSum);

        // check relative change in least squares model fit
        Matrix model = expandedSums * pointWise(fzv);
        err_old = err;
        err = (features - model).squaredNorm();
        //std::cout << "error: " << err << std::endl;
        lastChange = abs((err-err_old) / (U)(err + eps));
        //std::cout << "lastChange: " << lastChange << std::endl;

        iteration += 1;
    }
    //std::cout << "Terminated after " << iteration << " iterations." << std::endl;
    //std::cout << "Last relative change was " << lastChange << "." << std::endl;

    if(!options.normalized_component_weights)
    {
        // undo the normalization
        for(int k=0; k<numSamples; ++k)
            columnVector(zv, k) *= columnSums(0, k);
    }
}

template <class U, class C1, class C2, class C3>
inline void
pLSA(MultiArrayView<2, U, C1> const & features,
     MultiArrayView<2, U, C2> & fz,
     MultiArrayView<2, U, C3> & zv,
     PLSAOptions const & options = PLSAOptions())
{
    RandomNumberGenerator<> generator(RandomSeed);
    pLSA(features, fz, zv, generator, options);
}

//@}

} // namespace vigra

#endif // VIGRA_UNSUPERVISED_DECOMPOSITION_HXX

<commit_msg>added principleComponents() and test<commit_after>/************************************************************************/
/*                                                                      */
/*    Copyright 2008-2011 by Michael Hanselmann and Ullrich Koethe      */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://hci.iwr.uni-heidelberg.de/vigra/                       */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#ifndef VIGRA_UNSUPERVISED_DECOMPOSITION_HXX
#define VIGRA_UNSUPERVISED_DECOMPOSITION_HXX

#include <numeric>
#include "mathutil.hxx"
#include "matrix.hxx"
#include "singular_value_decomposition.hxx"
#include "random.hxx"

namespace vigra
{

/** \addtogroup Unsupervised_Decomposition Unsupervised Decomposition

    Unsupervised matrix decomposition methods.
**/
//@{

/*****************************************************************/
/*                                                               */
/*              principle component analysis (PCA)               */
/*                                                               */
/*****************************************************************/

   /** \brief Decompose a matrix according to the PCA algorithm.

        This function implements the PCA algorithm (principle component analysis).

        \arg features must be a matrix with shape <tt>(numFeatures * numSamples)</tt>, which is
        decomposed into the matrices
        \arg fz with shape <tt>(numFeatures * numComponents)</tt> and
        \arg zv with shape <tt>(numComponents * numSamples)</tt>

        such that
        \f[
            \mathrm{features} \approx \mathrm{fz} * \mathrm{zv}
        \f]
        (this formula requires that the features have been center around the mean by
        \ref <tt>prepareRows(features, features, ZeroMean)</tt>).

        The shape parameter <tt>numComponents</tt> determines the complexity of
        the decomposition model and therefore the approximation quality (if
        <tt>numComponents == numFeatures</tt>, the representation becomes exact).
        Intuitively, <tt>fz</tt> is a projection matrix from the reduced space
        into the original space, and <tt>zv</tt> is  the reduced representation
        of the data, using just <tt>numComponents</tt> features.

        Declaration:

        \#include \<vigra/unsupervised_decomposition.hxx\>

        \code
        namespace vigra {

            template <class U, class C1, class C2, class C3>
            void
            principleComponents(MultiArrayView<2, U, C1> const & features,
                                MultiArrayView<2, U, C2> fz,
                                MultiArrayView<2, U, C3> zv);
        }
        \endcode

        Usage:
        \code
        Matrix<double> data(numFeatures, numSamples);
        ... // fill the imput matrix

        int numComponents = 3;
        Matrix<double> fz(numFeatures, numComponents),
                       zv(numComponents, numSamples);

        // center the data
        prepareRows(data, data, ZeroMean);

        // compute the reduced representation
        principleComponents(data, fz, zv);

        Matrix<double> model = fz*zv;
        double meanSquaredError = squaredNorm(data - model) / numSamples;
        \endcode
   */
template <class T, class C1, class C2, class C3>
void
principleComponents(MultiArrayView<2, T, C1> const & features,
                    MultiArrayView<2, T, C2> fz,
                    MultiArrayView<2, T, C3> zv)
{
    using namespace linalg; // activate matrix multiplication and arithmetic functions

    int numFeatures = rowCount(features);
    int numSamples = columnCount(features);
    int numComponents = columnCount(fz);
    vigra_precondition(numSamples >= numFeatures,
      "principleComponents(): The number of samples has to be larger than the number of features.");
    vigra_precondition(numFeatures >= numComponents && numComponents >= 1,
      "principleComponents(): The number of features has to be larger or equal to the number of components in which the feature matrix is decomposed.");
    vigra_precondition(rowCount(fz) == numFeatures,
      "principleComponents(): The output matrix fz has to be of dimension numFeatures*numComponents.");
    vigra_precondition(columnCount(zv) == numSamples && rowCount(zv) == numComponents,
      "principleComponents(): The output matrix zv has to be of dimension numComponents*numSamples.");

    Matrix<T> U(numSamples, numFeatures), S(numFeatures, 1), V(numFeatures, numFeatures);
    singularValueDecomposition(features.transpose(), U, S, V);

    for(int k=0; k<numComponents; ++k)
    {
        rowVector(zv, k) = columnVector(U, k).transpose() * S(k, 0);
        columnVector(fz, k) = columnVector(V, k);
    }
}

/*****************************************************************/
/*                                                               */
/*         probabilistic latent semantic analysis (pLSA)         */
/*         see T Hofmann, Probabilistic Latent Semantic          */
/*         Indexing for details                                  */
/*                                                               */
/*****************************************************************/

   /** \brief Option object for the \ref pLSA algorithm.
   */
class PLSAOptions
{
  public:
        /** Initialize all options with default values.
        */
    PLSAOptions()
    : min_rel_gain(1e-4),
      max_iterations(50),
      normalized_component_weights(true)
    {}

        /** Maximum number of iterations which is performed by the pLSA algorithm.

            default: 50
        */
    PLSAOptions & maximumNumberOfIterations(unsigned int n)
    {
        vigra_precondition(n >= 1,
            "PLSAOptions::maximumNumberOfIterations(): number must be a positive integer.");
        max_iterations = n;
        return *this;
    }

        /** Minimum relative gain which is required for the algorithm to continue the iterations.

            default: 1e-4
        */
    PLSAOptions & minimumRelativeGain(double g)
    {
        vigra_precondition(g >= 0.0,
            "PLSAOptions::minimumRelativeGain(): number must be positive or zero.");
        min_rel_gain = g;
        return *this;
    }

        /** Normalize the entries of the zv result array.

            If true, the columns of zv sum to one. Otherwise, they have the same
            column sum as the original feature matrix.

            default: true
        */
    PLSAOptions & normalizedComponentWeights(bool v = true)
    {
        normalized_component_weights = v;
        return *this;
    }

    double min_rel_gain;
    int max_iterations;
    bool normalized_component_weights;
};

   /** \brief Decompose a matrix according to the pLSA algorithm.

        This function implements the pLSA algorithm (probabilistic latent semantic analysis)
        proposed in

        T. Hofmann:
        "Probabilistic Latent Semantic Analysis",
        in: UAI'99, Proc. 15th Conf. on Uncertainty in Artificial Intelligence,
        pp. 289-296, Morgan Kaufmann, 1999

        \arg features must be a matrix with shape <tt>(numFeatures * numSamples)</tt>, which is
        decomposed into the matrices
        \arg fz with shape <tt>(numFeatures * numComponents)</tt> and
        \arg zv with shape <tt>(numComponents * numSamples)</tt>

        such that
        \f[
            \mathrm{features} \approx \mathrm{fz} * \mathrm{zv}
        \f]
        (this formula applies when pLSA is called with
        <tt>PLSAOptions.normalizedComponentWeights(false)</tt>. Otherwise, you must
        normalize the features by calling \ref <tt>prepareColumns(features, features, UnitSum)</tt> to make the formula hold).

        The shape parameter <tt>numComponents</tt> determines the complexity of
        the decomposition model and therefore the approximation quality.
        Intuitively, features are a set of words, and the samples a set of
        documents. The entries of the <tt>features</tt> matrix denote the relative
        frequency of the words in each document. The components represents a
        (presumably small) set of topics. The matrix <tt>fz</tt> encodes the
        relative frequency of words in the different topics, and the matrix
        <tt>zv</tt> encodes to what extend each topic explains the content of each
        document.

        The option object determines the iteration termination conditions and the ouput
        normalization. In addition, you may pass a random number generator to pLSA()
        which is used to create the initial solution.

        Declarations:

        \#include \<vigra/unsupervised_decomposition.hxx\>

        \code
        namespace vigra {

            template <class U, class C1, class C2, class C3, class Random>
            void
            pLSA(MultiArrayView<2, U, C1> const & features,
                 MultiArrayView<2, U, C2> & fz,
                 MultiArrayView<2, U, C3> & zv,
                 Random const& random,
                 PLSAOptions const & options = PLSAOptions());

            template <class U, class C1, class C2, class C3>
            void
            pLSA(MultiArrayView<2, U, C1> const & features,
                 MultiArrayView<2, U, C2> & fz,
                 MultiArrayView<2, U, C3> & zv,
                 PLSAOptions const & options = PLSAOptions());
        }
        \endcode

        Usage:
        \code
        Matrix<double> words(numWords, numDocuments);
        ... // fill the imput matrix

        int numTopics = 3;
        Matrix<double> fz(numWords, numTopics),
                       zv(numTopics, numDocuments);

        pLSA(words, fz, zv, PLSAOptions().normalizedComponentWeights(false));

        Matrix<double> model = fz*zv;
        double meanSquaredError = (words - model).squaredNorm() / numDocuments;
        \endcode
   */
doxygen_overloaded_function(template <...> void pLSA)

template <class U, class C1, class C2, class C3, class Random>
void
pLSA(MultiArrayView<2, U, C1> const & features,
     MultiArrayView<2, U, C2> fz,
     MultiArrayView<2, U, C3> zv,
     Random const& random,
     PLSAOptions const & options = PLSAOptions())
{
    using namespace linalg; // activate matrix multiplication and arithmetic functions

    int numFeatures = rowCount(features);
    int numSamples = columnCount(features);
    int numComponents = columnCount(fz);
    vigra_precondition(numFeatures >= numComponents && numComponents >= 1,
      "pLSA(): The number of features has to be larger or equal to the number of components in which the feature matrix is decomposed.");
    vigra_precondition(rowCount(fz) == numFeatures,
      "pLSA(): The output matrix fz has to be of dimension numFeatures*numComponents.");
    vigra_precondition(columnCount(zv) == numSamples && rowCount(zv) == numComponents,
      "pLSA(): The output matrix zv has to be of dimension numComponents*numSamples.");

    // random initialization of result matrices, subsequent normalization
    UniformRandomFunctor<Random> randf(random);
    initMultiArray(destMultiArrayRange(fz), randf);
    initMultiArray(destMultiArrayRange(zv), randf);
    prepareColumns(fz, fz, UnitSum);
    prepareColumns(zv, zv, UnitSum);

    // init vars
    double eps = 1.0/NumericTraits::max(); // epsilon > 0
    double lastChange = NumericTraits::max(); // infinity
    double err = 0;
    double err_old;
    int iteration = 0;

    // expectation maximization (EM) algorithm
    Matrix columnSums(1, numSamples);
    features.sum(columnSums);
    Matrix expandedSums = ones(numFeatures, 1) * columnSums;

    while(iteration < options.max_iterations && (lastChange > options.min_rel_gain))
    {
        Matrix fzv = fz*zv;

        //if(iteration%25 == 0)
        //{
            //std::cout << "iteration: " << iteration << std::endl;
            //std::cout << "last relative change: " << lastChange << std::endl;
        //}

        Matrix factor = features / pointWise(fzv + (U)eps);
        zv *= (fz.transpose() * factor);
        fz *= (factor * zv.transpose());
        prepareColumns(fz, fz, UnitSum);
        prepareColumns(zv, zv, UnitSum);

        // check relative change in least squares model fit
        Matrix model = expandedSums * pointWise(fzv);
        err_old = err;
        err = (features - model).squaredNorm();
        //std::cout << "error: " << err << std::endl;
        lastChange = abs((err-err_old) / (U)(err + eps));
        //std::cout << "lastChange: " << lastChange << std::endl;

        iteration += 1;
    }
    //std::cout << "Terminated after " << iteration << " iterations." << std::endl;
    //std::cout << "Last relative change was " << lastChange << "." << std::endl;

    if(!options.normalized_component_weights)
    {
        // undo the normalization
        for(int k=0; k<numSamples; ++k)
            columnVector(zv, k) *= columnSums(0, k);
    }
}

template <class U, class C1, class C2, class C3>
inline void
pLSA(MultiArrayView<2, U, C1> const & features,
     MultiArrayView<2, U, C2> & fz,
     MultiArrayView<2, U, C3> & zv,
     PLSAOptions const & options = PLSAOptions())
{
    RandomNumberGenerator<> generator(RandomSeed);
    pLSA(features, fz, zv, generator, options);
}

//@}

} // namespace vigra

#endif // VIGRA_UNSUPERVISED_DECOMPOSITION_HXX

<|endoftext|>