ÿþ#include "StdAfx.h" #include <math.h> #include "RANSAC_LineFittingAlgorithm.h" CLineFitting::CLineFitting() { } CLineFitting::~CLineFitting() { } bool CLineFitting::find_in_samples (sPoint *samples, int no_samples, sPoint *data) { for (int i=0; i<no_samples; ++i) { if (samples[i].x == data->x && samples[i].y == data->y) { return true; } } return false; } void CLineFitting::get_samples (sPoint *samples, int no_samples, sPoint *data, int no_data) { // p³tÇ0ÑÐÅÁ Éõ¼´ÀÉ JŌ¬ N¬XÇ 4»‘ÇÇ HÁ ÕDÇ DÌèÍ\Õä². for (int i=0; i<no_samples; ) { int j = rand()%no_data; if (!find_in_samples(samples, i, &data[j])) { samples[i] = data[j]; ++i; } }; } int CLineFitting::compute_model_parameter(sPoint samples[], int no_samples, sLine &model) { // PCA )¼ÝÂ<Ç\¸ ÁÉ Á ¨ºx³XÇ Ó|·Tº0Ñ|¹ Æ!Î\Õä². double sx = 0, sy = 0; double sxx = 0, syy = 0; double sxy = 0, sw = 0; for(int i = 0; i<no_samples;++i) { double &x = samples[i].x; double &y = samples[i].y; sx += x; sy += y; sxx += x*x; sxy += x*y; syy += y*y; sw += 1; } //variance; double vxx = (sxx - sx*sx/sw)/sw; double vxy = (sxy - sx*sy/sw)/sw; double vyy = (syy - sy*sy/sw)/sw; //principal axis double theta = atan2(2*vxy, vxx - vyy)/2; model.mx = cos(theta); model.my = sin(theta); //center of mass(xc, yc) model.sx = sx/sw; model.sy = sy/sw; //ÁÉ ÁXÇ )¼ÈÝÂ: sin(theta)*(x - sx) = cos(theta)*(y - sy); return 1; } double CLineFitting::compute_distance(sLine &line, sPoint &x) { // \Õ È(x)\¸€½0Ñ ÁÉ Á(line)ÐÅ ´°°¹  ÁXÇ 8®tÇ(distance)|¹ Ĭ°À\Õä². return fabs((x.x - line.sx)*line.my - (x.y - line.sy)*line.mx)/sqrt(line.mx*line.mx + line.my*line.my); } double CLineFitting::model_verification (sPoint *inliers, int *no_inliers, sLine &estimated_model, sPoint *data, int no_data, double distance_threshold) { *no_inliers = 0; double cost = 0.; for(int i=0; i<no_data; i++){ // ÁÉ ÁÐÅ ´°°¹  ÁXÇ 8®tÇ|¹ Ĭ°À\Õä². double distance = compute_distance(estimated_model, data[i]); // Æ!δ ¨ºx³ÐÅÁ ǨÖ\Õ p³tÇ0ÑxÇ ½¬°Æ, ǨÖ\Õ p³tÇ0Ñ ÑÉiÕÐÅ T³\Õä². if (distance < distance_threshold) { cost += 1.; inliers[*no_inliers] = data[i]; ++(*no_inliers); } } return cost; } double CLineFitting::ransac_line_fitting(sPoint *data, int no_data, sLine &model, double distance_threshold) { const int no_samples = 2; if (no_data < no_samples) { return 0.; } sPoint *samples = new sPoint[no_samples]; int no_inliers = 0; sPoint *inliers = new sPoint[no_data]; sLine estimated_model; double max_cost = 0.; int max_iteration = (int)(1 + log(1. - 0.99)/log(1. - pow(0.5, no_samples))); for (int i = 0; i<max_iteration; i++) { // 1. hypothesis // ÐÆø¼ p³tÇ0ÑÐÅÁ „ÇXÇ\¸ N¬XÇ HÁ Õ p³tÇ0Ñ|¹ à¬x¹ä². get_samples (samples, no_samples, data, no_data); // tÇ p³tÇ0Ñ|¹ ÈÁÀÈxÇ p³tÇ0Ñ\¸ ô¼à¬ ¨ºx³ Ó|·Tº0Ñ|¹ Æ!Î\Õä². compute_model_parameter (samples, no_samples, estimated_model); // 2. Verification // ÐÆø¼ p³tÇ0Ѭ Æ!δ ¨ºx³ÐÅ ˜Ç Þ¹”²ÀÉ €¬¬À\Õä². double cost = model_verification (inliers, &no_inliers, estimated_model, data, no_data, distance_threshold); // ̹|Ç Æ!δ ¨ºx³tÇ ˜Ç Þ¹”²ä²tº, tÇ ¨ºx³ÐÅ ³\Õ Ç¨Ö\Õ p³tÇ0Ñ\¸ ÈÀ\¸´Æ ¨ºx³DÇ l­\Õä². if (max_cost < cost) { max_cost = cost; compute_model_parameter (inliers, no_inliers, model); } } delete [] samples; delete [] inliers; return max_cost; }