From 0a766c1ee5214f269f88f0e21720e7acd48c800a Mon Sep 17 00:00:00 2001 From: cr333 Date: Fri, 29 May 2015 16:56:44 +0200 Subject: [PATCH] USAC 1.0 code (no data) Source code from http://www.cs.unc.edu/~rraguram/usac/. --- license.txt | 17 + matlab/show_epipolar_lines.m | 83 ++ matlab/show_h_transformed.m | 35 + matlab/show_inliers.m | 63 ++ msvc/RunSingleTest/RunSingleTest.cpp | 208 +++++ msvc/RunSingleTest/RunSingleTest.vcproj | 186 ++++ msvc/USAC/USAC.sln | 26 + msvc/USAC/USAC.suo | Bin 0 -> 30720 bytes msvc/USAC/USAC.vcproj | 234 +++++ readme.txt | 72 ++ src/config/ConfigFileReader.cpp | 4 + src/config/ConfigFileReader.h | 59 ++ src/config/ConfigParams.cpp | 113 +++ src/config/ConfigParams.h | 110 +++ src/config/ConfigParamsFundmatrix.cpp | 75 ++ src/config/ConfigParamsFundmatrix.h | 35 + src/config/ConfigParamsHomog.cpp | 47 + src/config/ConfigParamsHomog.h | 27 + src/estimators/FundmatrixEstimator.h | 690 +++++++++++++++ src/estimators/HomogEstimator.h | 417 +++++++++ src/estimators/TemplateEstimator.h | 223 +++++ src/estimators/USAC.h | 1067 +++++++++++++++++++++++ src/utils/FundmatrixFunctions.cpp | 546 ++++++++++++ src/utils/FundmatrixFunctions.h | 27 + src/utils/HomographyFunctions.cpp | 52 ++ src/utils/HomographyFunctions.h | 11 + src/utils/MathFunctions.cpp | 505 +++++++++++ src/utils/MathFunctions.h | 40 + 28 files changed, 4972 insertions(+) create mode 100644 license.txt create mode 100644 matlab/show_epipolar_lines.m create mode 100644 matlab/show_h_transformed.m create mode 100644 matlab/show_inliers.m create mode 100644 msvc/RunSingleTest/RunSingleTest.cpp create mode 100644 msvc/RunSingleTest/RunSingleTest.vcproj create mode 100644 msvc/USAC/USAC.sln create mode 100644 msvc/USAC/USAC.suo create mode 100644 msvc/USAC/USAC.vcproj create mode 100644 readme.txt create mode 100644 src/config/ConfigFileReader.cpp create mode 100644 src/config/ConfigFileReader.h create mode 100644 src/config/ConfigParams.cpp create mode 100644 src/config/ConfigParams.h create mode 100644 src/config/ConfigParamsFundmatrix.cpp create mode 100644 src/config/ConfigParamsFundmatrix.h create mode 100644 src/config/ConfigParamsHomog.cpp create mode 100644 src/config/ConfigParamsHomog.h create mode 100644 src/estimators/FundmatrixEstimator.h create mode 100644 src/estimators/HomogEstimator.h create mode 100644 src/estimators/TemplateEstimator.h create mode 100644 src/estimators/USAC.h create mode 100644 src/utils/FundmatrixFunctions.cpp create mode 100644 src/utils/FundmatrixFunctions.h create mode 100644 src/utils/HomographyFunctions.cpp create mode 100644 src/utils/HomographyFunctions.h create mode 100644 src/utils/MathFunctions.cpp create mode 100644 src/utils/MathFunctions.h diff --git a/license.txt b/license.txt new file mode 100644 index 0000000..0af569e --- /dev/null +++ b/license.txt @@ -0,0 +1,17 @@ +//////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2012 University of North Carolina at Chapel Hill +// All Rights Reserved +// +// Permission to use, copy, modify and distribute this software and its +// documentation for educational, research and non-profit purposes, without +// fee, and without a written agreement is hereby granted, provided that the +// above copyright notice and the following paragraph appear in all copies. +// +// The University of North Carolina at Chapel Hill make no representations +// about the suitability of this software for any purpose. It is provided +// 'as is' without express or implied warranty. +// +// Please send BUG REPORTS to rraguram@cs.unc.edu +// +//////////////////////////////////////////////////////////////////////////// \ No newline at end of file diff --git a/matlab/show_epipolar_lines.m b/matlab/show_epipolar_lines.m new file mode 100644 index 0000000..7897624 --- /dev/null +++ b/matlab/show_epipolar_lines.m @@ -0,0 +1,83 @@ +%% reads in inliers and f matrix, shows epipolar lines +%function show_epipolar_lines + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% paths and constants + clear; + clc; + close all; + working_dir = 'E:\rraguram\projects\USAC\data\fundmatrix\test1'; + im1_fname = 'im1.jpg'; + im2_fname = 'im2.jpg'; + orig_pts_file = 'orig_pts.txt'; + inliers_file = 'inliers.txt'; + fund_file = 'F.txt'; + + skip = 5; % show subset of epipolar lines based on skip size + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in images + im1 = imread(fullfile(working_dir, im1_fname)); + im2 = imread(fullfile(working_dir, im2_fname)); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in original data points + fid_o = fopen(fullfile(working_dir, orig_pts_file), 'r'); + num_pts = str2num(fgetl(fid_o)); + m1 = zeros(2, num_pts); + m2 = zeros(2, num_pts); + for i = 1:num_pts + temp = textscan(fgetl(fid_o), '%s'); + m1(1, i) = str2num(temp{1,1}{1}); + m1(2, i) = str2num(temp{1,1}{2}); + m2(1, i) = str2num(temp{1,1}{3}); + m2(2, i) = str2num(temp{1,1}{4}); + end + fclose(fid_o); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in inlier data + inliers = textread(fullfile(working_dir, inliers_file)); + inliers_ind = find(inliers > 0); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in fundamental matrix + F = textread(fullfile(working_dir, fund_file)); + F = reshape(F(1:9), 3, 3)'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% show inlier matches and epipolar lines for all images + + % form vectors of inlying data points + x1 = m1(:,inliers_ind); x1 = [x1; ones(1, length(x1))]; + x2 = m2(:,inliers_ind); x2 = [x2; ones(1, length(x2))]; + + % display images side by side + I = []; + [M1,N1,K1]=size(im1); + [M2,N2,K2]=size(im2); + N3=N1+N2; + M3=max(M1,M2); + oj=N1; + oi=0; + I(1:M1,1:N1,:) = im1; + I(oi+(1:M2),oj+(1:N2),:) = im2; + + % step through each matched pair of points and display the + % corresponding epipolar lines on the two images + l2 = F*x1; % epipolar lines in image2 + l1 = F'*x2; % epipolar lines in image1 + + % solve for epipoles + [U,D,V] = svd(F); + e1 = hnormalise(V(:,3)); + e2 = hnormalise(U(:,3)); + + figure(1); imshow(uint8(im1)); hold on; axis image; axis off; + for n = 1:skip+1:length(inliers_ind) + hline(l1(:,n), 'g'); plot(e1(1), e1(2), 'g*'); + end + figure(2); imshow(uint8(im2)); hold on; axis image; axis off; + for n = 1:skip+1:length(inliers_ind) + hline(l2(:,n), 'g'); plot(e2(1), e2(2), 'g*'); + end \ No newline at end of file diff --git a/matlab/show_h_transformed.m b/matlab/show_h_transformed.m new file mode 100644 index 0000000..a4d92ed --- /dev/null +++ b/matlab/show_h_transformed.m @@ -0,0 +1,35 @@ +%% warp and display images using the estimated homography +%function show_h_transformed + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% paths and constants + working_dir = 'E:\rraguram\projects\USAC\data\homog\test1'; + im1_fname = 'im1.jpg'; + im2_fname = 'im2.jpg'; + h_file = 'H.txt'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in images + im1 = imread(fullfile(working_dir, im1_fname)); + im2 = imread(fullfile(working_dir, im2_fname)); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in homography + H = textread(fullfile(working_dir, h_file)); + H = reshape(H(1:9), 3, 3)'; + H = inv(H); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% transform image and display + + mytform = maketform('projective', H'); + [im2t xd yd] = imtransform(im2, mytform, 'FillValues', 255); + figure; imshow(im2t, 'XData', xd, 'YData', yd); hold on; + h = imshow(im1, gray(256)); + set(h, 'AlphaData', 0.6) + + mytform = maketform('projective', inv(H)'); + [im1t xd yd] = imtransform(im1, mytform, 'FillValues', 255); + figure; imshow(im1t, 'XData', xd, 'YData', yd); hold on; + h = imshow(im2, gray(256)); + set(h, 'AlphaData', 0.6) \ No newline at end of file diff --git a/matlab/show_inliers.m b/matlab/show_inliers.m new file mode 100644 index 0000000..3f7278a --- /dev/null +++ b/matlab/show_inliers.m @@ -0,0 +1,63 @@ +%% display inliers for an image pair +%function show_inliers + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% paths and constants +% clear; + clc; + working_dir = 'E:\rraguram\projects\USAC\data\homog\test2'; + im1_fname = 'im1.jpg'; + im2_fname = 'im2.jpg'; + orig_pts_file = 'orig_pts.txt'; + inliers_file = 'inliers.txt'; + + skip = 5; % show subset of inliers based on skip size + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in images + im1 = imread(fullfile(working_dir, im1_fname)); + im2 = imread(fullfile(working_dir, im2_fname)); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in original data points + fid_o = fopen(fullfile(working_dir, orig_pts_file), 'r'); + num_pts = str2num(fgetl(fid_o)); + m1 = zeros(2, num_pts); + m2 = zeros(2, num_pts); + for i = 1:num_pts + temp = textscan(fgetl(fid_o), '%s'); + m1(1, i) = str2num(temp{1,1}{1}); + m1(2, i) = str2num(temp{1,1}{2}); + m2(1, i) = str2num(temp{1,1}{3}); + m2(2, i) = str2num(temp{1,1}{4}); + end + fclose(fid_o); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% read in inlier data + inliers = textread(fullfile(working_dir, inliers_file)); + inliers_ind = find(inliers > 0); + n = randperm(length(inliers_ind)); + inliers_ind = inliers_ind(n); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% display images side by side and plot inliers + I = []; + padding = 15; + [M1,N1,K1]=size(im1); + [M2,N2,K2]=size(im2); + N3=N1+N2+padding; + M3=max(M1,M2); + oj=N1+padding; + oi=0; + I(1:M3, 1:N3, 1) = 255; I(1:M3, 1:N3, 2) = 255; I(1:M3, 1:N3, 3) = 255; + I(1:M1,1:N1,:) = im1; + I(oi+(1:M2),oj+(1:N2),:) = im2; + + figure(1); + title_str = sprintf('%s and %s: %d inliers',im1_fname, im2_fname, length(inliers_ind)); + imshow(uint8(I)); set(1,'name', title_str); hold on; axis image; axis off; + for m = 1:skip+1:length(inliers_ind) + plot([m1(1,inliers_ind(m)) m2(1,inliers_ind(m))+oj], [m1(2,inliers_ind(m)) m2(2,inliers_ind(m))+oi],'go-','LineWidth',2,'MarkerSize',2,'MarkerFaceColor','g'); + end + \ No newline at end of file diff --git a/msvc/RunSingleTest/RunSingleTest.cpp b/msvc/RunSingleTest/RunSingleTest.cpp new file mode 100644 index 0000000..ca05a64 --- /dev/null +++ b/msvc/RunSingleTest/RunSingleTest.cpp @@ -0,0 +1,208 @@ +#define NOMINMAX +#include +#include +#include +#include +#include +#include "config\ConfigParams.h" +#include "estimators\FundMatrixEstimator.h" +#include "estimators\HomogEstimator.h" + +// helper functions +bool readCorrsFromFile(std::string& inputFilePath, std::vector& pointData, unsigned int& numPts) +{ + // read data from from file + std::ifstream infile(inputFilePath.c_str()); + if (!infile.is_open()) + { + std::cerr << "Error opening input points file: " << inputFilePath << std::endl; + return false; + } + infile >> numPts; + pointData.resize(6*numPts); + for (unsigned int i = 0; i < numPts; ++i) + { + infile >> pointData[6*i] >> pointData[6*i+1] >> pointData[6*i+3] >> pointData[6*i+4]; + pointData[6*i+2] = 1.0; + pointData[6*i+5] = 1.0; + } + infile.close(); + return true; +} + +bool readPROSACDataFromFile(std::string& sortedPointsFile, unsigned int numPts, std::vector& prosacData) +{ + std::ifstream infile(sortedPointsFile.c_str()); + if (!infile.is_open()) + { + std::cerr << "Error opening sorted indices file: " << sortedPointsFile << std::endl; + return false; + } + for (unsigned int i = 0; i < numPts; ++i) + { + infile >> prosacData[i]; + } + infile.close(); + return true; +} + + +// --------------------------------------------------------------------------------------------------------------- +int main(int argc, char **argv) +{ + // check command line args + if (argc < 3) + { + std::cerr << "Usage: RunSingleTest " << std::endl; + std::cerr << "\t: 0 (fundamental matrix), 1 (homography)" << std::endl; + std::cerr << "\t: full path to configuration file" << std::endl; + return(EXIT_FAILURE); + } + int estimation_problem = atoi(argv[1]); + std::string cfg_file_path = argv[2]; + + // seed random number generator + srand((unsigned int)time(NULL)); + + // initialize the appropriate robust estimation problem + if (estimation_problem == 0) + { + // ------------------------------------------------------------------------ + // initialize the fundamental matrix estimation problem + ConfigParamsFund cfg; + if ( !cfg.initParamsFromConfigFile(std::string(cfg_file_path)) ) + { + std::cerr << "Error during initialization" << std::endl; + return(EXIT_FAILURE); + } + FundMatrixEstimator* fund = new FundMatrixEstimator; + fund->initParamsUSAC(cfg); + + // read in input data points + std::vector point_data; + if ( !readCorrsFromFile(cfg.fund.inputFilePath, point_data, cfg.common.numDataPoints) ) + { + return(EXIT_FAILURE); + } + + // read in prosac data if required + std::vector prosac_data; + if (cfg.common.randomSamplingMethod == USACConfig::SAMP_PROSAC) + { + prosac_data.resize(cfg.common.numDataPoints); + if ( !readPROSACDataFromFile(cfg.prosac.sortedPointsFile, cfg.common.numDataPoints, prosac_data) ) + { + return(EXIT_FAILURE); + } + cfg.prosac.sortedPointIndices = &prosac_data[0]; + } else { + cfg.prosac.sortedPointIndices = NULL; + } + + // set up the fundamental matrix estimation problem + + fund->initDataUSAC(cfg); + fund->initProblem(cfg, &point_data[0]); + if (!fund->solve()) + { + return(EXIT_FAILURE); + } + + // write out results + size_t pos = (cfg.fund.inputFilePath).find_last_of("/\\"); + std::string working_dir = (cfg.fund.inputFilePath).substr(0, pos); + std::ofstream outmodel((working_dir + "\\F.txt").c_str()); + for (unsigned int i = 0; i < 3; ++i) + { + for (unsigned int j = 0; j < 3; ++j) + { + outmodel << fund->final_model_params_[3*i+j] << " "; + } + } + outmodel.close(); + std::ofstream outinliers((working_dir + "\\inliers.txt").c_str()); + for (unsigned int i = 0; i < cfg.common.numDataPoints; ++i) + { + outinliers << fund->usac_results_.inlier_flags_[i] << std::endl; + } + outinliers.close(); + + // clean up + point_data.clear(); + prosac_data.clear(); + fund->cleanupProblem(); + delete fund; + + } else if (estimation_problem == 1) { + // ------------------------------------------------------------------------ + // initialize the homography estimation problem + ConfigParamsHomog cfg; + if ( !cfg.initParamsFromConfigFile(std::string(cfg_file_path)) ) + { + std::cerr << "Error during initialization" << std::endl; + return(EXIT_FAILURE); + } + + HomogEstimator* homog = new HomogEstimator; + homog->initParamsUSAC(cfg); + + // read in input data points + std::vector point_data; + if ( !readCorrsFromFile(cfg.homog.inputFilePath, point_data, cfg.common.numDataPoints) ) + { + return(EXIT_FAILURE); + } + + // read in prosac data if required + std::vector prosac_data; + if (cfg.common.randomSamplingMethod == USACConfig::SAMP_PROSAC) + { + prosac_data.resize(cfg.common.numDataPoints); + if ( !readPROSACDataFromFile(cfg.prosac.sortedPointsFile, cfg.common.numDataPoints, prosac_data) ) + { + return(EXIT_FAILURE); + } + cfg.prosac.sortedPointIndices = &prosac_data[0]; + } else { + cfg.prosac.sortedPointIndices = NULL; + } + + // set up the homography estimation problem + homog->initDataUSAC(cfg); + homog->initProblem(cfg, &point_data[0]); + if (!homog->solve()) + { + return(EXIT_FAILURE); + } + + // write out results + size_t pos = (cfg.homog.inputFilePath).find_last_of("/\\"); + std::string working_dir = (cfg.homog.inputFilePath).substr(0, pos); + std::ofstream outmodel((working_dir + "\\H.txt").c_str()); + for (unsigned int i = 0; i < 3; ++i) + { + for (unsigned int j = 0; j < 3; ++j) + { + outmodel << homog->final_model_params_[3*i+j] << " "; + } + } + outmodel.close(); + std::ofstream outinliers((working_dir + "\\inliers.txt").c_str()); + for (unsigned int i = 0; i < cfg.common.numDataPoints; ++i) + { + outinliers << homog->usac_results_.inlier_flags_[i] << std::endl; + } + outinliers.close(); + + // clean up + point_data.clear(); + prosac_data.clear(); + homog->cleanupProblem(); + delete homog; + } else { + std::cout << "Estimation problem currently not implemented" << std::endl; + } + + return(EXIT_SUCCESS); +} + diff --git a/msvc/RunSingleTest/RunSingleTest.vcproj b/msvc/RunSingleTest/RunSingleTest.vcproj new file mode 100644 index 0000000..352cb3c --- /dev/null +++ b/msvc/RunSingleTest/RunSingleTest.vcproj @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msvc/USAC/USAC.sln b/msvc/USAC/USAC.sln new file mode 100644 index 0000000..58af859 --- /dev/null +++ b/msvc/USAC/USAC.sln @@ -0,0 +1,26 @@ + +Microsoft Visual Studio Solution File, Format Version 10.00 +# Visual Studio 2008 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "USAC", "USAC.vcproj", "{42661DF6-BA9C-4C97-A38B-950C40360ADB}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RunSingleTest", "..\RunSingleTest\RunSingleTest.vcproj", "{20D489DB-EAB7-4CF4-97EC-FD56C313F8DA}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Release|Win32 = Release|Win32 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {42661DF6-BA9C-4C97-A38B-950C40360ADB}.Debug|Win32.ActiveCfg = Debug|Win32 + {42661DF6-BA9C-4C97-A38B-950C40360ADB}.Debug|Win32.Build.0 = Debug|Win32 + {42661DF6-BA9C-4C97-A38B-950C40360ADB}.Release|Win32.ActiveCfg = Release|Win32 + {42661DF6-BA9C-4C97-A38B-950C40360ADB}.Release|Win32.Build.0 = Release|Win32 + {20D489DB-EAB7-4CF4-97EC-FD56C313F8DA}.Debug|Win32.ActiveCfg = Debug|Win32 + {20D489DB-EAB7-4CF4-97EC-FD56C313F8DA}.Debug|Win32.Build.0 = Debug|Win32 + {20D489DB-EAB7-4CF4-97EC-FD56C313F8DA}.Release|Win32.ActiveCfg = Release|Win32 + {20D489DB-EAB7-4CF4-97EC-FD56C313F8DA}.Release|Win32.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/msvc/USAC/USAC.suo b/msvc/USAC/USAC.suo new file mode 100644 index 0000000000000000000000000000000000000000..05eacd14f3abb59ef7c59c1d7a39d3b5e15d620d GIT binary patch literal 30720 zcmeI534B%6wZ;#q41y@5iV8tyl^6!Yq+$p}RFEMI4p%W|fIvcM21Ug_ain!DD$WBI zeToxWi`HrtoJXp@zV=mHhgPk9R_(KOs0#gm=d7K367D_sW`gwny25w&I_s=+_Hg!I zd+l}hz5H@>*LT)D(EDAl9v$xOq0;Mu9^>HW&@Y zfU!Wj!oO+T7MSH#@K?>VnCvYft@O^I#nPu<^cr_p^@Aw7HRrjGWo=(=Bb#x=tUz-H zIaYX!yramy1a7joEb129oa==Za``LWMj?qneOENwEAXnk#okn}l(gEL!)GyNa`LJ` zE$Qa~uWcLkdsl{}ilGy-p&DPJN#u5_SK}?kv(e@Z1w zMY7u}v&fANrLqHiP&n~n8=&zoJD~9|JK)B@^fdmJE+Eb2T8)2|MK-{Vf7uq<05|^6 zg?Hm$*M`Tx?1skuWkBbImGH6w8vi<;L@FDg@m~gH12q2IkiQ%M(hHCO@ti*tbVBEF z(h1-QAiasCxyDZ>%`<*B=~3WlJDy59&3Iim1AZn@SI#&29MZX9o*f@gdIC5R#K1}5 zWH29`0t&#Xpb+Su#iS+1FC;Ami@;)VCU^lX0To~=_#UVP>MPZt2IxMglWHVsH0$^* z(&gZ6@O|(f;2hBQi(^JV(6R8C^|#!X|I}N*e-u%J=(F684brc#%;RVh>3Y>amsucI zJ9?|&D{0$>v_~2IB#tU*uT%M4#&Hd8SV&sPaSUxmQoLuFoW4~88cfpqa;ReETvqkTNNmw z2V~IuWXWo%Nf}4^$ZDu#Dd#ho4Ko=vF?3WfU0L7$Ohl$uSWIhGaFynSC_6L+`>Xj{ z_Ka?}l_Vv}{;#x#tDnE@e<3AUO1>)dlE~cUFErf#??V16uc&_hYUw4kS3Y^y&=x97 z6p?=$-B0#1-TKSN%4-|kHF^GL(C^e=W#N)jW9lz^;O%|Z41-!$%APOp<6IM2pHg?D<=QFxH~ADt;o zy8U0im*!TH#(On+R#G=vthl9fo?$U{uBKPY7D%(MMH)x`@=uhv$iHcdk8uB=#C;2C z(Q>;S4A5{S)wfRmv0p0o?ial(7#wl0E)`r`J$6(nQD-TGRa@2v?2L-N53d^ zGc!dYNwBuBBj6xiz53Th9k=tnHt$cG6L22SX}^9a9_QArL)+Gd8pfP7&n64p-I&kU z9rHbGdM%;d1aw$8csC$A>WQ z=5j6zSvH?JTsaYMER!d3x<5{yjhH-QS_9t z{)igJjFLmCHU2ab^Z3;G8;x9}GY_pp7=gKr!{H#0D<^S9x+76>uf$d&n5WcI9sDR( zdS%P^26Xx^=fF!hPI=(B>o#}4ectX@JQ&R&i)jz70kilU?yvTSM?Ty+w56<0fq&Pc+BwhV z7E`d^1b5^Z$MvrdxlFms1rqfTHp@6CWrh`BeUjxyrvyH-FV@i1}Z~ZPx0k@s!9K*SEO3 zdv8W}h3`=+q7sj2UDyQMr}P#(s$Pnm$X-gO`gZcJ-=X~{(Rhg4v{N4Z>~acO+v4Iy^=+DJb2M8c3#hr5%81ERan8tRuI}ovo18l?Hlwm)L1jUC zMqX)IaaG2UWn;33#-^1PR#sG1lvHQTEv>34D9gyNt|=<5$QYZM6`PwID~$3#N4V@F zQu&`MiDuKFL=nn%>M+vRjgQJ-@A6X-`TO}QZ&50Joj(TLVTw`tYwV?>#^iq-H+|Ng zZdCr-zmSU5{)%VHt4X*1Md;paH?FLXRaF+omQ__(mM+mI4zJyzOKQr>4iC;xDr{7@ zDqT`oR#Q|QJFU2|x}tKMyH1-tE>>Atu%L$C8e3Xfu?V%QSYZWM%*oHm9adRfR$Ne3 z94jiQE{K)XEGa54pm@t-)x}lS!(+wE3d)z36%VVaDk#h>EZMfu{m3wA3q^cfBxBpy zNtFK+M8wPREn{BX%Tau0DKq~WL@9zj1TH!rUQ5heJ5;92AGJyyX{-u*^m<>c5iQ#< znf8s?lCaj-o{sE5AktWrp6sBm7q9iPd*G+M2iR*P!0cb2@S zF6`GUTwvKY)m6|dGaYM9qT_7f=GciQlROtl?FFR-(D!GKc<_1Il3d%A)%=J z<)fz}<*$4lrP9~=)4;Q)IF}h_HW2{Dyo23o`OH|A=xAqGv1uhw)c%7w-3_P=$u;9= zj)m12@+r|v>E*Qxq`INJfn=KdW&_dnz+Jlr zJ`3Cyl5K?7{hl(J=7Jl5=(oUK`#bo7;2o36?|&1BWGAGjIjFrDXtc>@!jCc8F(#8O zm};^GCeu8(&}3C6Q*6E3WM`SI6q)vgi0&|%>ZCbTwAo~8lh;i4jmgwz-n}%Udb;fy#o9QtO8eptH9M@HMj;`3$6pQ6B_plh};Bj2DgA) z!5Sc&@nf(S+z##l{|W8{cY$@_Zg3B{7u*N#2kXHD;6d;c@DTVZ*Z>{|kARp`Mg8=uJL~${iE?8kbY?VN2GrS{|i0_e*vF>Pr+y4ui*c{ zZqz~d{5$*?;7jlo_!|5Jd;`7(-+?V)EAR)jHlsz4zNU=5S*gZedV6!m`*TuyJ8IIq zVpGc1w`@J7QQLH(mE&)aB~j!)k)s-%>+J8H(04Yf0zvSZ zAmEfh#5MZfK@hJ>{SG_6x(uS}J<015dzQiO!tL=rlJNHkmQe$JlQ7s%6ZkA8ow{C>HRwB z&9Y;6q->Ao`F2-R-x%*jS(|%5MBHvRwRBx5-Hk2UlFn`=?tO7PHQBch+wR!dn-aFZ z{TMwcRcl=3x1(H5-F=iS`jFhJCV%_gnewdMf&I(*6@lI8O=&LMp|haAU8(*1%YgP| z1n=ltYQ5jkC+2p!bfR2064hz!{^LY;zQ}*yQ^3qrl3>49%x@KW2*c4(`)DtozWY$j zLZk-UP(W*`#R6}(8zpQh?;#vzOXn?BN6NAzckHpqYqdKb6e(({-W`5U@p&ZrZLFeM z9kx@M8WLe^PnovIe3EWEIZL3`WXpP5iTs1s;OoBhe%U2dzBZP*2Yf%!6==lm3FHs& z4E6(($=B-xic-#ajq}y(&StqAk5$me&siR^pq0WtCX4?^Y`QRZS*#xiwh~5KEPcbv;DPAU-;xdY}i3THZdP88Occ956Ka3Bx z+TkWsYvsc0Sx*YdPB7VtA(`GQsI~M?fq#BWm%|K#Ut%(?GgrXtX`E*=W_aj{!l05=1{U=N|4}KoJ=yj8ce*<1+`IX6Jk$+=)AB6NiG`){Zrn-G> zdhId9s;{U6yzbY@WGmozgBOhm>5VkKY?B?&`7x$XL%_fP?e@vb05*@hu z%g;zR|M$_o$8Of}UyO>1^snnV&L7k1mZbH?XkNSPV1L-ATZ7geJ9+ckr|#2(PF|bi zuwP|->sv9G#eC{l2!r1rN$1zMwC}EMo4(|rXk_wSRE9WBhwWj;F{=OTw;fWE`oHYz zfgs)doc#;D z0j;?7Kqdg^nQNDVJtXgK`1wGVOERsr{s=_<5jed8Fw%3YFUhsVt{dq%QXR`9ngFvW z=@B8>T$9ZU$qM1M(kcqcO5wHAS`?B!1TXLPry<$%CVL?ydkbFH+U-Q<(EEZ9t*YD( zW69c3NUf^clKRK+T4{AAbt|pD@YS=o7d%>#>+)7L3+7Fp1Go*L0>1ijb%Oe|J zUUk^ho`FnW_3V(|3V3> zAL)FP$$ON?9?+T0Im$I$W65kh*4Lp3WL_v1;YFBB^?zYVKlD z8cFJ{RIfJrqaGt4Oe8z#u9c6eUaB>NWb&xhgGEOH$wWs3J%wpNGT~&PSl1~)$1}mP zKy(Ih*PaQlYtJ^BM%VX&$a(MLF9*`Q66jj3L9YU$$AG)v#eb>UpgAjR>P47t4t420O z7eK2_CjLs(yV_)Pk*zkp8$xD?aE(|%Ny=}wc4fL~{N4~F!9VtN}) zrjhcn={;dG#pfJp&sbaJJAs|SE}$J~4?2L3U{@f$&X#r|-5q3ruAm#}4m79t1iiqX zU@x#Y=neEt^epxRdKP*H{ebQ(Xcp1)&~q3B)aFBgo`=pWc6X5RnWXa4Bp*&X!gvmA zX*4qJ4c6?VnP)sW6e#YenBN3&1jqpsfh&ieZywO|)pM2n|1+sisDAp^RPt{_C$?sa zQ3sYtmVz7AM#a^;oYj&2mLJy_D4r-i&FRh($+Ac$ng!I$=K#rs5}^Apv}2vqXc9SV zB3{-+*Iom3omNEG0?|6)^0*sbdiR@b0;&8T(Wk)aeFiVR&rQ~c^h+R8?{j)tFwz@o zGUYqUPOA4A>-R$GsRv4S1W->D%`lmG{Z@kX=9sJqey-^)4C$4c-eQxlT9W*4_;+C%4D+IN5hM53F+NxdbgQu6zA8P-p@mN z@-KA1Crqa2_@wDQZ!$e=N0J4-(@7GieG#kpr-2M0QlFDdx1S8gf;`}47s0DPt~8nM zA*(8S8Kjf1bXS;9B3@BH)$e?hX$)KhFSZNW*VI=G%dK0~ zS4nsrzRvMo&=l{E!xuXFj&IS$)}l%$QpB84iCrnfB~QjDs-<+p^6k~4T5W94J{sR& zk!(NXi`HyPWg}W+D`T~+FLdTXx#y-gLYl|AnoO&R?)D&uk~&K_w2iwWer?0zAMxKN zOSFe3S<7nQLu-8VvpeP5z7d7iy`%TZ{-3Gi*Y+2A3H?A>2XqDBNo>BK z4PTCvn#o4y#(q6R|FMw!ndg@MeTC*4j4ks-56RY+@Thjw<_i5-Yu)^Lrs4nZ;VzD! z1HD6XLIFv?;hL)Z2Jd5<`&&JUcEUE!uB&%%pFOC1l9PV-LcistUx*IYW;^;DA1!PD zo9{W*oqxA|ucOxO1@t_d5>1`K^=@{b2`?{Yw#g(f2D<+OlSRiLMxzx?jfp>KFRF__ sMCYF!ojGS+{72C~=eec2NbhxAWH^;>I_H(IqjdC}TgT + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/readme.txt b/readme.txt new file mode 100644 index 0000000..58e6251 --- /dev/null +++ b/readme.txt @@ -0,0 +1,72 @@ +USAC Version 1.0 +================= + +Overview: +-------- + +USAC, or Universal RANSAC, is a framework for robust estimation. USAC extends the simple hypothesize-and-verify structure of standard RANSAC to incorporate a number of important practical and computational considerations. This implementation thus addresses many of the limitations of standard RANSAC (efficiency, robustness, degeneracy, accuracy) within a single unified package. + +A more in-depth discussion may be found in the following paper: + +R. Raguram, O. Chum, M. Pollefeys, J. Matas, J-M. Frahm. "USAC: A Universal Framework for Random Sample Consensus". PAMI (under review) + +------------------ +Using the library +------------------ + +This initial release contains a version of USAC that has been tested on Windows, with Visual Studio 2008/2010. To run the code, build the solution file located under msvc/USAC. The 'RunSingleTest' application provides a simple introduction to using the library. + +In brief, calling the estimator consists of these basic steps (shown below for homography estimation): + + HomogEstimator* homog = new HomogEstimator; + + // initialize the USAC parameters, either from a config file, or from your application + homog->initParamsUSAC(cfg); + + // get input data points/prosac ordering data (if required) + // set up point_data, cfg.common.numDataPoints, cfg.prosac.sortedPointIndices + + // set up the estimation problem + homog->initDataUSAC(cfg); + homog->initProblem(cfg, &point_data); + + // solve + if (!homog->solve()) + { + return(EXIT_FAILURE); + } + + // do stuff + + // cleanup + homog->cleanupProblem(); + delete homog; + +Configuring USAC: +----------------- +The data/ folder contains example configuration files that may be used to control the operation of USAC. Individual modules (e.g., non-uniform sampling with PROSAC) may be switched on or off in this file. Turning on all optimizations provides the best performance, corresponding to USAC-1.0. The library may also be configured to operate as a specific RANSAC variant (e.g., LO-RANSAC), by turning on only the appropriate option in the config file. + +Dependencies (ext/): +-------------------- +USAC currently requires the following external libraries to be provided: +LAPACK: For linear algebra routines (used in the fundamental matrix/homography estimators) +LIBCONFIG: Used to configure the operation of USAC (used to demonstrate the usage of parameters) + +Solving a specific estimation problem with USAC: +------------------------------------------------- +The src/estimators folder contains the main USAC estimator, along with sample implementations of fundamental matrix and homography estimators. Thus, the main robust estimator is decoupled from the specific estimation problem. + +To write your own estimator to solve a different problem (e.g., fitting planes to 3D points), you need to derive a class from the base USAC class, and implement the data-specific functionality (e.g., computing model paramters from minimal samples, etc.) within it. In particular, there should be an implementation for each of the pure virtual functions in USAC.h (however, many of these can be empty). + +To help with this process, a template (TemplateEstimator.h) is provided in the src/estimators folder, which can be used as a basis for new estimation problems. In most cases, you should not have to change anything in USAC.h, unless you want to alter the behaviour of the robust estimation algorithm itself. + +Misc: +----- +The matlab/ folder has some simple tools that can be used to visualize the output of the estimation process: + * show_inliers: reads in an image pair, and draws inlier correspondences between the images + * show_h_transformed: for homography estimation; warps image into alignment based on the estimated homography + * show_epipolar_lines: for fundamental matrix estimation; steps through epipolar lines corresponding to the computed epipolar geometry + +Bug reports: +------------ +Please contact rraguram@cs.unc.edu with bug reports, or questions about this library. \ No newline at end of file diff --git a/src/config/ConfigFileReader.cpp b/src/config/ConfigFileReader.cpp new file mode 100644 index 0000000..01f6c21 --- /dev/null +++ b/src/config/ConfigFileReader.cpp @@ -0,0 +1,4 @@ +#pragma warning(disable: 4290) // disable libconfig compile warnings +#include "ConfigFileReader.h" + + diff --git a/src/config/ConfigFileReader.h b/src/config/ConfigFileReader.h new file mode 100644 index 0000000..fa46d8e --- /dev/null +++ b/src/config/ConfigFileReader.h @@ -0,0 +1,59 @@ +#ifndef CONFIGFILEREADER_H +#define CONFIGFILEREADER_H +#pragma warning(disable: 4290) // disable libconfig compile warnings +#include +#include +#include +#include + +class ConfigFileReader: public libconfig::Config +{ + public: + template + bool getTopLevelParameterValue(const std::string settingName, const std::string parameterName, + T& value) const + { + const libconfig::Setting& root = getRoot(); + try + { + const libconfig::Setting &setting = root[settingName]; + if (!setting.lookupValue(parameterName.c_str(), value)) + { + std::cout << settingName << ": parameter " << parameterName << " not found" << std::endl; + return false; + } + } // end try + catch(...) + { + std::cerr << settingName << " block not found; recheck configuration file" << std::endl; + return false; + } + return true; + } + + template + bool getSubLevelParameterValue(const std::string settingName, const std::string subSettingName, + const std::string parameterName, T& value) const + { + const libconfig::Setting& root = getRoot(); + try + { + const libconfig::Setting &setting = root[settingName][subSettingName]; + if (!setting.lookupValue(parameterName.c_str(), value)) + { + std::cout << settingName << ":" << subSettingName + << ": parameter " << parameterName << " not found" << std::endl; + return false; + } + } + catch(...) + { + std::cerr << settingName << ":" << subSettingName << " block not found; recheck configuration file" << std::endl; + return false; + } + return true; + } + +}; + +#endif \ No newline at end of file diff --git a/src/config/ConfigParams.cpp b/src/config/ConfigParams.cpp new file mode 100644 index 0000000..9f2b8c1 --- /dev/null +++ b/src/config/ConfigParams.cpp @@ -0,0 +1,113 @@ +#include "ConfigFileReader.h" +#include "ConfigParams.h" + +bool ConfigParams::initUSACParamsFromConfigFile(const ConfigFileReader& cfr) +{ + // read in parameters + try + { + // first get all common parameters + std::string rand_sampling_method, verif_method, local_opt_method, est_problem; + if( !( cfr.getTopLevelParameterValue("common", "ransac_conf", common.confThreshold) && + cfr.getTopLevelParameterValue("common", "min_sample_size", common.minSampleSize) && + cfr.getTopLevelParameterValue("common", "inlier_threshold", common.inlierThreshold) && + cfr.getTopLevelParameterValue("common", "max_hypotheses", common.maxHypotheses) && + cfr.getTopLevelParameterValue("common", "max_solutions_per_sample", common.maxSolutionsPerSample) && + cfr.getTopLevelParameterValue("common", "prevalidate_sample", common.prevalidateSample) && + cfr.getTopLevelParameterValue("common", "prevalidate_model", common.prevalidateModel) && + cfr.getTopLevelParameterValue("common", "degen_check", common.testDegeneracy) && + cfr.getTopLevelParameterValue("common", "random_sampling_method", rand_sampling_method) && + cfr.getTopLevelParameterValue("common", "verif_method", verif_method) && + cfr.getTopLevelParameterValue("common", "local_opt", local_opt_method) ) ) + { + return false; + } + else + { + // verify parameter values + if (common.confThreshold < 0 || common.confThreshold > 1) + { + std::cout << "RANSAC confidence value must be between 0 and 1" << std::endl; + return false; + } + + if (rand_sampling_method.compare("UNIFORM") == 0) + common.randomSamplingMethod = USACConfig::SAMP_UNIFORM; + else if (rand_sampling_method.compare("PROSAC") == 0) + common.randomSamplingMethod = USACConfig::SAMP_PROSAC; + else + { + std::cerr << "Random sampling method " << rand_sampling_method << " not recognized" << std::endl; + return false; + } + + if (verif_method.compare("STANDARD") == 0) + common.verifMethod = USACConfig::VERIF_STANDARD; + else if (verif_method.compare("SPRT") == 0) + common.verifMethod = USACConfig::VERIF_SPRT; + else + { + std::cerr << "Verification method " << verif_method << " not recognized" << std::endl; + return false; + } + + if (local_opt_method.compare("NO_LO") == 0) + common.localOptMethod = USACConfig::LO_NONE; + else if (local_opt_method.compare("LOSAC") == 0) + common.localOptMethod = USACConfig::LO_LOSAC; + else + { + std::cerr << "Local optimization method " << local_opt_method << " not recognized" << std::endl; + return false; + } + } + + // read in PROSAC parameters if required + if (common.randomSamplingMethod == USACConfig::SAMP_PROSAC) + { + if (!( cfr.getTopLevelParameterValue("prosac", "max_prosac_samples", prosac.maxSamples) && + cfr.getTopLevelParameterValue("prosac", "beta", prosac.beta) && + cfr.getTopLevelParameterValue("prosac", "non_rand_conf", prosac.nonRandConf) && + cfr.getTopLevelParameterValue("prosac", "min_stopping_length", prosac.minStopLen) && + cfr.getTopLevelParameterValue("prosac", "sorted_points_path", prosac.sortedPointsFile) )) + { + return false; + } + } + + // read in SPRT parameters if required + if (common.verifMethod == USACConfig::VERIF_SPRT) + { + if ( !(cfr.getTopLevelParameterValue("sprt", "time_model", sprt.tM) && + cfr.getTopLevelParameterValue("sprt", "models_per_sample", sprt.mS) && + cfr.getTopLevelParameterValue("sprt", "delta", sprt.delta) && + cfr.getTopLevelParameterValue("sprt", "epsilon", sprt.epsilon)) ) + { + return false; + } + } + + // read in local optimization parameters if required + if (common.localOptMethod == USACConfig::LO_LOSAC) + { + if ( !(cfr.getTopLevelParameterValue("losac", "inner_sample_size", losac.innerSampleSize) && + cfr.getTopLevelParameterValue("losac", "inner_ransac_repetitions", losac.innerRansacRepetitions) && + cfr.getTopLevelParameterValue("losac", "threshold_multiplier", losac.thresholdMultiplier) && + cfr.getTopLevelParameterValue("losac", "num_steps", losac.numStepsIterative)) ) + { + return false; + } + } + } + catch(...) + { + return false; + } + + return true; +} + +bool ConfigParams::initParamsFromConfigFile(std::string& configFilePath) { + std::cerr << "Implement this in the derived class" << std::endl; + return false; +} \ No newline at end of file diff --git a/src/config/ConfigParams.h b/src/config/ConfigParams.h new file mode 100644 index 0000000..955d99b --- /dev/null +++ b/src/config/ConfigParams.h @@ -0,0 +1,110 @@ +#ifndef CONFIGPARAMS_H +#define CONFIGPARAMS_H + +#include +#include "ConfigFileReader.h" + +namespace USACConfig +{ + enum RandomSamplingMethod {SAMP_UNIFORM, SAMP_PROSAC, SAMP_UNIFORM_MM}; + enum VerifMethod {VERIF_STANDARD, VERIF_SPRT}; + enum LocalOptimizationMethod {LO_NONE, LO_LOSAC}; + + // common USAC parameters + struct Common + { + // change these parameters according to the problem of choice + Common(): confThreshold (0.95), + minSampleSize (7), + inlierThreshold (0.001), + maxHypotheses (100000), + maxSolutionsPerSample (3), + numDataPoints (0), + prevalidateSample (false), + prevalidateModel (false), + testDegeneracy (false), + randomSamplingMethod (SAMP_UNIFORM), + verifMethod (VERIF_STANDARD), + localOptMethod (LO_NONE) + {} + double confThreshold; + unsigned int minSampleSize; + double inlierThreshold; + unsigned int maxHypotheses; + unsigned int maxSolutionsPerSample; + unsigned int numDataPoints; + bool prevalidateSample; + bool prevalidateModel; + bool testDegeneracy; + RandomSamplingMethod randomSamplingMethod; + VerifMethod verifMethod; + LocalOptimizationMethod localOptMethod; + }; + + // PROSAC parameters + struct Prosac + { + Prosac(): maxSamples (200000), + beta (0.05), + nonRandConf (0.95), + minStopLen (20), + sortedPointsFile (""), // leave blank if not reading from file + sortedPointIndices (NULL) // this should point to an array of point indices + // sorted in decreasing order of quality scores + {} + unsigned int maxSamples; + double beta; + double nonRandConf; + unsigned int minStopLen; + std::string sortedPointsFile; + unsigned int* sortedPointIndices; + }; + + // SPRT parameters + struct Sprt + { + Sprt(): tM (200.0), + mS (2.38), + delta (0.05), + epsilon (0.2) + {} + double tM; + double mS; + double delta; + double epsilon; + }; + + // LOSAC parameters + struct Losac + { + Losac(): innerSampleSize (14), + innerRansacRepetitions (10), + thresholdMultiplier (2.0), + numStepsIterative (4) + {} + unsigned int innerSampleSize; + unsigned int innerRansacRepetitions; + double thresholdMultiplier; + unsigned int numStepsIterative; + }; + +} + +// main configuration struct that is passed to USAC +class ConfigParams +{ +public: + // to be overridden to read in model specific data + virtual bool initParamsFromConfigFile(std::string& configFilePath); + + // function to read in common usac parameters from config file + bool initUSACParamsFromConfigFile(const ConfigFileReader& cfr); + + USACConfig::Common common; + USACConfig::Prosac prosac; + USACConfig::Sprt sprt; + USACConfig::Losac losac; +}; + + +#endif \ No newline at end of file diff --git a/src/config/ConfigParamsFundmatrix.cpp b/src/config/ConfigParamsFundmatrix.cpp new file mode 100644 index 0000000..83c4bf0 --- /dev/null +++ b/src/config/ConfigParamsFundmatrix.cpp @@ -0,0 +1,75 @@ +#include "ConfigParamsFundmatrix.h" +#include "ConfigFileReader.h" + +bool ConfigParamsFund::initParamsFromConfigFile(std::string &configFilePath) +{ + // initialize the config file reader with the input config file + ConfigFileReader cfr; + try + { + cfr.readFile(configFilePath.c_str()); + } + catch(const libconfig::FileIOException) + { + std::cerr << "I/O error while reading file." << std::endl; + return false; + } + catch(const libconfig::ParseException &pex) + { + std::cerr << "Parse error at " << pex.getFile() << ":" << pex.getLine() + << " - " << pex.getError() << std::endl; + return false; + } + + // now read in parameters + try + { + // usac parameters + if (!initUSACParamsFromConfigFile(cfr)) + { + + std::cerr << "Error reading USAC parameters from config file." << std::endl; + return false; + } + + // fundmatrix parameters + if ( !cfr.getTopLevelParameterValue("problem_specific", "input_file_path", fund.inputFilePath) ) + { + return false; + } + + if ( common.testDegeneracy ) + { + if ( !(cfr.getTopLevelParameterValue("problem_specific", "h_degen_threshold", fund.hDegenThreshold) && + cfr.getTopLevelParameterValue("problem_specific", "max_upgrade_samples", fund.maxUpgradeSamples) )) + { + return false; + } + } + + std::string decomp_type; + if ( !cfr.getTopLevelParameterValue("problem_specific", "matrix_decomposition", decomp_type) ) + { + return false; + } + if (decomp_type.compare("LU") == 0) + { + fund.decompositionAlg = USACConfig::DECOMP_LU; + } + else if (decomp_type.compare("QR") == 0) + { + fund.decompositionAlg = USACConfig::DECOMP_QR; + } + else + { + std::cerr << "Matrix decomposition " << decomp_type << " not supported" << std::endl; + return false; + } + } + catch(...) + { + return false; + } + + return true; +} \ No newline at end of file diff --git a/src/config/ConfigParamsFundmatrix.h b/src/config/ConfigParamsFundmatrix.h new file mode 100644 index 0000000..2b22989 --- /dev/null +++ b/src/config/ConfigParamsFundmatrix.h @@ -0,0 +1,35 @@ +#ifndef CONFIGPARAMSFUND_H +#define CONFIGPARAMSFUND_H + +#include "ConfigParams.h" + +namespace USACConfig +{ + enum MatrixDecomposition {DECOMP_QR, DECOMP_LU}; + + // problem specific/data-related parameters: fundamental matrix + struct Fund + { + Fund(): decompositionAlg (DECOMP_QR), + hDegenThreshold (0.0), + maxUpgradeSamples (500), + inputFilePath ("") // leave blank if not using config file + {} + + MatrixDecomposition decompositionAlg; + double hDegenThreshold; + unsigned int maxUpgradeSamples; + std::string inputFilePath; + }; +} + +class ConfigParamsFund: public ConfigParams +{ +public: + // simple function to read in parameters from config file + bool initParamsFromConfigFile(std::string& configFilePath); + + USACConfig::Fund fund; +}; + +#endif \ No newline at end of file diff --git a/src/config/ConfigParamsHomog.cpp b/src/config/ConfigParamsHomog.cpp new file mode 100644 index 0000000..46c13ca --- /dev/null +++ b/src/config/ConfigParamsHomog.cpp @@ -0,0 +1,47 @@ +#include "ConfigParamsHomog.h" +#include "ConfigFileReader.h" + +bool ConfigParamsHomog::initParamsFromConfigFile(std::string &configFilePath) +{ + // initialize the config file reader with the input config file + ConfigFileReader cfr; + try + { + cfr.readFile(configFilePath.c_str()); + } + catch(const libconfig::FileIOException) + { + std::cerr << "I/O error while reading file." << std::endl; + return false; + } + catch(const libconfig::ParseException &pex) + { + std::cerr << "Parse error at " << pex.getFile() << ":" << pex.getLine() + << " - " << pex.getError() << std::endl; + return false; + } + + // now read in parameters + try + { + // usac parameters + if (!initUSACParamsFromConfigFile(cfr)) + { + + std::cerr << "Error reading USAC parameters from config file." << std::endl; + return false; + } + + // homog parameters + if ( !cfr.getTopLevelParameterValue("problem_specific", "input_file_path", homog.inputFilePath) ) + { + return false; + } + } + catch(...) + { + return false; + } + + return true; +} \ No newline at end of file diff --git a/src/config/ConfigParamsHomog.h b/src/config/ConfigParamsHomog.h new file mode 100644 index 0000000..0d141e6 --- /dev/null +++ b/src/config/ConfigParamsHomog.h @@ -0,0 +1,27 @@ +#ifndef CONFIGPARAMSHOMOG_H +#define CONFIGPARAMSHOMOG_H + +#include "ConfigParams.h" + +namespace USACConfig +{ + // problem specific/data-related parameters: fundamental matrix + struct Homog + { + Homog(): inputFilePath ("") // leave blank if not using config file + {} + + std::string inputFilePath; + }; +} + +class ConfigParamsHomog: public ConfigParams +{ +public: + // simple function to read in parameters from config file + bool initParamsFromConfigFile(std::string& configFilePath); + + USACConfig::Homog homog; +}; + +#endif \ No newline at end of file diff --git a/src/estimators/FundmatrixEstimator.h b/src/estimators/FundmatrixEstimator.h new file mode 100644 index 0000000..cac26d4 --- /dev/null +++ b/src/estimators/FundmatrixEstimator.h @@ -0,0 +1,690 @@ +#ifndef FUNDMATRIXESTIMATOR_H +#define FUNDMATRIXESTIMATOR_H + +#include +#include +#include +#include "config/ConfigParamsFundmatrix.h" +#include "utils/MathFunctions.h" +#include "utils/FundmatrixFunctions.h" +#include "utils/HomographyFunctions.h" +#include "USAC.h" + +class FundMatrixEstimator: public USAC +{ + public: + inline bool initProblem(const ConfigParamsFund& cfg, double* pointData); + // ------------------------------------------------------------------------ + // storage for the final result + std::vector final_model_params_; + std::vector degen_final_model_params_; + + public: + FundMatrixEstimator() + { + input_points_ = NULL; + data_matrix_ = NULL; + degen_data_matrix_ = NULL; + models_.clear(); + models_denorm_.clear(); + }; + ~FundMatrixEstimator() + { + if (input_points_) { delete[] input_points_; input_points_ = NULL; } + if (data_matrix_) { delete[] data_matrix_; data_matrix_ = NULL; } + if (degen_data_matrix_) { delete[] degen_data_matrix_; degen_data_matrix_ = NULL; } + for (size_t i = 0; i < models_.size(); ++i) + { + if (models_[i]) { delete[] models_[i]; } + } + models_.clear(); + for (size_t i = 0; i < models_denorm_.size(); ++i) + { + if (models_denorm_[i]) { delete[] models_denorm_[i]; } + } + models_denorm_.clear(); + }; + + public: + // ------------------------------------------------------------------------ + // problem specific functions + inline void cleanupProblem(); + inline unsigned int generateMinimalSampleModels(); + inline bool generateRefinedModel(std::vector& sample, const unsigned int numPoints, + bool weighted = false, double* weights = NULL); + inline bool validateSample(); + inline bool validateModel(unsigned int modelIndex); + inline bool evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested); + inline void testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel); + inline unsigned int upgradeDegenerateModel(); + inline void findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights); + inline void storeModel(unsigned int modelIndex, unsigned int numInliers); + + private: + double* input_points_denorm_; // stores pointer to original input points + double degen_homog_threshold_; // threshold for h-degen test + unsigned int degen_max_upgrade_samples_; // maximum number of upgrade attempts + USACConfig::MatrixDecomposition matrix_decomposition_method; // QR/LU decomposition + + // ------------------------------------------------------------------------ + // temporary storage + double* input_points_; // stores normalized data points + double* data_matrix_; // linearized input data + double* degen_data_matrix_; // only for degeneracy testing + std::vector degen_outlier_flags_; // for easy access to outliers to degeneracy + double m_T1_[9], m_T2_[9], m_T2_trans_[9]; // normalization matrices + std::vector models_; // stores vector of models + std::vector models_denorm_; // stores vector of (denormalized) models +}; + + +// ============================================================================================ +// initProblem: initializes problem specific data and parameters +// this function is called once per run on new data +// ============================================================================================ +bool FundMatrixEstimator::initProblem(const ConfigParamsFund& cfg, double* pointData) +{ + // copy pointer to input data + input_points_denorm_ = pointData; + input_points_ = new double[6*cfg.common.numDataPoints]; + if (input_points_denorm_ == NULL) + { + std::cerr << "Input point data not properly initialized" << std::endl; + return false; + } + if (input_points_ == NULL) + { + std::cerr << "Could not allocate storage for normalized data points" << std::endl; + return false; + } + + // normalize input data + // following this, input_points_ has the normalized points and input_points_denorm_ has + // the original input points + FTools::normalizePoints(input_points_denorm_, input_points_, cfg.common.numDataPoints, m_T1_, m_T2_); + MathTools::mattr(m_T2_trans_, m_T2_, 3, 3); + + // allocate storage for models + final_model_params_.clear(); final_model_params_.resize(9); + models_.clear(); models_.resize(usac_max_solns_per_sample_); + models_denorm_.clear(); models_denorm_.resize(usac_max_solns_per_sample_); + for (unsigned int i = 0; i < usac_max_solns_per_sample_; ++i) + { + models_[i] = new double[9]; + models_denorm_[i] = new double[9]; + } + + // precompute the data matrix + data_matrix_ = new double[9*usac_num_data_points_]; // 9 values per correspondence + FTools::computeDataMatrix(data_matrix_, usac_num_data_points_, input_points_); + + // if required, set up H-data matrix/storage for degeneracy testing + degen_outlier_flags_.clear(); degen_outlier_flags_.resize(usac_num_data_points_, 0); + if (usac_test_degeneracy_) + { + degen_homog_threshold_ = cfg.fund.hDegenThreshold; + degen_max_upgrade_samples_ = cfg.fund.maxUpgradeSamples; + degen_final_model_params_.clear(); degen_final_model_params_.resize(9); + degen_data_matrix_ = new double[2*9*usac_num_data_points_]; // 2 equations per correspondence + HTools::computeDataMatrix(degen_data_matrix_, usac_num_data_points_, input_points_); + } + else + { + degen_homog_threshold_ = 0.0; + degen_max_upgrade_samples_ = 0; + degen_final_model_params_.clear(); + degen_data_matrix_ = NULL; + } + + // read in the f-matrix specific parameters from the config struct + matrix_decomposition_method = cfg.fund.decompositionAlg; + degen_homog_threshold_ = cfg.fund.hDegenThreshold*cfg.fund.hDegenThreshold; + + return true; +} + + +// ============================================================================================ +// cleanupProblem: release any temporary problem specific data storage +// this function is called at the end of each run on new data +// ============================================================================================ +void FundMatrixEstimator::cleanupProblem() +{ + if (input_points_) { delete[] input_points_; input_points_ = NULL; } + if (data_matrix_) { delete[] data_matrix_; data_matrix_ = NULL; } + if (degen_data_matrix_) { delete[] degen_data_matrix_; degen_data_matrix_ = NULL; } + for (size_t i = 0; i < models_.size(); ++i) + { + if (models_[i]) { delete[] models_[i]; } + } + models_.clear(); + for (size_t i = 0; i < models_denorm_.size(); ++i) + { + if (models_denorm_[i]) { delete[] models_denorm_[i]; } + } + models_denorm_.clear(); + degen_outlier_flags_.clear(); +} + + +// ============================================================================================ +// generateMinimalSampleModels: generates minimum sample model(s) from the data points whose +// indices are currently stored in m_sample. +// the generated models are stored in a vector of models and are all evaluated +// ============================================================================================ +unsigned int FundMatrixEstimator::generateMinimalSampleModels() +{ + double A[9*9]; + unsigned int nsols = 0; + + // form the matrix of equations for this minimal sample + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < usac_min_sample_size_; ++i) + { + src_ptr = data_matrix_ + min_sample_[i]; + for (unsigned int j = 0; j < 9; ++j) + { + *dst_ptr = *src_ptr; + ++dst_ptr; + src_ptr += usac_num_data_points_; + } + } + + // LU/QR factorization + double sol[9*9]; + double poly[4], roots[3]; + double *f1, *f2; + int nullbuff [18]; + f1 = sol; + f2 = sol+9; + if (matrix_decomposition_method == USACConfig::DECOMP_QR) + { + FTools::nullspaceQR7x9(A, sol); + } + else if (matrix_decomposition_method == USACConfig::DECOMP_LU) + { + for (unsigned int i = 7*9; i < 9*9; ++i) + { + A[i] = 0.0; + } + int nullsize = FTools::nullspace(A, f1, 9, nullbuff); + if (nullsize != 2) + { + return 0; + } + } + + // solve polynomial + FTools::makePolynomial(f1, f2, poly); + nsols = FTools::rroots3(poly, roots); + + // form up to three fundamental matrices + double T2_F[9]; + for (unsigned int i = 0; i < nsols; ++i) + { + for (unsigned int j = 0; j < 9; ++j) + { + *(models_[i]+j) = f1[j] * roots[i] + f2[j] * (1 -roots[i]); + } + // store denormalized version as well + MathTools::mmul(T2_F, m_T2_trans_, models_[i], 3); + MathTools::mmul(models_denorm_[i], T2_F, m_T1_, 3); + } + + return nsols; +} + + +// ============================================================================================ +// generateRefinedModel: compute model using non-minimal set of samples +// default operation is to use a weight of 1 for every data point +// ============================================================================================ +bool FundMatrixEstimator::generateRefinedModel(std::vector& sample, + const unsigned int numPoints, + bool weighted, + double* weights) +{ + // form the matrix of equations for this non-minimal sample + double *A = new double[numPoints*9]; + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < numPoints; ++i) + { + src_ptr = data_matrix_ + sample[i]; + for (unsigned int j = 0; j < 9; ++j) + { + if (!weighted) + { + *dst_ptr = *src_ptr; + } + else + { + *dst_ptr = (*src_ptr)*weights[i]; + } + ++dst_ptr; + src_ptr += usac_num_data_points_; + } + } + + double Cv[9*9]; + FTools::formCovMat(Cv, A, numPoints, 9); + + double V[9*9], D[9], *p; + MathTools::svdu1v(D, Cv, 9, V, 9); + + unsigned int j = 0; + for (unsigned int i = 1; i < 9; ++i) + { + if (D[i] < D[j]) + { + j = i; + } + } + p = V + j; + + for (unsigned int i = 0; i < 9; ++i) + { + *(models_[0]+i) = *p; + p += 9; + } + FTools::singulF(models_[0]); + // store denormalized version as well + double T2_F[9]; + MathTools::mmul(T2_F, m_T2_trans_, models_[0], 3); + MathTools::mmul(models_denorm_[0], T2_F, m_T1_, 3); + + delete[] A; + + return true; +} + + +// ============================================================================================ +// validateSample: check if minimal sample is valid +// here, just returns true +// ============================================================================================ +bool FundMatrixEstimator::validateSample() +{ + return true; +} + + +// ============================================================================================ +// validateModel: check if model computed from minimal sample is valid +// checks oriented constraints to determine model validity +// ============================================================================================ +bool FundMatrixEstimator::validateModel(unsigned int modelIndex) +{ + // check oriented constraints + double e[3], sig1, sig2; + FTools::computeEpipole(e, models_[modelIndex]); + + sig1 = FTools::getOriSign(models_[modelIndex], e, input_points_ + 6*min_sample_[0]); + for(unsigned int i = 1; i < min_sample_.size(); ++i) + { + sig2 = FTools::getOriSign(models_[modelIndex], e, input_points_ + 6*min_sample_[i]); + if (sig1 * sig2 < 0) + { + return false; + } + } + return true; +} + + +// ============================================================================================ +// evaluateModel: test model against all/subset of the data points +// ============================================================================================ +bool FundMatrixEstimator::evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested) +{ + double rx, ry, rwc, ryc, rxc, r, temp_err; + double* model = models_denorm_[modelIndex]; + double* pt; + std::vector::iterator current_err_array = err_ptr_[0]; + bool good_flag = true; + double lambdaj, lambdaj_1 = 1.0; + *numInliers = 0; + *numPointsTested = 0; + unsigned int pt_index; + + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + // get index of point to be verified + if (eval_pool_index_ > usac_num_data_points_-1) + { + eval_pool_index_ = 0; + } + pt_index = evaluation_pool_[eval_pool_index_]; + ++eval_pool_index_; + + // compute sampson error + pt = input_points_denorm_ + 6*pt_index; + rxc = (*model) * (*(pt+3)) + (*(model+3)) * (*(pt+4)) + (*(model+6)); + ryc = (*(model+1)) * (*(pt+3)) + (*(model+4)) * (*(pt+4)) + (*(model+7)); + rwc = (*(model+2)) * (*(pt+3)) + (*(model+5)) * (*(pt+4)) + (*(model+8)); + r =((*(pt)) * rxc + (*(pt+1)) * ryc + rwc); + rx = (*model) * (*(pt)) + (*(model+1)) * (*(pt+1)) + (*(model+2)); + ry = (*(model+3)) * (*(pt)) + (*(model+4)) * (*(pt+1)) + (*(model+5)); + temp_err = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); + *(current_err_array+pt_index) = temp_err; + + if (temp_err < usac_inlier_threshold_) + { + ++(*numInliers); + } + + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + if (temp_err < usac_inlier_threshold_) + { + lambdaj = lambdaj_1 * (sprt_delta_/sprt_epsilon_); + } + else + { + lambdaj = lambdaj_1 * ( (1 - sprt_delta_)/(1 - sprt_epsilon_) ); + } + + if (lambdaj > decision_threshold_sprt_) + { + good_flag = false; + *numPointsTested = i+1; + return good_flag; + } + else + { + lambdaj_1 = lambdaj; + } + } + } + *numPointsTested = usac_num_data_points_; + return good_flag; +} + + +// ============================================================================================ +// testSolutionDegeneracy: check if model is degenerate +// test if >=5 points in the sample are on a plane +// ============================================================================================ +void FundMatrixEstimator::testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel) +{ + *degenerateModel = false; + *upgradeModel = false; + + // make up the tuples to be used to check for degeneracy + unsigned int degen_sample_indices[] = {0, 1, 2, 3, + 3, 4, 5, 6, + 0, 1, 5, 6, + 0, 2, 4, 5, + 1, 2, 4, 6, + 0, 3, 4, 6, + 1, 3, 4, 5, + 2, 3, 5, 6}; + + // the above tuples need to be tested on the remaining points for each case + unsigned int test_point_indices[] = {4, 5, 6, + 0, 1, 2, + 2, 3, 4, + 1, 3, 6, + 0, 3, 5, + 1, 2, 5, + 0, 2, 6, + 0, 1, 4}; + + unsigned int *sample_pos = degen_sample_indices; + unsigned int *test_pos = test_point_indices; + double h[9]; + double T2_inv[9], T2_H[9]; + for (unsigned int i = 0; i < 9; ++i) + { + T2_inv[i] = m_T2_[i]; + } + MathTools::minv(T2_inv, 3); + + std::vector sample(7), test(3); + std::vector errs; + for(unsigned int i = 0; i < 8; ++i) + { + // compute H from the current set of 4 points + for (unsigned int j = 0; j < 4; ++j) + { + sample[j] = min_sample_[sample_pos[j]]; + } + FTools::computeHFromMinCorrs(sample, 4, usac_num_data_points_, degen_data_matrix_, h); + MathTools::mmul(T2_H, T2_inv, h, 3); + MathTools::mmul(h, T2_H, m_T1_, 3); + + // check test points to see how many are consistent + for (unsigned int j = 0; j < 3; ++j) + { + test[j] = min_sample_[test_pos[j]]; + } + unsigned int num_inliers = FTools::getHError(test, 3, errs, input_points_denorm_, h, degen_homog_threshold_); + for (unsigned int j = 0, count = 4; j < 3; ++j) + { + if (errs[j] < degen_homog_threshold_) + { + sample[count++] = test[j]; + } + } + + // if at least 1 inlier in the test points, then h-degenerate sample found + if (num_inliers > 0) + { + // find inliers from all data points + num_inliers = FTools::getHError(evaluation_pool_, usac_num_data_points_, errs, input_points_denorm_, h, degen_homog_threshold_); + //std::cout << "Degenerate sample found with " << num_inliers << " inliers" << std::endl; + + // refine with least squares fit + unsigned int count = 0; + std::vector inlier_sample(num_inliers); + for (unsigned int j = 0; j < usac_num_data_points_; ++j) + { + if (errs[j] < degen_homog_threshold_) + { + inlier_sample[count++] = evaluation_pool_[j]; + } + } + FTools::computeHFromCorrs(inlier_sample, inlier_sample.size(), usac_num_data_points_, degen_data_matrix_, h); + MathTools::mmul(T2_H, T2_inv, h, 3); + MathTools::mmul(h, T2_H, m_T1_, 3); + + // find support of homography + num_inliers = FTools::getHError(evaluation_pool_, usac_num_data_points_, errs, input_points_denorm_, h, degen_homog_threshold_); + //std::cout << "Refined model has " << num_inliers << " inliers" << std::endl; +#if 1 + if (num_inliers < usac_results_.best_inlier_count_/5) + { + sample_pos += 4; + test_pos += 3; + continue; + } +#endif + // set flag + *degenerateModel = true; + + // if largest degenerate model found so far, store results + if (num_inliers > usac_results_.degen_inlier_count_) + { + // set flag + *upgradeModel = true; + + // refine with final least squares fit + count = 0; + inlier_sample.resize(num_inliers); + for (unsigned int j = 0; j < usac_num_data_points_; ++j) + { + if (errs[j] < degen_homog_threshold_) + { + inlier_sample[count++] = evaluation_pool_[j]; + } + } + FTools::computeHFromCorrs(inlier_sample, inlier_sample.size(), usac_num_data_points_, degen_data_matrix_, h); + + usac_results_.degen_inlier_count_ = num_inliers; + // store homography + for (unsigned int j = 0; j < 9; ++j) + { + degen_final_model_params_[j] = h[j]; + } + // store inliers and outliers - for use in model completion + for (unsigned int j = 0; j < usac_num_data_points_; ++j) + { + if (errs[j] < degen_homog_threshold_) + { + usac_results_.degen_inlier_flags_[evaluation_pool_[j]] = 1; + degen_outlier_flags_[evaluation_pool_[j]] = 0; + } + else + { + degen_outlier_flags_[evaluation_pool_[j]] = 1; + usac_results_.degen_inlier_flags_[evaluation_pool_[j]] = 0; + } + } + // store the degenerate points from the minimal sample + usac_results_.degen_sample_ = sample; + + } // end store denerate results + } // end check for one model degeneracy + + sample_pos += 4; + test_pos += 3; + + } // end check for all combinations in the minimal sample +} + + +// ============================================================================================ +// upgradeDegenerateModel: try to upgrade degenerate model to non-degenerate by sampling from +// the set of outliers to the degenerate model +// ============================================================================================ +unsigned int FundMatrixEstimator::upgradeDegenerateModel() +{ + unsigned int best_upgrade_inliers = usac_results_.best_inlier_count_; + unsigned int num_outliers = usac_num_data_points_ - usac_results_.degen_inlier_count_; + + if (num_outliers < 2) { + return 0; + } + + std::vector outlier_indices(num_outliers); + unsigned int count = 0; + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + if (degen_outlier_flags_[i]) + { + outlier_indices[count++] = i; + } + } + std::vector outlier_sample(2); + std::vector::iterator current_err_array = err_ptr_[0]; + + double* pt1_index, *pt2_index; + double x1[3], x1p[3], x2[3], x2p[3]; + double temp[3], l1[3], l2[3], ep[3]; + double skew_sym_ep[9]; + double T2_F[9]; + for (unsigned int i = 0; i < degen_max_upgrade_samples_; ++i) + { + generateUniformRandomSample(num_outliers, 2, &outlier_sample); + + pt1_index = input_points_ + 6*outlier_indices[outlier_sample[0]]; + pt2_index = input_points_ + 6*outlier_indices[outlier_sample[1]]; + + x1[0] = pt1_index[0]; x1[1] = pt1_index[1]; x1[2] = 1.0; + x1p[0] = pt1_index[3]; x1p[1] = pt1_index[4]; x1p[2] = 1.0; + x2[0] = pt2_index[0]; x2[1] = pt2_index[1]; x2[2] = 1.0; + x2p[0] = pt2_index[3]; x2p[1] = pt2_index[4]; x2p[2] = 1.0; + + MathTools::vmul(temp, °en_final_model_params_[0], x1, 3); + MathTools::crossprod(l1, temp, x1p, 1); + + MathTools::vmul(temp, °en_final_model_params_[0], x2, 3); + MathTools::crossprod(l2, temp, x2p, 1); + + MathTools::crossprod(ep, l1, l2, 1); + + MathTools::skew_sym(skew_sym_ep, ep); + MathTools::mmul(models_[0], skew_sym_ep, °en_final_model_params_[0], 3); + MathTools::mmul(T2_F, m_T2_trans_, models_[0], 3); + MathTools::mmul(models_denorm_[0], T2_F, m_T1_, 3); + + unsigned int num_inliers, num_pts_tested; + evaluateModel(0, &num_inliers, &num_pts_tested); + + if (num_inliers > best_upgrade_inliers) + { + usac_results_.degen_sample_[5] = outlier_indices[outlier_sample[0]]; + usac_results_.degen_sample_[6] = outlier_indices[outlier_sample[1]]; + min_sample_ = usac_results_.degen_sample_; + storeSolution(0, num_inliers); + best_upgrade_inliers = num_inliers; + + unsigned int count = 0; + for (size_t j = 0; j < outlier_indices.size(); ++j) + { + if (*(current_err_array+outlier_indices[j]) < usac_inlier_threshold_) + { + ++count; + } + } + unsigned int num_samples = updateStandardStopping(count, num_outliers, 2); + //std::cout << "Inliers = " << num_inliers << ", in/out = " << count << "/" << num_outliers + // << ". Num samples = " << num_samples << std::endl; + if (num_samples < degen_max_upgrade_samples_) + { + degen_max_upgrade_samples_ = num_samples; + } + + } + } + + std::cout << "Upgraded model has " << best_upgrade_inliers << " inliers" << std::endl; + return best_upgrade_inliers; +} + + +// ============================================================================================ +// findWeights: given model and points, compute weights to be used in local optimization +// ============================================================================================ +void FundMatrixEstimator::findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights) +{ + double rx, ry, ryc, rxc; + double* model = models_[modelIndex]; + double* pt; + unsigned int pt_index; + + for (unsigned int i = 0; i < numInliers; ++i) + { + // get index of point to be verified + pt_index = inliers[i]; + + // compute weight (ref: torr dissertation, eqn. 2.25) + pt = input_points_ + 6*pt_index; + rxc = (*model) * (*(pt+3)) + (*(model+3)) * (*(pt+4)) + (*(model+6)); + ryc = (*(model+1)) * (*(pt+3)) + (*(model+4)) * (*(pt+4)) + (*(model+7)); + rx = (*model) * (*(pt)) + (*(model+1)) * (*(pt+1)) + (*(model+2)); + ry = (*(model+3)) * (*(pt)) + (*(model+4)) * (*(pt+1)) + (*(model+5)); + + weights[i] = 1/sqrt(rxc*rxc + ryc*ryc + rx*rx + ry*ry); + } +} + + +// ============================================================================================ +// storeModel: stores current best model +// this function is called (by USAC) every time a new best model is found +// ============================================================================================ +void FundMatrixEstimator::storeModel(unsigned int modelIndex, unsigned int numInliers) +{ + // save the current model as the best solution so far + for (unsigned int i = 0; i < 9; ++i) + { + final_model_params_[i] = *(models_denorm_[modelIndex]+i); + } +} + +#endif + diff --git a/src/estimators/HomogEstimator.h b/src/estimators/HomogEstimator.h new file mode 100644 index 0000000..bb0c699 --- /dev/null +++ b/src/estimators/HomogEstimator.h @@ -0,0 +1,417 @@ +#ifndef HOMOGESTIMATOR_H +#define HOMOGESTIMATOR_H + +#include +#include +#include +#include "config/ConfigParamsHomog.h" +#include "utils/MathFunctions.h" +#include "utils/FundmatrixFunctions.h" +#include "utils/HomographyFunctions.h" +#include "USAC.h" + +class HomogEstimator: public USAC +{ + public: + inline bool initProblem(const ConfigParamsHomog& cfg, double* pointData); + // ------------------------------------------------------------------------ + // storage for the final result + std::vector final_model_params_; + + public: + HomogEstimator() + { + input_points_ = NULL; + data_matrix_ = NULL; + models_.clear(); + models_denorm_.clear(); + }; + ~HomogEstimator() + { + if (input_points_) { delete[] input_points_; input_points_ = NULL; } + if (data_matrix_) { delete[] data_matrix_; data_matrix_ = NULL; } + for (size_t i = 0; i < models_.size(); ++i) + { + if (models_[i]) { delete[] models_[i]; } + } + models_.clear(); + for (size_t i = 0; i < models_denorm_.size(); ++i) + { + if (models_denorm_[i]) { delete[] models_denorm_[i]; } + } + models_denorm_.clear(); + }; + + public: + // ------------------------------------------------------------------------ + // problem specific functions + inline void cleanupProblem(); + inline unsigned int generateMinimalSampleModels(); + inline bool generateRefinedModel(std::vector& sample, const unsigned int numPoints, + bool weighted = false, double* weights = NULL); + inline bool validateSample(); + inline bool validateModel(unsigned int modelIndex); + inline bool evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested); + inline void testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel); + inline unsigned int upgradeDegenerateModel(); + inline void findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights); + inline void storeModel(unsigned int modelIndex, unsigned int numInliers); + + private: + double* input_points_denorm_; // stores pointer to original input points + + // ------------------------------------------------------------------------ + // temporary storage + double* input_points_; // stores normalized data points + double* data_matrix_; // linearized input data + double m_T1_[9], m_T2_[9], m_T2inv_[9]; // normalization matrices + std::vector models_; // stores vector of models + std::vector models_denorm_; // stores vector of (denormalized) models +}; + + +// ============================================================================================ +// initProblem: initializes problem specific data and parameters +// this function is called once per run on new data +// ============================================================================================ +bool HomogEstimator::initProblem(const ConfigParamsHomog& cfg, double* pointData) +{ + // copy pointer to input data + input_points_denorm_ = pointData; + input_points_ = new double[6*cfg.common.numDataPoints]; + if (input_points_denorm_ == NULL) + { + std::cerr << "Input point data not properly initialized" << std::endl; + return false; + } + if (input_points_ == NULL) + { + std::cerr << "Could not allocate storage for normalized data points" << std::endl; + return false; + } + + // normalize input data + // following this, input_points_ has the normalized points and input_points_denorm_ has + // the original input points + FTools::normalizePoints(input_points_denorm_, input_points_, cfg.common.numDataPoints, m_T1_, m_T2_); + for (unsigned int i = 0; i < 9; ++i) + { + m_T2inv_[i] = m_T2_[i]; + } + MathTools::minv(m_T2inv_, 3); + + // allocate storage for models + final_model_params_.clear(); final_model_params_.resize(9); + models_.clear(); models_.resize(usac_max_solns_per_sample_); + models_denorm_.clear(); models_denorm_.resize(usac_max_solns_per_sample_); + for (unsigned int i = 0; i < usac_max_solns_per_sample_; ++i) + { + models_[i] = new double[9]; + models_denorm_[i] = new double[9]; + } + + // precompute the data matrix + data_matrix_ = new double[18*usac_num_data_points_]; // 2 equations per correspondence + HTools::computeDataMatrix(data_matrix_, usac_num_data_points_, input_points_); + + return true; +} + + +// ============================================================================================ +// cleanupProblem: release any temporary problem specific data storage +// this function is called at the end of each run on new data +// ============================================================================================ +void HomogEstimator::cleanupProblem() +{ + if (input_points_) { delete[] input_points_; input_points_ = NULL; } + if (data_matrix_) { delete[] data_matrix_; data_matrix_ = NULL; } + for (size_t i = 0; i < models_.size(); ++i) + { + if (models_[i]) { delete[] models_[i]; } + } + models_.clear(); + for (size_t i = 0; i < models_denorm_.size(); ++i) + { + if (models_denorm_[i]) { delete[] models_denorm_[i]; } + } + models_denorm_.clear(); +} + + +// ============================================================================================ +// generateMinimalSampleModels: generates minimum sample model from the data points whose +// indices are currently stored in min_sample_. +// in this case, only one model per minimum sample +// ============================================================================================ +unsigned int HomogEstimator::generateMinimalSampleModels() +{ + double A[8*9]; + double At[9*8]; + + // form the matrix of equations for this minimal sample + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < usac_min_sample_size_; ++i) + { + for (unsigned int j = 0; j < 2; ++j) + { + src_ptr = data_matrix_ + 2*min_sample_[i] + j; + for (unsigned int k = 0; k < 9; ++k) + { + *dst_ptr = *src_ptr; + ++dst_ptr; + src_ptr += 2*usac_num_data_points_; + } + } + } + + MathTools::mattr(At, A, 8, 9); + + double D[9], U[9*9], V[8*8], *p; + MathTools::svduv(D, At, U, 9, V, 8); + p = U + 8; + + double T2_H[9]; + for (unsigned int i = 0; i < 9; ++i) + { + *(models_[0]+i) = *p; + p += 9; + } + MathTools::mmul(T2_H, m_T2inv_, models_[0], 3); + MathTools::mmul(models_denorm_[0], T2_H, m_T1_, 3); + + return 1; +} + + +// ============================================================================================ +// generateRefinedModel: compute model using non-minimal set of samples +// default operation is to use a weight of 1 for every data point +// ============================================================================================ +bool HomogEstimator::generateRefinedModel(std::vector& sample, + unsigned int numPoints, + bool weighted, + double* weights) +{ + // form the matrix of equations for this non-minimal sample + double *A = new double[numPoints*2*9]; + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < numPoints; ++i) + { + for (unsigned int j = 0; j < 2; ++j) + { + src_ptr = data_matrix_ + 2*sample[i] + j; + for (unsigned int k = 0; k < 9; ++k) + { + if (!weighted) + { + *dst_ptr = *src_ptr; + } + else + { + *dst_ptr = (*src_ptr)*weights[i]; + } + ++dst_ptr; + src_ptr += 2*usac_num_data_points_; + } + } + } + + // decompose + double V[9*9], D[9], *p; + MathTools::svdu1v(D, A, 2*numPoints, V, 9); + + unsigned int j = 0; + for (unsigned int i = 1; i < 9; ++i) + { + if (D[i] < D[j]) + j = i; + } + p = V + j; + + for (unsigned int i = 0; i < 9; ++i) + { + *(models_[0]+i) = *p; + p += 9; + } + double T2_H[9]; + MathTools::mmul(T2_H, m_T2inv_, models_[0], 3); + MathTools::mmul(models_denorm_[0], T2_H, m_T1_, 3); + + delete[] A; + + return true; +} + + +// ============================================================================================ +// validateSample: check if minimal sample is valid +// ============================================================================================ +bool HomogEstimator::validateSample() +{ + // check oriented constraints + double p[3], q[3]; + double *a, *b, *c, *d; + + a = input_points_ + 6*min_sample_[0]; + b = input_points_ + 6*min_sample_[1]; + c = input_points_ + 6*min_sample_[2]; + d = input_points_ + 6*min_sample_[3]; + + HTools::crossprod(p, a, b, 1); + HTools::crossprod(q, a+3, b+3, 1); + + if ((p[0]*c[0]+p[1]*c[1]+p[2]*c[2])*(q[0]*c[3]+q[1]*c[4]+q[2]*c[5])<0) + return false; + if ((p[0]*d[0]+p[1]*d[1]+p[2]*d[2])*(q[0]*d[3]+q[1]*d[4]+q[2]*d[5])<0) + return false; + + HTools::crossprod(p, c, d, 1); + HTools::crossprod(q, c+3, d+3, 1); + + if ((p[0]*a[0]+p[1]*a[1]+p[2]*a[2])*(q[0]*a[3]+q[1]*a[4]+q[2]*a[5])<0) + return false; + if ((p[0]*b[0]+p[1]*b[1]+p[2]*b[2])*(q[0]*b[3]+q[1]*b[4]+q[2]*b[5])<0) + return false; + + return true; +} + + +// ============================================================================================ +// validateModel: check if model computed from minimal sample is valid +// ============================================================================================ +bool HomogEstimator::validateModel(const unsigned int modelIndex) +{ + return true; +} + + +// ============================================================================================ +// evaluateModel: test model against all/subset of the data points +// ============================================================================================ +bool HomogEstimator::evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested) +{ + double* model = models_denorm_[modelIndex]; + double inv_model[9]; + double h_x[3], h_inv_xp[3], temp_err; + double* pt; + std::vector::iterator current_err_array = err_ptr_[0]; + bool good_flag = true; + double lambdaj, lambdaj_1 = 1.0; + *numInliers = 0; + *numPointsTested = 0; + unsigned int pt_index; + + for (unsigned int i = 0; i < 9; ++i) + { + inv_model[i] = model[i]; + } + MathTools::minv(inv_model, 3); + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + // get index of point to be verified + if (eval_pool_index_ > usac_num_data_points_-1) + { + eval_pool_index_ = 0; + } + pt_index = evaluation_pool_[eval_pool_index_]; + ++eval_pool_index_; + + // compute symmetric transfer error + pt = input_points_denorm_ + 6*pt_index; + MathTools::vmul(h_x, model, pt, 3); + MathTools::vmul(h_inv_xp, inv_model, pt+3, 3); + + double err1 = 0.0, err2 = 0.0; + for (unsigned int j = 0; j < 2; ++j) + { + err1 += (h_x[j]/h_x[2] - pt[3+j]) * (h_x[j]/h_x[2] - pt[3+j]); + err2 += (h_inv_xp[j]/h_inv_xp[2] - pt[j]) * (h_inv_xp[j]/h_inv_xp[2] - pt[j]); + } + temp_err = err1 + err2; + *(current_err_array+pt_index) = temp_err; + + if (temp_err < usac_inlier_threshold_) + { + ++(*numInliers); + } + + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + if (temp_err < usac_inlier_threshold_) + { + lambdaj = lambdaj_1 * (sprt_delta_/sprt_epsilon_); + } + else + { + lambdaj = lambdaj_1 * ( (1 - sprt_delta_)/(1 - sprt_epsilon_) ); + } + + if (lambdaj > decision_threshold_sprt_) + { + good_flag = false; + *numPointsTested = i+1; + return good_flag; + } + else + { + lambdaj_1 = lambdaj; + } + } + } + *numPointsTested = usac_num_data_points_; + + return good_flag; +} + +// ============================================================================================ +// testSolutionDegeneracy: check if model is degenerate +// ============================================================================================ +void HomogEstimator::testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel) +{ + *degenerateModel = false; + *upgradeModel = false; +} + +// ============================================================================================ +// upgradeDegenerateModel: try to upgrade degenerate model to non-degenerate by sampling from +// the set of outliers to the degenerate model +// ============================================================================================ +unsigned int HomogEstimator::upgradeDegenerateModel() +{ + return 0; +} + + +// ============================================================================================ +// findWeights: given model and points, compute weights to be used in local optimization +// ============================================================================================ +void HomogEstimator::findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights) +{ + for (unsigned int i = 0; i < numInliers; ++i) + { + weights[i] = 1.0; + } +} + + +// ============================================================================================ +// storeModel: stores current best model +// this function is called (by USAC) every time a new best model is found +// ============================================================================================ +void HomogEstimator::storeModel(const unsigned int modelIndex, unsigned int numInliers) +{ + // save the current model as the best solution so far + for (unsigned int i = 0; i < 9; ++i) + { + final_model_params_[i] = *(models_denorm_[modelIndex]+i); + } +} + +#endif + diff --git a/src/estimators/TemplateEstimator.h b/src/estimators/TemplateEstimator.h new file mode 100644 index 0000000..fee4fa8 --- /dev/null +++ b/src/estimators/TemplateEstimator.h @@ -0,0 +1,223 @@ +#ifndef TEMPLATEESTIMATOR_H +#define TEMPLATEESTIMATOR_H + +#include +#include +#include +#include "config/ConfigParamsTemplate.h" +#include "utils/MathFunctions.h" +#include "USAC.h" + +class TemplateEstimator: public USAC +{ + public: + inline bool initProblem(const ConfigParamsTemplate& cfg, double* pointData); + + // ------------------------------------------------------------------------ + // problem specific functions: implement these + inline void cleanupProblem(); + inline unsigned int generateMinimalSampleModels(); + inline bool generateRefinedModel(std::vector& sample, const unsigned int numPoints, + bool weighted = false, double* weights = NULL); + inline bool validateSample(); + inline bool validateModel(unsigned int modelIndex); + inline bool evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested); + inline void testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel); + inline unsigned int upgradeDegenerateModel(); + inline void findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights); + inline void storeModel(unsigned int modelIndex, unsigned int numInliers); + + public: + // ------------------------------------------------------------------------ + // storage for the final result + std::vector final_model_params_; + + private: +}; + + +// ============================================================================================ +// initProblem: initializes problem specific data and parameters +// call this function once per run on new data +// ============================================================================================ +bool TemplateEstimator::initProblem(const ConfigParamsTemplate& cfg, double* pointData) +{ + // set up problem specific data here (e.g., data matrices, parameter vectors, etc.) + return true; +} + + +// ============================================================================================ +// cleanupProblem: release any temporary problem specific data storage +// call this function once at the end of each run on new data +// ============================================================================================ +void TemplateEstimator::cleanupProblem() +{ + +} + + +// ============================================================================================ +// generateMinimalSampleModels: generates minimum sample model from the data points whose +// indices are currently stored in min_sample_. +// in this case, only one model per minimum sample +// ============================================================================================ +unsigned int TemplateEstimator::generateMinimalSampleModels() +{ + // this function must be implemented + + return num_models; // return the number of minimal sample models +} + + +// ============================================================================================ +// generateRefinedModel: compute model using non-minimal set of samples +// default operation is to use a weight of 1 for every data point +// ============================================================================================ +bool TemplateEstimator::generateRefinedModel(std::vector& sample, + unsigned int numPoints, + bool weighted, + double* weights) +{ + // implement this function if you want to use local optimization + + return true; +} + + +// ============================================================================================ +// validateSample: check if minimal sample is valid +// ============================================================================================ +bool TemplateEstimator::validateSample() +{ + // implement this function if you want to pre-verify the minimal sample, otherwise + // simply return true + return true; +} + + +// ============================================================================================ +// validateModel: check if model computed from minimal sample is valid +// ============================================================================================ +bool TemplateEstimator::validateModel(const unsigned int modelIndex) +{ + // implement this function if you want to pre-verify a model, otherwise + // simply return true + return true; +} + + +// ============================================================================================ +// evaluateModel: test model against all/subset of the data points +// ============================================================================================ +bool TemplateEstimator::evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested) +{ + // test model against all data points, or a randomized subset (in the case of early + // termination with, for e.g., SPRT) + double* model = models_denorm_[modelIndex]; + std::vector::iterator current_err_array = err_ptr_[0]; + double err; + bool good_flag = true; + double lambdaj, lambdaj_1 = 1.0; + *numInliers = 0; + *numPointsTested = 0; + unsigned int pt_index; + + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + // get index of point to be verified + if (eval_pool_index_ > usac_num_data_points_-1) + { + eval_pool_index_ = 0; + } + pt_index = evaluation_pool_[eval_pool_index_]; + ++eval_pool_index_; + + // compute point-model error for the problem of interest + // + // --- implement this + // + + *(current_err_array+pt_index) = err; + + if (err < usac_inlier_threshold_) + { + ++(*numInliers); + } + + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + if (err < usac_inlier_threshold_) + { + lambdaj = lambdaj_1 * (sprt_delta_/sprt_epsilon_); + } + else + { + lambdaj = lambdaj_1 * ( (1 - sprt_delta_)/(1 - sprt_epsilon_) ); + } + + if (lambdaj > decision_threshold_sprt_) + { + good_flag = false; + *numPointsTested = i+1; + return good_flag; + } + else + { + lambdaj_1 = lambdaj; + } + } + } + *numPointsTested = usac_num_data_points_; + + return good_flag; +} + +// ============================================================================================ +// testSolutionDegeneracy: check if model is degenerate +// ============================================================================================ +void TemplateEstimator::testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel) +{ + // implement this if you want to detect degenerate models, otherwise return false + *degenerateModel = false; + *upgradeModel = false; +} + +// ============================================================================================ +// upgradeDegenerateModel: try to upgrade degenerate model to non-degenerate by sampling from +// the set of outliers to the degenerate model +// ============================================================================================ +unsigned int TemplateEstimator::upgradeDegenerateModel() +{ + // implement this if you want to upgrade degenerate models to non-degenerate, otherwise + // return 0; + return num_inliers; +} + + +// ============================================================================================ +// findWeights: given model and points, compute weights to be used in local optimization +// ============================================================================================ +void TemplateEstimator::findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights) +{ + // implement this if you want weighted local refinement, otherwise return an array of ones + for (unsigned int i = 0; i < numInliers; ++i) + { + weights[i] = 1.0; + } +} + + +// ============================================================================================ +// storeModel: stores current best model +// this function is called (by USAC) every time a new best model is found +// ============================================================================================ +void TemplateEstimator::storeModel(const unsigned int modelIndex, unsigned int numInliers) +{ + // save the current model as the best solution so far +} + +#endif + diff --git a/src/estimators/USAC.h b/src/estimators/USAC.h new file mode 100644 index 0000000..262cdad --- /dev/null +++ b/src/estimators/USAC.h @@ -0,0 +1,1067 @@ +#ifndef USAC_HH +#define USAC_HH +#define NOMINMAX + +#include +#include +#include +#include +#include +#include +#include "config/ConfigParams.h" + +struct UsacResults { + void reset() { + hyp_count_ = 0; + model_count_ = 0; + rejected_sample_count_ = 0; + rejected_model_count_ = 0; + best_inlier_count_ = 0; + degen_inlier_count_ = 0; + total_points_verified_ = 0; + num_local_optimizations_ = 0; + total_runtime_ = 0; + inlier_flags_.clear(); + best_sample_.clear(); + degen_inlier_flags_.clear(); + degen_sample_.clear(); + } + unsigned int hyp_count_; + unsigned int model_count_; + unsigned int rejected_sample_count_; + unsigned int rejected_model_count_; + unsigned int total_points_verified_; + unsigned int num_local_optimizations_; + unsigned int best_inlier_count_; + std::vector inlier_flags_; + std::vector best_sample_; + unsigned int degen_inlier_count_; + std::vector degen_inlier_flags_; + std::vector degen_sample_; + double total_runtime_; +}; + +template +class USAC +{ + public: + // ------------------------------------------------------------------------ + // initialization/main functions + USAC() {}; + virtual ~USAC() {}; + void initParamsUSAC(const ConfigParams& cfg); + void initDataUSAC(const ConfigParams& cfg); + bool solve(); + UsacResults usac_results_; + + protected: + // ------------------------------------------------------------------------ + // pure virtual functions to be overridden in the derived class + virtual inline unsigned int generateMinimalSampleModels() = 0; + virtual inline bool generateRefinedModel(std::vector& sample, const unsigned int numPoints, + bool weighted, double* weights) = 0; + virtual inline bool validateSample() = 0; + virtual inline bool validateModel(unsigned int modelIndex) = 0; + virtual inline bool evaluateModel(unsigned int modelIndex, unsigned int* numInliers, unsigned int* numPointsTested) = 0; + virtual inline void testSolutionDegeneracy(bool* degenerateModel, bool* upgradeModel) = 0; + virtual inline unsigned int upgradeDegenerateModel() = 0; + virtual inline void findWeights(unsigned int modelIndex, const std::vector& inliers, + unsigned int numInliers, double* weights) = 0; + virtual inline void storeModel(unsigned int modelIndex, unsigned int numInliers) = 0; + + // ------------------------------------------------------------------------ + // USAC input parameters + protected: + // common parameters + double usac_conf_threshold_; + unsigned int usac_min_sample_size_; + double usac_inlier_threshold_; + unsigned int usac_max_hypotheses_; + unsigned int usac_max_solns_per_sample_; + bool usac_prevalidate_sample_; + bool usac_prevalidate_model_; + bool usac_test_degeneracy_; + unsigned int usac_num_data_points_; + USACConfig::RandomSamplingMethod usac_sampling_method_; + USACConfig::VerifMethod usac_verif_method_; + USACConfig::LocalOptimizationMethod usac_local_opt_method_; + + // PROSAC parameters + unsigned int prosac_growth_max_samples_; + double prosac_beta_; + double prosac_non_rand_conf_; + unsigned int prosac_min_stop_length_; + std::vector prosac_sorted_point_indices_; + + // SPRT parameters + double sprt_tM_; + double sprt_mS_; + double sprt_delta_; + double sprt_epsilon_; + + // LOSAC parameters + unsigned int lo_inner_sample_size; + unsigned int lo_num_inner_ransac_reps_; + double lo_threshold_multiplier_; + unsigned int lo_num_iterative_steps_; + + // ------------------------------------------------------------------------ + // data-specific parameters/temporary storage + protected: + // common + std::vector min_sample_; + std::vector errs_; + std::vector::iterator err_ptr_[2]; // err_ptr_[0] points to the temporary error value storage + // err_ptr_[1] points to the error values for the best solution + + // PROSAC + unsigned int subset_size_prosac_; + unsigned int largest_size_prosac_; + unsigned int stop_len_prosac_; + std::vector growth_function_prosac_; + std::vector non_random_inliers_prosac_; + std::vector maximality_samples_prosac_; + + // SPRT + double decision_threshold_sprt_; + std::vector evaluation_pool_; // randomized evaluation - holds ordering of points + unsigned int eval_pool_index_; // index to the first point to be verified + struct TestHistorySPRT + { + double epsilon_, delta_, A_; + unsigned int k_; + struct TestHistorySPRT *prev_; + }; + unsigned int last_wald_history_update_; + TestHistorySPRT* wald_test_history_; + + // LO + unsigned int num_prev_best_inliers_lo_; + std::vector prev_best_inliers_lo_; + + // ------------------------------------------------------------------------ + // helper functions + protected: + // random sampling + inline void generateUniformRandomSample(unsigned int dataSize, unsigned int sampleSize, std::vector* sample); + inline void generatePROSACMinSample(unsigned int hypCount, std::vector* sample); + + // PROSAC initialization + inline void initPROSAC(); + + // SPRT updates + inline void designSPRTTest(); + double computeExpSPRT(double new_epsilon, double epsilon, double delta); + TestHistorySPRT* addTestHistorySPRT(double epsilon, double delta, unsigned int numHyp, + TestHistorySPRT* testHistory, unsigned int* lastUpdate); + + // local optimization + unsigned int locallyOptimizeSolution(const unsigned int bestInliers); + unsigned int findInliers(const std::vector::iterator& errPtr, double threshold, + std::vector* inliers); + + // stopping criterion + unsigned int updateStandardStopping(unsigned int numInliers, unsigned int totPoints, unsigned int sampleSize); + unsigned int updatePROSACStopping(unsigned int hypCount); + unsigned int updateSPRTStopping(unsigned int numInliers, unsigned int totPoints, TestHistorySPRT* testHistory); + + // store USAC result/statistics + void storeSolution(unsigned int modelIndex, unsigned int numInliers); +}; + + +// ============================================================================================ +// initParamsUSAC: initializes USAC +// this function is called once to initialize the basic USAC parameters +// all problem specific/data-specific initialization is done using initDataUSAC() +// ============================================================================================ +template +void USAC::initParamsUSAC(const ConfigParams& cfg) +{ + // store common parameters + usac_conf_threshold_ = cfg.common.confThreshold; + usac_min_sample_size_ = cfg.common.minSampleSize; + usac_inlier_threshold_ = cfg.common.inlierThreshold; + usac_max_hypotheses_ = cfg.common.maxHypotheses; + usac_max_solns_per_sample_ = cfg.common.maxSolutionsPerSample; + usac_prevalidate_sample_ = cfg.common.prevalidateSample; + usac_prevalidate_model_ = cfg.common.prevalidateModel; + usac_test_degeneracy_ = cfg.common.testDegeneracy; + usac_sampling_method_ = cfg.common.randomSamplingMethod; + usac_verif_method_ = cfg.common.verifMethod; + usac_local_opt_method_ = cfg.common.localOptMethod; + + // read in PROSAC parameters if required + if (usac_sampling_method_ == USACConfig::SAMP_PROSAC) + { + prosac_growth_max_samples_ = cfg.prosac.maxSamples; + prosac_beta_ = cfg.prosac.beta; + prosac_non_rand_conf_ = cfg.prosac.nonRandConf; + prosac_min_stop_length_ = cfg.prosac.minStopLen; + } + + // read in SPRT parameters if required + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + sprt_tM_ = cfg.sprt.tM; + sprt_mS_ = cfg.sprt.mS; + sprt_delta_ = cfg.sprt.delta; + sprt_epsilon_ = cfg.sprt.epsilon; + } + + // read in LO parameters if required + if (usac_local_opt_method_ == USACConfig::LO_LOSAC) + { + lo_inner_sample_size = cfg.losac.innerSampleSize; + lo_num_inner_ransac_reps_ = cfg.losac.innerRansacRepetitions; + lo_threshold_multiplier_ = cfg.losac.thresholdMultiplier; + lo_num_iterative_steps_ = cfg.losac.numStepsIterative; + } + + min_sample_.clear(); min_sample_.resize(usac_min_sample_size_); +} + +// ============================================================================================ +// initDataUSAC: initializes problem specific/data-specific parameters +// this function is called once per USAC run on new data +// ============================================================================================ +template +void USAC::initDataUSAC(const ConfigParams& cfg) +{ + // clear output data + usac_results_.reset(); + + // initialize some data specfic stuff + usac_num_data_points_ = cfg.common.numDataPoints; + usac_inlier_threshold_ = cfg.common.inlierThreshold*cfg.common.inlierThreshold; + + // set up PROSAC if required + if (usac_sampling_method_ == USACConfig::SAMP_PROSAC) + { + prosac_sorted_point_indices_.assign(cfg.prosac.sortedPointIndices, cfg.prosac.sortedPointIndices+cfg.common.numDataPoints); + initPROSAC(); // init the PROSAC data structures + } + + // set up SPRT if required + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + sprt_tM_ = cfg.sprt.tM; + sprt_mS_ = cfg.sprt.mS; + sprt_delta_ = cfg.sprt.delta; + sprt_epsilon_ = cfg.sprt.epsilon; + + last_wald_history_update_ = 0; + wald_test_history_ = NULL; + designSPRTTest(); + } + + // set up LO if required + if (usac_local_opt_method_ == USACConfig::LO_LOSAC) + { + num_prev_best_inliers_lo_ = 0; + prev_best_inliers_lo_.clear(); + prev_best_inliers_lo_.resize(usac_num_data_points_, 0); + } + + // space to store point-to-model errors + errs_.clear(); errs_.resize(2*usac_num_data_points_, 0); + err_ptr_[0] = errs_.begin(); + err_ptr_[1] = errs_.begin()+usac_num_data_points_; + + // inititalize evaluation ordering to a random permutation of 0...num_data_points_-1 + eval_pool_index_ = 0; + evaluation_pool_.clear(); evaluation_pool_.resize(usac_num_data_points_); + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + evaluation_pool_[i] = i; + } + std::random_shuffle(evaluation_pool_.begin(), evaluation_pool_.end()); + + // storage for results + usac_results_.inlier_flags_.resize(usac_num_data_points_, 0); + usac_results_.best_sample_.resize(usac_min_sample_size_, 0); + // if degeneracy testing option is selected + if (usac_test_degeneracy_) + { + usac_results_.degen_inlier_flags_.resize(usac_num_data_points_, 0); + usac_results_.degen_sample_.resize(usac_num_data_points_, 0); + usac_results_.degen_sample_.resize(usac_min_sample_size_, 0); + } +} + + +// ============================================================================================ +// solve: the main USAC function +// computes a solution to the robust estimation problem +// ============================================================================================ +template +bool USAC::solve() +{ + unsigned int adaptive_stopping_count = usac_max_hypotheses_; // initialize with worst case + bool update_sprt_stopping = true; + + // abort if too few data points + if (usac_num_data_points_ < usac_min_sample_size_ || (usac_sampling_method_ == USACConfig::SAMP_PROSAC && + usac_num_data_points_ < prosac_min_stop_length_)) + { + return false; + } + + // timing stuff + LARGE_INTEGER tick, tock, freq; + QueryPerformanceCounter(&tick); + + // ------------------------------------------------------------------------ + // main USAC loop + while (usac_results_.hyp_count_ < adaptive_stopping_count && usac_results_.hyp_count_ < usac_max_hypotheses_) + { + ++usac_results_.hyp_count_; + + // ----------------------------------------- + // 1. generate sample + switch (usac_sampling_method_) + { + case USACConfig::SAMP_UNIFORM: + { + generateUniformRandomSample(usac_num_data_points_, usac_min_sample_size_, &min_sample_); + break; + } + + case USACConfig::SAMP_UNIFORM_MM: + { + generateUniformRandomSample(usac_num_data_points_, usac_min_sample_size_, &min_sample_); + break; + } + + case USACConfig::SAMP_PROSAC: + { + generatePROSACMinSample(usac_results_.hyp_count_, &min_sample_); + break; + } + } + + // ----------------------------------------- + // 2. validate sample + if (usac_prevalidate_sample_) + { + // pre-validate sample before testing generating model + bool valid_sample = static_cast(this)->validateSample(); + if (!valid_sample) + { + ++usac_results_.rejected_sample_count_; + continue; + } + } + + // ----------------------------------------- + // 3. generate model(s) + unsigned int num_solns = static_cast + (this)->generateMinimalSampleModels(); + usac_results_.model_count_ += num_solns; + + // ----------------------------------------- + // 4-5. validate + evaluate model(s) + bool update_best = false; + for (unsigned int i = 0; i < num_solns; ++i) + { + if (usac_prevalidate_model_) + { + // pre-validate model before testing against data points + bool valid_model = static_cast(this)->validateModel(i); + if (!valid_model) + { + ++usac_results_.rejected_model_count_; + continue; + } + } + + // valid model, perform evaluation + unsigned int inlier_count, num_points_tested; + bool good = static_cast + (this)->evaluateModel(i, &inlier_count, &num_points_tested); + + // update based on verification results + switch (usac_verif_method_) + { + case USACConfig::VERIF_STANDARD: + { + usac_results_.total_points_verified_ += usac_num_data_points_; + // check if best so far + if (inlier_count > usac_results_.best_inlier_count_) + { + update_best = true; + usac_results_.best_inlier_count_ = inlier_count; + storeSolution(i, usac_results_.best_inlier_count_); // store result + } + break; + } // end case standard verification + + case USACConfig::VERIF_SPRT: + { + if (!good) + { + // model rejected + usac_results_.total_points_verified_ += num_points_tested; + double delta_new = (double)inlier_count/num_points_tested; + if (delta_new > 0 && abs(sprt_delta_ - delta_new)/sprt_delta_ > 0.1) + { + // update parameters + wald_test_history_ = addTestHistorySPRT(sprt_epsilon_, sprt_delta_, + usac_results_.hyp_count_, wald_test_history_, &last_wald_history_update_); + sprt_delta_ = delta_new; + designSPRTTest(); + } + } + else + { + // model accepted + usac_results_.total_points_verified_ += usac_num_data_points_; + if (inlier_count > usac_results_.best_inlier_count_) + { + // and best so far + update_best = true; + usac_results_.best_inlier_count_ = inlier_count; + wald_test_history_ = addTestHistorySPRT(sprt_epsilon_, sprt_delta_, + usac_results_.hyp_count_, wald_test_history_, &last_wald_history_update_); + sprt_epsilon_ = (double)usac_results_.best_inlier_count_/usac_num_data_points_; + designSPRTTest(); + update_sprt_stopping = true; + storeSolution(i, usac_results_.best_inlier_count_); // store model + } + } + } // end case sprt + + } // end switch verification method + } // end evaluating all models for one minimal sample + + // ----------------------------------------- + // 6. if new best model, check for degeneracy + bool degenerate_model = false, upgrade_model = false, upgrade_successful = false; + + if (update_best && usac_test_degeneracy_) + { + std::cout << "Testing for degeneracy (" << usac_results_.best_inlier_count_ << ")" << std::endl; + static_cast(this)->testSolutionDegeneracy(°enerate_model, &upgrade_model); + if (degenerate_model && upgrade_model) + { + // complete model + unsigned int upgrade_inliers = static_cast(this)->upgradeDegenerateModel(); + if (upgrade_inliers > usac_results_.best_inlier_count_) + { + usac_results_.best_inlier_count_ = upgrade_inliers; + upgrade_successful = true; + } + } + } + + // ----------------------------------------- + // 7. perform local optimization if specified + if (usac_local_opt_method_ == USACConfig::LO_LOSAC && update_best == true) + { + std::cout << "(" << usac_results_.hyp_count_ << ") Performing LO. Inlier count before: " << usac_results_.best_inlier_count_; + unsigned int lo_inlier_count = locallyOptimizeSolution(usac_results_.best_inlier_count_); + if (lo_inlier_count > usac_results_.best_inlier_count_) + { + usac_results_.best_inlier_count_ = lo_inlier_count; + } + std::cout << ", inlier count after: " << lo_inlier_count << std::endl; + + if (num_prev_best_inliers_lo_ < usac_results_.best_inlier_count_) + { + // backup the old set of inliers for reference in LO + num_prev_best_inliers_lo_ = usac_results_.best_inlier_count_; + prev_best_inliers_lo_ = usac_results_.inlier_flags_; + } + } + + // ----------------------------------------- + // 8. update the stopping criterion + if (update_best) + { + // update the number of samples required + if ( usac_sampling_method_ == USACConfig::SAMP_PROSAC && usac_results_.hyp_count_ <= prosac_growth_max_samples_ ) + { + adaptive_stopping_count = updatePROSACStopping(usac_results_.hyp_count_); + } + else + { + adaptive_stopping_count = updateStandardStopping(usac_results_.best_inlier_count_, usac_num_data_points_, usac_min_sample_size_); + } + } + // update adaptive stopping count to take SPRT test into account + if (usac_verif_method_ == USACConfig::VERIF_SPRT && usac_sampling_method_ != USACConfig::SAMP_PROSAC) + { + if (usac_results_.hyp_count_ >= adaptive_stopping_count && update_sprt_stopping) + { + adaptive_stopping_count = updateSPRTStopping(usac_results_.best_inlier_count_, usac_num_data_points_, wald_test_history_); + update_sprt_stopping = false; + } + } + + } // end the main USAC loop + + // ------------------------------------------------------------------------ + // output statistics + QueryPerformanceCounter(&tock); + QueryPerformanceFrequency(&freq); + std::cout << "Number of hypotheses/models: " << usac_results_.hyp_count_ << "/" << usac_results_.model_count_ << std::endl; + std::cout << "Number of samples rejected by pre-validation: " << usac_results_.rejected_sample_count_ << std::endl; + std::cout << "Number of models rejected by pre-validation: " << usac_results_.rejected_model_count_ << std::endl; + std::cout << "Number of verifications per model: " << + (double)usac_results_.total_points_verified_/(usac_results_.model_count_-usac_results_.rejected_model_count_) << std::endl; + std::cout << "Max inliers/total points: " << usac_results_.best_inlier_count_ << "/" << usac_num_data_points_ << std::endl; + + // ------------------------------------------------------------------------ + // timing stuff + usac_results_.total_runtime_ = (double)(tock.QuadPart - tick.QuadPart)/(double)freq.QuadPart; + std::cout << "Time: " << usac_results_.total_runtime_ << std::endl << std::endl; + + // ------------------------------------------------------------------------ + // clean up + if (usac_verif_method_ == USACConfig::VERIF_SPRT) + { + while (wald_test_history_) + { + TestHistorySPRT *temp = wald_test_history_->prev_; + delete wald_test_history_; + wald_test_history_ = temp; + } + } + + return true; +} + + +// ============================================================================================ +// generateUniformRandomSample: generate random sample uniformly distributed between +// [0...dataSize-1] +// note that the sample vector needs to be properly sized before calling this function +// ============================================================================================ +template +void USAC::generateUniformRandomSample(unsigned int dataSize, unsigned int sampleSize, + std::vector* sample) +{ + unsigned int count=0; + unsigned int index; + std::vector::iterator pos; + pos = sample->begin(); + do { + index = rand() % dataSize; + if (find(sample->begin(), pos, index) == pos) + { + (*sample)[count] = index; + ++count; + ++pos; + } + } while (count < sampleSize); +} + + +// ============================================================================================ +// initPROSAC: initializes PROSAC +// sets up growth function and stopping criterion +// ============================================================================================ +template inline +void USAC::initPROSAC() +{ + // ------------------------------------------------------------------------ + // growth function + + growth_function_prosac_.clear(); growth_function_prosac_.resize(usac_num_data_points_); + double T_n; + unsigned int T_n_p = 1; + // compute initial value for T_n + T_n = prosac_growth_max_samples_; + for (unsigned int i = 0; i < usac_min_sample_size_; ++i) + { + T_n *= (double)(usac_min_sample_size_-i)/(usac_num_data_points_-i); + } + // compute values using recurrent relation + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + if (i+1 <= usac_min_sample_size_) + { + growth_function_prosac_[i] = T_n_p; + continue; + } + double temp = (double)(i+1)*T_n/(i+1-usac_min_sample_size_); + growth_function_prosac_[i] = T_n_p + (unsigned int)ceil(temp - T_n); + T_n = temp; + T_n_p = growth_function_prosac_[i]; + } + + // ------------------------------------------------------------------------ + // initialize the data structures that determine stopping + + // non-randomness constraint + // i-th entry - inlier counts for termination up to i-th point (term length = i+1) + non_random_inliers_prosac_.clear(); + non_random_inliers_prosac_.resize(usac_num_data_points_, 0); + double pn_i = 1.0; // prob(i inliers) with subset size n + for (size_t n = usac_min_sample_size_+1; n <= usac_num_data_points_; ++n) + { + if (n-1 > 1000) + { + non_random_inliers_prosac_[n-1] = non_random_inliers_prosac_[n-2]; + continue; + } + + std::vector pn_i_vec(usac_num_data_points_, 0); + // initial value for i = m+1 inliers + pn_i_vec[usac_min_sample_size_] = (prosac_beta_)*std::pow((double)1-prosac_beta_, (double)n-usac_min_sample_size_-1)*(n-usac_min_sample_size_); + pn_i = pn_i_vec[usac_min_sample_size_]; + for (size_t i = usac_min_sample_size_+2; i <= n; ++i) + { + // use recurrent relation to fill in remaining values + if (i == n) + { + pn_i_vec[n-1] = std::pow((double)prosac_beta_, (double)n-usac_min_sample_size_); + break; + } + pn_i_vec[i-1] = pn_i * ((prosac_beta_)/(1-prosac_beta_)) * ((double)(n-i)/(i-usac_min_sample_size_+1)); + pn_i = pn_i_vec[i-1]; + } + // find minimum number of inliers satisfying the non-randomness constraint + double acc = 0.0; + unsigned int i_min = 0; + for (size_t i = n; i >= usac_min_sample_size_+1; --i) + { + acc += pn_i_vec[i-1]; + if (acc < 1-prosac_non_rand_conf_) + { + i_min = i; + } + else + { + break; + } + } + non_random_inliers_prosac_[n-1] = i_min; + } + + // maximality constraint + // i-th entry - number of samples for pool [0...i] (pool length = i+1) + maximality_samples_prosac_.clear(); + maximality_samples_prosac_.resize(usac_num_data_points_); + for (size_t i = 0; i < usac_num_data_points_; ++i) + { + maximality_samples_prosac_[i] = usac_max_hypotheses_; + } + + // other initializations + largest_size_prosac_ = usac_min_sample_size_; // largest set sampled in PROSAC + subset_size_prosac_ = usac_min_sample_size_; // size of the current sampling pool + stop_len_prosac_ = usac_num_data_points_; // current stopping length +} + + +// ============================================================================================ +// generatePROSACMinSample: generate a minimal sample using ordering information +// uses growth function to determine size of current sampling pool +// ============================================================================================ +template +void USAC::generatePROSACMinSample(unsigned int hypCount, std::vector* sample) +{ + // revert to RANSAC-style sampling if maximum number of PROSAC samples have been tested + if (hypCount > prosac_growth_max_samples_) + { + generateUniformRandomSample(usac_num_data_points_, usac_min_sample_size_, sample); + return; + } + + // if current stopping length is less than size of current pool, use only points up to the stopping length + if (subset_size_prosac_ > stop_len_prosac_) + { + generateUniformRandomSample(stop_len_prosac_, usac_min_sample_size_, sample); + } + + // increment the size of the sampling pool if required + if (hypCount > growth_function_prosac_[subset_size_prosac_-1]) + { + ++subset_size_prosac_; + if (subset_size_prosac_ > usac_num_data_points_) + { + subset_size_prosac_ = usac_num_data_points_; + } + if (largest_size_prosac_ < subset_size_prosac_) + { + largest_size_prosac_ = subset_size_prosac_; + } + } + + // generate PROSAC sample + generateUniformRandomSample(subset_size_prosac_-1, usac_min_sample_size_-1, sample); + (*sample)[usac_min_sample_size_-1] = subset_size_prosac_-1; + for (unsigned int i = 0; i < sample->size(); ++i) + { + (*sample)[i] = prosac_sorted_point_indices_[(*sample)[i]]; + } +} + + +// ============================================================================================ +// updatePROSACStopping: determines number of samples required for termination +// checks the non-randomness and maximality constraints +// ============================================================================================ +template inline +unsigned int USAC::updatePROSACStopping(unsigned int hypCount) +{ + unsigned int max_samples = maximality_samples_prosac_[stop_len_prosac_-1]; + + // go through sorted points and track inlier counts + unsigned int inlier_count = 0; + + // just accumulate the count for the first prosac_min_stop_length_ points + for (unsigned int i = 0; i < prosac_min_stop_length_; ++i) + { + inlier_count += usac_results_.inlier_flags_[prosac_sorted_point_indices_[i]]; + } + + // after this initial subset, try to update the stopping length if possible + for (unsigned int i = prosac_min_stop_length_; i < usac_num_data_points_; ++i) + { + inlier_count += usac_results_.inlier_flags_[prosac_sorted_point_indices_[i]]; + + if (non_random_inliers_prosac_[i] < inlier_count) + { + non_random_inliers_prosac_[i] = inlier_count; // update the best inliers for the the subset [0...i] + + // update the number of samples based on this inlier count + if ( (i == usac_num_data_points_-1) || + (usac_results_.inlier_flags_[prosac_sorted_point_indices_[i]] && !usac_results_.inlier_flags_[prosac_sorted_point_indices_[i+1]]) ) + { + unsigned int new_samples = updateStandardStopping(inlier_count, i+1, usac_min_sample_size_); + if (i+1 < largest_size_prosac_) + { + // correct for number of samples that have points in [i+1, largest_size_prosac_-1] + new_samples += hypCount - growth_function_prosac_[i]; + } + + if (new_samples < maximality_samples_prosac_[i]) + { + // if number of samples can be lowered, store values and update stopping length + maximality_samples_prosac_[i] = new_samples; + if ( (new_samples < max_samples) || ( (new_samples == max_samples) && (i+1 >= stop_len_prosac_) ) ) + { + stop_len_prosac_ = i+1; + max_samples = new_samples; + } + } + } + } + } + return max_samples; +} + + +// ============================================================================================ +// designSPRTTest: designs a new SPRT test (i.e., updates the SPRT decision threshold) based +// on current values of delta and epsilon +// ============================================================================================ +template inline +void USAC::designSPRTTest() +{ + double An_1, An, C, K; + + C = (1 - sprt_delta_)*log( (1 - sprt_delta_)/(1-sprt_epsilon_) ) + + sprt_delta_*(log( sprt_delta_/sprt_epsilon_ )); + K = (sprt_tM_*C)/sprt_mS_ + 1; + An_1 = K; + + // compute A using a recursive relation + // A* = lim(n->inf)(An), the series typically converges within 4 iterations + for (unsigned int i = 0; i < 10; ++i) + { + An = K + log(An_1); + if (An - An_1 < 1.5e-8) + { + break; + } + An_1 = An; + } + + decision_threshold_sprt_ = An; +} + +// ============================================================================================ +// locallyOptimizeSolution: iteratively refines the current model based on its inlier set +// ============================================================================================ +template inline +unsigned int USAC::locallyOptimizeSolution(const unsigned int bestInliers) +{ + // return if insufficient number of points + if (bestInliers < 2*lo_inner_sample_size) + { + return 0; + } + + unsigned int lo_sample_size = std::min(lo_inner_sample_size, bestInliers/2); + std::vector sample(lo_sample_size); + std::vector orig_inliers(usac_num_data_points_); + std::vector iter_inliers(usac_num_data_points_); + unsigned int num_points_tested; + + // find all inliers less than threshold + unsigned int lo_inliers = bestInliers; + unsigned int temp_inliers = 0; + findInliers(err_ptr_[1], usac_inlier_threshold_, &orig_inliers); +#if 0 + // check if there is substantial overlap between the current and the best inlier sets + // if yes, the local refinement is unlikely to help + std::vector ind_best, ind_curr(bestInliers); + for (size_t i = 0; i < usac_num_data_points_; ++i) + { + if (m_prevBestInliers[i]) ind_best.push_back(i); + } + for (size_t i = 0; i < bestInliers; ++i) + { + ind_curr[i] = orig_inliers[i]; + } + std::vector temp_intersection(bestInliers); + std::vector::iterator it = std::set_intersection (ind_best.begin(), ind_best.end(), + ind_curr.begin(), ind_curr.end(), + temp_intersection.begin()); + unsigned int num_elements_intersection = it - temp_intersection.begin(); + std::cout << " (" << num_elements_intersection << ") " ; + if ((double)num_elements_intersection/bestInliers > 0.95) + { + std::cout << "****"; + return 0; + } +#endif + ++usac_results_.num_local_optimizations_; + + double *weights = new double[usac_num_data_points_]; + double threshold_step_size = (lo_threshold_multiplier_*usac_inlier_threshold_ - usac_inlier_threshold_) + /lo_num_iterative_steps_; + // perform number of inner RANSAC repetitions + for (unsigned int i = 0; i < lo_num_inner_ransac_reps_; ++i) + { + // generate non-minimal sample model and find inliers + generateUniformRandomSample(bestInliers, lo_sample_size, &sample); + for (unsigned int j = 0; j < lo_sample_size; ++j) + { + sample[j] = orig_inliers[sample[j]]; // we want points only from the current inlier set + } + if ( !static_cast(this)->generateRefinedModel(sample, lo_sample_size) ) + { + continue; + } + + if (! static_cast(this)->evaluateModel(0, &temp_inliers, &num_points_tested) ) + { + continue; + } + temp_inliers = findInliers(err_ptr_[0], lo_threshold_multiplier_*usac_inlier_threshold_, &iter_inliers); + + // generate least squares model from all inliers + if (! static_cast(this)->generateRefinedModel(iter_inliers, temp_inliers) ) + { + continue; + } + + // iterative (reweighted) refinement - reduce threshold in steps, find new inliers and refit fundamental matrix + // using weighted least-squares + for (unsigned int j = 0; j < lo_num_iterative_steps_; ++j) + { + if (! static_cast(this)->evaluateModel(0, &temp_inliers, &num_points_tested) ) + { + continue; + } + findInliers(err_ptr_[0], (lo_threshold_multiplier_*usac_inlier_threshold_) - (j+1)*threshold_step_size, &iter_inliers); + static_cast(this)->findWeights(0, iter_inliers, temp_inliers, weights); + if (! static_cast(this)->generateRefinedModel(iter_inliers, temp_inliers, true, weights) ) + { + continue; + } + } + + // find final set of inliers for this round + if (! static_cast(this)->evaluateModel(0, &temp_inliers, &num_points_tested) ) + { + continue; + } + //findInliers(err_ptr_[0], iter_inliers, usac_inlier_threshold_); + + if (temp_inliers > lo_inliers) + { + // store model + lo_inliers = temp_inliers; + static_cast(this)->storeSolution(0, lo_inliers); + } + } + + delete[] weights; + return lo_inliers; +} + + +// ============================================================================================ +// findInliers: given an error vector and threshold, returns the indices of inliers +// ============================================================================================ +template inline +unsigned int USAC::findInliers(const std::vector::iterator& errPtr, double threshold, + std::vector* inliers) +{ + unsigned int inlier_count = 0; + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + if (*(errPtr+i) < threshold) + { + (*inliers)[inlier_count] = i; + ++inlier_count; + } + } + return inlier_count; +} + + +// ============================================================================================ +// updateStandardStopping: updates the standard RANSAC stopping criterion +// ============================================================================================ +template inline +unsigned int USAC::updateStandardStopping(unsigned int numInliers, unsigned int totPoints, + unsigned int sampleSize) +{ + double n_inliers = 1.0; + double n_pts = 1.0; + + for (unsigned int i = 0; i < sampleSize; ++i) + { + n_inliers *= numInliers - i; + n_pts *= totPoints - i; + } + double prob_good_model = n_inliers/n_pts; + + if ( prob_good_model < std::numeric_limits::epsilon() ) + { + return usac_max_hypotheses_; + } + else if ( 1 - prob_good_model < std::numeric_limits::epsilon() ) + { + return 1; + } + else + { + double nusample_s = log(1-usac_conf_threshold_)/log(1-prob_good_model); + return (unsigned int) ceil(nusample_s); + } +} + + +// ============================================================================================ +// updateSPRTStopping: updates the stopping criterion accounting for erroneous rejections in +// the SPRT test +// ============================================================================================ +template inline +unsigned int USAC::updateSPRTStopping(unsigned int numInliers, unsigned int totPoints, TestHistorySPRT* testHistory) +{ + double n_inliers = 1.0; + double n_pts = 1.0; + double h = 0.0, k = 0.0, prob_reject_good_model = 0.0, log_eta = 0.0; + double new_eps = (double)numInliers/totPoints; + TestHistorySPRT* current_test = testHistory; + + for (unsigned int i = 0; i < usac_min_sample_size_; ++i) + { + n_inliers *= numInliers - i; + n_pts *= totPoints - i; + } + double prob_good_model = n_inliers/n_pts; + + if ( prob_good_model < std::numeric_limits::epsilon() ) + { + return usac_max_hypotheses_; + } + else if ( 1 - prob_good_model < std::numeric_limits::epsilon() ) + { + return 1; + } + + while (current_test != NULL) + { + k += current_test->k_; + h = computeExpSPRT(new_eps, current_test->epsilon_, current_test->delta_); + prob_reject_good_model = 1/(exp( h*log(current_test->A_) )); + log_eta += (double) current_test->k_ * log( 1 - prob_good_model*(1-prob_reject_good_model) ); + current_test = current_test->prev_; + } + + double nusample_s = k + ( log(1-usac_conf_threshold_) - log_eta ) / log( 1-prob_good_model * (1-(1/decision_threshold_sprt_)) ); + return (unsigned int) ceil(nusample_s); +} + + +// ============================================================================================ +// computeExpSPRT: computes the value of the exponent h_i required to determine the number +// of iterations when using SPRT +// ============================================================================================ +template inline +double USAC::computeExpSPRT(double newEpsilon, double epsilon, double delta) +{ + double al, be, x0, x1, v0, v1, h; + + al = log(delta/epsilon); + be = log( (1-delta)/(1-epsilon) ); + + x0 = log( 1/(1-newEpsilon) )/be; + v0 = newEpsilon * exp(x0 *al); + x1 = log( (1-2*v0) / (1-newEpsilon) )/be; + v1 = newEpsilon * exp(x1 * al) + (1-newEpsilon) * exp(x1 * be); + h = x0 - (x0 - x1)/(1+v0 - v1)*v0; + return h; +} + + +// ============================================================================================ +// addTestHistorySPRT: store statistics for each SPRT test (since each test is adjusted to +// reflect current estimates of these parameters) +// this is required to compute the number of samples for termination +// ============================================================================================ +template inline +typename USAC::TestHistorySPRT* USAC::addTestHistorySPRT(double epsilon, double delta, unsigned int numHyp, + TestHistorySPRT* testHistory, + unsigned int* lastUpdate) +{ + TestHistorySPRT *new_test_history = new TestHistorySPRT; + new_test_history->epsilon_ = epsilon; + new_test_history->delta_ = delta; + new_test_history->A_ = decision_threshold_sprt_; + new_test_history->k_ = numHyp - *lastUpdate; + new_test_history->prev_ = testHistory; + *lastUpdate = numHyp; + + return new_test_history; +} + + +// ============================================================================================ +// storeSolution: stores inlier flag vector, best sample indices and best inlier count +// also swaps error pointers so that err_ptr_[1] points to the error values for the best model +// calls a model-specific function to store model data as appropriate +// ============================================================================================ +template inline +void USAC::storeSolution(unsigned int modelIndex, unsigned int numInliers) +{ + // save the current best set of inliers + usac_results_.best_inlier_count_ = numInliers; + std::vector::iterator current_err_array = err_ptr_[0]; + for (unsigned int i = 0; i < usac_num_data_points_; ++i) + { + if (*(current_err_array+i) < usac_inlier_threshold_) + usac_results_.inlier_flags_[i] = 1; + else + usac_results_.inlier_flags_[i] = 0; + } + + // save the current best sample indices + usac_results_.best_sample_ = min_sample_; + + // and switch the error pointers + std::swap(err_ptr_[0], err_ptr_[1]); + + // store model + static_cast(this)->storeModel(modelIndex, numInliers); +} + +#endif diff --git a/src/utils/FundmatrixFunctions.cpp b/src/utils/FundmatrixFunctions.cpp new file mode 100644 index 0000000..1d0da53 --- /dev/null +++ b/src/utils/FundmatrixFunctions.cpp @@ -0,0 +1,546 @@ +#include "FundmatrixFunctions.h" +#include + +namespace FTools +{ + void normalizePoints(double* inputPointsUnNorm, double* inputPoints, unsigned int numPoints, + double* T1, double* T2) + { + for (unsigned int i = 0; i < 3; ++i) + { + for (unsigned int j = 0; j < 3; ++j) + { + T1[3*i+j] = 0; + T2[3*i+j] = 0; + } + } + + // compute mean + double mean1[2] = {0.0}; double mean2[2] = {0.0}; + for (unsigned int i = 0; i < numPoints; ++i) + { + mean1[0] += inputPointsUnNorm[6*i]; mean1[1] += inputPointsUnNorm[6*i+1]; + mean2[0] += inputPointsUnNorm[6*i+3]; mean2[1] += inputPointsUnNorm[6*i+4]; + } + mean1[0] /= (double)numPoints; mean2[0] /= (double)numPoints; + mean1[1] /= (double)numPoints; mean2[1] /= (double)numPoints; + + // compute mean distance from center + double meandist1(0), meandist2(0); + for (unsigned int i = 0; i < numPoints; ++i) + { + meandist1 += sqrt( (inputPointsUnNorm[6*i] - mean1[0]) * (inputPointsUnNorm[6*i] - mean1[0]) + + (inputPointsUnNorm[6*i+1] - mean1[1]) * (inputPointsUnNorm[6*i+1] - mean1[1]) ); + meandist2 += sqrt( (inputPointsUnNorm[6*i+3] - mean2[0]) * (inputPointsUnNorm[6*i+3] - mean2[0]) + + (inputPointsUnNorm[6*i+4] - mean2[1]) * (inputPointsUnNorm[6*i+4] - mean2[1]) ); + } + meandist1 /= (double)numPoints; meandist2 /= (double)numPoints; + double scale1 = sqrt(2.0)/meandist1; double scale2 = sqrt(2.0)/meandist2; + + // form normalization matrices + T1[0] = scale1; + T1[2] = -scale1*mean1[0]; + T1[4] = scale1; + T1[5] = -scale1*mean1[1]; + T1[8] = 1.0; + + T2[0] = scale2; + T2[2] = -scale2*mean2[0]; + T2[4] = scale2; + T2[5] = -scale2*mean2[1]; + T2[8] = 1.0; + + // form array of normazlied data points + for (unsigned int i = 0; i < numPoints; ++i) + { + MathTools::vmul( (inputPoints+6*i), T1, (inputPointsUnNorm + 6*i), 3); + MathTools::vmul( (inputPoints+6*i+3), T2, (inputPointsUnNorm + 6*i+3), 3); + } + } + + void computeDataMatrix(double* data_matrix, unsigned int num_points, double* points) + { + // linearizes corresp. with respect to entries of fundamental matrix + // so that x' F x -> A f + const double *data_ptr; + double *matrix_ptr = data_matrix; + unsigned int offset; + + for (unsigned int i = 0; i < num_points; ++i) + { + data_ptr = points + 6*i; + offset = 0; + for (unsigned int j = 0; j < 3; ++j) + { + for (unsigned int k = 0; k < 3; ++k) + { + *(matrix_ptr+offset) = *(data_ptr+j+3) * (*(data_ptr+k)); + offset += num_points; + } + } + ++matrix_ptr; + } + } // end computeDataMatrix + + int nullspace(double* matrix, double* nullspace, int n, int* buffer) + { + int *pnopivot = buffer, nonpivot = 0; + int *ppivot = buffer + n; + int i, j, k, l, ptr, max; + double pivot, t; + double tol = 1e-12; + + ptr = 0; + i = 0; + for (j = 0; j < n; ++j) + { + /* find pivot, start with diagonal element */ + pivot = matrix[n*i+j]; max = i; + for (k=i+1; k max */ + for (k=j; kcolumn organization for Fortran + double T[7*9]; + double tau[9]; + double work[3*9+1]; + int p[9]; + + int work_size = 3*cols+1; + int info; + + // assume underdetermined system with full possible rank... + int null_size = cols - rows; + int k,r,c; + double *sol = N; + double a; + + for (i=0; i=0; r--) + { + a=0; + if (T[r*rows+r]==0.0) + return -1; + for (c=r+1;c 0) + { + A = sqrt(D) - q; + if (A > 0) + { + v = pow(A,1.0/3); + *r = v - p/v - bt; + } else + { + v = pow(-A,1.0/3); + *r = p/v - v - bt; + } + + return 1; + } + else + { + if (q > 0) e = 1; else e = -1; + R = e * sqrt(-p); + _2R = R *2; + cosphi = q / (R*R*R); + if (cosphi > 1) cosphi = 1; else + if (cosphi < -1) cosphi = -1; + phit = acos(cosphi) /3; + pit = 3.14159265358979/3; + + r[0] = -_2R * cos(phit) -bt; + r[1] = _2R * cos(pit - phit) -bt; + r[2] = _2R * cos(pit + phit) -bt; + + return 3; + } + } // end rroots3 + + void formCovMat(double* Cv, const double* Z, unsigned int len, unsigned int siz) + { + unsigned int lenM = len*siz; + double val; + + for (unsigned int i = 0; i < siz; ++i) + { + for (unsigned int j = 0; j <= i; ++j) + { + val = 0; + for (unsigned int k = 0; k < lenM; k += siz) + { + val += Z[k+i] * Z[k+j]; + } + Cv[siz*i + j] = val; + Cv[i + siz*j] = val; + } + } + } // end formCovMat + + void singulF(double *F) + { + double U[9], D[3], V[9]; + int i, j, k=0, l=0; + MathTools::svduv(D, F, V, 3, U, 3); + + j = 0; + for (i = 1; i < 3; i ++) + if (abs(D[j]) > abs(D[i])) j = i; + + switch (j) + { + case 0: k = 1; l = 2; break; + case 1: k = 0; l = 2; break; + case 2: k = 0; l = 1; break; + } + + for (i = 0; i < 9; i+=3) + { + V[i+k] *= D[k]; + V[i+l] *= D[l]; + } + + for (j = 0; j < 9; j+=3) + for (i = 0; i < 9; i+=3, F++) + *F = U[i+k] * V[j+k] + U[i+l] * V[j+l]; + } // end singulF + + void computeEpipole(double* epi, const double* F) + { + double xeps = 1.9984e-15; + MathTools::crossprod(epi, F, F+6, 1); + for(unsigned int i = 0; i < 3; ++i) + { + if ( (epi[i] > xeps) || (epi[i] < -xeps) ) + return; + } + MathTools::crossprod(epi, F+3, F+6, 1); + } + + double getOriSign(double* F, double* e, double* pt) + { + double s1, s2; + s1 = F[0]*pt[3] + F[3]*pt[4] + F[6]*pt[5]; + s2 = e[1]*pt[2] - e[2]*pt[1]; + return (s1 * s2); + } + + void computeHFromF(const std::vector& sample, double* u, double* ep, double* F, double* H) + { + double A[9], Ex[9], M[9]; + double p1[3], p2[3], b[3]; + double h[3], norm; + double *pt1, *pt2, *m; + + // A = [e']_x * F + MathTools::skew_sym(Ex, ep); + MathTools::mmul(A, Ex, F, 3); + + m = M; + for(unsigned int i = 0; i < 3; ++i) + { + pt1 = u + 6*sample[i]; + pt2 = pt1 + 3; + + // rows of M are the points x_i + std::memcpy(m, pt1, sizeof(double) * 3); + m += 3; + + // compute b_i + MathTools::vmul(h, A, pt1, 3); + MathTools::crossprod(p1, pt2, h, 1); + MathTools::crossprod(p2, pt2, ep, 1); + norm = p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2]; + b[i] = (p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2]) / norm; + } + + // compute H = A - epipole*(inv(M)*b)^T + MathTools::minv(M, 3); + MathTools::vmul(h, M, b, 3); + unsigned int k = 0; + for (unsigned int i = 0; i < 3; ++i) + { + for(unsigned int j = 0; j < 3; ++j) + { + H[k] = A[k] - ep[i]*h[j]; + k++; + } + } + } + + unsigned int getHError(const std::vector& test, unsigned int numPoints, std::vector& errs, + double* u, double* H, double threshold) + { + double* model = H; + double inv_model[9]; + double h_x[3], h_inv_xp[3], temp_err; + double* pt; + unsigned int num_inliers = 0; + errs.clear(); errs.resize(numPoints); + + for (unsigned int i = 0; i < 9; ++i) + { + inv_model[i] = model[i]; + } + MathTools::minv(inv_model, 3); + + // check each point for symmetric transfer error + for (unsigned int i = 0; i < numPoints; ++i) + { + // compute symmetric transfer error + pt = u + 6*test[i]; + MathTools::vmul(h_x, model, pt, 3); + MathTools::vmul(h_inv_xp, inv_model, pt+3, 3); + + double err1 = 0.0, err2 = 0.0; + for (unsigned int j = 0; j < 2; ++j) + { + err1 += (h_x[j]/h_x[2] - pt[3+j]) * (h_x[j]/h_x[2] - pt[3+j]); + err2 += (h_inv_xp[j]/h_inv_xp[2] - pt[j]) * (h_inv_xp[j]/h_inv_xp[2] - pt[j]); + } + temp_err = err1 + err2; + errs[i] = temp_err; + + if (temp_err < threshold) + { + ++num_inliers; + } + } + return num_inliers; + } + + unsigned int computeHFromCorrs(const std::vector& sample, unsigned int numPoints, + unsigned int numDataPoints, double* u, double* H) + { + // form the matrix of equations for this non-minimal sample + double *A = new double[numPoints*2*9]; + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < numPoints; ++i) + { + for (unsigned int j = 0; j < 2; ++j) + { + src_ptr = u + 2*sample[i] + j; + for (unsigned int k = 0; k < 9; ++k) + { + *dst_ptr = *src_ptr; + ++dst_ptr; + src_ptr += 2*numDataPoints; + } + } + } + + // decompose + double V[9*9], D[9], *p; + MathTools::svdu1v(D, A, 2*numPoints, V, 9); + + unsigned int j = 0; + for (unsigned int i = 1; i < 9; ++i) + { + if (D[i] < D[j]) + j = i; + } + p = V + j; + + for (unsigned int i = 0; i < 9; ++i) + { + H[i] = *p; + p += 9; + } + + delete A; + + return 1; + } + + unsigned int computeHFromMinCorrs(const std::vector& sample, unsigned int numPoints, + unsigned int numDataPoints, double* u, double* H) + { + double A[8*9]; + double At[9*8]; + + // form the matrix of equations for this minimal sample + double *src_ptr; + double *dst_ptr = A; + for (unsigned int i = 0; i < numPoints; ++i) + { + for (unsigned int j = 0; j < 2; ++j) + { + src_ptr = u + 2*sample[i] + j; + for (unsigned int k = 0; k < 9; ++k) + { + *dst_ptr = *src_ptr; + ++dst_ptr; + src_ptr += 2*numDataPoints; + } + } + } + + MathTools::mattr(At, A, 8, 9); + + double D[9], U[9*9], V[8*8], *p; + MathTools::svduv(D, At, U, 9, V, 8); + p = U + 8; + + for (unsigned int i = 0; i < 9; ++i) + { + H[i] = *p; + p += 9; + } + return 1; + } + +} \ No newline at end of file diff --git a/src/utils/FundmatrixFunctions.h b/src/utils/FundmatrixFunctions.h new file mode 100644 index 0000000..098af2c --- /dev/null +++ b/src/utils/FundmatrixFunctions.h @@ -0,0 +1,27 @@ +#ifndef FTOOLS_H +#define FTOOLS_H +#include +#include "MathFunctions.h" + +namespace FTools +{ + void normalizePoints(double* inputPointsUnNorm, double* inputPoints, unsigned int numPoints, + double* T1, double* T2); + void computeDataMatrix(double* data_matrix, unsigned int num_points, double* points); + int nullspaceQR7x9(const double* A, double* N); + int nullspace(double* matrix, double* nullspace, int n, int* buffer); + void makePolynomial(double* A, double* B, double* p); + unsigned int rroots3 (double* po, double* r); + void formCovMat(double* Cv, const double* A, unsigned int len, unsigned int siz); + void singulF(double* F); + void computeEpipole(double* e, const double* F); + double getOriSign(double* F, double* e, double* pt); + void computeHFromF(const std::vector& sample, double* u, double* ep, double* F, double* H); + unsigned int getHError(const std::vector& test, unsigned int numPoints, std::vector& errs, + double* u, double* H, double threshold); + unsigned int computeHFromCorrs(const std::vector& sample, unsigned int numPoints, + unsigned int numDataPoints, double* u, double* H); + unsigned int computeHFromMinCorrs(const std::vector& sample, unsigned int numPoints, + unsigned int numDataPoints, double* u, double* H); +} +#endif diff --git a/src/utils/HomographyFunctions.cpp b/src/utils/HomographyFunctions.cpp new file mode 100644 index 0000000..6c90b6d --- /dev/null +++ b/src/utils/HomographyFunctions.cpp @@ -0,0 +1,52 @@ +#include "HomographyFunctions.h" + +namespace HTools +{ + + void computeDataMatrix(double* data_matrix, unsigned int num_points, double* points) + { + // linearizes corresp. with respect to entries of homography matrix, + // so that u' = H u -> A h + + const double *data_ptr; + double *p; + unsigned int offset = 2*num_points; + + for (unsigned int i = 0; i < num_points; ++i) + { + data_ptr = points + 6*i; + p = data_matrix + 2*i; + + *p = 0; + *(p + offset) = 0; + *(p + 2*offset) = 0; + *(p + 3*offset) = -data_ptr[0]; + *(p + 4*offset) = -data_ptr[1]; + *(p + 5*offset) = -data_ptr[2]; + *(p + 6*offset) = data_ptr[4] * data_ptr[0]; + *(p + 7*offset) = data_ptr[4] * data_ptr[1]; + *(p + 8*offset) = data_ptr[4] * data_ptr[2]; + + p = data_matrix + 2*i + 1; + + *p = data_ptr[0]; + *(p + offset) = data_ptr[1]; + *(p + 2*offset) = data_ptr[2]; + *(p + 3*offset) = 0; + *(p + 4*offset) = 0; + *(p + 5*offset) = 0; + *(p + 6*offset) = -data_ptr[3] * data_ptr[0]; + *(p + 7*offset) = -data_ptr[3] * data_ptr[1]; + *(p + 8*offset) = -data_ptr[3] * data_ptr[2]; + } + } // end computeDataMatrix + + void crossprod(double *out, const double *a, const double *b, unsigned int st) + { + unsigned int st2 = 2 * st; + out[0] = a[st]*b[st2] - a[st2]*b[st]; + out[1] = a[st2]*b[0] - a[0]*b[st2]; + out[2] = a[0]*b[st] - a[st]*b[0]; + } + +} \ No newline at end of file diff --git a/src/utils/HomographyFunctions.h b/src/utils/HomographyFunctions.h new file mode 100644 index 0000000..51131da --- /dev/null +++ b/src/utils/HomographyFunctions.h @@ -0,0 +1,11 @@ +#ifndef HTOOLS_H +#define HTOOLS_H + +#include "MathFunctions.h" + +namespace HTools +{ + void computeDataMatrix(double* data_matrix, unsigned int num_points, double* points); + void crossprod(double *out, const double *a, const double *b, unsigned int st); +} +#endif \ No newline at end of file diff --git a/src/utils/MathFunctions.cpp b/src/utils/MathFunctions.cpp new file mode 100644 index 0000000..84f0987 --- /dev/null +++ b/src/utils/MathFunctions.cpp @@ -0,0 +1,505 @@ +#include +#include +#include "MathFunctions.h" + +namespace MathTools +{ + /* svdu1v.c CCMATH mathematics library source code. + * + * Copyright (C) 2000 Daniel A. Atkinson All rights reserved. + * This code may be redistributed under the terms of the GNU library + * public license (LGPL). ( See the lgpl.license file for details.) + * ------------------------------------------------------------------------ + */ + int svdu1v(double *d,double *a,int m,double *v,int n) + { double *p,*p1,*q,*pp,*w,*e; + double s,h,r,t,sv; + int i,j,k,mm,nm,ms; + if(m1){ sv=h=0.; + for(j=0,q=p,s=0.; j0.){ + h=sqrt(s); if(*p<0.) h= -h; + s+= *p*h; s=1./s; t=1./(w[0]+=h); + sv=1.+fabs(*p/h); + for(k=1,ms=n-i; k1){ + for(j=0,q=p1,s=0.; j0.){ + h=sqrt(s); if(*p1<0.) h= -h; + sv=1.+fabs(*p1/h); + s+= *p1*h; s=1./s; t=1./(*p1+=h); + for(k=n,ms=n*(m-i); k0 ;--i,p0-=n+1,q0-=n+1,++mm){ + if(*(p0-1)!=0.){ + for(j=0,p=p0,h=1.; j=0 ;--i,++mm,p0-=n+1){ + if(*p0!=0.){ + for(j=0,p=p0+n; jt) t=s; + t*=1.e-15; n=100*m; nm=m; + for(j=0; m>1 && j0 ;--k){ + if(fabs(em[k-1])0.){ + c=sqrt((u+a)/(u+u)); + if(c!=0.) s/=(c*u); else s=1.; + for(i=k; ik){ + a=s*em[i]; b*=c; + em[i-1]=u=sqrt(x*x+a*a); + c=x/u; s=a/u; + } + a=c*y+s*b; b=c*b-s*y; + for(jj=0,p=vm+i; jj1){ sv=h=0.; + for(j=0,q=p,s=0.; j0.){ + h=sqrt(s); if(*p<0.) h= -h; + s+= *p*h; s=1./s; t=1./(w[0]+=h); + sv=1.+fabs(*p/h); + for(k=1,ms=n-i; k1){ + for(j=0,q=p1,s=0.; j0.){ + h=sqrt(s); if(*p1<0.) h= -h; + sv=1.+fabs(*p1/h); + s+= *p1*h; s=1./s; t=1./(*p1+=h); + for(k=n,ms=n*(m-i); k=0 ;--i,++mm,p0-=n+1,q0-=m+1){ + if(*p0!=0.){ + for(j=0,p=p0+n,h=1.; jt) t=s; + t*=1.e-15; n=100*m; nm=m; + for(j=0; m>1 && j0 ;--k){ + if(fabs(em[k-1])k){ + a=s*em[i]; b*=c; + em[i-1]=u=sqrt(x*x+a*a); + c=x/u; s=a/u; + } + a=c*y+s*b; b=c*b-s*y; + for(jj=0,p=vm+i; jj0){ + for(i=0,q=q0,p=pa; is){ s=t; lc=k;} + } + tq=tq>s?tq:s; if(s=0 ;--j){ --pa; pd-=n+1; + for(i=0,m=n-j-1,q=q0,p=pd+n; ij ;--k,ps-=n){ t= -(*ps); + for(i=j+1,p=ps,q=q0; ik){ t=0.; p=ps+j; i=j;} + else{ t=q0[j]; p=ps+k+1; i=k+1;} + for(; i=0 ;--j){ + for(k=0,p=a+j,q=a+ *(--le); k +#include + +namespace MathTools +{ + typedef int integer; + typedef double doublereal; + + extern "C" { + int dgeqp3_(integer const *m, + integer const *n, + doublereal *a, + integer const *lda, + integer *jpvt, + doublereal *tau, + doublereal *work, + integer *lwork, + integer *info); + } + + // the below functions are from ccmath + int svdu1v(double *d, double *a, int m, double *v, int n); + int svduv(double *d, double *a, double *u, int m, double *v, int n); + void ldumat(double *x, double *y, int i, int k); + void ldvmat(double *x, double *y, int k); + void atou1(double *r, int i, int j); + int qrbdu1(double *w, double *x, double *y, int k, double *z, int l); + int qrbdv(double *x, double *y, double *z, int i, double *w, int j); + int minv(double *a,int n); + void vmul(double *vp,double *mat,double *v,int n); + void mattr(double *a,double *b,int m,int n) ; + void skew_sym(double *A, double *a); + void mmul(double *c,double *a,double *b,int n); + void crossprod(double *out, const double *a, const double *b, unsigned int st); + void trnm(double *a,int n); +} +#endif