From e3c215549571a1379cd83543da5727ab9d399d9d Mon Sep 17 00:00:00 2001 From: astraw Date: Mon, 29 May 2006 22:54:46 +0000 Subject: [PATCH] initial import of v1.0 of toolbox git-svn-id: https://code.astraw.com/kookaburra/trunk/MultiCamSelfCal@1070 e6c0db7a-28da-0310-a2fc-f6ab65d9114a --- BlueCFindingPoints/findpointsBlueC | 55 ++ BlueCFindingPoints/im2pointsLocally.m | 144 ++++ BlueCFindingPoints/localconfig.m | 73 ++ CalTechCal/README.txt | 3 + CalTechCal/analyse_error.m | 157 +++++ CalTechCal/apply_distortion.m | 97 +++ CalTechCal/check_active_images.m | 3 + CalTechCal/comp_distortion_oulu.m | 48 ++ CalTechCal/comp_error_calib.m | 46 ++ CalTechCal/comp_ext_calib.m | 62 ++ CalTechCal/compute_extrinsic_init.m | 214 ++++++ CalTechCal/compute_extrinsic_refine.m | 114 ++++ CalTechCal/export_calib_data.m | 164 +++++ CalTechCal/extract_parameters.m | 61 ++ CalTechCal/ginput3.m | 216 ++++++ CalTechCal/go_calib_optim_iter.m | 629 ++++++++++++++++++ CalTechCal/gorad.m | 70 ++ CalTechCal/goradf.m | 82 +++ CalTechCal/normalize.m | 47 ++ CalTechCal/preparedata.m | 12 + CalTechCal/project_points2.m | 321 +++++++++ CalTechCal/quiver.m | 146 ++++ CalTechCal/rigid_motion.m | 66 ++ CalTechCal/rodrigues.m | 221 ++++++ CalTechCal/visualize_distortions.m | 217 ++++++ CommonCfgAndIO/configdata.m | 414 ++++++++++++ CommonCfgAndIO/expname.m | 35 + CommonCfgAndIO/loaddata.m | 104 +++ CommonCfgAndIO/oldexperiments.m | 136 ++++ MultiCamSelfCal/BlueCLocal/bluechoengg.m | 68 ++ MultiCamSelfCal/BlueCLocal/bluecrz.m | 64 ++ MultiCamSelfCal/CoreFunctions/align3d.m | 10 + .../CoreFunctions/estimateLambda.m | 57 ++ MultiCamSelfCal/CoreFunctions/estimatePX.m | 75 +++ MultiCamSelfCal/CoreFunctions/estsimt.m | 71 ++ MultiCamSelfCal/CoreFunctions/euclidize.m | 170 +++++ MultiCamSelfCal/CoreFunctions/findinl.m | 61 ++ MultiCamSelfCal/CoreFunctions/findoutl.m | 26 + MultiCamSelfCal/CoreFunctions/reprerror.m | 29 + MultiCamSelfCal/CoreFunctions/rq.m | 33 + MultiCamSelfCal/CoreFunctions/selfcalib.m | 81 +++ MultiCamSelfCal/FindingPoints/atlantic.pm | 4 + MultiCamSelfCal/FindingPoints/atlanticall.pm | 32 + MultiCamSelfCal/FindingPoints/getpoint.m | 322 +++++++++ MultiCamSelfCal/FindingPoints/im2imstat.m | 140 ++++ .../FindingPoints/im2pmultiproc.pl | 92 +++ MultiCamSelfCal/FindingPoints/im2points.m | 185 ++++++ MultiCamSelfCal/FindingPoints/mycorr2.m | 24 + MultiCamSelfCal/FindingPoints/virooms.pm | 16 + MultiCamSelfCal/FindingPoints/virooms4.pm | 8 + MultiCamSelfCal/LocalAlignments/erlangen.m | 61 ++ MultiCamSelfCal/LocalAlignments/g9.m | 52 ++ MultiCamSelfCal/LocalAlignments/joinoscars.m | 54 ++ MultiCamSelfCal/LocalAlignments/nfi2r.m | 37 ++ MultiCamSelfCal/LocalAlignments/planarcams.m | 55 ++ MultiCamSelfCal/LocalAlignments/planarmove.m | 53 ++ MultiCamSelfCal/LocalAlignments/planefit.m | 20 + .../MartinecPajdla/fill_mm/L2depths.m | 54 ++ MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m | 44 ++ .../MartinecPajdla/fill_mm/approximate.m | 119 ++++ .../MartinecPajdla/fill_mm/balance_triplets.m | 83 +++ .../MartinecPajdla/fill_mm/create_nullspace.m | 151 +++++ .../MartinecPajdla/fill_mm/depth_estimation.m | 39 ++ .../MartinecPajdla/fill_mm/fill_mm.m | 319 +++++++++ .../MartinecPajdla/fill_mm/fill_mm_sub.m | 53 ++ .../MartinecPajdla/fill_mm/fill_prmm.m | 101 +++ .../MartinecPajdla/fill_mm/normu.m | 20 + .../fill_mm/spread_depths_col.m | 19 + .../MartinecPajdla/fill_mm/subseq_longest.m | 34 + MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m | 73 ++ .../fill_mm_test/bundle_PX_proj.m | 358 ++++++++++ .../fill_mm_test/fill_mm_bundle.m | 46 ++ .../fill_mm_test/qPXbundle_cmp.m | 203 ++++++ MultiCamSelfCal/MartinecPajdla/setpaths.m | 9 + MultiCamSelfCal/MartinecPajdla/startup.m | 18 + MultiCamSelfCal/MartinecPajdla/test_.m | 7 + MultiCamSelfCal/MartinecPajdla/utils/comb.m | 8 + .../MartinecPajdla/utils/combfirst.m | 12 + .../MartinecPajdla/utils/combnext.m | 28 + .../MartinecPajdla/utils/diff_rand_ints.m | 23 + MultiCamSelfCal/MartinecPajdla/utils/dist.m | 22 + .../MartinecPajdla/utils/eucl_dist.m | 11 + .../MartinecPajdla/utils/eucl_dist_only.m | 32 + .../MartinecPajdla/utils/file_exists.m | 17 + MultiCamSelfCal/MartinecPajdla/utils/k2i.m | 20 + .../MartinecPajdla/utils/kill_spaces.m | 5 + .../MartinecPajdla/utils/load_scene.m | 226 +++++++ .../MartinecPajdla/utils/normalize.m | 33 + .../MartinecPajdla/utils/normalize_cut.m | 16 + MultiCamSelfCal/MartinecPajdla/utils/p2e.m | 15 + .../MartinecPajdla/utils/random_int.m | 7 + .../MartinecPajdla/utils/str_cut.m | 16 + .../MartinecPajdla/utils/tiles_set.m | 10 + .../OutputFunctions/dispcamstats.m | 13 + MultiCamSelfCal/OutputFunctions/drawcloud.m | 28 + MultiCamSelfCal/OutputFunctions/drawobject.m | 37 ++ MultiCamSelfCal/OutputFunctions/drawscene.m | 61 ++ .../OutputFunctions/evalreprerror.m | 53 ++ MultiCamSelfCal/OutputFunctions/savecalpar.m | 101 +++ MultiCamSelfCal/OutputFunctions/showimg.m | 30 + MultiCamSelfCal/README.txt | 435 ++++++++++++ MultiCamSelfCal/Ransac/FDs.m | 11 + MultiCamSelfCal/Ransac/fFDs.c | 45 ++ MultiCamSelfCal/Ransac/fFDs.mexglx | Bin 0 -> 6228 bytes MultiCamSelfCal/Ransac/fslcm.c | 68 ++ MultiCamSelfCal/Ransac/fslcm.mexglx | Bin 0 -> 6744 bytes MultiCamSelfCal/Ransac/fu2F7.m | 28 + MultiCamSelfCal/Ransac/lin_fm.c | 32 + MultiCamSelfCal/Ransac/lin_fm.mexglx | Bin 0 -> 6102 bytes MultiCamSelfCal/Ransac/mfFDs.m | 28 + MultiCamSelfCal/Ransac/normu.m | 19 + MultiCamSelfCal/Ransac/nsamples.m | 14 + MultiCamSelfCal/Ransac/p2e.m | 3 + MultiCamSelfCal/Ransac/rEG.m | 85 +++ MultiCamSelfCal/Ransac/rroots.c | 71 ++ MultiCamSelfCal/Ransac/rroots.mexglx | Bin 0 -> 6903 bytes MultiCamSelfCal/Ransac/seig.m | 5 + MultiCamSelfCal/Ransac/u2F.m | 42 ++ MultiCamSelfCal/changes.txt | 56 ++ MultiCamSelfCal/gocal.m | 348 ++++++++++ MultiCamSelfCal/showpoints.m | 111 ++++ MultiCamValidation/CoreFunctions/P2KRtC.m | 19 + MultiCamValidation/CoreFunctions/estimateX.m | 128 ++++ MultiCamValidation/CoreFunctions/findinl.m | 61 ++ MultiCamValidation/CoreFunctions/isptnorm.m | 43 ++ MultiCamValidation/CoreFunctions/rq.m | 33 + MultiCamValidation/CoreFunctions/uP2X.m | 39 ++ MultiCamValidation/CoreFunctions/workvolume.m | 65 ++ MultiCamValidation/FindingPoints/atlantic.pm | 4 + MultiCamValidation/FindingPoints/getpoint.m | 261 ++++++++ MultiCamValidation/FindingPoints/im2imstat.m | 140 ++++ .../FindingPoints/im2pmultiproc.pl | 92 +++ MultiCamValidation/FindingPoints/im2points.m | 159 +++++ MultiCamValidation/FindingPoints/mycorr2.m | 24 + MultiCamValidation/FindingPoints/virooms.pm | 16 + MultiCamValidation/FindingPoints/virooms4.pm | 8 + .../InputOutputFunctions/drawcloud.m | 28 + .../InputOutputFunctions/drawobject.m | 37 ++ .../InputOutputFunctions/drawscene.m | 61 ++ .../InputOutputFunctions/showimg.m | 30 + MultiCamValidation/Ransac/FDs.m | 11 + MultiCamValidation/Ransac/fFDs.c | 45 ++ MultiCamValidation/Ransac/fFDs.mexglx | Bin 0 -> 6228 bytes MultiCamValidation/Ransac/fslcm.c | 68 ++ MultiCamValidation/Ransac/fslcm.mexglx | Bin 0 -> 6744 bytes MultiCamValidation/Ransac/fu2F7.m | 33 + MultiCamValidation/Ransac/lin_fm.c | 32 + MultiCamValidation/Ransac/lin_fm.mexglx | Bin 0 -> 6102 bytes MultiCamValidation/Ransac/normu.m | 19 + MultiCamValidation/Ransac/nsamples.m | 14 + MultiCamValidation/Ransac/p2e.m | 3 + MultiCamValidation/Ransac/rEG.m | 83 +++ MultiCamValidation/Ransac/rroots.c | 71 ++ MultiCamValidation/Ransac/rroots.mexglx | Bin 0 -> 6903 bytes MultiCamValidation/Ransac/seig.m | 5 + MultiCamValidation/Ransac/u2F.m | 42 ++ MultiCamValidation/gorec.m | 173 +++++ RadialDistortions/comp_distortion_oulu.m | 48 ++ RadialDistortions/isptnorm.m | 43 ++ RadialDistortions/readradfile.m | 35 + RadialDistortions/undoheikk.m | 36 + RadialDistortions/undoradial.m | 33 + RansacM/Fsampson.m | 21 + RansacM/nsamples.m | 16 + RansacM/pointnormiso.m | 28 + RansacM/rEG.m | 75 +++ RansacM/u2Fdlt.m | 59 ++ 167 files changed, 12073 insertions(+) create mode 100755 BlueCFindingPoints/findpointsBlueC create mode 100644 BlueCFindingPoints/im2pointsLocally.m create mode 100644 BlueCFindingPoints/localconfig.m create mode 100644 CalTechCal/README.txt create mode 100644 CalTechCal/analyse_error.m create mode 100644 CalTechCal/apply_distortion.m create mode 100644 CalTechCal/check_active_images.m create mode 100644 CalTechCal/comp_distortion_oulu.m create mode 100644 CalTechCal/comp_error_calib.m create mode 100644 CalTechCal/comp_ext_calib.m create mode 100644 CalTechCal/compute_extrinsic_init.m create mode 100644 CalTechCal/compute_extrinsic_refine.m create mode 100644 CalTechCal/export_calib_data.m create mode 100644 CalTechCal/extract_parameters.m create mode 100644 CalTechCal/ginput3.m create mode 100644 CalTechCal/go_calib_optim_iter.m create mode 100644 CalTechCal/gorad.m create mode 100644 CalTechCal/goradf.m create mode 100644 CalTechCal/normalize.m create mode 100644 CalTechCal/preparedata.m create mode 100644 CalTechCal/project_points2.m create mode 100644 CalTechCal/quiver.m create mode 100644 CalTechCal/rigid_motion.m create mode 100644 CalTechCal/rodrigues.m create mode 100644 CalTechCal/visualize_distortions.m create mode 100644 CommonCfgAndIO/configdata.m create mode 100644 CommonCfgAndIO/expname.m create mode 100644 CommonCfgAndIO/loaddata.m create mode 100644 CommonCfgAndIO/oldexperiments.m create mode 100644 MultiCamSelfCal/BlueCLocal/bluechoengg.m create mode 100644 MultiCamSelfCal/BlueCLocal/bluecrz.m create mode 100644 MultiCamSelfCal/CoreFunctions/align3d.m create mode 100644 MultiCamSelfCal/CoreFunctions/estimateLambda.m create mode 100644 MultiCamSelfCal/CoreFunctions/estimatePX.m create mode 100644 MultiCamSelfCal/CoreFunctions/estsimt.m create mode 100644 MultiCamSelfCal/CoreFunctions/euclidize.m create mode 100644 MultiCamSelfCal/CoreFunctions/findinl.m create mode 100644 MultiCamSelfCal/CoreFunctions/findoutl.m create mode 100644 MultiCamSelfCal/CoreFunctions/reprerror.m create mode 100644 MultiCamSelfCal/CoreFunctions/rq.m create mode 100644 MultiCamSelfCal/CoreFunctions/selfcalib.m create mode 100644 MultiCamSelfCal/FindingPoints/atlantic.pm create mode 100644 MultiCamSelfCal/FindingPoints/atlanticall.pm create mode 100644 MultiCamSelfCal/FindingPoints/getpoint.m create mode 100644 MultiCamSelfCal/FindingPoints/im2imstat.m create mode 100755 MultiCamSelfCal/FindingPoints/im2pmultiproc.pl create mode 100644 MultiCamSelfCal/FindingPoints/im2points.m create mode 100644 MultiCamSelfCal/FindingPoints/mycorr2.m create mode 100644 MultiCamSelfCal/FindingPoints/virooms.pm create mode 100644 MultiCamSelfCal/FindingPoints/virooms4.pm create mode 100644 MultiCamSelfCal/LocalAlignments/erlangen.m create mode 100644 MultiCamSelfCal/LocalAlignments/g9.m create mode 100644 MultiCamSelfCal/LocalAlignments/joinoscars.m create mode 100644 MultiCamSelfCal/LocalAlignments/nfi2r.m create mode 100644 MultiCamSelfCal/LocalAlignments/planarcams.m create mode 100644 MultiCamSelfCal/LocalAlignments/planarmove.m create mode 100644 MultiCamSelfCal/LocalAlignments/planefit.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/L2depths.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/approximate.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/balance_triplets.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/create_nullspace.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/depth_estimation.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm_sub.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/fill_prmm.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/normu.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/spread_depths_col.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/subseq_longest.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm_test/bundle_PX_proj.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm_test/fill_mm_bundle.m create mode 100644 MultiCamSelfCal/MartinecPajdla/fill_mm_test/qPXbundle_cmp.m create mode 100644 MultiCamSelfCal/MartinecPajdla/setpaths.m create mode 100644 MultiCamSelfCal/MartinecPajdla/startup.m create mode 100644 MultiCamSelfCal/MartinecPajdla/test_.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/comb.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/combfirst.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/combnext.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/diff_rand_ints.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/dist.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/eucl_dist.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/eucl_dist_only.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/file_exists.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/k2i.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/kill_spaces.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/load_scene.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/normalize.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/normalize_cut.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/p2e.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/random_int.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/str_cut.m create mode 100644 MultiCamSelfCal/MartinecPajdla/utils/tiles_set.m create mode 100644 MultiCamSelfCal/OutputFunctions/dispcamstats.m create mode 100644 MultiCamSelfCal/OutputFunctions/drawcloud.m create mode 100644 MultiCamSelfCal/OutputFunctions/drawobject.m create mode 100644 MultiCamSelfCal/OutputFunctions/drawscene.m create mode 100644 MultiCamSelfCal/OutputFunctions/evalreprerror.m create mode 100644 MultiCamSelfCal/OutputFunctions/savecalpar.m create mode 100644 MultiCamSelfCal/OutputFunctions/showimg.m create mode 100644 MultiCamSelfCal/README.txt create mode 100644 MultiCamSelfCal/Ransac/FDs.m create mode 100644 MultiCamSelfCal/Ransac/fFDs.c create mode 100755 MultiCamSelfCal/Ransac/fFDs.mexglx create mode 100644 MultiCamSelfCal/Ransac/fslcm.c create mode 100755 MultiCamSelfCal/Ransac/fslcm.mexglx create mode 100644 MultiCamSelfCal/Ransac/fu2F7.m create mode 100644 MultiCamSelfCal/Ransac/lin_fm.c create mode 100755 MultiCamSelfCal/Ransac/lin_fm.mexglx create mode 100644 MultiCamSelfCal/Ransac/mfFDs.m create mode 100644 MultiCamSelfCal/Ransac/normu.m create mode 100644 MultiCamSelfCal/Ransac/nsamples.m create mode 100644 MultiCamSelfCal/Ransac/p2e.m create mode 100644 MultiCamSelfCal/Ransac/rEG.m create mode 100644 MultiCamSelfCal/Ransac/rroots.c create mode 100755 MultiCamSelfCal/Ransac/rroots.mexglx create mode 100644 MultiCamSelfCal/Ransac/seig.m create mode 100644 MultiCamSelfCal/Ransac/u2F.m create mode 100644 MultiCamSelfCal/changes.txt create mode 100644 MultiCamSelfCal/gocal.m create mode 100644 MultiCamSelfCal/showpoints.m create mode 100644 MultiCamValidation/CoreFunctions/P2KRtC.m create mode 100644 MultiCamValidation/CoreFunctions/estimateX.m create mode 100644 MultiCamValidation/CoreFunctions/findinl.m create mode 100644 MultiCamValidation/CoreFunctions/isptnorm.m create mode 100644 MultiCamValidation/CoreFunctions/rq.m create mode 100644 MultiCamValidation/CoreFunctions/uP2X.m create mode 100644 MultiCamValidation/CoreFunctions/workvolume.m create mode 100644 MultiCamValidation/FindingPoints/atlantic.pm create mode 100644 MultiCamValidation/FindingPoints/getpoint.m create mode 100644 MultiCamValidation/FindingPoints/im2imstat.m create mode 100755 MultiCamValidation/FindingPoints/im2pmultiproc.pl create mode 100644 MultiCamValidation/FindingPoints/im2points.m create mode 100644 MultiCamValidation/FindingPoints/mycorr2.m create mode 100644 MultiCamValidation/FindingPoints/virooms.pm create mode 100644 MultiCamValidation/FindingPoints/virooms4.pm create mode 100644 MultiCamValidation/InputOutputFunctions/drawcloud.m create mode 100644 MultiCamValidation/InputOutputFunctions/drawobject.m create mode 100644 MultiCamValidation/InputOutputFunctions/drawscene.m create mode 100644 MultiCamValidation/InputOutputFunctions/showimg.m create mode 100644 MultiCamValidation/Ransac/FDs.m create mode 100644 MultiCamValidation/Ransac/fFDs.c create mode 100755 MultiCamValidation/Ransac/fFDs.mexglx create mode 100644 MultiCamValidation/Ransac/fslcm.c create mode 100755 MultiCamValidation/Ransac/fslcm.mexglx create mode 100644 MultiCamValidation/Ransac/fu2F7.m create mode 100644 MultiCamValidation/Ransac/lin_fm.c create mode 100755 MultiCamValidation/Ransac/lin_fm.mexglx create mode 100644 MultiCamValidation/Ransac/normu.m create mode 100644 MultiCamValidation/Ransac/nsamples.m create mode 100644 MultiCamValidation/Ransac/p2e.m create mode 100644 MultiCamValidation/Ransac/rEG.m create mode 100644 MultiCamValidation/Ransac/rroots.c create mode 100755 MultiCamValidation/Ransac/rroots.mexglx create mode 100644 MultiCamValidation/Ransac/seig.m create mode 100644 MultiCamValidation/Ransac/u2F.m create mode 100644 MultiCamValidation/gorec.m create mode 100644 RadialDistortions/comp_distortion_oulu.m create mode 100644 RadialDistortions/isptnorm.m create mode 100644 RadialDistortions/readradfile.m create mode 100644 RadialDistortions/undoheikk.m create mode 100644 RadialDistortions/undoradial.m create mode 100644 RansacM/Fsampson.m create mode 100644 RansacM/nsamples.m create mode 100644 RansacM/pointnormiso.m create mode 100644 RansacM/rEG.m create mode 100644 RansacM/u2Fdlt.m diff --git a/BlueCFindingPoints/findpointsBlueC b/BlueCFindingPoints/findpointsBlueC new file mode 100755 index 0000000..fc97cd9 --- /dev/null +++ b/BlueCFindingPoints/findpointsBlueC @@ -0,0 +1,55 @@ +#!/bin/sh + +# convert the *PVI files from on a local discs and copy only the *.jpg files + +echo "Tool to process PVI sequences, Tomas Svoboda, ETHZ, 07/2002" + +# basepath for the selfcalib codes +selfcalib_basepath=/home/svoboda/Work/BlueCCal/BlueCFindingPoints + +####################################################### +### config definitions +### the variables should be specified in .cshrc +### or something like that +####################################################### +# basepath for binaries and auxiliary scripts +basepath=$BlueC_BASEPATH +# image basename +imname=$BlueC_IMNAME +# machine basename +mname=$BlueC_MNAME +workmachine=$BlueC_WORKMACHINE +# working directory on local machines +localdir=$BlueC_LOCALDIR +# for all available indexes +indexes=$BlueC_INDEXES + +# first clean the old local data +for i in $indexes; do + command="ssh ${mname}${i} rm -f ${localdir}/*.dat*" + echo $command + eval $command +done + +for i in $indexes; do + command="ssh ${mname}${i} rm -f ${localdir}/*.tiff" + echo $command + eval $command +done + +wait + +# call the local finding procedure +for i in $indexes; do + command="ssh ${mname}${i} /usr/sepp/bin/matlab -nosplash -nojvm < ${selfcalib_basepath}/im2pointsLocally.m > ${selfcalib_basepath}/im2pointsLocally.out.${i} &" + + echo $command + eval $command + sleep 1 +done + +wait + +# clean output files +rm im2pointsLocally.out.* + diff --git a/BlueCFindingPoints/im2pointsLocally.m b/BlueCFindingPoints/im2pointsLocally.m new file mode 100644 index 0000000..97c44e6 --- /dev/null +++ b/BlueCFindingPoints/im2pointsLocally.m @@ -0,0 +1,144 @@ +% im2pointsLocally computes image statistics from several images +% and finds the projections of laser points +% +% The version for local computation on each machine +% +% The computation core is taken from im2points.m +% computes average image and image of standard deviations +% requires configdata.m +% The name of the experiment has to be specified +% it determines the location of files etc ... +% +% the scripts serves as a template for a multiprocessing +% it assumes a vector of camera IDs CamIds to be known +% indexes in the CamIds are supposed to be correct +% + +donefile = '.done'; + +addpath /home/svoboda/Work/BlueCCal/BlueCFindingPoints + +% config = localconfig('BlueCHoengg') +config = localconfig('BlueCRZ') + +STEP4STAT = 5; % step for computing average and std images, if 1 then all images taken + +im.dir = config.paths.data; +im.ext = config.files.imgext; + +% get the information about machine +machinename = getenv('HOST'); +CamsIds = str2num(machinename(find(machinename>47 & machinename<58))); + +NoCams = size(CamsIds,2); +CamsIds + +% load image names +for i=1:NoCams, + seq(i).camId = CamsIds(i); + if seq(i).camId > -1 + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + [sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext] + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + i, seq + error('Not enough images found. Wrong image path or name pattern?'); + end +end + +% Expected number of 3D points is equal to the number of frames. +% In fact, some frames might be without any calibration point + +% Because of non-consistent stopping of capturing, the sequences might +% have different number of images, select the minimal value as the right one +NoPoints = min([seq.size]); + +% compute the average images that will be used for finding LEDs +% if not already computed + +pointsIdx = [1:STEP4STAT:NoPoints]; + +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.avIM,seq(i).camId)), + disp(sprintf('The average image of the camera %d is being computed',seq(i).camId)); + avIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + avIM = avIM + double(IM); + end + avIM = uint8(round(avIM./size(pointsIdx,2))); + imwrite(avIM,sprintf(config.files.avIM,seq(i).camId)); + else disp('Average files already exist'); + end +end +disp(sprintf('Elapsed time for average images: %d [sec]',cputime-t)) +% compute the standard deviations images that will be used for finding LEDs +% if not already computed +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.stdIM,seq(i).camId)), + avIM = double(imread(sprintf(config.files.avIM,seq(i).camId))); + disp(sprintf('The image of standard deviations of the camera %d is being computed',seq(i).camId)); + stdIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + stdIM = stdIM + (double(IM)-avIM).^2; + end + stdIM = uint8(round(sqrt(stdIM./(size(pointsIdx,2)-1)))); + imwrite(stdIM,sprintf(config.files.stdIM,seq(i).camId)); + else + disp('Image of standard deviations already exist') + end +end +disp(sprintf('Elapsed time for computation of images [sec]: %d',cputime-t)) + +% find points in the images +Ws = []; +IdWs = []; +Res = []; + +IdMat = ones(NoCams,NoPoints); +% IdMat is very important for Martinec&Pajdla filling [ECCV2002] +% it is a NoCams x NoPoints matrix, +% IdMat(i,j) = 0 -> no j-th point in i-th +% IdMat(i,j) = 1 -> point successfully detected + +for i=1:NoCams, + Points = []; + avIM = imread(sprintf(config.files.avIM,seq(i).camId)); + stdIM = imread(sprintf(config.files.stdIM,seq(i).camId)); + for j=1:NoPoints, + [pos,err] = getpoint([sprintf(im.dir,seq(i).camId),seq(i).data(j).name], 0, config.imgs, avIM, stdIM); + if err + IdMat(i,j) = 0; + Points = [Points, [NaN; NaN; NaN]]; + else + Points = [Points, [pos; 1]]; + end + end + Ws = [Ws; Points]; + Res= [Res; size(avIM,2), size(avIM,1)]; +end + +idx = '.'; +for i=CamsIds, + idx = sprintf('%s%02d',idx,i); +end + +save([config.files.points,idx], 'Ws','-ASCII') +% save(config.files.IdPoints,'IdWs','-ASCII') +save([config.files.Res,idx], 'Res', '-ASCII') +save([config.files.IdMat,idx], 'IdMat', '-ASCII') + +% write auxiliary file that is done +done=1; +save(donefile,'done','-ascii'); + +% exit the Matlab +% this script is to be used in the batch mode +% hence exit at the end is necessary +exit; diff --git a/BlueCFindingPoints/localconfig.m b/BlueCFindingPoints/localconfig.m new file mode 100644 index 0000000..a6cd13d --- /dev/null +++ b/BlueCFindingPoints/localconfig.m @@ -0,0 +1,73 @@ +% LocalConfig configuration file for self-calibration experiments +% +% +% config = configdata(experiment) +% +% experiment ... string with an experiment name + +function config = configdata(experiment) + +HOME_PATH = '/home/svoboda/Work/BlueCCal/'; + +% add paths +addpath([HOME_PATH,'MultiCamSelfCal/FindingPoints']); +addpath([HOME_PATH,'BlueCFindingPoints']); + + +if nargin<1, + display('No name of the experiment specified: >>basic<< used as default') + experiment = 'basic'; +end + +if strcmp(experiment,'basic') + error; +elseif strcmp(experiment,'BlueCHoengg') + config.paths.data = '/local/tomas/'; + config.paths.img = config.paths.data; + config.files.imnames = 'arctic%d.pvi.*.'; + config.files.idxcams = [1:16]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/5; +elseif strcmp(experiment,'BlueCRZ') + config.paths.data = '/local/tomas/'; + config.paths.img = config.paths.data; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:12,14:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/5; +else + error('Configdata: wrong identifier of the data set'); +end + + +% image resolution +try config.imgs.res; catch, config.imgs.res = [640,480]; end; + +% scale for the subpixel accuracy +% 1/3 is a good compromise between speed and accuracy +% for high-resolution images or bigger LEDs you may try 1/1 or 1/2 +try config.imgs.subpix; catch, config.imgs.subpix = 1/3; end; + +% data names +try config.files.Pmats; catch, config.files.Pmats = [config.paths.data,'Pmatrices.dat']; end; +try config.files.points; catch, config.files.points = [config.paths.data,'points.dat']; end; +try config.files.IdPoints; catch, config.files.IdPoints = [config.paths.data,'IdPoints.dat']; end; +try config.files.Res; catch, config.files.Res = [config.paths.data,'Res.dat']; end; +try config.files.IdMat; catch, config.files.IdMat = [config.paths.data,'IdMat.dat']; end; +try config.files.inidx; catch, config.files.inidx = [config.paths.data,'idxin.dat']; end; +try config.files.avIM; catch, config.files.avIM = [config.paths.data,'camera%d.average.tiff']; end; +try config.files.stdIM; catch, config.files.stdIM = [config.paths.data,'camera%d.std.tiff']; end; +try config.files.CalPar; catch, config.files.CalPar = [config.paths.data,'camera%d.cal']; end; +try config.files.CalPmat; catch, config.files.CalPmat = [config.paths.data,'camera%d.Pmat.cal']; end; +try config.files.StCalPar; catch, config.files.StCalPar = [config.paths.data,'atlantic%d.ethz.ch.cal']; end; + + + + + + + diff --git a/CalTechCal/README.txt b/CalTechCal/README.txt new file mode 100644 index 0000000..9421c32 --- /dev/null +++ b/CalTechCal/README.txt @@ -0,0 +1,3 @@ +Source codes by courtesy of Jean-Yves Bouguet at jean-yves.bouguet@intel.com + +http://www.vision.caltech.edu/bouguetj/calib_doc/ diff --git a/CalTechCal/analyse_error.m b/CalTechCal/analyse_error.m new file mode 100644 index 0000000..bff8abb --- /dev/null +++ b/CalTechCal/analyse_error.m @@ -0,0 +1,157 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if n_ima ~=0, +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + + +%if ~exist('no_grid'), +no_grid = 0; +%end; + +colors = 'brgkcm'; + + +figure(5); + +for kk = 1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if ~no_grid, + eval(['XX_kk = X_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + end; + + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + end; +end; + +hold off; +axis('equal'); +if 1, %~no_grid, + title('Reprojection error (in pixel) - To exit: right button'); +else + title('Reprojection error (in pixel)'); +end; +xlabel('x'); +ylabel('y'); + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + +if n_ima == 0, + + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); + +else + +err_std = std(ex')'; + +fprintf(1,'Pixel error: err = [ %3.5f %3.5f] (all active images)\n\n',err_std); + + +b = 1; + +while b==1, + + [xp,yp,b] = ginput3(1); + + if b==1, + ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2; + + [mind,indmin] = min(ddd); + + + done = 0; + kk_ima = 1; + while (~done)&(kk_ima<=n_ima), + %fprintf(1,'%d...',kk_ima); + eval(['ex_kk = ex_' num2str(kk_ima) ';']); + sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin))); + if isempty(sol_kk), + kk_ima = kk_ima + 1; + else + done = 1; + end; + end; + + eval(['x_kk = x_' num2str(kk_ima) ';']); + xpt = x_kk(:,sol_kk); + + if ~no_grid, + + eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']); + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + y1 = floor((sol_kk-1)./Nx); + x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx); + + y1 = (n_sq_y+1) - y1; + x1 = x1 + 1; + + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Pattern coordinates (in units of (dX,dY)): (X,Y)=(%d,%d)\n',[x1-1 y1-1]); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + else + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + end; + + + if exist(['wintx_' num2str(kk_ima)]), + + eval(['wintx = wintx_' num2str(kk_ima) ';']); + eval(['winty = winty_' num2str(kk_ima) ';']); + + fprintf(1,'Window size: (wintx,winty) = (%d,%d)\n',[wintx winty]); + end; + + + end; + +end; + +disp('done'); + +end; diff --git a/CalTechCal/apply_distortion.m b/CalTechCal/apply_distortion.m new file mode 100644 index 0000000..ac11ae0 --- /dev/null +++ b/CalTechCal/apply_distortion.m @@ -0,0 +1,97 @@ +function [xd,dxddk] = apply_distortion(x,k) + + +% Complete the distortion vector if you are using the simple distortion model: +length_k = length(k); +if length_k <5 , + k = [k ; zeros(5-length_k,1)]; +end; + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +r6 = r2.^3; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + dxd1dk = zeros(2*n,5); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdk = zeros(2*n,5); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; + if length_k < 5, + dxddk = dxddk(:,1:length_k); + end; +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +lk = 1; + +x = 10*randn(2,n); +k = 0.5*randn(lk,1); + +[xd,dxddk] = apply_distortion(x,k); + + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(lk,1); +k2 = k + dk; + +[x2] = apply_distortion(x,k2); + +x_pred = xd + reshape(dxddk * dk,2,n); + + +norm(x2-xd)/norm(x2 - x_pred) diff --git a/CalTechCal/check_active_images.m b/CalTechCal/check_active_images.m new file mode 100644 index 0000000..8641232 --- /dev/null +++ b/CalTechCal/check_active_images.m @@ -0,0 +1,3 @@ +ind_active = 1; +n_ima = 1; +active_images = 1; \ No newline at end of file diff --git a/CalTechCal/comp_distortion_oulu.m b/CalTechCal/comp_distortion_oulu.m new file mode 100644 index 0000000..c2253af --- /dev/null +++ b/CalTechCal/comp_distortion_oulu.m @@ -0,0 +1,48 @@ +function [x] = comp_distortion_oulu(xd,k); + +%comp_distortion_oulu.m +% +%[x] = comp_distortion_oulu(xd,k) +% +%Compensates for radial and tangential distortion. Model From Oulu university. +%For more informatino about the distortion model, check the forward projection mapping function: +%project_points.m +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + + +if length(k) == 1, + + [x] = comp_distortion(xd,k); + +else + + k1 = k(1); + k2 = k(2); + k3 = k(5); + p1 = k(3); + p2 = k(4); + + x = xd; % initial guess + + for kk=1:20, + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (xd - delta_x)./(ones(2,1)*k_radial); + + end; + +end; + + \ No newline at end of file diff --git a/CalTechCal/comp_error_calib.m b/CalTechCal/comp_error_calib.m new file mode 100644 index 0000000..b787f05 --- /dev/null +++ b/CalTechCal/comp_error_calib.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/CalTechCal/comp_ext_calib.m b/CalTechCal/comp_ext_calib.m new file mode 100644 index 0000000..e8aa30c --- /dev/null +++ b/CalTechCal/comp_ext_calib.m @@ -0,0 +1,62 @@ +%%% Computes the extrinsic parameters for all the active calibration images + +check_active_images; + +N_points_views = zeros(1,n_ima); + +for kk = 1:n_ima, + + if exist(['x_' num2str(kk)]), + + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + end; + if active_images(kk), + N_points_views(kk) = size(x_kk,2); + [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + end; + end; + if isnan(omckk(1,1)), + %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) + active_images(kk) = 0; + end; + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + + active_images(kk) = 0; + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +check_active_images; diff --git a/CalTechCal/compute_extrinsic_init.m b/CalTechCal/compute_extrinsic_init.m new file mode 100644 index 0000000..9ebbb74 --- /dev/null +++ b/CalTechCal/compute_extrinsic_init.m @@ -0,0 +1,214 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(5,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + + +%keyboard; + +% Compute the normalized coordinates: + +xn = normalize(x_kk,fc,cc,kc,alpha_c); + + + +Np = size(xn,2); + +%% Check for planarity of the structure: + +X_mean = mean(X_kk')'; + +Y = X_kk - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +%keyboard; + + +if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity + + %fprintf(1,'Planar structure detected: r=%f\n',r); + + % Transform the plane to bring it in the Z=0 plane: + + R_transform = V'; + + %norm(R_transform(1:2,3)) + + if norm(R_transform(1:2,3)) < 1e-6, + R_transform = eye(3); + end; + + if det(R_transform) < 0, R_transform = -R_transform; end; + + T_transform = -(R_transform)*X_mean; + + X_new = R_transform*X_kk + T_transform*ones(1,Np); + + + % Compute the planar homography: + + H = compute_homography(xn,X_new(1:2,:)); + + % De-embed the motion parameters from the homography: + + sc = mean([norm(H(:,1));norm(H(:,2))]); + + H = H/sc; + + % Extra normalization for some reasons... + %H(:,1) = H(:,1)/norm(H(:,1)); + %H(:,2) = H(:,2)/norm(H(:,2)); + + if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!! + + % Case#1: no opposite sign: + + omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk1 = rodrigues(omckk1); + Tckk1 = H(:,3); + + Hs1 = [Rckk1(:,1:2) Tckk1]; + xn1 = Hs1*[X_new(1:2,:);ones(1,Np)]; + xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)]; + e1 = xn1 - xn; + + % Case#2: opposite sign: + + omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk2 = rodrigues(omckk2); + Tckk2 = -H(:,3); + + Hs2 = [Rckk2(:,1:2) Tckk2]; + xn2 = Hs2*[X_new(1:2,:);ones(1,Np)]; + xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)]; + e2 = xn2 - xn; + + if 1, %norm(e1) < norm(e2), + omckk = omckk1; + Tckk = Tckk1; + Rckk = Rckk1; + else + omckk = omckk2; + Tckk = Tckk2; + Rckk = Rckk2; + end; + + else + + u1 = H(:,1); + u1 = u1 / norm(u1); + u2 = H(:,2) - dot(u1,H(:,2)) * u1; + u2 = u2 / norm(u2); + u3 = cross(u1,u2); + RRR = [u1 u2 u3]; + omckk = rodrigues(RRR); + + %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk = rodrigues(omckk); + Tckk = H(:,3); + + end; + + + + %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform + + Tckk = Tckk + Rckk* T_transform; + Rckk = Rckk * R_transform; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + + +else + + %fprintf(1,'Non planar structure detected: r=%f\n',r); + + % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!): + % The DLT method is applied here!! + + J = zeros(2*Np,12); + + xX = (ones(3,1)*xn(1,:)).*X_kk; + yX = (ones(3,1)*xn(2,:)).*X_kk; + + J(1:2:end,[1 4 7]) = -X_kk'; + J(2:2:end,[2 5 8]) = X_kk'; + J(1:2:end,[3 6 9]) = xX'; + J(2:2:end,[3 6 9]) = -yX'; + J(1:2:end,12) = xn(1,:)'; + J(2:2:end,12) = -xn(2,:)'; + J(1:2:end,10) = -ones(Np,1); + J(2:2:end,11) = ones(Np,1); + + JJ = J'*J; + [U,S,V] = svd(JJ); + + RR = reshape(V(1:9,12),3,3); + + if det(RR) < 0, + V(:,12) = -V(:,12); + RR = -RR; + end; + + [Ur,Sr,Vr] = svd(RR); + + Rckk = Ur*Vr'; + + sc = norm(V(1:9,12)) / norm(Rckk(:)); + Tckk = V(10:12,12)/sc; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + +end; diff --git a/CalTechCal/compute_extrinsic_refine.m b/CalTechCal/compute_extrinsic_refine.m new file mode 100644 index 0000000..2eba96a --- /dev/null +++ b/CalTechCal/compute_extrinsic_refine.m @@ -0,0 +1,114 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(5,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + ex = x_kk - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); diff --git a/CalTechCal/export_calib_data.m b/CalTechCal/export_calib_data.m new file mode 100644 index 0000000..096caf6 --- /dev/null +++ b/CalTechCal/export_calib_data.m @@ -0,0 +1,164 @@ +%% Export calibration data (corners + 3D coordinates) to +%% text files (in Willson-Heikkila's format or Zhang's format) + +%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion +%% of adding this export function to the toolbox. + + +if ~exist('n_ima'), + fprintf(1,'ERROR: No calibration data to export\n'); + +else + + check_active_images; + +% check_extracted_images; + + check_active_images; + + % fprintf(1,'Tool that exports calibration data to Willson-Heikkila, Zhang formats or blue-c formats\n'); + + qformat = 3; + + if 0 + while (qformat ~=0)&(qformat ~=1)&(qformat ~=2)&(qformat ~=3), + + fprintf(1,'Three possible formats of export: 0=Willson and Heikkila, 1=Zhang, 2=blue-c, complete, 3=blue-c, intrinsic\n') + qformat = input('Format of export (enter 0, 1, 2, or 3): '); + + if isempty(qformat) + qformat = -1; + end; + + if (qformat ~=0)&(qformat ~=1)&(qformat ~=2)&(qformat ~=3), + + fprintf(1,'Invalid entry. Try again.\n') + + end; + + end; + end + + if qformat == 0 + + fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n'); + outputfile = input('File basename: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Xx = [X_kk ; x_kk]'; + + file_name = [outputfile num2str(kk)]; + + disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']); + + eval(['save ' file_name ' Xx -ASCII']); + + end; + + elseif qformat == 1 + + fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n'); + modelfile = input('File basename for the 3D world coordinates: ','s'); + datafile = input('File basename for the 2D image coordinates: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + if ~exist(['n_sq_x_' num2str(kk)]), + n_sq_x = 1; + n_sq_y = size(X_kk,2); + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + end; + + X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1)); + + x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1)); + + disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']); + + eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']); + eval(['save ' datafile num2str(kk) '.txt xy -ASCII']); + end; + + elseif qformat == 2 + + fprintf(1,'\nExport of complete calibration data to blue-c configuration file\n'); + % outputfile = input('File basename: ', 's'); + configfile = [outputfile '.cal']; + disp(['Writing ' configfile]); + + fid = fopen(configfile, 'w'); + fprintf(fid, 'R11 = %.16f\n', Rc_ext(1,1)); + fprintf(fid, 'R12 = %.16f\n', Rc_ext(1,2)); + fprintf(fid, 'R13 = %.16f\n', Rc_ext(1,3)); + fprintf(fid, 'R21 = %.16f\n', Rc_ext(2,1)); + fprintf(fid, 'R22 = %.16f\n', Rc_ext(2,2)); + fprintf(fid, 'R23 = %.16f\n', Rc_ext(2,3)); + fprintf(fid, 'R31 = %.16f\n', Rc_ext(3,1)); + fprintf(fid, 'R32 = %.16f\n', Rc_ext(3,2)); + fprintf(fid, 'R33 = %.16f\n\n', Rc_ext(3,3)); + + fprintf(fid, 't11 = %.16f\n', Tc_ext(1,1)); + fprintf(fid, 't21 = %.16f\n', Tc_ext(2,1)); + fprintf(fid, 't31 = %.16f\n\n', Tc_ext(3,1)); + + fprintf(fid, 'K11 = %.16f\n', KK(1,1)); + fprintf(fid, 'K12 = %.16f\n', KK(1,2)); + fprintf(fid, 'K13 = %.16f\n', KK(1,3)); + fprintf(fid, 'K21 = %.16f\n', KK(2,1)); + fprintf(fid, 'K22 = %.16f\n', KK(2,2)); + fprintf(fid, 'K23 = %.16f\n', KK(2,3)); + fprintf(fid, 'K31 = %.16f\n', KK(3,1)); + fprintf(fid, 'K32 = %.16f\n', KK(3,2)); + fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); + + fprintf(fid, 'kc1 = %.16f\n', kc(1)); + fprintf(fid, 'kc2 = %.16f\n', kc(2)); + fprintf(fid, 'kc3 = %.16f\n', kc(3)); + fprintf(fid, 'kc4 = %.16f\n', kc(4)); + + status = fclose(fid); + + else + + fprintf(1,'\nExport of intrinsic calibration data to blue-c configuration file\n'); + % outputfile = input('File basename: ', 's'); + configfile = outputfile; + disp(['Writing ' configfile]); + + fid = fopen(configfile, 'w'); + + fprintf(fid, 'K11 = %.16f\n', KK(1,1)); + fprintf(fid, 'K12 = %.16f\n', KK(1,2)); + fprintf(fid, 'K13 = %.16f\n', KK(1,3)); + fprintf(fid, 'K21 = %.16f\n', KK(2,1)); + fprintf(fid, 'K22 = %.16f\n', KK(2,2)); + fprintf(fid, 'K23 = %.16f\n', KK(2,3)); + fprintf(fid, 'K31 = %.16f\n', KK(3,1)); + fprintf(fid, 'K32 = %.16f\n', KK(3,2)); + fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); + + fprintf(fid, 'kc1 = %.16f\n', kc(1)); + fprintf(fid, 'kc2 = %.16f\n', kc(2)); + fprintf(fid, 'kc3 = %.16f\n', kc(3)); + fprintf(fid, 'kc4 = %.16f\n', kc(4)); + + status = fclose(fid); + +end; + +fprintf(1,'done\n'); + +end; diff --git a/CalTechCal/extract_parameters.m b/CalTechCal/extract_parameters.m new file mode 100644 index 0000000..660faba --- /dev/null +++ b/CalTechCal/extract_parameters.m @@ -0,0 +1,61 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +if ~exist('solution_error') + solution_error = zeros(6*n_ima + 15,1); +end; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:10);%*** + +fc_error = solution_error(1:2); +cc_error = solution_error(3:4); +alpha_c_error = solution_error(5); +kc_error = solution_error(6:10); + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + + omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + Rckk = rodrigues(omckk); + + Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hkk = Hkk / Hkk(3,3); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + Hkk = NaN*ones(3,3); + omckk_error = NaN*ones(3,1); + Tckk_error = NaN*ones(3,1); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + eval(['H_' num2str(kk) '= Hkk;']); + eval(['omc_error_' num2str(kk) ' = omckk_error;']); + eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); + +end; diff --git a/CalTechCal/ginput3.m b/CalTechCal/ginput3.m new file mode 100644 index 0000000..026b5ad --- /dev/null +++ b/CalTechCal/ginput3.m @@ -0,0 +1,216 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 2.0 $ $Date: 2003/06/19 12:05:59 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + +pointer = get(gcf,'pointer'); + +set(gcf,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); +%set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/CalTechCal/go_calib_optim_iter.m b/CalTechCal/go_calib_optim_iter.m new file mode 100644 index 0000000..3983cbd --- /dev/null +++ b/CalTechCal/go_calib_optim_iter.m @@ -0,0 +1,629 @@ +%go_calib_optim_iter +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%%%%% +%% The following note is very important if you use different cameras!!! Then you should clear +%% the already estimated parameters +%%%%% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + +if ~exist('initFOV') + initFOV = 70; % Initial camera field of view in degrees % initially 35 +end + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if ~exist('recompute_extrinsic'), + recompute_extrinsic = 1 % Set this variable to 0 in case you do not want to recompute the extrinsic parameters + % at each iterstion. +end; + + +if ~exist('MaxIter'), + MaxIter = 50; % Maximum number of iterations in the gradient descent +end; + +% Leave it 1, otherways some serious problems appear. I do not why for the moment +if ~exist('check_cond'), + check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically +end; + +if ~exist('center_optim'), + center_optim = 0; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +if exist('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if ~exist('est_dist'), + est_dist = [1;1;0;0;0]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = double(~~center_optim); +est_alpha = double(~~est_alpha); +est_dist = double(~~est_dist); +est_fc = double(~~est_fc); +est_aspect_ratio = double(~~est_aspect_ratio); + + + +fprintf(1,'\n'); + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + % if is3D(X_kk), + rig3D = 1; + % end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5) & ~rig3D, + fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + %if ~dont_ask, + % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + % center_optim = isempty(quest); + %end; +end; + + +% A quick fix for solving conflict +if ~isequal(est_fc,[1;1]), + est_aspect_ratio=1; +end; +if ~est_aspect_ratio, + est_fc=[1;1]; +end; + + +if ~est_aspect_ratio, + fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n'); +else + if isequal(est_fc,[1;1]), + fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n'); + end; +end; + +if ~isequal(est_fc,[1;1]), + if isequal(est_fc,[1;0]), + fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n'); + else + if isequal(est_fc,[0;1]), + fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n'); + else + fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n'); + end; + end; +end; + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). '); + if ~exist('cc'), + fprintf(1,'It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + else + fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n'); + end; +else + fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n'); +end; + + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n'); + alpha_c = 0; +else + fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n'); +end; + + +if ~prod(double(est_dist)), + fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n'); + if ~est_dist(1), + fprintf(1,' Second order distortion not estimated (est_dist(1)=0).\n'); + end; + if ~est_dist(2), + fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0).\n'); + end; + if ~est_dist(5), + fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n'); + end; + if ~prod(double(est_dist(3:4))), + fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n'); + end; +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + % if is3D(X_kk), + rig3D = 1; + % end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 1; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.4; % slow convergence +end; + + +if exist('kc'), + if length(kc) == 4; + fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree'); + kc = [kc;0]; + end; +end; + + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(5,1); + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('fc')& quick_init, + FOV_angle = initFOV; % Initial camera field of view in degrees % initially 35 + fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']); + fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.4; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n') + init_intrinsic_param; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.4; % slow convergence + est_fc = [1;1]; +end; + + +if ~est_aspect_ratio, + fc(1) = (fc(1)+fc(2))/2; + fc(2) = fc(1); +end; + +if ~prod(double(est_dist)), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + +if ~prod(double(est_fc)), + fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n'); +end; + + +%%% Initialization of the extrinsic parameters for global minimization: +comp_ext_calib; + + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(5,1)]; + +for kk = 1:n_ima, + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + init_param = [init_param; omckk ; Tckk]; +end; + + + +%-------------------- Main Optimization: + +fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active)); + + +param = init_param; +change = 1; + +iter = 0; + +fprintf(1,'Gradient descent iterations: '); + +param_list = param; + + +while (change > 1e-9)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:10); + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + %if active_images(kk), + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + ex3(1:10) = ex3(1:10) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + % Check if this view is ill-conditioned: + if check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + %end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ3 = JJ3(ind_Jac,ind_Jac); + ex3 = ex3(ind_Jac); + + JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex3; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + alpha_current = param(5); + kc_current = param(6:10); + + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + + %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!! + %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes) + %% The complete gradient descent method is useful to precisely update the intrinsic parameters. + + + if recompute_extrinsic, + MaxIter2 = 20; + for kk =ind_active, %1:n_ima, + %if active_images(kk), + omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk); + desactivated_images = [desactivated_images kk]; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + end; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk; + %end; + end; + end; + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:10); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); + +JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[fc;fc_error]); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ± [ %3.5f %3.5f ]\n',[cc;cc_error]); +fprintf(1,'Skew: alpha_c = [ %3.5f ] ± [ %3.5f ] => angle of pixel axes = %3.5f ± %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ± [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc;kc_error]); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); +fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') +%fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n') + + + +%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode. + +alpha_c_min = alpha_c - alpha_c_error/2; +alpha_c_max = alpha_c + alpha_c_error/2; + +if (alpha_c_min < 0) & (alpha_c_max > 0), + fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n'); + fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n'); +end; + +kc_min = kc - kc_error/2; +kc_max = kc + kc_error/2; + +prob_kc = (kc_min < 0) & (kc_max > 0); + +if ~(prob_kc(3) & prob_kc(4)) + prob_kc(3:4) = [0;0]; +end; + + +if sum(prob_kc), + fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n'); + fprintf(1,' To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc); +end; + + +return; diff --git a/CalTechCal/gorad.m b/CalTechCal/gorad.m new file mode 100644 index 0000000..2ff3e52 --- /dev/null +++ b/CalTechCal/gorad.m @@ -0,0 +1,70 @@ +% main script to launch the estimation +% of the non-linear parameters by using the CalTech +% calibration toolbox and the output from the Svoboda's +% Multicamera self-calibration +% +% How to create the input data: +% 1) Run the MultiCamSelfCam +% 2) Run the MultiCamValidation +% +% $Id: gorad.m,v 2.0 2003/06/19 12:06:00 svoboda Exp $ + +clear all; + +addpath ../MultiCamSelfCalib/Cfg +config = configdata(expname); + +% if problem with desactivated images -> some problems with the estimation in general +desactivated_images = []; + +idxcams = config.cal.cams2use; +selfcalib.goradproblem = 0; + +for i = idxcams, + [X_1,x_1] = preparedata(sprintf(config.files.points4cal,i)); + go_calib_optim_iter + if any(isnan(param)) + % when the iteration fails insert null distortion + % it is better than nonsense + kc(1:4) = [0,0,0,0]; + selfcalib.goradproblem=1; + else + visualize_distortions + end + + disp(sprintf('***** camera %d **********************************',i)) + % + outputfile = sprintf(config.files.rad,i); + + fprintf(1,'\nExport of intrinsic calibration data to blue-c configuration file\n'); + % outputfile = input('File basename: ', 's'); + configfile = outputfile; + disp(['Writing ' configfile]); + + fid = fopen(configfile, 'w'); + + fprintf(fid, 'K11 = %.16f\n', KK(1,1)); + fprintf(fid, 'K12 = %.16f\n', KK(1,2)); + fprintf(fid, 'K13 = %.16f\n', KK(1,3)); + fprintf(fid, 'K21 = %.16f\n', KK(2,1)); + fprintf(fid, 'K22 = %.16f\n', KK(2,2)); + fprintf(fid, 'K23 = %.16f\n', KK(2,3)); + fprintf(fid, 'K31 = %.16f\n', KK(3,1)); + fprintf(fid, 'K32 = %.16f\n', KK(3,2)); + fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); + + fprintf(fid, 'kc1 = %.16f\n', kc(1)); + fprintf(fid, 'kc2 = %.16f\n', kc(2)); + fprintf(fid, 'kc3 = %.16f\n', kc(3)); + fprintf(fid, 'kc4 = %.16f\n', kc(4)); + + status = fclose(fid); + +disp('Press any key to continue'), pause + +%%% +% clear already estimated parameters +clear fc kc alpha_c cc +end + + diff --git a/CalTechCal/goradf.m b/CalTechCal/goradf.m new file mode 100644 index 0000000..daff66e --- /dev/null +++ b/CalTechCal/goradf.m @@ -0,0 +1,82 @@ +% main function to launch the estimation +% of the non-linear parameters by using the CalTech +% calibration toolbox and the output from the Svoboda's +% Multicamera self-calibration +% +% How to create the input data: +% 1) Run the MultiCamSelfCam +% 2) Run the MultiCamValidation +% +% $Id: goradf.m,v 2.2 2003/07/30 10:32:22 svoboda Exp $ + +function selfcalib = goradf(config,par2estimate,INL_TOL) + +% assignment of the parameters to estimate +initFOV = par2estimate(1); +center_optim = par2estimate(2); +est_dist = par2estimate(3:6)'; + +% if problem with desactivated images -> some problems with the estimation in general +desactivated_images = []; + +idxcams = config.cal.cams2use; +selfcalib.goradproblem = 0; + +count = 0; + +for i = idxcams, + count = count+1; + [X_1,x_1] = preparedata(sprintf(config.files.points4cal,i)); + % handle image resolutions correctly + nx = config.cal.Res(count,1); + ny = config.cal.Res(count,2); + go_calib_optim_iter + if any(isnan(param)) | any(err_std > 2*INL_TOL) + % when the iteration fails insert null distortion + % it is better than nonsense + KK = [700 0 320; 0 700 240; 0 0 1]; % void calibration matrix + kc(1:4) = [0,0,0,0]; + selfcalib.goradproblem=1; + else + visualize_distortions + figure(2), + eval(['print -depsc ', config.paths.data, sprintf('NonLinModel.cam%d.eps',i)]) + end + % + disp(sprintf('***** camera %d **********************************',i)) + % + outputfile = sprintf(config.files.rad,i); + fprintf(1,'\nExport of intrinsic calibration data to blue-c configuration file\n'); + % outputfile = input('File basename: ', 's'); + configfile = outputfile; + disp(['Writing ' configfile]); + + fid = fopen(configfile, 'w'); + + fprintf(fid, 'K11 = %.16f\n', KK(1,1)); + fprintf(fid, 'K12 = %.16f\n', KK(1,2)); + fprintf(fid, 'K13 = %.16f\n', KK(1,3)); + fprintf(fid, 'K21 = %.16f\n', KK(2,1)); + fprintf(fid, 'K22 = %.16f\n', KK(2,2)); + fprintf(fid, 'K23 = %.16f\n', KK(2,3)); + fprintf(fid, 'K31 = %.16f\n', KK(3,1)); + fprintf(fid, 'K32 = %.16f\n', KK(3,2)); + fprintf(fid, 'K33 = %.16f\n\n', KK(3,3)); + + fprintf(fid, 'kc1 = %.16f\n', kc(1)); + fprintf(fid, 'kc2 = %.16f\n', kc(2)); + fprintf(fid, 'kc3 = %.16f\n', kc(3)); + fprintf(fid, 'kc4 = %.16f\n', kc(4)); + + status = fclose(fid); + + % disp('Press any key to continue'), pause + + %%% + % clear already estimated parameters + clear fc kc alpha_c cc nx ny + +end + +return + diff --git a/CalTechCal/normalize.m b/CalTechCal/normalize.m new file mode 100644 index 0000000..4d049fb --- /dev/null +++ b/CalTechCal/normalize.m @@ -0,0 +1,47 @@ +function [xn] = normalize(x_kk,fc,cc,kc,alpha_c), + +%normalize +% +%[xn] = normalize(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: +% +%comp_distortion_oulu: undistort pixel coordinates. + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +if norm(kc) ~= 0, + % Third: Compensate for lens distortion: + xn = comp_distortion_oulu(x_distort,kc); +else + xn = x_distort; +end; diff --git a/CalTechCal/preparedata.m b/CalTechCal/preparedata.m new file mode 100644 index 0000000..6ec953a --- /dev/null +++ b/CalTechCal/preparedata.m @@ -0,0 +1,12 @@ +% preparedata +% +% reads BlueC *.rad file and transform data +% for the CalTech Camera calibration toolbox + +function [X,x] = preparedata(filename); + +datamat = load(filename,'-ASCII'); +X = datamat(:,1:3)'; % 3D points +x = datamat(:,5:6)'; % 2D projections + +return; diff --git a/CalTechCal/project_points2.m b/CalTechCal/project_points2.m new file mode 100644 index 0000000..bb28f1d --- /dev/null +++ b/CalTechCal/project_points2.m @@ -0,0 +1,321 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha) + +%project_points.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(5,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + +dxdom = zeros(2*n,3); +dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); +dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + +dxdT = zeros(2*n,3); +dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); +dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +r6 = r2.^3; + +dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; +dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; +dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; +dcdistdk = [ r2' r4' zeros(n,2) r6']; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,5); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +%ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,5); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +dxd3dom = zeros(2*n,3); +dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); +dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); +dxd3dT = zeros(2*n,3); +dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); +dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); +dxd3dk = zeros(2*n,5); +dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); +dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); +dxd3dalpha = zeros(2*n,1); +dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,5)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; +else + xp = f * xd3 + c*ones(1,n); + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha); + + +% Test on om: OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points2(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points2(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points2(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points2(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = project_points2(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points2(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/CalTechCal/quiver.m b/CalTechCal/quiver.m new file mode 100644 index 0000000..c20a63b --- /dev/null +++ b/CalTechCal/quiver.m @@ -0,0 +1,146 @@ +function hh = quiver(varargin) +%QUIVER Quiver plot. +% QUIVER(X,Y,U,V) plots velocity vectors as arrows with components (u,v) +% at the points (x,y). The matrices X,Y,U,V must all be the same size +% and contain corresponding position and velocity components (X and Y +% can also be vectors to specify a uniform grid). QUIVER automatically +% scales the arrows to fit within the grid. +% +% QUIVER(U,V) plots velocity vectors at equally spaced points in +% the x-y plane. +% +% QUIVER(U,V,S) or QUIVER(X,Y,U,V,S) automatically scales the +% arrows to fit within the grid and then stretches them by S. Use +% S=0 to plot the arrows without the automatic scaling. +% +% QUIVER(...,LINESPEC) uses the plot linestyle specified for +% the velocity vectors. Any marker in LINESPEC is drawn at the base +% instead of an arrow on the tip. Use a marker of '.' to specify +% no marker at all. See PLOT for other possibilities. +% +% QUIVER(...,'filled') fills any markers specified. +% +% H = QUIVER(...) returns a vector of line handles. +% +% Example: +% [x,y] = meshgrid(-2:.2:2,-1:.15:1); +% z = x .* exp(-x.^2 - y.^2); [px,py] = gradient(z,.2,.15); +% contour(x,y,z), hold on +% quiver(x,y,px,py), hold off, axis image +% +% See also FEATHER, QUIVER3, PLOT. + +% Clay M. Thompson 3-3-94 +% Copyright 1984-2002 The MathWorks, Inc. +% $Revision: 2.1 $ $Date: 2005/05/23 16:20:12 $ + +% Arrow head parameters +alpha = 0.33; % Size of arrow head relative to the length of the vector +beta = 0.33; % Width of the base of the arrow head relative to the length +autoscale = 1; % Autoscale if ~= 0 then scale by this. +plotarrows = 1; % Plot arrows +sym = ''; + +filled = 0; +ls = '-'; +ms = ''; +col = ''; + +nin = nargin; +% Parse the string inputs +while isstr(varargin{nin}), + vv = varargin{nin}; + if ~isempty(vv) & strcmp(lower(vv(1)),'f') + filled = 1; + nin = nin-1; + else + [l,c,m,msg] = colstyle(vv); + if ~isempty(msg), + error(sprintf('Unknown option "%s".',vv)); + end + if ~isempty(l), ls = l; end + if ~isempty(c), col = c; end + if ~isempty(m), ms = m; plotarrows = 0; end + if isequal(m,'.'), ms = ''; end % Don't plot '.' + nin = nin-1; + end +end + +error(nargchk(2,5,nin)); + +% Check numeric input arguments +if nin<4, % quiver(u,v) or quiver(u,v,s) + [msg,x,y,u,v] = xyzchk(varargin{1:2}); +else + [msg,x,y,u,v] = xyzchk(varargin{1:4}); +end +if ~isempty(msg), error(msg); end + +if nin==3 | nin==5, % quiver(u,v,s) or quiver(x,y,u,v,s) + autoscale = varargin{nin}; +end + +% Scalar expand u,v +if prod(size(u))==1, u = u(ones(size(x))); end +if prod(size(v))==1, v = v(ones(size(u))); end + +if autoscale, + % Base autoscale value on average spacing in the x and y + % directions. Estimate number of points in each direction as + % either the size of the input arrays or the effective square + % spacing if x and y are vectors. + if min(size(x))==1, n=sqrt(prod(size(x))); m=n; else [m,n]=size(x); end + delx = diff([min(x(:)) max(x(:))])/n; + dely = diff([min(y(:)) max(y(:))])/m; + del = delx.^2 + dely.^2; + if del>0 + len = sqrt((u.^2 + v.^2)/del); + maxlen = max(len(:)); + else + maxlen = 0; + end + + if maxlen>0 + autoscale = autoscale*0.9 / maxlen; + else + autoscale = autoscale*0.9; + end + u = u*autoscale; v = v*autoscale; +end + +ax = newplot; +next = lower(get(ax,'NextPlot')); +hold_state = ishold; + +% Make velocity vectors +x = x(:).'; y = y(:).'; +u = u(:).'; v = v(:).'; +uu = [x;x+u;repmat(NaN,size(u))]; +vv = [y;y+v;repmat(NaN,size(u))]; + +h1 = plot(uu(:),vv(:),[col ls]); + +if plotarrows, + % Make arrow heads and plot them + hu = [x+u-alpha*(u+beta*(v+eps));x+u; ... + x+u-alpha*(u-beta*(v+eps));repmat(NaN,size(u))]; + hv = [y+v-alpha*(v-beta*(u+eps));y+v; ... + y+v-alpha*(v+beta*(u+eps));repmat(NaN,size(v))]; + hold on + h2 = plot(hu(:),hv(:),[col ls]); +else + h2 = []; +end + +if ~isempty(ms), % Plot marker on base + hu = x; hv = y; + hold on + h3 = plot(hu(:),hv(:),[col ms]); + if filled, set(h3,'markerfacecolor',get(h1,'color')); end +else + h3 = []; +end + +if ~hold_state, hold off, view(2); set(ax,'NextPlot',next); end + +if nargout>0, hh = [h1;h2;h3]; end diff --git a/CalTechCal/rigid_motion.m b/CalTechCal/rigid_motion.m new file mode 100644 index 0000000..02904cd --- /dev/null +++ b/CalTechCal/rigid_motion.m @@ -0,0 +1,66 @@ +function [Y,dYdom,dYdT] = rigid_motion(X,om,T); + +%rigid_motion.m +% +%[Y,dYdom,dYdT] = rigid_motion(X,om,T) +% +%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% +%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) +% dYdom: Derivative of Y with respect to om ((3N)x3 matrix) +% dYdT: Derivative of Y with respect to T ((3N)x3 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Y = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector + + + +if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure as input (in rigid_motion.m)'); + return; + end; + end; +end; + + +[R,dRdom] = rodrigues(om); + +[m,n] = size(X); + +Y = R*X + repmat(T,[1 n]); + +if nargout > 1, + + +dYdR = zeros(3*n,9); +dYdT = zeros(3*n,3); + +dYdR(1:3:end,1:3:end) = X'; +dYdR(2:3:end,2:3:end) = X'; +dYdR(3:3:end,3:3:end) = X'; + +dYdT(1:3:end,1) = ones(n,1); +dYdT(2:3:end,2) = ones(n,1); +dYdT(3:3:end,3) = ones(n,1); + +dYdom = dYdR * dRdom; + +end; + + + + diff --git a/CalTechCal/rodrigues.m b/CalTechCal/rodrigues.m new file mode 100644 index 0000000..0f724ef --- /dev/null +++ b/CalTechCal/rodrigues.m @@ -0,0 +1,221 @@ +function [out,dout]=rodrigues(in) + +% RODRIGUES Transform rotation matrix into rotation vector and viceversa. +% +% Sintax: [OUT]=RODRIGUES(IN) +% If IN is a 3x3 rotation matrix then OUT is the +% corresponding 3x1 rotation vector +% if IN is a rotation 3-vector then OUT is the +% corresponding 3x3 rotation matrix +% + +%% +%% Copyright (c) March 1993 -- Pietro Perona +%% California Institute of Technology +%% + +%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995. +%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !! + +%% BUG when norm(om)=pi fixed -- April 6th, 1997; +%% Jean-Yves Bouguet + +%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003; +%% Jean-Yves Bouguet + +[m,n] = size(in); +%bigeps = 10e+4*eps; +bigeps = 10e+20*eps; + +if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector + theta = norm(in); + if theta < eps + R = eye(3); + + %if nargout > 1, + + dRdin = [0 0 0; + 0 0 1; + 0 -1 0; + 0 0 -1; + 0 0 0; + 1 0 0; + 0 1 0; + -1 0 0; + 0 0 0]; + + %end; + + else + if n==length(in) in=in'; end; %% make it a column vec. if necess. + + %m3 = [in,theta] + + dm3din = [eye(3);in'/theta]; + + omega = in/theta; + + %m2 = [omega;theta] + + dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; + + alpha = cos(theta); + beta = sin(theta); + gamma = 1-cos(theta); + omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; + A = omega*omega'; + + %m1 = [alpha;beta;gamma;omegav;A]; + + dm1dm2 = zeros(21,4); + dm1dm2(1,4) = -sin(theta); + dm1dm2(2,4) = cos(theta); + dm1dm2(3,4) = sin(theta); + dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]'; + + w1 = omega(1); + w2 = omega(2); + w3 = omega(3); + + dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; + dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; + dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; + + R = eye(3)*alpha + omegav*beta + A*gamma; + + dRdm1 = zeros(9,21); + + dRdm1([1 5 9],1) = ones(3,1); + dRdm1(:,2) = omegav(:); + dRdm1(:,4:12) = beta*eye(9); + dRdm1(:,3) = A(:); + dRdm1(:,13:21) = gamma*eye(9); + + dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; + + + end; + out = R; + dout = dRdin; + + %% it is prob. a rot matr. + elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... + & (abs(det(in)-1) < bigeps)) + R = in; + + % project the rotation matrix to SO(3); + [U,S,V] = svd(R); + R = U*V'; + + tr = (trace(R)-1)/2; + dtrdR = [1 0 0 0 1 0 0 0 1]/2; + theta = real(acos(tr)); + + + if sin(theta) >= 1e-5, + + dthetadtr = -1/sqrt(1-tr^2); + + dthetadR = dthetadtr * dtrdR; + % var1 = [vth;theta]; + vth = 1/(2*sin(theta)); + dvthdtheta = -vth*cos(theta)/sin(theta); + dvar1dtheta = [dvthdtheta;1]; + + dvar1dR = dvar1dtheta * dthetadR; + + + om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; + + dom1dR = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]; + + % var = [om1;vth;theta]; + dvardR = [dom1dR;dvar1dR]; + + % var2 = [om;theta]; + om = vth*om1; + domdvar = [vth*eye(3) om1 zeros(3,1)]; + dthetadvar = [0 0 0 0 1]; + dvar2dvar = [domdvar;dthetadvar]; + + + out = om*theta; + domegadvar2 = [theta*eye(3) om]; + + dout = domegadvar2 * dvar2dvar * dvardR; + + + else + if tr > 0; % case norm(om)=0; + + out = [0 0 0]'; + + dout = [0 0 0 0 0 1/2 0 -1/2 0; + 0 0 -1/2 0 0 0 1/2 0 0; + 0 1/2 0 -1/2 0 0 0 0 0]; + else % case norm(om)=pi; %% fixed April 6th + + + out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); + %keyboard; + + if nargout > 1, + fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); + dout = NaN*ones(3,9); + end; + end; + end; + + else + error('Neither a rotation matrix nor a rotation vector were provided'); + end; + +return; + +%% test of the Jacobians: + +%%%% TEST OF dRdom: +om = randn(3,1); +dom = randn(3,1)/1000000; + +[R1,dR1] = rodrigues(om); +R2 = rodrigues(om+dom); + +R2a = R1 + reshape(dR1 * dom,3,3); + +gain = norm(R2 - R1)/norm(R2 - R2a) + +%%% TEST OF dOmdR: +om = randn(3,1); +R = rodrigues(om); +dom = randn(3,1)/10000; +dR = rodrigues(om+dom) - R; + +[omc,domdR] = rodrigues(R); +[om2] = rodrigues(R+dR); + +om_app = omc + domdR*dR(:); + +gain = norm(om2 - omc)/norm(om2 - om_app) + + +%%% OTHER BUG: (FIXED NOW!!!) + +omu = randn(3,1); +omu = omu/norm(omu) +om = pi*omu; +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +%%% NORMAL OPERATION + +om = randn(3,1); +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + diff --git a/CalTechCal/visualize_distortions.m b/CalTechCal/visualize_distortions.m new file mode 100644 index 0000000..75914a2 --- /dev/null +++ b/CalTechCal/visualize_distortions.m @@ -0,0 +1,217 @@ +% visualize_distortions +% +% +% A script to run in conjunction with calib_gui in TOOLBOX_calib to plot +% the distortion models. +% +% This is a slightly modified version of the script plot_CCT_distortion.m written by Mr. Oshel +% Thank you Mr. Oshel for your contribution! + + +[mx,my] = meshgrid(0:nx/20:(nx-1),0:ny/20:(ny-1)); +[nnx,nny]=size(mx); +px=reshape(mx',nnx*nny,1); +py=reshape(my',nnx*nny,1); +kk_new=[fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2);0 0 1]; +rays=inv(kk_new)*[px';py';ones(1,length(px))]; +x=[rays(1,:)./rays(3,:);rays(2,:)./rays(3,:)]; + + +title2=strcat('Complete Distortion Model'); + +fh1 = 2; + +%if ishandle(fh1), +% close(fh1); +%end; +figure(fh1); clf; +xd=apply_distortion(x,kc); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2-px'; +dy=py2-py'; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh1,'color',[1,1,1]); + +hold off; + + + + + +title2=strcat('Tangential Component of the Distortion Model'); + +fh2 = 3; + +%if ishandle(fh2), +% close(fh2); +%end; +figure(fh2); clf; +xd=apply_distortion(x,[0 0 kc(3) kc(4) 0]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2-px'; +dy=py2-py'; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +% clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh2,'color',[1,1,1]); + +hold off; + + + + + + + + +title2=strcat('Radial Component of the Distortion Model'); + +fh3 = 4; + +%if ishandle(fh3), +% close(fh3); +%end; +figure(fh3); clf; +xd=apply_distortion(x,[kc(1) kc(2) 0 0 kc(5)]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2-px'; +dy=py2-py'; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh3,'color',[1,1,1]); + +hold off; + +figure(fh1); diff --git a/CommonCfgAndIO/configdata.m b/CommonCfgAndIO/configdata.m new file mode 100644 index 0000000..65c7516 --- /dev/null +++ b/CommonCfgAndIO/configdata.m @@ -0,0 +1,414 @@ +% Config configuration file for self-calibration experiments +% +% config = configdata(experiment) +% +% experiment ... string with an experiment name + +% $Author: svoboda $ +% $Revision: 2.9 $ +% $Id: configdata.m,v 2.9 2005/05/23 16:23:35 svoboda Exp $ +% $State: Exp $ + +function config = configdata(experiment) + +if nargin<1, + display('No name of the experiment specified: >>basic<< used as default') + experiment = 'basic'; +end + +if strcmp(experiment,'basic') + error; +elseif strcmp(experiment,'G9') + config.paths.data = ['/local/MultiCam/Data/CalibrationG9_20050502/']; + config.files.basename = 'cam'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = ['img0%d','_*.']; + % config.files.idxcams = [1:11,13:16]; % related to the imnames + config.files.idxcams = [1:4]; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'red'; % color of the laser pointer + config.imgs.LEDthr = 30; + config.imgs.subpix = 1/3; + config.cal.nonlinpar = [50,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,0,1,0,0,0]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_THR = 0.5; + config.cal.GLOBAL_ITER_MAX = 5; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; % CalTech (BlueC compatible) + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; +% config.cal.cams2use = [1:11,13:16]; % if use of some cams only is required +elseif strcmp(experiment,'2410ViRoom') + config.paths.data = ['/home/svoboda/viroomData/ViRoom/Calib/20021024Calib/']; + config.files.basename = 'vr'; + config.paths.img = config.paths.data; + config.files.imnames = 'vr%0.2d_image.*.'; + config.files.idxcams = [0:3]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'intensity'; % color of the laser pointer + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 1.5; + config.cal.NL_UPDATE = [1,0,1,0,0,0]; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.DO_BA = 1; + config.cal.START_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.NUM_CAMS_FILL = 1; +elseif strcmp(experiment,'oscar2c1p') + config.paths.data = ['/home/svoboda/viroomData/oscar/oscar_2c1p/']; + config.paths.img = [config.paths.data,'cam%d/']; + config.files.imnames = 'oscar2c1p_'; + config.files.basename = 'oscar'; + config.files.idxcams = [1:2]; % related to the imnames + config.files.idxproj = [3]; % related to the projectors + config.cal.cams2use = [1:3]; + config.files.imgext = 'jpg'; + config.files.projdata = [config.paths.data,'files.txt']; % contains the projector data + config.imgs.LEDsize = 25; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/3; % scale of the required subpixel accuracy + config.imgs.res = [1392,1024]; + config.imgs.projres = [1024,768]; % projector resolution + config.cal.DO_GLOBAL_ITER = 0; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 0.5; + config.cal.nonlinpar = [30,1,1,1,1,1]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.INL_TOL = 2; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 0; + config.cal.UNDO_RADIAL= 0; + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 2; + config.cal.SQUARE_PIX = 1; +elseif strcmp(experiment,'oscardemo') + config.paths.data = ['/home.zam/svoboda/Work/SelfCalibExtern/']; + config.paths.img = [config.paths.data,'cam%d/']; + config.files.imnames = 'demo3p3c_'; + config.files.basename = 'oscar'; + config.files.idxcams = [1:3]; % related to the imnames + config.files.idxproj = [4:6]; % related to the projectors + config.files.imgext = 'jpg'; + config.files.projdata = [config.paths.data,'calib_3p3c_p3.txt']; % contains the projector data + config.imgs.LEDsize = 25; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/3; % scale of the required subpixel accuracy + config.imgs.res = [1392,1024]; + config.imgs.projres = [1024,768]; % projector resolution + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 1; + config.cal.nonlinpar = [15,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,0,1,1,0,0]; + config.cal.INL_TOL = 3; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 3; + config.cal.DO_BA = 0; + config.cal.START_BA = 0; % do BA in intermediate steps + config.cal.UNDO_RADIAL= 0; + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; + config.cal.SQUARE_PIX = 1; % 0 works surprisingly far better than 1 (special cameras or projectors?) + config.cal.cams2use = [1,2,3] %,4,5,6]; +elseif strcmp(experiment,'0801BlueCRZ') + config.paths.data = ['/local/MultiCam/Data/CalibData4Testing/20030108_BigBlueC/Calib/']; + config.files.basename = 'atlantic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.LEDthr = 30; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 0.2; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 15; + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.MIN_PTS_VAL = 200; + config.cal.NTUPLES = 3; + config.imgs.subpix = 1/5; + % config.cal.cams2use = 4; +elseif strcmp(experiment,'BlueCRZ') + config.paths.data = ['/local/MultiCam/Data/CalibData4Testing/20030626_BigBlueC/']; + config.files.basename = 'atlantic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [3:12,14:18]; % related to the imnames + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/3; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 0.3; + config.cal.INL_TOL = 10; % + config.cal.NUM_CAMS_FILL = 15; + config.cal.DO_BA = 0; + config.cal.MIN_PTS_VAL = 30; + config.cal.UNDO_RADIAL= 0; + config.cal.NTUPLES = 3; + config.cal.SQUARE_PIX = 1; + % config.cal.cams2use = [3:12,14:18]; + % config.cal.cams2use = [5,6,12]; + % config.cal.cams2use = [3,4,5,6,9,10,17,18]; +elseif strcmp(experiment,'BlueCHoengg') + config.paths.data = ['/local/MultiCam/Data/CalibData4Testing/20030615_Hoengg/']; + config.files.basename = 'arctic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + % config.files.idxcams = [1:11,13:16]; % related to the imnames + config.files.idxcams = [1:16]; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/5; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_THR = 0.5; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 0; + config.cal.UNDO_RADIAL= 0; % CalTech (BlueC compatible) + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; +% config.cal.cams2use = [1:11,13:16]; % if use of some cams only is required + elseif strcmp(experiment,'TestData') + config.paths.data = ['/local/MultiCam/Data/TestData/']; + config.files.basename = 'arctic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [1:13,15,16]; + config.imgs.LEDsize = 3; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/3; + config.imgs.LEDthr = 30; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_THR = 0.2; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.INL_TOL = 5; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; % CalTech (BlueC compatible) + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; + config.cal.cams2use = [2:3,16]; +elseif strcmp(experiment,'OneSideH0105') + config.paths.data = ['/datagrid/MultiCam/Data/OneSideCams/Hoengg0105/']; + config.files.basename = 'arctic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [1:5]; % related to the imnames + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/5; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_THR = 0.2; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.INL_TOL = 5; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 0; + config.cal.UNDO_RADIAL= 0; % CalTech (BlueC compatible) + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; + config.cal.cams2use = [3,4,5]; % if use of some cams only is required +elseif strcmp(experiment,'OneSideH1416') + config.paths.data = ['/datagrid/MultiCam/Data/OneSideCams/Hoengg1416/']; + config.files.basename = 'arctic'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [14:16]; % related to the imnames + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/5; + config.cal.nonlinpar = [70,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_THR = 0.15; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 10; + config.cal.DO_BA = 0; + config.cal.UNDO_RADIAL= 0; % CalTech (BlueC compatible) + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 3; + config.cal.cams2use = [14:16]; % if use of some cams only is required +elseif strcmp(experiment,'ViRoom20030611') + config.paths.data = ['/datagrid/MultiCam/Data/Calib.ViRoom/20030611Calib2/']; + config.files.basename = 'viroom'; + config.paths.img = [config.paths.data,'Images/']; + config.files.imnames = 'calib2_%0.2d_*'; + config.files.idxcams = [10,11,20,21,30,40]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 9; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'red'; % color of the laser pointer + config.imgs.LEDthr = 100; % acceptance threshold for the LED + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 1; + config.cal.nonlinpar = [50,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,0,1,0,0,0]; + config.cal.INL_TOL = 10; + config.cal.START_BA = 1; % do BA in all intermediate steps + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.NUM_CAMS_FILL = 3; + config.cal.NTUPLES = 3; + config.cal.SQUARE_PIX = 1; + config.cal.planarcams = [1:6]; + % config.cal.cams2use = [20,21,30,40]; + config.imgs.subpix = 1/2; + config.cal.MIN_PTS_VAL = 20; +elseif strcmp(experiment,'ViRoom20030724') + config.paths.data = ['/home/svoboda/viroomData/ViRoom/Calib/20030724Calib/']; + config.files.basename = 'viroom'; + config.paths.img = [config.paths.data,'cam%d/']; + config.files.imnames = 'calib%d*'; + config.files.idxcams = [10,11,20,21,30,40,41]; % related to the imnames + config.files.imgext = 'jpg'; +% config.files.maxid = 499; % maximum number of allowed img indexes +% config.files.posid = [10:13]; % position of the Id + config.imgs.LEDsize = 9; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'red'; % color of the laser pointer + config.imgs.LEDthr = 70; % acceptance threshold for the LED + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 1; + config.cal.nonlinpar = [50,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,0,1,0,0,0]; + config.cal.INL_TOL = 10; + config.cal.START_BA = 1; % do BA in all intermediate steps + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.NUM_CAMS_FILL = 4; + config.cal.NTUPLES = 3; + config.cal.SQUARE_PIX = 1; + % config.cal.cams2use = [10,11,40,41]; + config.cal.planarcams = [1,3:7]; + config.imgs.subpix = 1/3; + config.cal.MIN_PTS_VAL = 20; +elseif strcmp(experiment,'Erlangen') + config.paths.data = ['/local/MultiCam/Data/CalibData4Testing/Erlangen_Viroom/']; + config.files.basename = 'erlangen'; + config.paths.img = [config.paths.data,'Images/']; + config.files.imnames = 'cal%0.2d_*'; + config.files.idxcams = [70,71,72,80,81,82]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 9; % avg diameter of a LED in pixels + config.imgs.subpix = 1/3; + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.cal.DO_GLOBAL_ITER = 0; + config.cal.GLOBAL_ITER_MAX = 5; + config.cal.GLOBAL_ITER_THR = 1.5; + config.cal.nonlinpar = [50,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,0,1,0,0,0]; + config.cal.INL_TOL = 5; + config.cal.START_BA = 1; % do BA in all intermediate steps + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.NUM_CAMS_FILL = 2; + config.cal.NTUPLES = 3; + config.cal.SQUARE_PIX = 1; + % config.cal.cams2use = [70,71,72,80,82]; + config.cal.MIN_PTS_VAL = 20; +elseif strcmp(experiment,'extern') + config.paths.data = ['/home.zam/svoboda/Work/SelfCalibExtern/']; + config.files.basename = 'void'; + config.paths.img = [config.paths.data,config.files.basename,'%d/']; + config.files.imnames = [config.files.basename,'%d.pvi.*.']; + config.files.idxcams = [1:32]; % related to the imnames + config.files.imgext = 'jpg'; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.cal.DO_GLOBAL_ITER = 1; + config.cal.GLOBAL_ITER_MAX = 10; + config.cal.GLOBAL_ITER_THR = 0.3; + config.cal.nonlinpar = [50,0,1,0,0,0]; + config.cal.NL_UPDATE = [1,1,1,1,1,1]; + config.cal.INL_TOL = 10; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 15; % with 30 it gets unstable after some iteration loops + config.cal.DO_BA = 1; + config.cal.UNDO_RADIAL= 0; + config.cal.MIN_PTS_VAL = 30; + config.cal.NTUPLES = 2; + config.imgs.subpix = 1/5; + config.cal.SQUARE_PIX = 1; + % config.cal.cams2use = 4; +else + error('Configdata: wrong identifier of the data set'); +end + +% camera indexes handling +try, config.cal.cams2use; catch, config.cal.cams2use = config.files.idxcams; end + +% Default initial settings for the estiamtion of the nonlinear distortion +% (1) ... camera view angle +% (2) ... estimate principal point? +% (3:4) ... parameters of the radial distortion +% (5:6) ... parameters of the tangential distortion +try, config.cal.nonlinpar; catch, config.cal.nonlinpar = [50,0,1,0,0,0]; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% adding more and more non-linear paramaters might be tricky +% in case of bad data. You may fall in the trap of overfitting +% You may want to disable this +% update all possible parameters by default +try, config.cal.NL_UPDATE; catch, config.cal.NL_UPDATE = [1,1,1,1,1,1]; end + + + +% configuration of the for the calibration process +try, config.cal.SQUARE_PIX; catch, config.cal.SQUARE_PIX = 1;end % most of the cameras have square pixels +try, config.cal.START_BA; catch, config.cal.START_BA = 0; end +try, config.cal.DO_GLOBAL_ITER; catch, config.cal.DO_GLOBAL_ITER = 1; end +try, config.cal.GLOBAL_ITER_THR; catch, config.cal.GLOBAL_ITER_THR = 1; end +try, config.cal.GLOBAL_ITER_MAX; catch, config.cal.GLOBAL_ITER_MAX = 10; end +try, config.cal.INL_TOL; catch, config.cal.INL_TOL = 5; end; +try, config.cal.NUM_CAMS_FILL; catch, config.cal.NUM_CAMS_FILL = 12; end; +try, config.cal.DO_BA; catch, config.cal.DO_BA = 0; end; +try, config.cal.UNDO_RADIAL; catch, config.cal.UNDO_RADIAL = 0; end; +try, config.cal.UNDO_HEIKK; catch, config.cal.UNDO_HEIKK = 0; end; % only for testing, not a part of standard package +try, config.cal.NTUPLES; catch, config.cal.NTUPLES = 3; end; % size of the camera tuples, 2-5 implemented +try, config.cal.MIN_PTS_VAL; catch, config.cal.MIN_PTS_VAL = 30; end; % minimal number of correnspondences in the sample + +% image extensions +try, config.files.imgext; catch, config.files.imgext = 'jpg'; end; + +% image resolution +try, config.imgs.res; catch, config.imgs.res = [640,480]; end; + +% scale for the subpixel accuracy +% 1/3 is a good compromise between speed and accuracy +% for high-resolution images or bigger LEDs you may try, 1/1 or 1/2 +try, config.imgs.subpix; catch, config.imgs.subpix = 1/3; end; + +% data names +try, config.files.Pmats; catch, config.files.Pmats = [config.paths.data,'Pmatrices.dat']; end; +try, config.files.points; catch, config.files.points = [config.paths.data,'points.dat']; end; +try, config.files.IdPoints; catch, config.files.IdPoints = [config.paths.data,'IdPoints.dat']; end; +try, config.files.Res; catch, config.files.Res = [config.paths.data,'Res.dat']; end; +try, config.files.IdMat; catch, config.files.IdMat = [config.paths.data,'IdMat.dat']; end; +try, config.files.inidx; catch, config.files.inidx = [config.paths.data,'idxin.dat']; end; +try, config.files.avIM; catch, config.files.avIM = [config.paths.data,'camera%d.average.tiff']; end; +try, config.files.stdIM; catch, config.files.stdIM = [config.paths.data,'camera%d.std.tiff']; end; +try, config.files.CalPar; catch, config.files.CalPar = [config.paths.data,'camera%d.cal']; end; +try, config.files.CalPmat; catch, config.files.CalPmat = [config.paths.data,'camera%d.Pmat.cal']; end; +try, config.files.StCalPar; catch, config.files.StCalPar = [config.paths.data,config.files.basename,'%d.cal']; end; +try, config.files.rad; catch, config.files.rad = [config.paths.data,config.files.basename,'%d.rad']; end; +try, config.files.heikkrad; catch, config.files.heikkrad = [config.paths.data,config.files.basename,'%d.heikk']; end; +try, config.files.Pst; catch, config.files.Pst = [config.paths.data,'Pst.dat']; end; +try, config.files.Cst; catch, config.files.Cst = [config.paths.data,'Cst.dat']; end; +try, config.files.points4cal; catch, config.files.points4cal = [config.paths.data,'cam%d.points4cal.dat']; end; diff --git a/CommonCfgAndIO/expname.m b/CommonCfgAndIO/expname.m new file mode 100644 index 0000000..3994beb --- /dev/null +++ b/CommonCfgAndIO/expname.m @@ -0,0 +1,35 @@ +% EXPeriment NAME +% returns the name of the current experiment +% this name indexes the CONFIGDATA +% +% $Id: expname.m,v 2.7 2005/05/23 16:23:35 svoboda Exp $ + +function name = expname() + +% the name of a calibration fo BlueC must +% contain the following sub-string 'BigBlue' + +% the name for the Hoenggeberg calibration must contain +% the sub-string Hoengg + +% the name of the oscar setup must contain +% the string 'oscar' + +% name = 'G9'; +% name = '0801BlueCRZ'; +% name = 'BlueCHoengg'; +% name = 'TestData'; +% name = 'OneSideH0105'; +% name = 'OneSideH1416'; +name = 'BlueCRZ'; +% name = 'ViRoom20030611'; +% name = '2410ViRoom'; +% name = 'Erlangen'; +% name = 'oscar2c1p'; +% name = 'oscardemo'; +% name = 'ViRoom20030724'; +% name = 'extern'; + +return + + diff --git a/CommonCfgAndIO/loaddata.m b/CommonCfgAndIO/loaddata.m new file mode 100644 index 0000000..51a527c --- /dev/null +++ b/CommonCfgAndIO/loaddata.m @@ -0,0 +1,104 @@ +% loaddata ... load the input data +% +% loaded = loaddata(config) +% +% config ... configuration structure, see the CONFIGDATA +% +% M cameras and N frames +% loaded.Ws ... 3M x N joint image matrix +% .IdMat ... M x N point identification matrix +% .Res ... M x 2 image resolutions +% .Pmat ... {1xM} cell array of the projection matrices +% +% see the FindingPoint and MulticamSelfCalib for more details +% +% $Id: loaddata.m,v 2.2 2005/05/23 16:23:35 svoboda Exp $ + +function loaded = loaddata(config) + +USED_MULTIPROC = 0; % was the multipropcessing used? + % if yes then multiple IdMat.dat and points.dat have to be loaded + % setting to 1 it forces to read the multiprocessor data against the + % monoprocessor see the IM2POINTS, IM2PMULTIPROC.PL + +%%% +% read the data structures +if ~USED_MULTIPROC + try, + Ws = load(config.files.points); % distorted points as found by Im2Points + IdMat = load(config.files.IdMat); % see function im2points for detailed comments + %%% + % try,load the file with Images resolutions which is on of the output files + % from finding LEDs procedure or take the pre-defined resolutions specified in the configdata + try, Res = load(config.files.Res); catch, Res = repmat(config.imgs.res,size(IdMat,1),1); end + catch + warning('Data from mono-processor version not found, trying the multi-proc ones ...') + USED_MULTIPROC=1; + end +end + +if USED_MULTIPROC + pointsfiles = dir([config.files.points,'.*']); + IdMatfiles = dir([config.files.IdMat,'.*']); + Resfiles = dir([config.files.Res,'.*']); + pp = []; + for i=1:size(pointsfiles,1), + W{i} = load([config.paths.data,pointsfiles(i).name],'-ASCII'); + pp = [pp,size(W{i},2)]; + IdM{i} = load([config.paths.data,IdMatfiles(i).name],'-ASCII'); + try, Rs{i} = load([config.paths.data,Resfiles(i).name],'-ASCII'); catch, Rs{i} = repmat(config.imgs.res,size(IdM{i},1),1); end + end + % throw away some point to preserve consistency + [minpp,minidx] = min(pp); + if minpp == 0 + error('loaddata: Problem in loading input data. Check CONFIGDATA, EXPNAME settings'); + end + % merge the matrices + Ws = []; + IdMat = []; + Res = []; + for i=1:size(pointsfiles,1), + Ws = [Ws; W{i}(:,1:minpp)]; + IdMat = [IdMat; IdM{i}(:,1:minpp)]; + Res = [Res; Rs{i}]; + end + if isempty(Ws) | isempty(IdMat) + error('Error in loading parallel data. Did you really use multi-processor version of finding points'); + end +end + +% oscar data hack. The projectors are handled as the cameras +try,config.files.idxcams = [config.files.idxcams,config.files.idxproj]; catch, config.files.idxcams; end + +if size(Ws,1)/3 ~= size(config.files.idxcams,2) + error('Problem in loading points data. Less or more data than expected found') +end + + +%%% read the P matrices +count = 1; +for i=config.cal.cams2use, + try,P = load(sprintf(config.files.CalPmat,i),'-ASCII'); catch, warning(sprintf('The calibration file config.files.CalPmat does not exist',i)); end; + try,Pmat{count} = P; catch, warning('No P mat available'); end; + count = count+1; +end + +idx2use = zeros(size(config.cal.cams2use)); +for i=1:size(config.cal.cams2use,2), + idx2use(i) = find(config.cal.cams2use(i) == config.files.idxcams); +end + +idx2use3 = []; +for i=1:size(idx2use,2), + idx2use3 = [idx2use3, [idx2use(i)*3-2:idx2use(i)*3]]; +end + +%%% +% fill the loaded structure +loaded.Ws = Ws(idx2use3,:); +loaded.IdMat = IdMat(idx2use,:); +loaded.Res = Res(idx2use,:); + +try,loaded.Pmat = Pmat; catch, warning('No Pmat available'); end; + +return; diff --git a/CommonCfgAndIO/oldexperiments.m b/CommonCfgAndIO/oldexperiments.m new file mode 100644 index 0000000..53826cb --- /dev/null +++ b/CommonCfgAndIO/oldexperiments.m @@ -0,0 +1,136 @@ +elseif strcmp(experiment,'1704BigBlue') + config.paths.data = ['/local/Calibration/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1/3; + config.cal.INL_TOL = 1; % + config.cal.NUM_CAMS_FILL = 6; + config.cal.DO_BA = 0; + +elseif strcmp(experiment,'DeRoeck01') + config.paths.data = ['/data/ViRoom/Calib/DeRoeck20030224/']; + config.paths.img = ['/data/ViRoom/Calib/DeRoeck20030224/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'vr%0.2d_image.*.'; + config.files.idxcams = [0:2]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.img,'Pst.dat']; + config.files.Cst = [config.paths.img,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + +elseif strcmp(experiment,'RolandBigBlue') + config.paths.data = ['/data/BigBlueC/20030117_BigBlueC/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer +elseif strcmp(experiment,'Roland2BigBlue') + config.paths.data = ['/data/BigBlueC/Roland_BigBlue_20030217/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:12,15:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer +elseif strcmp(experiment,'Roland3BigBlue') + config.paths.data = ['/data/BigBlueC/Roland_BigBlue_20030219/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:12,14:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 9; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.imgs.subpix = 1; + + +elseif strcmp(experiment,'0801BigBlue') + config.paths.data = ['/home/svoboda/viroomData/BigBlueC/20030108_BigBlueC/Calib/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + config.cal.INL_TOL = 3; % if UNDO_RADIAL than it may be relatively small <1 + config.cal.NUM_CAMS_FILL = 12; + config.cal.DO_BA = 0; + config.cal.UNDO_RADIAL= 1; +elseif strcmp(experiment,'1301BigBlue') + config.paths.data = ['/data/BigBlueC/20030113_BigBlueC/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + +elseif strcmp(experiment,'2911BigBlue') + config.paths.data = ['/data/BigBlueC/20021129_BigBlueC/Calib/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer +elseif strcmp(experiment,'0301BigBlue') + config.paths.data = ['/data/BigBlueC/20030103_BigBlueC/Calib2/']; + config.paths.img = [config.paths.data,'atlantic%d/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.data,'Pst.dat']; + config.files.Cst = [config.paths.data,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + +elseif strcmp(experiment,'2910BigBlue') + config.paths.data = ['/data/BigBlueC/20021029_BigBlueC/Calib/']; + config.paths.img = ['/data/BigBlueC/20021029_BigBlueC/Calib/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.img,'Pst.dat']; + config.files.Cst = [config.paths.img,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer + +elseif strcmp(experiment,'2210BigBlue') + config.paths.img = ['/data/BigBlueC/20022210_BigBlueC/Calib1/']; + config.paths.radial = [HOME_PATH,'none']; + config.files.imnames = 'atlantic%d.pvi.*.'; + config.files.idxcams = [3:18]; % related to the imnames + config.files.imgext = 'jpg'; + config.files.Pst = [config.paths.img,'Pst.dat']; + config.files.Cst = [config.paths.img,'Cst.dat']; + config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + config.imgs.LEDcolor = 'green'; % color of the laser pointer diff --git a/MultiCamSelfCal/BlueCLocal/bluechoengg.m b/MultiCamSelfCal/BlueCLocal/bluechoengg.m new file mode 100644 index 0000000..02c3d67 --- /dev/null +++ b/MultiCamSelfCal/BlueCLocal/bluechoengg.m @@ -0,0 +1,68 @@ +function [align] = bluechoengg(in,config) +% bluechoengg ... local routines for the hoengg installation +% +% [align] = bluechoengg(in,config) +% in, config ... see the main GOCAL script +% +% align ... structures aligned wit the specified world frame +% +% $Id: bluechoengg.m,v 2.1 2005/05/20 15:27:37 svoboda Exp $ + +Cst = in.Cst; +Rot = in.Rot; + +drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); + +horizplane.idx(1) = find(config.cal.cams2use==13); +horizplane.idx(2) = find(config.cal.cams2use==14); +horizplane.idx(3) = find(config.cal.cams2use==15); +horizplane.idx(4) = find(config.cal.cams2use==16); +horizplane.vec = pinv([Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*Cst(horizplane.idx,3); +horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; +horizplane.n = horizplane.par(1:3)'; + +% set the camera on top +set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); +set(gca,'CameraPosition',mean(Cst(horizplane.idx,:))+3*horizplane.n'); +% figure(41), print -depsc grapheval.eps + +% definition of the absolute world frame +% ccam(11).C = [1.40, -2.05, 2.70]; +cam(13).C = [-1.40, -2.55, 2.70]'; +cam(14).C = [-1.40, 2.70, 2.70]'; +cam(15).C = [0, 2.70, 2.70]'; +cam(16).C = [1.40, 2.70, 2.70]'; + +cam(1).C = [-3.70, -2.55, 1.55]'; % relatively ad hoc values to improve the stability +cam(5).C = [-3.70, 4.06, 1.55]'; +cam(6).C = [3.70, 4.06, 1.55]'; +cam(10).C = [3.70, -2.55, 1.55]'; +% of the similarity computation + +[align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(find(config.cal.cams2use==1),:)', Cst(find(config.cal.cams2use==5),:)', Cst(find(config.cal.cams2use==6),:)', Cst(find(config.cal.cams2use==10),:)',Cst(horizplane.idx,1:3)'],[cam(:).C]); +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +% plot the 3D results from a better perspective by estimating the plane of the Cams 9,10,17,18 +% let call this plane "horizontal". +% this plot makes sense only for the BigBlueC +horizplane.vec = pinv([align.Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*align.Cst(horizplane.idx,3); +horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; +horizplane.n = horizplane.par(1:3)'; +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: View from the top camera',config.cal.cams2use); +% set the camera on top +set(gca,'CameraTarget',mean(align.Cst(horizplane.idx,:))); +set(gca,'CameraPosition',mean(align.Cst(horizplane.idx,:))+3*horizplane.n'); +% set(gca,'CameraPosition',align.Cst(find(config.cal.cams2use==15),:)); % view from the perspective of the camera4 + +% drawscene(in.Xe,Cst',Rot,42,'cloud','Graphical Output Validation: View from side',config.cal.cams2use); +% set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); +% set(gca,'CameraPosition',mean(Cst(horizplane.idx,:)+Cst(horizplane(4),:)-Cst(horizplane(1),:))'); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'graphevalaligned.eps']) + +return diff --git a/MultiCamSelfCal/BlueCLocal/bluecrz.m b/MultiCamSelfCal/BlueCLocal/bluecrz.m new file mode 100644 index 0000000..fb71726 --- /dev/null +++ b/MultiCamSelfCal/BlueCLocal/bluecrz.m @@ -0,0 +1,64 @@ +function [align] = bluecrz(in,config) +% bluecrz ... localized output function for the BlueCRZ installation +% +% [align] = bluecrz(in,config) +% in, cam, config ... see the main GOCAL script +% +% align ... structures aligned wit the specified world frame +% +% $Id: bluecrz.m,v 2.1 2005/05/20 15:27:37 svoboda Exp $ + +Cst = in.Cst; +Rot = in.Rot; + +drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); + +horizplane.idx(1) = find(config.cal.cams2use==9); +horizplane.idx(2) = find(config.cal.cams2use==10); +horizplane.idx(3) = find(config.cal.cams2use==17); +horizplane.idx(4) = find(config.cal.cams2use==18); +horizplane.vec = pinv([Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*Cst(horizplane.idx,3); +horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; +horizplane.n = horizplane.par(1:3)'; + +% set the camera on top +set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); +set(gca,'CameraPosition',mean(Cst(horizplane.idx,:))+3*horizplane.n'); +% figure(41), print -depsc grapheval.eps + +% definition of the absolute world frame +cave.x=2.8; cave.y=2.8; cave.z=2.36; +cam(9).C = [cave.x/2, -cave.y/2, cave.z]'; +cam(10).C = [cave.x/2, cave.y/2, cave.z]'; +cam(17).C = [-cave.x/2, -cave.y/2, cave.z]'; +cam(18).C = [-cave.x/2, cave.y/2, cave.z]'; +cam(4).C = [0,0.05,3.7]'; % relatively ad hoc values to improve the stability +% of the similarity computation + +[align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(find(config.cal.cams2use==4),:)',Cst(horizplane.idx,1:3)'],[cam(:).C]); +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +% plot the 3D results from a better perspective by estimating the plane of the Cams 9,10,17,18 +% let call this plane "horizontal". +% this plot makes sense only for the BigBlueC +horizplane.vec = pinv([align.Cst(horizplane.idx,1:2),ones(size(horizplane.idx))'])*align.Cst(horizplane.idx,3); +horizplane.par = [-horizplane.vec(1),-horizplane.vec(2),1,-horizplane.vec(3)]; +horizplane.n = horizplane.par(1:3)'; +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: View from the top camera',config.cal.cams2use); +% set the camera on top +set(gca,'CameraTarget',mean(align.Cst(horizplane.idx,:))); +set(gca,'CameraPosition',mean(align.Cst(horizplane.idx,:))+3*horizplane.n'); +% set(gca,'CameraPosition',align.Cst(find(config.cal.cams2use==4),:)); % view from the perspective of the camera4 + +% drawscene(in.Xe,Cst',Rot,42,'cloud','Graphical Output Validation: View from side',config.cal.cams2use); +% set(gca,'CameraTarget',mean(Cst(horizplane.idx,:))); +% set(gca,'CameraPosition',mean(Cst(horizplane.idx,:)+Cst(horizplane(4),:)-Cst(horizplane(1),:))'); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'graphevalaligned.eps']) + +return; diff --git a/MultiCamSelfCal/CoreFunctions/align3d.m b/MultiCamSelfCal/CoreFunctions/align3d.m new file mode 100644 index 0000000..d0bd7d5 --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/align3d.m @@ -0,0 +1,10 @@ +function [P,X] = align3d(inP,inX,simT); + +T = [simT.s*simT.R;0 0 0]; +T = [T, [simT.t(:);1]]; + +X = T*inX; + +P = inP*inv(T); + +return; diff --git a/MultiCamSelfCal/CoreFunctions/estimateLambda.m b/MultiCamSelfCal/CoreFunctions/estimateLambda.m new file mode 100644 index 0000000..ae8829f --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/estimateLambda.m @@ -0,0 +1,57 @@ +% estimateLambda ... estimate some initial projective depth given +% the measurement matrix +% +% This algorithm is based on a paper by P. Sturm and B. Triggs called +% "A Factorization Based Algorithm for Multi-Image Projective Structure +% and motion" (1996) +% Projective depths are estimated using fundamental matrices. +% +% [Lambda] = estimateLambda(Ws) +% +% Ws ...... the 3*nxm measurement matrix (the data should be normalized +% before calling this functions) +% pair .... array of camera pairs containing Fundamental matrices and +% indexes of points used for their computations +% it is the output from RANSAC validation step +% +% Lambda .. nxm matrix containing the estimated projective depths +% +% 09/2001, Dejan Radovic +% 05/2002, Tomas Svoboda + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: estimateLambda.m,v 2.0 2003/06/19 12:06:49 svoboda Exp $ +% $State: Exp $ + + +function [Lambda] = estimateLambda(Ws,pair) +n = size(Ws,1)/3; % cameras +m = size(Ws,2); % frames + +% estimate (n-1) fundamental matrices F_12, F_23, ..., F_(n-1)n +F = []; % the fundamental matrices +e = []; % the epipoles (as columns) +Lambda = ones(n,m); % the estimated projective depths +for i=1:n-1 + j=i+1; + F_ij = pair(i).F; + % compute epipole from F_ij*e_ij == 0 + [U,S,V] = svd(F_ij,0); + % diag(S)' + e_ij = V(:,size(V,2)); + for p=pair(i).idxin, + q_ip = Ws(i*3-2:i*3,p); + q_jp = Ws(j*3-2:j*3,p); + Lambda(j,p) = Lambda(i,p)*((norm(cross(e_ij,q_ip)))^2/sum(cross(e_ij,q_ip).*(F_ij'*q_jp))); + % Lambda(j,p) = Lambda(i,p)*(sum((F_ij'*q_jp).*cross(e_ij,q_ip))/(norm(F_ij'*q_jp))^2); + end +end + +return + + + + + + diff --git a/MultiCamSelfCal/CoreFunctions/estimatePX.m b/MultiCamSelfCal/CoreFunctions/estimatePX.m new file mode 100644 index 0000000..a90d89a --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/estimatePX.m @@ -0,0 +1,75 @@ +% estimatePX ... estimate the projective shape and motion +% +% [P,X,Lambda] = estimatePX(Ws, Lambda) +% +% Ws ....... 3*nxm measurement matrix +% Lambda ... nxm matrix containg some initial projective depths +% (see also: ESTIMATELAMBDA) +% +% P ........ 3*nx4 matrix containing the projective motion +% X ........ 4xm matrix containing the projective shape +% Lambda ... the new estimation of the projective depths + +function [P,X,Lambda] = estimatePX(Ws, Lambda) +n = size(Ws,1)/3; +m = size(Ws,2); + +% compute 1st updated Ws +for i = 1:n + for j = 1:m + Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda(i, j); + Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda(i, j); + Ws_updated(3*i, j) = Ws(3*i, j) * Lambda(i, j); + end +end + +Lambda_new = Lambda; +iterations = 0; +errs = 1e10*[99.9,99]; +tol = 1e-3; +while (errs(iterations+1)-errs(iterations+2)>tol), + [U,D,V] = svd(Ws_updated); + % the following loop is not needed since these elements of D + % are not considered in further computations + for i = 5:size(D,2) + D(i, i) = 0; + end + + % projective shape X and motion P + P = U*D(1:size(U,2),1:4); + X = V(:,1:4)'; + % U*D*V' == P*X + + % correct projective depths + normfact = sum(P(3:3:(3*n),:)'.^2); + Lambda_old = Lambda_new; + Lambda_new = P(3:3:(3*n),:)./repmat(sqrt(normfact'),1,4)*X; + + % normalize lambdas + lambnfr = sum(Lambda_new.^2); + Lambda_new = sqrt(n)*Lambda_new./repmat(sqrt(lambnfr),n,1); + lambnfc = sum(Lambda_new'.^2); + Lambda_new = sqrt(m)*Lambda_new./repmat(sqrt(lambnfc'),1,m); + + for i = 1:n + for j = 1:m + Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda_new(i, j); + Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda_new(i, j); + Ws_updated(3*i, j) = Ws(3*i, j) * Lambda_new(i, j); + end + end + iterations = iterations + 1; + errs = [errs,sum(sum(abs(Lambda_old-Lambda_new)))]; + % errs(iterations+2) +end +%iterations + +[U,D,V] = svd(Ws_updated); +% compute new projective shape and motion +P = U*D(1:size(U,2),1:4); +X = V(:,1:4)'; +X = X./repmat(X(4,:),4,1); +Lambda = Lambda_new; + + + diff --git a/MultiCamSelfCal/CoreFunctions/estsimt.m b/MultiCamSelfCal/CoreFunctions/estsimt.m new file mode 100644 index 0000000..7ee2c60 --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/estsimt.m @@ -0,0 +1,71 @@ +function [s,R,T] = estsimt(X1,X2) +% ESTimate SIMilarity Transformation +% +% [s,R,T] = estsimt(X1,X2) +% +% X1,X2 ... 3xN matrices with corresponding 3D points +% +% X2 = s*R*X1 + T +% s ... scalar scale +% R ... 3x3 rotation matrix +% T ... 3x1 translation vector +% +% This is done according to the paper: +% "Least-Squares Fitting of Two 3-D Point Sets" +% by K.S. Arun, T. S. Huang and S. D. Blostein +% +% $Id: estsimt.m,v 2.1 2005/05/23 16:24:59 svoboda Exp $ + +% number of points +N = size(X1,2); + +if N ~= size(X2,2) + error('estsimt: both sets must contain the same number of points') +end + +X1cent = mean(X1,2); +X2cent = mean(X2,2); +% normalize coordinate systems for both set of points +x1 = X1 - repmat(X1cent,1,N); +x2 = X2 - repmat(X2cent,1,N); + +% first estimate the scale s +% dists1 = sum(sqrt(x1.^2)); +% dists2 = sum(sqrt(x2.^2)); + +% mutual distances; +d1 = x1(:,2:end)-x1(:,1:(end-1)); +d2 = x2(:,2:end)-x2(:,1:(end-1)); +ds1 = sum(d1.^2).^(1/2); +ds2 = sum(d2.^2).^(1/2); + +scales = ds2./ds1; + +% the scales should be the same for all points +% because of noise they are not +% scales = dists2./dists1 +s = median(sort(scales)); + +% undo scale +x1s = s*x1; + +% finding rotation +H = zeros(3,3); +for i=1:N + H = H+ x1s(:,i)*x2(:,i)'; +end + +[U,S,V] = svd(H,0); +X = V*U'; +if det(X) > 0 + R = X; % estimated rotation matrix +else + % V = [V(:,1:2),-V(:,3)]; + % R = V*U'; + % s = -s; + R = X; +end + +T = X2cent - s*R*X1cent; + + diff --git a/MultiCamSelfCal/CoreFunctions/euclidize.m b/MultiCamSelfCal/CoreFunctions/euclidize.m new file mode 100644 index 0000000..2e6950a --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/euclidize.m @@ -0,0 +1,170 @@ +% euclidize ... perform euclidian reconstruction +% under assumption of unknown focal lengths, const. principal points = 0, +% and aspect ratio = 1 +% +% [Pe,Xe,C,Rot] = euclidize(Ws,Lambda,P,X,config) +% +% n is the number of cameras and m is the number of points +% +% Ws ....... 3*nxm measurement matrix +% Lambda ... nxm matrix containing the projective depths +% P ........ 3*nx4 projective motion matrix +% X ........ 4xm projective shape matrix +% config ... see the CONFIGDATA +% .cal.pp and .cal.SQUARE_PIX are expected +% +% Pe ....... 3*nx4 euclidian motion matrix +% Xe ....... 4xm euclidian shape matrix +% C ........ 4xn matrix containg the camera centers +% Rot ...... 3*nx3 matrix containing the camera rotation matrices + +% $Author: svoboda $ +% $Revision: 2.1 $ +% $Id: euclidize.m,v 2.1 2003/07/09 14:40:48 svoboda Exp $ +% $State: Exp $ + +function [Pe,Xe,C,Rot] = euclidize(Ws,Lambda,P,X,config) +n = size(Ws,1)/3; % number of cameras +m = size(Ws,2); % number of points + +% compute B +a = []; b = []; c = []; +for i = 1:n + a = [a; sum(Ws(3*i-2,:).*Lambda(i,:))]; + b = [b; sum(Ws(3*i-1,:).*Lambda(i,:))]; + c = [c; sum(Lambda(i,:))]; +end +TempA = -P(3:3:3*n, :); +TempB = -P(3:3:3*n, :); +for i = 1:n + TempA(i, :) = TempA(i, :)*a(i)/c(i); + TempB(i, :) = TempB(i, :)*b(i)/c(i); +end +TempA = TempA + P(1:3:3*n, :); +TempB = TempB + P(2:3:3*n, :); +Temp = [TempA; TempB]; +[U,S,V] = svd(Temp,0); +B = V(:,4); % least square solution (of Temp*B == 0) + +% compute A +% +% M * M^T == P * Q *P^T, thus +% +% ( m_x ) ( P1 ) +% ( m_y )*(m_x m_y m_z) == ( P2 ) * Q * (P1 P2 P3) (let Pi denote the i-th row of P), thus +% ( m_z ) ( P3 ) +% +% ( |m_x|^2 m_x*m_y m_x*m_z ) ( P1*Q*P1^T P1*Q*P2^T P1*Q*P3^T ) +% ( . |m_y|^2 m_y*m_z ) == ( . P2*Q*P2^T P2*Q*P3^T ) +% ( . ....... |m_z|^2 ) ( . ......... P3*Q*P3^T ) +% +Temp = []; b = []; +for i = 1:n + P1 = P(3*i-2,:); % 1st row of i-th camera + P2 = P(3*i-1,:); % 2nd row of i-th camera + P3 = P(3*i, :); % 3rd row of i-th camera + u = P1; v = P2; + Temp = [Temp; u(1)*v(1) u(1)*v(2)+u(2)*v(1) u(3)*v(1)+u(1)*v(3) u(1)*v(4)+u(4)*v(1) u(2)*v(2) u(2)*v(3)+u(3)*v(2) u(2)*v(4)+u(4)*v(2) u(3)*v(3) u(4)*v(3)+u(3)*v(4) u(4)*v(4)]; + if config.cal.SQUARE_PIX + Temp = [Temp; u(1)^2-v(1)^2 2*u(1)*u(2)-2*v(1)*v(2) 2*u(1)*u(3)-2*v(1)*v(3) 2*u(1)*u(4)-2*v(1)*v(4) u(2)^2-v(2)^2 2*u(2)*u(3)-2*v(2)*v(3) 2*u(2)*u(4)-2*v(2)*v(4) u(3)^2-v(3)^2 2*u(3)*u(4)-2*v(3)*v(4) u(4)^2-v(4)^2]; + end + u = P1; v = P3; + Temp = [Temp; u(1)*v(1) u(1)*v(2)+u(2)*v(1) u(3)*v(1)+u(1)*v(3) u(1)*v(4)+u(4)*v(1) u(2)*v(2) u(2)*v(3)+u(3)*v(2) u(2)*v(4)+u(4)*v(2) u(3)*v(3) u(4)*v(3)+u(3)*v(4) u(4)*v(4)]; + u = P2; v = P3; + Temp = [Temp; u(1)*v(1) u(1)*v(2)+u(2)*v(1) u(3)*v(1)+u(1)*v(3) u(1)*v(4)+u(4)*v(1) u(2)*v(2) u(2)*v(3)+u(3)*v(2) u(2)*v(4)+u(4)*v(2) u(3)*v(3) u(4)*v(3)+u(3)*v(4) u(4)*v(4)]; +end +% one additional equation only if needed +if n<4 & ~config.cal.SQUARE_PIX + u = P(3,:); + Temp = [Temp; u(1)^2 2*u(1)*u(2) 2*u(1)*u(3) 2*u(1)*u(4) u(2)^2 2*u(2)*u(3) 2*u(2)*u(4) u(3)^2 2*u(3)*u(4) u(4)^2]; + b = [zeros(size(Temp(1:end-1,1)));1]; + % TLS solution of Temp*q=b + [U,S,V] = svd([Temp,b],0); + q = -(1/V(11,end))*V(1:10,end); +else + [U,S,V] = svd(Temp,0); + q = -V(:,size(V,2)); +end + +Q = [ + q(1) q(2) q(3) q(4) + q(2) q(5) q(6) q(7) + q(3) q(6) q(8) q(9) + q(4) q(7) q(9) q(10) +]; +% test which solution to take for q (-V or V) +% diagonal entries of M_M should be positive +M_M = P(1:3,:)*Q*P(1:3,:)'; +if (M_M(1,1)<=0) + q = -q; % V(:,size(V,2)); + Q = [ + q(1) q(2) q(3) q(4) + q(2) q(5) q(6) q(7) + q(3) q(6) q(8) q(9) + q(4) q(7) q(9) q(10) + ]; +end + +[U,S,V] = svd(Q,0); +A = U(:,1:3)*sqrt(S(1:3,1:3)); + +H = [A, B]; + +% euclidian motion and shape +Pe = P*H; +Xe = inv(H)*X; + +% normalize coordiates +Xe = Xe./repmat(Xe(4,:),4,1); + +PeRT = []; +Rot = []; +if 1 + Rot = []; + for i=1:n, + sc = norm(Pe(i*3,1:3),'fro'); + % first normalize the Projection matrices to get normalized pixel points + Pe(i*3-2:i*3,:) = Pe(i*3-2:i*3,:)./sc; + % correct it of points behind the camera + xe = Pe(i*3-2:i*3,:)*Xe; + if sum(xe(3,:)<0), + Pe(i*3-2:i*3,:) = -Pe(i*3-2:i*3,:); + end + + % decompose the matrix by using rq decomposition + [K,R] = rq(Pe(i*3-2:i*3,1:3)); + Cc = -R'*inv(K)*Pe(i*3-2:i*3,4);% camera center + % Stephi calib params + Pst(i*3-2:i*3,:) = R'*inv(K); + Cst(i,:) = Cc'; + % modify the Kalibaration matrix to get consistent + % euclidean motion Pe + K(1,3) = K(1,3)-config.cal.pp(i,1); + K(2,3) = K(2,3)-config.cal.pp(i,2); + PeRT = [PeRT; K*[R,-R*Cc]]; + Rot = [Rot;R]; + end + Pe = PeRT; + C = Cst'; +end + + + + + + + + + + + + + + + + + + + + + diff --git a/MultiCamSelfCal/CoreFunctions/findinl.m b/MultiCamSelfCal/CoreFunctions/findinl.m new file mode 100644 index 0000000..a6b699a --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/findinl.m @@ -0,0 +1,61 @@ +% FindInl find inliers in joint image matrix +% by pairwise epipolar geometry +% +% function IdMatIn = findinl(Ws,IdMat,tol) +% Ws ... 3MxN joint image matrix +% IdMat ... MxN ... 0 -> no point detected +% 1 -> point detected +% tol ... [pixels] tolerance for the epipolar geometry +% the point are accpted as outliers only if they +% are closer to the epipolar line than tol + +% $Author: svoboda $ +% $Revision: 2.1 $ +% $Id: findinl.m,v 2.1 2003/07/30 10:28:29 svoboda Exp $ +% $State: Exp $ + +function IdMatIn = findinl(Ws,IdMat,tol) + +NoCams = size(IdMat,1); + +% fill the array of structures not_used denoted as 0 +% allocate the array of structures for used +for i=1:NoCams, + not_used(i).pts = sum(IdMat(i,:)); + used(i).pts = -1; +end + +% allocate IdMat for outliers +IdMatIn = zeros(size(IdMat)); + +while (sum([not_used.pts])>1-NoCams), + [buff, id.cam_max] = max([not_used.pts]); + used = add(used, id.cam_max, not_used(id.cam_max).pts); + not_used = remove(not_used, id.cam_max); + Mask = repmat(IdMat(id.cam_max,:),NoCams,1); + Corresp = Mask & IdMat; + Corresp(id.cam_max,:) = 0; + [buff, id.cam_to_pair] = max(sum(Corresp')); % find the camera with most correspondences + idx.corr_to_pair = find(sum(IdMat([id.cam_max,id.cam_to_pair],:))==2); + % used = add(used, id.cam_to_pair, not_used(id.cam_to_pair).pts); + % not_used = remove(not_used, id.cam_to_pair); + if size(idx.corr_to_pair,2)<8, + error('Not enough points to compute epipolar geometry in RANSAC validation') + end + Wspair = []; + Wspair = Ws(id.cam_max*3-2:id.cam_max*3, idx.corr_to_pair); + Wspair = [Wspair; Ws(id.cam_to_pair*3-2:id.cam_to_pair*3, idx.corr_to_pair)]; + % id + [F, inls] = rEG(Wspair,tol,tol,0.99); + IdMatIn(id.cam_max, idx.corr_to_pair(inls)) = 1; + IdMat(id.cam_max, :) = 0; + IdMat(id.cam_max, idx.corr_to_pair(inls)) = 1; +end + +function list = add(list, id, value) +list(id).pts = value; +return + +function list = remove(list, id) +list(id).pts = -1; +return diff --git a/MultiCamSelfCal/CoreFunctions/findoutl.m b/MultiCamSelfCal/CoreFunctions/findoutl.m new file mode 100644 index 0000000..dc5e049 --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/findoutl.m @@ -0,0 +1,26 @@ +% find outliers in cameras +% +% [outliers, inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL); +% +% $Id: findoutl.m,v 2.1 2005/05/20 11:58:23 svoboda Exp $ + +function [outliers, inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL); + +CAMS = size(cam,2); + +idxoutMat = zeros(size(inliers.IdMat)); +for i=1:CAMS, + if (cam(i).std2Derr > cam(i).mean2Derr) | (cam(i).mean2Derr > INL_TOL) + reprerrs = cam(i).err2d - cam(i).mean2Derr; + idxout = find((reprerrs > 3*cam(i).std2Derr) & reprerrs > INL_TOL); + else + idxout = []; + end + idxoutMat(i,cam(i).idlin(cam(i).visandrec(idxout))) = 1; +end +inliers.IdMat(:,sum(idxoutMat)>0) = 0; % zero all columns with at least one outlier +inliers.idx = find(sum(inliers.IdMat)>=size(inliers.IdMat,1)-NUM_CAMS_FILL); +outliers = sum(sum(idxoutMat)>0); + +return; + diff --git a/MultiCamSelfCal/CoreFunctions/reprerror.m b/MultiCamSelfCal/CoreFunctions/reprerror.m new file mode 100644 index 0000000..8805b43 --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/reprerror.m @@ -0,0 +1,29 @@ +% reprerror Estimate reprojection error +% +% [cam] = reprerror(cam,Pe,Xe,FRAMES,inliers); +% +% $Id: reprerror.m,v 2.0 2003/06/19 12:06:50 svoboda Exp $ + +function [cam] = reprerror(cam,Pe,Xe,FRAMES,inliers); + +CAMS = size(Pe,1)/3; + +for i=1:CAMS, + xe = Pe(((3*i)-2):(3*i),:)*Xe; + cam(i).xe = xe./repmat(xe(3,:),3,1); + % these points were the input into Martinec and Pajdla filling + mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far + mask.vis = zeros(1,FRAMES); % maks of visible points + mask.rec(inliers.idx) = 1; + mask.vis(cam(i).idlin) = 1; + mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera + unmask.rec = cumsum(mask.rec); + unmask.vis = cumsum(mask.vis); + cam(i).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); + cam(i).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); + cam(i).err2d = sqrt(sum([cam(i).xe(1:2,cam(i).recandvis) - cam(i).xgt(1:2,cam(i).visandrec)].^2)); + cam(i).mean2Derr = mean(cam(i).err2d); + cam(i).std2Derr = std(cam(i).err2d); +end + +return; diff --git a/MultiCamSelfCal/CoreFunctions/rq.m b/MultiCamSelfCal/CoreFunctions/rq.m new file mode 100644 index 0000000..3176a2a --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/rq.m @@ -0,0 +1,33 @@ +%RQ Pajdla: Returns a 3x3 upper triangular R and a unitary Q so that X = R*Q +% +% function [R,Q] = rq(X) +% +% X = input matrix, +% Q = unitary matrix +% R = upper triangular matrix +% +% See also QR. + +% Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be +% pajdla@vision.felk.cvut.cz +% 05/28/94 ESAT-MI2, KU Leuven +% Documentation: +% Language: Matlab 4.1, (c) MathWorks +% +function [R,Q] = rq(X) + + [Qt,Rt] = qr(X'); + Rt = Rt'; + Qt = Qt'; + + Qu(1,:) = cross(Rt(2,:),Rt(3,:)); + Qu(1,:) = Qu(1,:)/norm(Qu(1,:)); + + Qu(2,:) = cross(Qu(1,:),Rt(3,:)); + Qu(2,:) = Qu(2,:)/norm(Qu(2,:)); + + Qu(3,:) = cross(Qu(1,:),Qu(2,:)); + + R = Rt * Qu'; + Q = Qu * Qt; + diff --git a/MultiCamSelfCal/CoreFunctions/selfcalib.m b/MultiCamSelfCal/CoreFunctions/selfcalib.m new file mode 100644 index 0000000..407b828 --- /dev/null +++ b/MultiCamSelfCal/CoreFunctions/selfcalib.m @@ -0,0 +1,81 @@ +function [Xe,Pe,C,R,T,foc] = selfcalib(Ws,IdMat) + +% selfcalib .... performs the self-calibration algorithm on +% a measurement matrix +% +% [Xe,Pe,C,R,T,foc] = selfcalib(Ws) +% +% Ws ........ the 3*nxm measurement matrix +% +% Xe ........ 4xm matrix containg reconstructed calibration points +% Pe ........ 3*CAMSx4 matrix containing estimated camera matrices +% C ......... 4xn matrix containg the camera centers +% R ......... 3*CAMSx3 matrix containing estimated camera rotation matrices +% T ......... 3*n matrix containing the camera translation vectors +% foc ....... CAMSx1 vector containing the focal lengths of the cameras + + +POINTS = size(Ws,2); +CAMS = size(Ws,1)/3; + +if 1 + % normalize image data + % (see Hartley, p.91) + T = []; % the CAMS*3x3 normalization transformations + for i=1:CAMS + [X_i, T_i] = isptnorm(Ws(i*3-2:i*3-1,IdMat(i,:)>0)'); + Ws(i*3-2:i*3-1,IdMat(i,:)>0) = [X_i'; ones(1,sum(IdMat(i,:)>0))]; + T = [T; T_i]; + end +else + T = repmat(eye(3),CAMS,1); +end + +% estimate projective depths +Lambda_est = estimateLambda(Ws,IdMat); +% Lambda_est = ones(CAMS,POINTS); + +if 1 + % normalize estimated lambdas. + % it is more balancing than normalization + % Check it again. Probably not correct? + lambnfr = sum(Lambda_est.^2); + Lambda_est = sqrt(CAMS)*Lambda_est./repmat(sqrt(lambnfr),CAMS,1); + lambnfc = sum(Lambda_est'.^2); + Lambda_est = sqrt(POINTS)*Lambda_est./repmat(sqrt(lambnfc'),1,POINTS); +end + +% no need for negative lambdas +Lambda_est = abs(Lambda_est); + +% Lambda check +% employing lambdas, the Ws should have rank 4 ! +if 0 + lambdaMat = []; + for i=1:CAMS + lambdaMat = [lambdaMat; repmat(Lambda_est(i,:),3,1)]; + end + Ws_rankcheck = lambdaMat.*Ws; + [svd(Ws_rankcheck),svd(Ws)] +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% compute projective shape and motion +[P,X,Lambda] = estimatePX(Ws,Lambda_est); + +% undo normalization +for i=1:CAMS + P(3*i-2:3*i,:) = inv(T(3*i-2:3*i,:))*P(3*i-2:3*i,:); +end + +% Euclidian reconstruction +warn = 0; +[Pe,Xe,C,R,T,foc,warn] = euclidize(Ws,Lambda,P,X); + + + + + + + + diff --git a/MultiCamSelfCal/FindingPoints/atlantic.pm b/MultiCamSelfCal/FindingPoints/atlantic.pm new file mode 100644 index 0000000..486f1f0 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/atlantic.pm @@ -0,0 +1,4 @@ +$process={"compname"=>"atlantic2","camIds"=>[3,4,5,6,7,8,9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic2","camIds"=>[11,12,13,14,15,16,17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamSelfCal/FindingPoints/atlanticall.pm b/MultiCamSelfCal/FindingPoints/atlanticall.pm new file mode 100644 index 0000000..f2b6439 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/atlanticall.pm @@ -0,0 +1,32 @@ +$process={"compname"=>"atlantic3","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic4","camIds"=>[4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic5","camIds"=>[5],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic6","camIds"=>[6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic7","camIds"=>[7],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic8","camIds"=>[8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic9","camIds"=>[9],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic10","camIds"=>[10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic11","camIds"=>[11],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic12","camIds"=>[12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic13","camIds"=>[13],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic14","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic15","camIds"=>[15],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic16","camIds"=>[16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic17","camIds"=>[17],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic18","camIds"=>[18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamSelfCal/FindingPoints/getpoint.m b/MultiCamSelfCal/FindingPoints/getpoint.m new file mode 100644 index 0000000..f18e897 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/getpoint.m @@ -0,0 +1,322 @@ +function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM) +% GETPOINT ... extracts position of an LED from an image +% only one or none LED is expected +% +% function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM) +% +% imname ... name of the image (full path should be specified) +% showfig .. show figures (1->on/0->off) +% imconfig . config.imgs, see CONFIGDATA +% avIM ... average image of the camera, see IM2POINTS +% stdIM ... image of standard deviations, see IM2POINTS +% +% pos ...... 2x1 vector containing (x,y)'-coordinates of the point +% if error then 0 is returned +% err ...... boolean, indicates an error (ambiguous blobs, point too +% eccentric, etc.) + +% $Author: svoboda $ +% $Revision: 2.6 $ +% $Id: getpoint.m,v 2.6 2005/05/20 12:05:40 svoboda Exp $ +% $State: Exp $ + + +err = 0; + +foo = ver('images'); +IPT_VER = str2num(foo.Version(1)); +if IPT_VER >= 5 + region_properties_function = 'regionprops'; +else + region_properties_function = 'imfeature'; +end + +SHOW_WARN = 0; % show warnings? +BLK_PROC = 0; % blkproc may be faster for bigger LEDs, rather not +SUB_PIX = 1/imconfig.subpix; % required sub-pixel precision 3 -> 1/3 pixel + +TEST_AREA = 0; % check the size of the thresholded blob? (mostly not necessary) +TEST_ECC = 0; % perform the eccentricity check? (mostly not necessary) +ECC_THR = 0.99; % eccentricity threshold (for validity check) + % this threshold is not usable in the current implementation + +LEDSIZE = imconfig.LEDsize; % avg diameter of a LED in pixels + +im.name = imname; + +% threshold for the LED detection, set to a default value if not specified +try imconfig.LEDthr; catch imconfig.LEDthr = 70; end + +%%% +% set figure handles +fig.im4thr = 1; % image used for thresholding +fig.imOrig = 2; % original image +fig.blob = 3; % ouput of bwlabel +fig.subI = 4; % subimage (local neighbourhood of est. LED pos.) + +im.info = imfinfo(im.name); +im.orig = imread(im.name); + +if strcmp(im.info.ColorType,'grayscale'); + im.I = im.orig; +else + [im.r,im.c] = size(im.orig(:,:,1)); + im.R = im.orig(:,:,1); % Red component + im.G = im.orig(:,:,2); % Green component +end + +% find possible location of the point by thresholding +if strcmp(imconfig.LEDcolor,'green') + im.thr = uint8(abs(double(im.G(:,:))-double(avIM(:,:,2)))); % on which image the thresholding will be done + im.std = stdIM(:,:,2); % use green component + im.fit = im.G; % image for fitting of the PSF +elseif strcmp(imconfig.LEDcolor,'red') + im.thr = uint8(abs(double(im.R(:,:))-double(avIM(:,:,1)))); % on which image the thresholding will be done + im.std = stdIM(:,:,1); % use red component + im.fit = im.R; % image for fitting of the PSF +elseif strcmp(im.info.ColorType,'grayscale') + im.thr = uint8(abs(double(im.I)-double(avIM))); + im.std = stdIM; + im.fit = im.I; +elseif ~strcmp(im.info.ColorType,'grayscale') & strcmp(imconfig.LEDcolor,'intensity') + try im.I; catch im.I = uint8(round(mean(double(im.orig),3))); end + im.thr = uint8(round(abs(double(im.I)-double(mean(avIM,3))))); + try im.std = stdIM; catch im.std = uint8(round(mean(double(stdIM),3))); end + try im.fit = im.I; catch im.fit = im.I; end +else + error('getpoint: no valid color of the laser pointer, see CONFIGDATA'); +end + +% show figures if required, may be useful when debugging +if showfig + figure(fig.imOrig), + clf + imshow(im.orig) + title(strcat(im.name, ' original')) + hold on + figure(fig.im4thr), + clf + imshow(im.thr); + title(strcat(im.name, ' image to be thresholded')) + drawnow + hold on +end + +[maxint,idx] = max(im.thr(:)); +leds.thr = double(maxint)*0.99; %4/5; + +if ( (im.thr(idx) < 5*double(im.std(idx))) | ( im.thr(idx)< imconfig.LEDthr )) + if SHOW_WARN + warning('Perhaps no LED in the image, detected maximum of image difference is too low') + disp(sprintf('Max: %d, Thr4Max1: %d, Thr4Max2: %d',double(im.thr(idx)), 10*double(im.std(idx)), imconfig.LEDthr)) + end + err=1; + pos=0; + return; +else + [L,num]=bwlabel(im.thr>leds.thr); +end + +if num>1 % sum(diff(any(im.thr>leds.thr,2))>0)>1 + if SHOW_WARN + warning('More than one blob detected') + figure(99), imshow(im.thr>leds.thr), title('thresholded image'); + disp('press a key to continue') + pause + end + err=1; + pos=0; + return; +else + im.stats = eval([region_properties_function,'(L,',char(39),'Centroid',char(39),')']); + rawpos = round([im.stats.Centroid(2),im.stats.Centroid(1)]); + % rawpos = zeros(1,2); + % [rawpos(1),rawpos(2)] = ind2sub(size(im.thr),idx); +end + + +leds.size = round(LEDSIZE/1.2); +% (2*leds.size+1)x(2*leds.size+1) is the area of interest around each detected LED +% check if the LED lies in the allowed position (not very close to the image border +% it is because of the implementation not because of principle +if rawpos(1)-leds.size < 1 | rawpos(1)+leds.size > size(im.thr,1) | ... + rawpos(2)-leds.size < 1 | rawpos(2)+leds.size > size(im.thr,2) + if SHOW_WARN + warning('LED position lies outside allowed boundary'); + end + err = 1; + pos = 0; + return; +end + +leds.rows = (rawpos(1)-leds.size):(rawpos(1)+leds.size); +leds.cols = (rawpos(2)-leds.size):(rawpos(2)+leds.size); +% [L,num] = bwlabel(im.thr(leds.rows,leds.cols)>leds.thr); + +% perform checks of the thresholded blob if required +if TEST_ECC + im.stats = eval([region_properties_function,'(L,',char(39),'Eccentricity',char(39),')']); + if (im.stats.Eccentricity > ECC_THR) + if SHOW_WARN + warning(sprintf('eccentricity treshold %2.4f exceeded by %2.4f', ECC_THR, im.stats.Eccentricity)); + end + err = 1; pos = 0; return; + end +end +if TEST_AREA + im.stats = eval([region_properties_function,'(L,',char(39),'Area',char(39),')']); + if im.stats.Area > size(leds.rows,2)*size(leds.cols,2)/1.5 + if SHOW_WARN + warning('Detected LED to too big') + end + err=1; + pos=0; + return; + end +end + + +% Crop the sub-image of interest around found LED +IM = im.fit(leds.rows,leds.cols); + +% visual check of LED position +if showfig + figure(fig.subI), clf + imshow(im.thr>leds.thr) + hold on + plot(rawpos(2),rawpos(1),'g+','EraseMode','Back'); + drawnow + % pause +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% interpolate local neighborhood and find the maxima +% by correlation with the gaussian (see declaration of Gsize) +leds.scale = SUB_PIX; % the area of interest will be inreased by leds.scale using bilinear interpolation + % leds.size should be comparable to the leds.scale. If leds.size is assumed too small + % then the correlation based detection does not work + % properly +finepos = getfinepos(IM,rawpos,leds,LEDSIZE,BLK_PROC,showfig,SHOW_WARN); + + + +%%% plot information about detected LED +if showfig + figure(fig.im4thr) + plot(finepos(2),finepos(1),'r+','EraseMode','Back'); + figure(fig.imOrig) + plot(finepos(2),finepos(1),'r+','EraseMode','Back','MarkerSize',25,'LineWidth',3); + drawnow +end + +if showfig + pause +end + +pos = [finepos(2); finepos(1)]; + +return % end of the getpoint function + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% +% internal function for finding the fine position +function finepos = getfinepos(IM,rawpos,leds,LEDSIZE,BLK_PROC,showfig,SHOW_WARN) + + Gsize = round(leds.scale*LEDSIZE/2); % (2*Gsize+1)x(2*Gsize+1) is the dimension of the Gaussian mask that models PSF + + % t1 = cputime; + IM2 = imresize(IM,leds.scale,'bicubic'); % zoom in + % disp(sprintf('elapsed for resize: %f',cputime-t1')) + + % Correlation mask that approximates point spread function (PSF) of a LED + Gaussian = fspecial('Gaussian',2*Gsize+1,leds.scale*LEDSIZE/3); + + % activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2)); + % activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2)); + + %%% + % generally it evaluates only 5x5 region aroung the rough position + % it is scaled accordingly. The 5x5 grid is a compromise between the speed + % and the robustness against bad rough position + sc = 2; % 2 is for 5x5 neigh, 1 is for 3x3, 3 is for 9x9 and so on + activerows = ceil(size(IM2,1)/2)-sc*round(leds.scale):ceil(size(IM2,1)/2)+sc*round(leds.scale); + activecols = ceil(size(IM2,2)/2)-sc*round(leds.scale):ceil(size(IM2,2)/2)+sc*round(leds.scale); + im2activerows = ceil(size(IM2,1)/2)-floor(size(Gaussian,1)/2)-sc*round(leds.scale):ceil(size(IM2,1)/2)+floor(size(Gaussian,1)/2)+sc*round(leds.scale); + im2activecols = ceil(size(IM2,2)/2)-floor(size(Gaussian,2)/2)-sc*round(leds.scale):ceil(size(IM2,2)/2)+floor(size(Gaussian,2)/2)+sc*round(leds.scale); + + % CHECK if leds.size and leds.scale have reasonable values + % and correct them if not. + % typically, if the assumed LED size is fairly small, a complete IM2 must be taken + if (min([im2activerows,im2activecols])<1) + if SHOW_WARN + warning('probably incorect setting of leds.size and leds.scale variables') + end + im2activerows = 1:size(IM2,1); + im2activecols = 1:size(IM2,2); + activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2)); + activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2)); + end + + corrcoefmat = zeros(size(IM2)); + % t1 = cputime; + if BLK_PROC % blkproc may be faster for big neighbourhoods + corrcoefmat(activerows,activecols) = blkproc(IM2(activerows,activecols),[1,1],[Gsize,Gsize],'corr2',Gaussian); + else + G = double(Gaussian(:)); + Gn = G-mean(G); + Gn2 = sum(Gn.^2); + B = im2col(double(IM2(im2activerows,im2activecols)),size(Gaussian),'sliding'); + corrcoefmat(activerows,activecols) = col2im(mycorr2(B,G,Gn,Gn2), size(Gaussian), size(IM2(im2activerows,im2activecols)),'sliding'); + % corrcoefmat(activerows,activecols) = colfilt(double(IM2(activerows,activecols)),size(Gaussian),'sliding','mycorr2',G,Gn,Gn2); + end + % disp(sprintf('elapsed for coorrelations: %f',cputime-t1')) + + [maxcorrcoef,idxmaxcorrcoef] = max(corrcoefmat(:)); + [rmax,cmax] = ind2sub(size(corrcoefmat),idxmaxcorrcoef); + finepos = rawpos+([rmax,cmax]-ceil(size(IM2)/2))./leds.scale; + + %%% + % plot the subimage with detected position of the maximal correlation + %%% + + if showfig + figure(5), + clf + subplot(2,2,4) + showimg(IM2,5); + title('Interpolated ROI') + % colormap('gray'); + hold on + axis on + plot(cmax,rmax,'g+','EraseMode','Back','MarkerSize',15,'LineWidth',3) + hold off + subplot(2,2,3) + mesh(corrcoefmat) + title('Correlation coeffs') + subplot(2,2,1) + mesh(double(IM2(activerows,activecols))) + title('Active ROI') + subplot(2,2,2) + mesh(Gaussian) + title('PSF approx by 2D Gaussian') + drawnow + % pause + end + +return + + + + + + + + + + + + + + + + diff --git a/MultiCamSelfCal/FindingPoints/im2imstat.m b/MultiCamSelfCal/FindingPoints/im2imstat.m new file mode 100644 index 0000000..b0e7697 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/im2imstat.m @@ -0,0 +1,140 @@ +% im2imstat computes image statistics from several images +% and finds the projections of laser points +% +% The computation core is taken from im2points.m +% computes average image and image of standard deviations +% requires configdata.m +% The name of the experiment has to be specified +% it determines the location of files etc ... +% +% the scripts serves as a template for a multiprocessing +% it assumes a vector of camera IDs CamIds to be known +% indexes in the CamIds are supposed to be correct +% +% It is used by im2pmultiproc.pl + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: im2imstat.m,v 2.0 2003/06/19 12:06:51 svoboda Exp $ +% $State: Exp $ + +config = configdata(expname); + +STEP4STAT = 5; % step for computing average and std images, if 1 then all images taken + +im.dir = config.paths.img; +im.ext = config.files.imgext; + +% NoCams = size(config.files.idxcams,2); % number of cameras +% Use concrete CameraIds instead of all cameras +NoCams = size(CamsIds,2); +CamsIds + +% load image names +for i=1:NoCams, + seq(i).camId = CamsIds(i); + if seq(i).camId > -1 + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + [sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext] + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + error('Not enough images found. Wrong image path or name pattern?'); + end +end + +% Expected number of 3D points is equal to the number of frames. +% In fact, some frames might be without any calibration point + +% Because of non-consistent stopping of capturing, the sequences might +% have different number of images, select the minimal value as the right one +NoPoints = min([seq.size]); + +% compute the average images that will be used for finding LEDs +% if not already computed + +pointsIdx = [1:STEP4STAT:NoPoints]; + +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.avIM,seq(i).camId)), + disp(sprintf('The average image of the camera %d is being computed',seq(i).camId)); + avIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + avIM = avIM + double(IM); + end + avIM = uint8(round(avIM./size(pointsIdx,2))); + imwrite(avIM,sprintf(config.files.avIM,seq(i).camId)); + else disp('Average files already exist'); + end +end +disp(sprintf('Elapsed time for average images: %d [sec]',cputime-t)) +% compute the standard deviations images that will be used for finding LEDs +% if not already computed +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.stdIM,seq(i).camId)), + avIM = double(imread(sprintf(config.files.avIM,seq(i).camId))); + disp(sprintf('The image of standard deviations of the camera %d is being computed',seq(i).camId)); + stdIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + stdIM = stdIM + (double(IM)-avIM).^2; + end + stdIM = uint8(round(sqrt(stdIM./(size(pointsIdx,2)-1)))); + imwrite(stdIM,sprintf(config.files.stdIM,seq(i).camId)); + else + disp('Image of standard deviations already exist') + end +end +disp(sprintf('Elapsed time for computation of images [sec]: %d',cputime-t)) + +% find points in the images +Ws = []; +IdWs = []; +Res = []; + +IdMat = ones(NoCams,NoPoints); +% IdMat is very important for Martinec&Pajdla filling [ECCV2002] +% it is a NoCams x NoPoints matrix, +% IdMat(i,j) = 0 -> no j-th point in i-th +% IdMat(i,j) = 1 -> point successfully detected + +for i=1:NoCams, + Points = []; + avIM = imread(sprintf(config.files.avIM,seq(i).camId)); + stdIM = imread(sprintf(config.files.stdIM,seq(i).camId)); + for j=1:NoPoints, + [pos,err] = getpoint([sprintf(im.dir,seq(i).camId),seq(i).data(j).name], 0, config.imgs, avIM, stdIM); + if err + IdMat(i,j) = 0; + Points = [Points, [NaN; NaN; NaN]]; + else + Points = [Points, [pos; 1]]; + end + end + Ws = [Ws; Points]; + Res= [Res; size(avIM,2), size(avIM,1)]; +end + +idx = '.'; +for i=CamsIds, + idx = sprintf('%s%02d',idx,i); +end + +save([config.files.points,idx], 'Ws','-ASCII') +% save(config.files.IdPoints,'IdWs','-ASCII') +save([config.files.Res,idx], 'Res', '-ASCII') +save([config.files.IdMat,idx], 'IdMat', '-ASCII') + +% write auxiliary file that is done +done=1; +save(donefile,'done','-ascii'); + +% exit the Matlab +% this script is to be used in the batch mode +% hence exit at the end is necessary +exit; \ No newline at end of file diff --git a/MultiCamSelfCal/FindingPoints/im2pmultiproc.pl b/MultiCamSelfCal/FindingPoints/im2pmultiproc.pl new file mode 100755 index 0000000..c086907 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/im2pmultiproc.pl @@ -0,0 +1,92 @@ +#! /usr/bin/perl -w + +# find projections of LEDs in images +# it spreads the job to all specified processors/machines +# It requires: +# - matlab +# - configdata.m +# - expname +# +# script is useful if more machines or more processors +# are available. +# automatic ssh authentication is necessary, see +# http://www.cs.umd.edu/~arun/misc/ssh.html +# http://www.cvm.uiuc.edu/~ms-drake/linux/ssh_nopw.html + +# $Author: svoboda $ +# $Revision: 2.0 $ +# $Id: im2pmultiproc.pl,v 2.0 2003/06/19 12:06:52 svoboda Exp $ +# $State: Exp $ + +use Env; +# use XML::Simple; +# use Data::Dumper; + +# name of the m-file template +$mfile="im2imstat"; +$mvariable="CamsIds"; +$donefile=".done"; + +# not yet ready +# $config = XMLin('multiprocAtlantic.xml'); +# print Dumper($config); +# @processes = @$config-> + +# load the info about local machines and parallel processes +use atlantic; + +# just a debug cycle +foreach $process (@processes) { + print "$process->{'compname'} \n"; + foreach $camId (@{$process->{'camIds'}}) { + print "$camId \n"; + } +} + +# create temporary m-files with using the template m-file +# store their names for delete at the very end +# each process must have its own temporary m-file + +# compose the names and commands +foreach $process (@processes) { + $idfile=""; + $str4cmd=""; + foreach $camId (@{$process->{'camIds'}}) { + $idfile = "$idfile$camId"; + $str4cmd = "$str4cmd $camId"; + } + $process->{'scriptName'} = "$ENV{'PWD'}/$mfile${idfile}.m"; + $process->{'donefile'} = "$ENV{'PWD'}/${donefile}${idfile}"; + $process->{'catcmd'} = "echo \"${mvariable} = [${str4cmd}]; donefile='$process->{'donefile'}'; addpath $ENV{'PWD'}; addpath $ENV{'PWD'}/../Cfg; \" | cat - ${mfile}.m > $process->{'scriptName'}"; + $process->{'mcmd'} = "ssh $process->{'compname'} /usr/sepp/bin/matlab -nosplash -nojvm < $process->{'scriptName'} > $process->{'scriptName'}.out &"; +} + +print "*************************** \n"; + +# create the auxiliary scripts +foreach $process (@processes) { + system($process->{'catcmd'}); +} + +# run the parallel matlabs +foreach $process (@processes) { + system($process->{'mcmd'}); +} + +# wait until all processing is done +print "Image processing in progress. Plase be patient \n"; +do { + sleep 10; + @donefiles = glob("${donefile}*"); +} while (@donefiles < @processes); + +# copy the data files to a coomon disc space yet to be implemented + +# final cleaning of the auxiliary files +# foreach $process (@processes) { +# system("rm -f $process->{'donefile'}"); +# system("rm -f $process->{'scriptName'}"); +# system("rm -f $process->{'scriptName'}.out"); +# } +# system("rm -f *.out"); + diff --git a/MultiCamSelfCal/FindingPoints/im2points.m b/MultiCamSelfCal/FindingPoints/im2points.m new file mode 100644 index 0000000..cac1238 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/im2points.m @@ -0,0 +1,185 @@ +% Read all images and extract point coordinates. +% +% All information needed are stored and retrieved +% from the function CONFIGDATA + +% $Author: svoboda $ +% $Revision: 2.6 $ +% $Id: im2points.m,v 2.6 2005/05/23 16:26:03 svoboda Exp $ +% $State: Exp $ + +clear all; + +% add path to config data +addpath ../../CommonCfgAndIO +% add path for graphical output if needed +addpath ../OutputFunctions + +SHOWFIG = 0; % show images during point extraction +STEP4STAT = 1; % step for computing average and std images, if 1 then all images taken + +config = configdata(expname); + +im.dir = config.paths.img; +im.ext = config.files.imgext; + +NoCams = size(config.files.idxcams,2); % number of cameras + +% load image names +for i=1:NoCams, + seq(i).camId = config.files.idxcams(i); + if seq(i).camId > -1 + if findstr(expname,'oscar') + seq(i).data = dir([sprintf(im.dir,seq(i).camId),config.files.imnames,'*.',im.ext]); + else + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + end + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + error('Not enough images found. Wrong image path or name pattern?'); + end +end + + +% create an occupancy matrix for image frames +occmat=1; try config.files.maxid; catch occmat=0; end +if occmat, + NoPoints = config.files.maxid; + FrameMat = zeros(config.files.maxid,NoCams); + for i=1:NoCams, + seq(i).imgidx = zeros(size(1:NoPoints)); + for j=1:size(seq(i).data,1) + FrameMat(str2num(seq(i).data(j).name(config.files.posid)),i)=j; + end + end +else + NoPoints = min([seq.size]); + FrameMat = zeros(NoPoints,NoCams); + for i=1:NoCams, + FrameMat(:,i) = [1:NoPoints]'; + end +end + +% In fact, some frames might be without any calibration point + +% Becouse of non-consistent stopping of capturing, the sequences might +% have different number of images, select the minimal value as the right one + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% beginning of the findings + +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.avIM,seq(i).camId)), + disp(sprintf('The average image of the camera %d is being computed',seq(i).camId)); + avIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + pointIdx = 1:STEP4STAT:seq(i).size; + for j=pointIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + avIM = avIM + double(IM); + end + avIM = uint8(round(avIM./size(pointIdx,2))); + imwrite(avIM,sprintf(config.files.avIM,seq(i).camId)); + else disp('Average file already exists'); + end +end +disp(sprintf('Elapsed time for computation of average images: %4.2f [sec]',cputime-t)) +% compute the standard deviations images that will be used for finding LEDs +% if not already computed +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.stdIM,seq(i).camId)), + avIM = double(imread(sprintf(config.files.avIM,seq(i).camId))); + disp(sprintf('The image of standard deviations of the camera %d is being computed',seq(i).camId)); + stdIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + pointIdx = 1:STEP4STAT:seq(i).size; + for j=pointIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + stdIM = stdIM + (double(IM)-avIM).^2; + end + stdIM = uint8(round(sqrt(stdIM./(size(pointIdx,2)-1)))); + imwrite(stdIM,sprintf(config.files.stdIM,seq(i).camId)); + else + disp('Image of standard deviations already exists') + end +end + +disp(sprintf('Elapsed time for computation of variance images: %4.2f [sec]',cputime-t)) + +% find points in the images +Ws = []; % joint image matrix +Res = []; % resolution of cameras +% UsableFramesIdx = find(sum(FrameMat')>2); +IdMat = ones(NoCams,NoPoints); +% IdMat is very important for Martinec&Pajdla filling [ECCV2002] +% it is a NoCams x NoPoints matrix, +% IdMat(i,j) = 0 -> no j-th point in i-th +% IdMat(i,j) = 1 -> point successfully detected + + +disp('*********************************************') +disp('Finding points (laser projections) in cameras') +disp(sprintf('Totally %d cameras, %d images for each cam', NoCams, NoPoints')) +disp('*********************************************') +for i=1:NoCams, + t1 = cputime; + disp(sprintf('Finding points in camera No: %0.2d',config.files.idxcams(i))) + Points = []; + avIM = imread(sprintf(config.files.avIM,seq(i).camId)); + stdIM = imread(sprintf(config.files.stdIM,seq(i).camId)); + for j=1:NoPoints, + fprintf(1,'\b\b\b\b\b\b %5d',j); + idx2data = FrameMat(j,i); + if idx2data + [pos,err] = getpoint([sprintf(im.dir,seq(i).camId),seq(i).data(idx2data).name], SHOWFIG, config.imgs, avIM, stdIM); + else + err = 1; + end + if err + IdMat(i,j) = 0; + Points = [Points, [NaN; NaN; NaN]]; + else + Points = [Points, [pos; 1]]; + end + end + Ws = [Ws; Points]; + Res= [Res; size(avIM,2), size(avIM,1)]; + t2 = cputime; + disp(sprintf('\nElapsed time for finding points in one camera: %d minutes %d seconds',floor((t2-t1)/60), round(mod((t2-t1),60)))) + disp(sprintf('%4d points found in camera No: %0.2d',sum(Points(3,:)>0),config.files.idxcams(i))); +end + +%%% End of the findings +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +if findstr(expname,'oscar') + % needs special care for handling projector data + ProjPoints = load(config.files.projdata,'-ASCII'); + Ws = [Ws; ProjPoints(:,end-1:end)'; ones(size(ProjPoints(:,1)'))]; + IdMat = [IdMat; ones(size(ProjPoints(:,1)'))]; + Res = [Res; config.imgs.projres]; +end + +save(config.files.points, 'Ws','-ASCII') +save(config.files.Res, 'Res', '-ASCII') +save(config.files.IdMat, 'IdMat', '-ASCII') + +% display the overall statistics +disp('Overall statistics from im2points: ************************ ') +disp(sprintf('Total number of frames (possible 3D points): %d',NoPoints)) +disp(sprintf('Total number of cameras %d', NoCams)) +disp('More important statistics: ********************************* ') +disp(sprintf('Detected 3D points: %d', sum(sum(IdMat)>0))) +disp(sprintf('Detected 3D points in at least 3 cams: %d', sum(sum(IdMat)>2))) +disp(sprintf('Detected 3D points in ALL cameras: %d', sum(sum(IdMat)==NoCams))) + + + + + + + + diff --git a/MultiCamSelfCal/FindingPoints/mycorr2.m b/MultiCamSelfCal/FindingPoints/mycorr2.m new file mode 100644 index 0000000..9c4320e --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/mycorr2.m @@ -0,0 +1,24 @@ +% mycorr2 modified version of the 2D correlation +% for the use with im2col and col2im +% see GETPOINT +% +% +% $Id: mycorr2.m,v 2.0 2003/06/19 12:06:52 svoboda Exp $ + +% Note: It written in order to gain speed. The clarity of the code suffers accordingly + +function R = mycorr2(X,G,Gn,Gn2) + +% Gn = G-mean(G); +% Gn2 = sqrt(sum(Gn.^2)); + +mX = repmat(mean(X),size(X,1),1); +mXn = X - mX; +smX = sum(mXn.^2); + +numerator = (mXn'*Gn)'; +denominator = smX*Gn2; + +R = numerator./denominator; + +return \ No newline at end of file diff --git a/MultiCamSelfCal/FindingPoints/virooms.pm b/MultiCamSelfCal/FindingPoints/virooms.pm new file mode 100644 index 0000000..d490ba5 --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/virooms.pm @@ -0,0 +1,16 @@ +$process={"compname"=>"viroom00","camIds"=>[3,4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom00","camIds"=>[5,6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom01","camIds"=>[7,8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom01","camIds"=>[9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom02","camIds"=>[11,12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom02","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[15,16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamSelfCal/FindingPoints/virooms4.pm b/MultiCamSelfCal/FindingPoints/virooms4.pm new file mode 100644 index 0000000..d2a7b2f --- /dev/null +++ b/MultiCamSelfCal/FindingPoints/virooms4.pm @@ -0,0 +1,8 @@ +$process={"compname"=>"viroom00","camIds"=>[0],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom00","camIds"=>[1],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[2],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamSelfCal/LocalAlignments/erlangen.m b/MultiCamSelfCal/LocalAlignments/erlangen.m new file mode 100644 index 0000000..d45ff93 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/erlangen.m @@ -0,0 +1,61 @@ +function [align] = erlangen(in,config) +% erlangen ... local routines for the hoengg installation +% +% [align] = erlangen(in,config) +% in, cam, config ... see the main GOCAL script +% +% align ... structures aligned wit the specified world frame +% +% $Id: erlangen.m,v 1.3 2005/05/20 15:31:30 svoboda Exp $ + +REALVIZ = 0; + +Cst = in.Cst; +Rot = in.Rot; + +drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); + +% definition of the absolute world frame + +if REALVIZ + cam(1).C = [1.20055 -1.85769 4.02702]'; + cam(2).C = [0.382772 -4.49652 5.28274]'; + cam(3).C = [-2.99133 -3.90767 6.1968]'; + cam(4).C = [-2.92944 -2.40276 -7.78579]'; + cam(5).C = [-9.00505 -5.61218 -9.73132]'; + cam(6).C = [-7.76965 -2.94546 -1.63609]'; +else % own measurement + cam(1).C = [1.16, 0.3, 1.8]'; + cam(2).C = [1.1, 3.0, 2.5]'; + cam(3).C = [-0.9, 2.4, 1.56]'; + cam(4).C = [0.15, -2.5, 2.2]'; + cam(5).C = [-1.95, -2.65, 2.45]'; + cam(6).C = [-1.75, -2.2, 1.42]'; +end +% of the similarity computation + +[align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst'],[cam(:).C]); +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0]); +set(gca,'CameraPosition',[0,0,1]); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'topview.eps']) + +drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0.9]); +set(gca,'CameraPosition',[2,0,0.9]); + +figure(62), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'sideview.eps']) + +return diff --git a/MultiCamSelfCal/LocalAlignments/g9.m b/MultiCamSelfCal/LocalAlignments/g9.m new file mode 100644 index 0000000..8945491 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/g9.m @@ -0,0 +1,52 @@ +function [align] = g9(in,config) +% g9 ... local alignment for room G9 +% +% [align] = G9(in,config) +% in, cam, config ... see the main GOCAL script +% +% align ... structures aligned wit the specified world frame +% +% $Id: g9.m,v 1.1 2005/05/20 15:31:31 svoboda Exp $ + +REALVIZ = 0; + +Cst = in.Cst; +Rot = in.Rot; + +drawscene(in.Xe,Cst',Rot,41,'cloud','Graphical Output Validation: View from top or bottom',config.cal.cams2use); + +% definition of the absolute world frame +% cam(1).C = [3, 1.25, 0.57]'; +cam(2).C = [3, 1.8, 0.2]'; +cam(3).C = [-2.2, 2.05, 0.1]'; +cam(4).C = [-2.2, 2.05, 3.2]'; + +idx = [2:4]; + +% of the similarity computation + +[align.simT.s, align.simT.R, align.simT.t] = estsimt([Cst(idx,:)'],[cam(idx).C]); +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0]); +set(gca,'CameraPosition',[0,1,0]); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'topview.eps']) + +drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data',config.cal.cams2use); + +set(gca,'CameraTarget',[0,2.05,0]); +set(gca,'CameraPosition',[0,2.05,3]); + +figure(62), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'sideview.eps']) + +return diff --git a/MultiCamSelfCal/LocalAlignments/joinoscars.m b/MultiCamSelfCal/LocalAlignments/joinoscars.m new file mode 100644 index 0000000..48e7105 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/joinoscars.m @@ -0,0 +1,54 @@ +% auxiliary script for joining particular oscar settings +% +% The current version is still not optimal. It would be good to have +% some nicer version. It is not very automatic and it does not use the configdata +% +% $Id: joinoscars.m,v 1.5 2004/04/06 12:57:57 svoboda Exp $ +% + +clear all; + +datapath = '/home/svoboda/viroomData/oscar/demo_3p3c_p%d/'; +setups = [1:3]; % related to the data path +path2save = '/home/svoboda/viroomData/oscar/oscardemo3/'; + +% Some dirty scale factors to make ETH Oscar setup compatible +% with Leuven libraries +% the coordinates and resolutions will be divided by theses values +scalefactor.cameras = 1; +scalefactor.projectors = 1; + +% the following indexes are related to the setups +idxcams = [1:4]; +idxproj = [5]; + +% load the partial data sets and join them in a consitent way +% tested with three setups, three cameras and one projector each. +% the three cameras are the same for all the particular datasets +Ws = []; IdMat = []; Res = []; +for i=1:size(setups,2), + pts = load([sprintf(datapath,setups(i)),'points.dat']); + proj(1:3*size(setups,2),1:size(pts,2)) = NaN; + proj(i*3-2:i*3,:) = pts(idxproj*3-2:idxproj*3,:); + proj(i*3-2:i*3-1,:) = proj(i*3-2:i*3-1,:)/scalefactor.projectors; + pts(1:3:3*size(idxcams,2),:) = pts(1:3:3*size(idxcams,2),:)/scalefactor.cameras; + pts(2:3:3*size(idxcams,2),:) = pts(2:3:3*size(idxcams,2),:)/scalefactor.cameras; + Ws = [Ws, [pts(1:3*size(idxcams,2),:); proj]]; + id = load([sprintf(datapath,setups(i)),'IdMat.dat']); + projid(1:size(setups,2),1:size(pts,2)) = 0; + projid(i,:)=id(idxproj,:); + IdMat = [IdMat,[id(idxcams,:); projid]]; + Res = [Res; load([sprintf(datapath,setups(i)),'Res.dat'])]; + proj =[]; projid=[]; +end +% The Res matrix needs a special care when putting +% camera and projector resolutions together +Res2 = Res([idxcams,idxproj:size(idxcams,2)+1:end],:); +Res2(idxcams,:) = Res2(idxcams,:)/scalefactor.cameras; +Res2(end-2:end,:) = Res2(end-2:end,:)/scalefactor.projectors; + +% savings +save([path2save,'points.dat'],'Ws','-ASCII'); +save([path2save,'IdMat.dat'],'IdMat','-ASCII'); +save([path2save,'Res.dat'],'Res2','-ASCII'); + diff --git a/MultiCamSelfCal/LocalAlignments/nfi2r.m b/MultiCamSelfCal/LocalAlignments/nfi2r.m new file mode 100644 index 0000000..7c10574 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/nfi2r.m @@ -0,0 +1,37 @@ +% nfi2r Computes rotation matrix, axis of rotation and the angle are given +% +% R = nfi2r(n,fi) +% Inputs: +% n[3x1] ... axis of rotation (vector of direction) +% fi[rad] ... angle of rotation (counter clockwise) +% Return: +% R ... rotation matrix +% +% T. Svoboda, 3/1998, CMP Prague +% +% $Id: nfi2r.m,v 1.1 2003/07/03 15:38:40 svoboda Exp $ + +% page 203 of the book: +% @BOOK{Kanatani90, +% AUTHOR = {Kanatani, Kenichi}, +% PUBLISHER = {Springer-{V}erlag}, +% TITLE = {Group-{T}heoretical Methods in Image Understanding}, +% YEAR = {1990}, +% HARDCOPY = { CMPlib.book.BC14 }, +% SIGNATURE = {X-copy}, +% ISSN_ISBN = {3-540-51263-5}, +% + +function R = nfi2r(n,fi) + +n = n./norm(n,2); +cfi = cos(fi); +sfi = sin(fi); + +R(1,1:3) = [ cfi+n(1)^2*(1-cfi), n(1)*n(2)*(1-cfi)-n(3)*sfi, n(1)*n(3)*(1-cfi)+n(2)*sfi ]; +R(2,1:3) = [ n(1)*n(2)*(1-cfi)+n(3)*sfi, cfi+n(2)^2*(1-cfi), n(2)*n(3)*(1-cfi)-n(1)*sfi ]; +R(3,1:3) = [ n(3)*n(1)*(1-cfi)-n(2)*sfi, n(3)*n(2)*(1-cfi)+n(1)*sfi, cfi+n(3)^2*(1-cfi) ]; + +R = R'; % due to reverse notation of Kanatani + +return diff --git a/MultiCamSelfCal/LocalAlignments/planarcams.m b/MultiCamSelfCal/LocalAlignments/planarcams.m new file mode 100644 index 0000000..63f89bc --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/planarcams.m @@ -0,0 +1,55 @@ +% planarcams ... alignment under assumption of planar camera arrangements +% +% [align,cam] = planarmove(in,cam,config,idx) +% in, cam, config ... see the main GOCAL script +% idx ... indexes of cameras which are suppose to be in one plane +% +% align ... structures aligned wit the specified world frame +% +% $Id: planarcams.m,v 1.1 2003/07/03 15:36:55 svoboda Exp $ + +function [align,cam] = planarcams(in,cam,config,idx) + +% fit a plane to the reconstructed points and estimate normal + +plane.n = planefit(in.Ce(:,idx)'); + +new.n = [0,0,1]'; % align the xy plane horizontally + +rotaxis = cross(new.n,plane.n); +rotangle = acos( (plane.n'*new.n)/norm(plane.n)*norm(new.n) ); + +R = nfi2r(rotaxis,rotangle); +s = 3; +t = [0,0,1]' - s*R*mean(in.Xe(1:3,:)')'; + +align.simT.s = s; +align.simT.R = R; +align.simT.t = t; + +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data, TopView',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0]); +set(gca,'CameraPosition',[0,0,1]); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'topview.eps']) + +drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data, SideView',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0.9]); +set(gca,'CameraPosition',[2,0,0.9]); + +figure(62), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'sideview.eps']) + +return + + diff --git a/MultiCamSelfCal/LocalAlignments/planarmove.m b/MultiCamSelfCal/LocalAlignments/planarmove.m new file mode 100644 index 0000000..a97f829 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/planarmove.m @@ -0,0 +1,53 @@ +function [align] = planarmove(in,config) +% planarmove ... alignment under assumption of planar motion +% +% [align] = planarmove(in,config) +% in, cam, config ... see the main GOCAL script +% +% align ... structures aligned wit the specified world frame +% +% $Id: planarmove.m,v 1.2 2005/05/20 15:31:31 svoboda Exp $ + +% fit a plane to the reconstructed points and estimate normal + +plane.n = planefit(in.Xe(1:3,:)'); + +new.n = [0,0,1]'; % align the xy plane horizontally + +rotaxis = cross(plane.n,new.n); +rotangle = acos( (plane.n'*new.n)/norm(plane.n)*norm(new.n) ); + +R = nfi2r(rotaxis,rotangle); +s = 3; +t = [0,0,1]' - s*R*mean(in.Xe(1:3,:)')'; + +align.simT.s = s; +align.simT.R = R; +align.simT.t = t; + +[align.P, align.X] = align3d(in.Pe,in.Xe,align.simT); +% save aligned data +if 1 % SAVE_STEPHI | SAVE_PGUHA + [align.Cst,align.Rot] = savecalpar(align.P,config); +end +drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data, TopView',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,1]); +set(gca,'CameraPosition',[0,0,2]); + +figure(61), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'topview.eps']) + +drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data, SideView',config.cal.cams2use); + +set(gca,'CameraTarget',[0,0,0.9]); +set(gca,'CameraPosition',[2,0,0.9]); + +figure(62), +% print -depsc graphevalaligned.eps +eval(['print -depsc ', config.paths.data, 'sideview.eps']) + +return + + diff --git a/MultiCamSelfCal/LocalAlignments/planefit.m b/MultiCamSelfCal/LocalAlignments/planefit.m new file mode 100644 index 0000000..ace0578 --- /dev/null +++ b/MultiCamSelfCal/LocalAlignments/planefit.m @@ -0,0 +1,20 @@ +% planefit ... fit a plane to a point cloud +% algebraic minimization applied +% +% n = planefit(X); +% X ... Nx3 matrix with 3D points +% +% n ... 3x1 normal of the fitted plane +% +% $Id: planefit.m,v 1.1 2003/07/03 15:34:43 svoboda Exp $ + +function n = planefit(X); + +X = X-repmat(mean(X),size(X,1),1); + +[u,s,v] =svd(X); + +n = v(:,3); +n = n./norm(n); + +return diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/L2depths.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/L2depths.m new file mode 100644 index 0000000..93c82e2 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/L2depths.m @@ -0,0 +1,54 @@ +%L2depths Compute depths of PRMM from basis L. +% +% No known depths are exploited. These can be different because of noise +% etc. +% +% Parameters: +% opt.verbose(1) .. whether display info + +function [Mdepths, lambda] = L2depths(L, M, Idepths, opt) + +if nargin < 4, opt = []; end +if ~isfield(opt,'verbose') + opt.verbose = 1; end + +if opt.verbose, fprintf('Computing depths...'); tic; end + +Mdepths = M; + +[m n] = size(M); m = m/3; +lambda(m, n) = 0; % memory allocation + +for j = 1:n + full = find(~isnan(M(1:3:end,j))); + mis_rows = intersect(find(Idepths(:,j)==0),full); + if length(mis_rows) > 0 + submatrix = spread_depths_col(M(k2i(full),j),Idepths(full,j)); + + % We want submatrix to be in the space L -> + % we search for combination of columns of the base of L i.e. + % L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))] = 0 + right = submatrix(:,1); + A = [ L(k2i(full),:) -(submatrix(:,2:size(submatrix,2))) ]; + if rank(A) < size(A, 2) % depths cannot be computed => kill the data + kill = full(~Idepths(full,j)); + Mdepths(k2i(kill),j) = NaN; lambda(kill,j) = NaN; + else + res = A \ right; + + %test: er should be near to zero + %er=L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))']' + + % depth corresponding to right is/are set to 1 + i = full(find(right(1:3:end))); lambda(i,j) = 1; + Mdepths(k2i(i),j) = M(k2i(i),j); + + for ii = 1:size(submatrix,2)-1 + i = full(find(submatrix(1:3:end,1+ii))); lambda(i,j) = res(4+ii); + Mdepths(k2i(i),j) = M(k2i(i),j)*lambda(i,j); + end + end + end +end + +if opt.verbose, disp(['(' num2str(toc) ' sec)']); end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m new file mode 100644 index 0000000..52ecfec --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/M2Fe.m @@ -0,0 +1,44 @@ +%M2Fe Estimate epipolar geometry of MM in sequence or using the central image. +% +% [F,e,rows,nonrows] = M2Fe(M, central) +% +% Parametres: +% central ... If zero, the concept of sequence is used otherwise the +% concept of the central image is used with central +% image number ``central''. + +function [F,ep,rows,nonrows] = M2Fe(M, central) + +m = size(M,1)/3; nonrows = []; F = []; ep = []; + +if central, rows = [1:central-1, central+1:m]; +else, rows = [2:m]; end + +%estimate the fundamental matrices and epipoles with the method of [Har95] +for k = rows + if central, j = central; + else, j = k-1; end + G = u2FI([M(3*k-2:3*k,:);M(3*j-2:3*j,:)], 'donorm'); + if G==0 + rows = setdiff(rows,k); + nonrows = [nonrows k]; + else + %ep=null(G'); %it must be transposed otherwise it's the second epipole + % sometimes returns empty matrix => compute it "by hand" by svd + [u,s,v] = svd(G); + epip = u(:,3); + + F(k,j,1:3,1:3) = G; + ep(k,j,1:3) = epip; + end +end + +if central, rows = union(rows, central); +else rows = [1 rows]; end + +if ~isempty(nonrows) & ~central % find the longest continuous subsequence + I_(rows,1) = 1; + [b, len] = subseq_longest(I_); + rows = b:b+len-1; + nonrows = setdiff(1:m, rows); +end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/approximate.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/approximate.m new file mode 100644 index 0000000..de8a985 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/approximate.m @@ -0,0 +1,119 @@ +%approximate Compute r-rank approximation of MM using null-space. +% +% The approximated matrix is P*X. + +function [P,X, u1,u2] = approximate(M, r, P, opt) + +[m n] = size(M); +rows = find(mean(abs(P')) > opt.tol); %find(sum(P'))'; +nonrows = setdiff(1:m, rows); + +if opt.verbose, fprintf(1,'Immersing columns of MM into the basis...'); tic;end +[Mapp, noncols, X] = approx_matrix(M(rows,:), P(rows,:),r, opt); +if opt.verbose, disp(['(' num2str(toc) ' sec)']); end + +% The rows of NULLSPACE now contain all the nullspaces of the crossproduct +% spaces. Use the nullspace to approximate M, taking r least principal +% components. +cols = setdiffJ(1:n, noncols); +if length(cols) < r; % it will not be able to extend the matrix correctly + u1 = 1:m; u2 = 1:n; +else + if isempty(nonrows), u1 = []; r1 = 1:m; else + if opt.verbose, + disp('Filling rows of MM which have not been computed yet...'); end + [Mapp1, u1, Pnonrows] = extend_matrix(M(:,cols), Mapp(:,cols), ... + X(:,cols), rows, nonrows, r,opt.tol); + Mapp = []; Mapp(:,cols) = Mapp1; + if length(u1) < length(nonrows), P(setdiff(nonrows,u1),:) = Pnonrows; end + r1 = union(rows, setdiff(nonrows,u1)); P = P(r1,:); + end + if isempty(noncols), u2 = []; else + if opt.verbose + disp('Filling columns of MM which have not been computed yet...'); end + [Mapp_tr, u2, Xnoncols_tr] = extend_matrix(M(r1,:)', Mapp(r1,cols)', P',... + cols, noncols, r, opt.tol); + Mapp2 = Mapp_tr'; Xnoncols = Xnoncols_tr'; + if length(u2) < length(noncols), X(:,setdiff(noncols,u2)) = Xnoncols; end + X = X(:, union(cols, setdiff(noncols, u2))); + if sum(~sum(X)), keyboard; end + end +% These extensions are necessary because the nullspace might not allow us to +% compute every row of the approximating rank r linear space, and then this +% linear space might not allow us to fill in some columns, if they are +% missing too much data. +end +if sum(~sum(X)), keyboard; end + + +%approx_matrix Immerse columns of MM into the basis. + +function [Mapp, misscols, X] = approx_matrix(M, P, r, opt) + +[m n] = size(M); misscols = []; +Mapp(m, n) = NaN; X(r,n) = NaN; % memory allocation + +if ~isempty(P) +% P has r columns, which span the r-D space that gives the best linear +% surface to approximate M. + for j = 1:n + rows = find(~isnan(M(:,j))); + if rank(P(rows,:), opt.tol) == r + X(:,j) = P(rows,:)\M(rows,j); Mapp(:,j) = P*X(:,j); + else + misscols = [misscols, j]; + end + end +end + + +%extend_matrix Fill rows of MM which have not been computed yet. +% +% [E, unrecovered] = extend_matrix(M,INC,subM, rows, nonrows, r) +% +% Parameters: +% rows ... rows of M which have been used to find the solution +% subM ... is a fit to just these rows +% nonrows ... indicate rows of that still need to be fit +% +% Output parameters: +% Pnonrows ... in terms of unrecovered rows + +function [E, unrecovered, Pnonrows] = extend_matrix(M, subM, X, rows, ... + nonrows, r, tol) + +E(size(M,1),size(M,2)) = NaN; +E(rows,:) = subM; unrecovered = []; row = 0; Pnonrows = []; +for i = nonrows + cols = find(~isnan(M(i,:))); + if rank(X(:,cols)) == r + if max(abs(M(i,cols)/X(:,cols) * X)) > 1/tol + unrecovered = [unrecovered i]; + else, row = row + 1; + Pnonrows(row,:) = M(i,cols)/X(:,cols); E(i,:) = Pnonrows(row,:)*X; + end + else + unrecovered = [unrecovered i]; + end +end + + +function c = setdiffJ(a,b) %J as Jacobs so that matlab's setdiff isn't damaged +c = []; +for i = a + if ~member(i,b) + c = [c,i]; + end +end + + +%member(e,s) Return 1 if element e is a member of set s. Otherwise return 0. + +function c = member(e,s) +c = 0; +for i = s + if i == e + c = 1; + break + end +end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/balance_triplets.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/balance_triplets.m new file mode 100644 index 0000000..b384906 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/balance_triplets.m @@ -0,0 +1,83 @@ +%balance_triplets Balance PRMM by column-wise and "triplet-of-rows"-wise +% scalar multiplications. +% +% After balancing, overall weight of M will be m*n where 3m*n is size of M +% (i.e. 3 coordinates of each image point will give together 1 in average). +% +% Parameters: +% opt.info_separately .. 1(default) .. display info on separate row +% 0 .. display info in brackets +% .verbose(1) .. whether display info + +function B = balance_triplets(M, opt); + +if nargin < 2, opt = []; end +if ~isfield(opt,'info_separately') + opt.info_separately = 1; end +if ~isfield(opt,'verbose') + opt.verbose = 1; end + +if opt.verbose + if opt.info_separately, fprintf(1,'Balancing PRMM...'); tic + else, fprintf(1,'(balancing PRMM...'); tic; end; end + +m=size(M,1)/3; %number of cameras +n=size(M,2); %number of points + +B=M; change=inf; diff_rows = inf; iteration = 0; +while (change > 0.01 | diff_rows > 1 | diff_cols > 1) & iteration <= 20 + Bold=B; + + % 1. rescale each column l so that sum w_r_l^2=1 i.e. column of unit + % length. However, due to the missing data, the length must be smaller + % in (linear) dependance on amount of missing data. + diff_cols = -inf; + for l=1:n + rows = find(~isnan(M(1:3:end,l))); + if length(rows) > 0 + rowsb = k2i(rows); + s = sum(B(rowsb,l) .^ 2); + supposed_weight = length(rows); % the less data, the slighter impact + diff_cols = max(abs(s-supposed_weight), diff_cols); + B(rowsb,l) = B(rowsb,l) ./ sqrt(s/supposed_weight); + end + end + + % 2. rescale each triplet of rows so that it's sum w_i_l^2=1 i.e. unit + % area. However, due to the missing data, the area must be smaller in + % (linear) dependance on amount of missing data. + diff_rows = -inf; + for k=1:m + cols = find(~isnan(M(3*k,:))); + if length(cols) > 0 + s = sum(sum( B(3*k-2:3*k,cols) .^ 2 )); + supposed_weight = length(cols); % the less data, the slighter impact + diff_rows = max(abs(s-supposed_weight), diff_rows); + B(3*k-2:3*k,cols) = B(3*k-2:3*k,cols) ./ sqrt(s/supposed_weight); + end + end + + % repeat steps 1 and 2 if W changed significantly + % Note: It is not ensured that sums (s) would not change significantly + % in the (hypothetical) next step. The reason is that rescaling + % No. 1 rescales n columns to overall weight n whereas rescaling + % No. 2 rescales m triplets of rows to overall weight m. + change = 0; + for k=1:m + cols = find(~isnan(M(3*k,:))); + change = change + sum(sum( (B (3*k-2:3*k,cols)-... + Bold(3*k-2:3*k,cols)) .^ 2 )); + end + %disp(sprintf('change=%f, diff_cols=%f, diff_rows=%f', ... + % change, diff_cols, diff_rows)); + iteration = iteration +1; + if opt.verbose, fprintf(1,'.'); end +end + + +if opt.verbose + if opt.info_separately, disp(['(' num2str(toc) ' sec)']); + else, fprintf(1,[num2str(toc) ' sec)']); end; end + +%disp(sprintf('change: %f, diff_rows: %f, diff_cols: %f, iterations: %d', ... +% change, diff_rows, diff_cols, iteration)); diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/create_nullspace.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/create_nullspace.m new file mode 100644 index 0000000..63c86ed --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/create_nullspace.m @@ -0,0 +1,151 @@ +%create_nullspace Create null-space for scene with perspective camera. +% +% Parameters: +% depths .. matrix of zeros and ones meaning whether corresponding element +% in M is scaled (i.e. multiplied by known perspective depth) + +function [nullspace, result] = create_nullspace(M, depths, central, opt) + +if nargin < 4, opt.trial_coef = 1; + opt.threshold = .01; end + +I = ~isnan(M(1:3:end,:)); [m n] = size(I); show_mod = 10; use_maxtuples = 0; +if opt.verbose, fprintf(1, 'Used 4-tuples (.=%d): ', show_mod); tic; end + +if central, cols_scaled(1:n) = 0; cols_scaled(find(I(central,:) > 0)) = 1; +else, cols_scaled = []; end + +num_trials = round(opt.trial_coef * n); + +if opt.verbose, fprintf(1,'(Allocating memory...'); end +nullspace(size(M,1),num_trials) = 0; % Memory allocation: at least this + % because at least one column is + % added per one 4-tuple. +width = 0; +if opt.verbose, fprintf(1, ')'); end + +tenth = .1; % because of the first % +result.used = 0; result.failed = 0; +for i = 1:num_trials + % choose a 4/max-tuple + cols = 1:n; + rows = 1:m; + + cols_chosen = []; t=1; failed = 0; + if central, + scaled_ensured = 0; + else + scaled_ensured = 1; % trial version: no scale controling when cutting + end + for t = 1:4 + % choose one column, cut useless parts etc. + [c, cols] = random_element(cols); + cols_chosen = [cols_chosen c]; + + % check just added column + rows = intersect(rows, find(I(:,c) > 0)); + + if t < 4, + [rows, cols, scaled_ensured] = cut_useless(I, cols_scaled, ... + cols_chosen, rows, cols, 4-t, scaled_ensured); + end + + if isempty(rows), failed = 1; break; end + end + + if ~failed + % use the 4/max-tuple + d = depths(rows,cols_chosen); + % see ``debug code'' in the comment lower + + rowsbig = k2i(rows); + submatrix=[]; for j=1:length(cols_chosen) % 4, + submatrix=[ submatrix ... + spread_depths_col(M(rowsbig,cols_chosen(j)), d(:,j)) ]; end + debug=1; if debug, if size(submatrix, 1)<=size(submatrix,2) & opt.verbose + fprintf(1,'-'); end;end + subnull = nulleps(submatrix,opt.threshold); %svd(submatrix) + if size(subnull,2)>0 & ( use_maxtuples | ... + size(submatrix,1) == size(submatrix,2) + size(subnull,2)) + nulltemp = zeros(size(M,1),size(subnull,2)); + nulltemp(rowsbig,:) = subnull; % * (length(rows)/m); % weighting + if width+size(nulltemp,2) > size(nullspace,2) % Memory allocation: + if opt.verbose, fprintf(1,'(Allocating memory...)'); end + mean_added = width/i; + nullspace(size(M,1), size(nullspace,2) ... + + round(mean_added * (num_trials-i))) = 0; + end + nullspace(:, width+1 : width+size(nulltemp,2)) = nulltemp; + width = width +size(nulltemp,2); + result.used = result.used +1; + if mod(result.used, show_mod)==0 & opt.verbose, fprintf(1,'.'); end + end + else + result.failed = result.failed +1; + end + + if i/num_trials > .1*tenth + if opt.verbose, fprintf(1,'%d%%', tenth*10); end + if tenth < 1, tenth=0; end + tenth = tenth + 1; + end +end + +if size(nullspace,2) > width, % cut off unused allocated memory + if opt.verbose, fprintf(1,'(Cutting unused memory...'); end + nullspace = nullspace(:,1:width); + if opt.verbose, fprintf(1, ')'); end +end +if opt.verbose, fprintf(1, ['(' num2str(toc) ' sec)\n']); end +result.tried = i; + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% debug code +% if size(d, 1) == 1, bla; end +% if sum(d(1,:)==0) > 2, blabla; end +% if use_maxtuples | ... +% size(d,1)*3 > size(d,2) + sum(d(:)==0)... % i.e. size(submatrix,1)> +% ... % size(submatrix,2) +% - sum(cols_scaled(cols_chosen)==0) % i.e. subtract 1 per each +% % column full of zeros +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [rows, cols, scaled_ensured] = cut_useless( ... %) + I, cols_scaled, ... % this is always same + cols_chosen, rows, cols, demanded, scaled_ensured) + +if ~scaled_ensured + % check scaled columns + if length(rows) == 2, demanded_scaled = 3; demanded_rows = 2; + else demanded_scaled = 2; demanded_rows = 3; end + cols_scaled_chosen = sum(cols_scaled(cols_chosen) > 0); + + % if no unscaled are allowed, they must be all cut + if demanded == demanded_scaled - cols_scaled_chosen, + cols = intersect(cols, find(cols_scaled > 0)); + scaled_ensured = 1; + end +else demanded_rows = 2; end + +% check columns +cols = cols(find(sum(I(rows,cols)) >= demanded_rows)); + +% check rows +rows = rows(find(sum(I(rows,cols)') >= demanded)); + +function [element, rest] = random_element(set) +% Take a random element out of a set. +element = set(random_int(1, length(set))); +rest = setdiff(set, element); + +function y = random_int(from,to) +y = floor(from + (1 + to - from)*rand); + +function [N,s] = nulleps(M,tol) +% Find the nullspace of M. This is the regular nullspace, augmented by +% vectors that aren't really in the nullspace, but have low singular +% values associated with them. tol is the threshold on these singular values. +[u,s,v] = svd(M); +sigsvs = sum(diag(s)>tol); +N = u(:,sigsvs+1:size(u,2)); diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/depth_estimation.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/depth_estimation.m new file mode 100644 index 0000000..7e1677b --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/depth_estimation.m @@ -0,0 +1,39 @@ +%depths_estimation Determine scale factors (proj. depths) of PRMM. + +function [lambda, Ilamb] = depth_estimation(M,F,ep,rows,central) + +[m n] = size(M); m = m/3; + +if central + j = central; + ps = 1:n; + Ilamb(j,:) = ~isnan(M(3*j,:)); +else + j = 1; + b = subseq_longest(~isnan(M(1:3:end,:))); + for p = 1:n + Ilamb(b(p),p) = ~isnan(M(3*b(p),p)); % =1 is sufficient if b(p) is correct + end +end + +lambda = ones(m,n); + +for i = setdiff(1:m, j) + if ~central + j = i-1; + ps = find(b <= j); + end + G = reshape(F(rows(i),rows(j),1:3,1:3),3,3); + epip = reshape(ep(rows(i),rows(j),1:3),3,1); + for p = ps + Ilamb(i,p) = Ilamb(j,p) & ~isnan(M(3*i,p)); + + if Ilamb(i,p) + u = cross(epip,M(3*i-2:3*i,p)); %q(i,p) + v = G*M(3*j-2:3*j,p); %q(j,p) + lambda(i,p) = u'*v/norm(u)^2*lambda(j,p); + else + lambda(i,p) = 1; %so that it's possible to recover scene at the end + end + end +end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm.m new file mode 100644 index 0000000..7d3c0a7 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm.m @@ -0,0 +1,319 @@ +%fill_mm Proj. reconstruction from MM. +% +% [P,X, u1,u2, info] = fill_mm(M, opt) +% +% The concept of the central image strength and of the sequence is used. +% +% Parameters: +% M .. measurement matrix (MM) with homogeneous image points with NaNs +% standing for the unknown elements in all three coordinates +% size(M) = [3m, n] +% opt .. options with default values in (): +% .strategy(-1) .. -1 choose the best of the following: +% 0 sequence +% -2 all strategies with the central image +% 1..m the central image of the corresponding No. +% .create_nullspace.trial_coef(1) +% .create_nullspace.threshold(.01) +% .tol(1e-6) .. tolerance on rows of well computed basis of MM, +% see approximate +% .no_factorization(0) .. if 1, no factorization performed +% .metric(1) .. the metric used for reproj. error +% 1 .. square root of sum of squares of x- and y-coordinates +% 2 .. std of x- and y-coordinates +% .verbose(1) .. whether display info +% +% Return parameters: +% P, X .. estimated projective structure (X) and motion (P) +% u1, u2 .. unrecovered images (u1) and points (u2) +% size(P) = [3*(m-length(u1)),4], size(X) = [4,n-length(u2)] +% info.omega .. info, e.g. predictor functions, about the used strategy + +function [P,X, u1,u2, info] = fill_mm(M, opt) + +if ~isfield(opt, 'strategy'), opt.strategy = -1; end +if ~isfield(opt, 'create_nullspace') | ... + ~isfield(opt.create_nullspace, 'trial_coef') + opt.create_nullspace.trial_coef = 1; end % 10 +if ~isfield(opt.create_nullspace, 'threshold') + opt.create_nullspace.threshold = .01; end +if ~isfield(opt, 'tol'), opt.tol = 1e-6; end +if ~isfield(opt, 'no_factorization'), + opt.no_factorization = 0; end +if ~isfield(opt, 'metric'), + opt.metric = 1; end +if ~isfield(opt, 'verbose'), + opt.verbose = 1; end +if ~isfield(opt.create_nullspace, 'verbose') + opt.create_nullspace.verbose = opt.verbose; end + +if ~opt.verbose, fprintf('Repr. error in proj. space (no fact.'); + if ~opt.no_factorization, fprintf('/fact.'); end + if ~opt.no_BA, fprintf('/BA'); end; fprintf(') is ... '); end + +nbeg = size(M,2); +ptbeg = find(sum(~isnan(M(1:3:end,:))) >= 2); u2beg = setdiff(1:nbeg,ptbeg); +M = M(:,ptbeg); +if ~isempty(u2beg), + fprintf('Removed correspondences in < 2 images (%d):%s\n', length(u2beg),... + sprintf(' %d', u2beg(1:min(20, length(u2beg))))); end + +[m n] = size(M); m = m/3; M0 = M; +cols = []; recoverable = inf; Omega = []; + +% names of strategies +switch opt.strategy + case -1 % use all strategies + Omega{end+1}.name = 'seq'; + for i = 1:m, Omega{end+1}.name = 'cent'; Omega{end}.ind = i; end + case 0 % sequence + Omega{end+1}.name = 'seq'; + case -2 % use all strategies with the central image + for i = 1:m, Omega{end+1}.name = 'cent'; Omega{end}.ind = i; end + otherwise % the central image of corresponding No. + Omega{end+1}.name = 'cent'; Omega{end}.ind = opt.strategy; +end + +added = 1; I = ~isnan(M(1:3:end,:)); info = []; iter = 0; +while recoverable > 0 ... % not all parts of JIM are restored + & added % something was added during the last trial + if opt.verbose, + disp(sprintf('%d (from %d) recovered columns...', length(cols), n)); end + + added = 0; iter = iter + 1; if iter > 10, keyboard; end + + [S, F, strengths] = compute_predictions(Omega, I); % for all stragegies + + % try the best strategy(s) + while (~added & max(F) > 0) | ... + sum(F==0) == length(F) % when no data missing, only rescaling needed + + % choose the actually best strategy (sg) + Omega_F = find(F==max(F)); if length(Omega_F) == 1, sg = Omega_F; else + if opt.verbose, disp(sprintf('Omega_F:%s.', sprintf(' %d',Omega_F))); end + ns = S(Omega_F); + Omega_S = Omega_F(find(ns==max(ns))); if length(Omega_S) >1 & opt.verbose + disp(sprintf('Omega_S:%s.', sprintf(' %d', Omega_S))); + disp(['!!! Maybe some other criteria is needed to choose' ... + ' the better candidate (now the 1st is taken)! !!!']); end + sg = Omega_S(1); % an arbitrary of Omega_S + end + + [rows, cols, central, info] = set_rows_cols(Omega, sg, F, S, strengths, ... + I, info, opt); + F(sg) = -1; % "don't try this strategy anymore" + if opt.verbose, disp(sprintf('Used images:%s.', sprintf(' %d', rows))); end + [Mn,T] = normM(M); + [Pn,X, lambda, u1,u2, info] = fill_mm_sub(Mn(k2i(rows),:), ... + Mn(k2i(rows),cols), ... + find(central==rows),opt,info); + if length(u1)==length(rows) & length(u2)==length(cols) + if opt.verbose, disp('Nothing recovered.'); end + else + if opt.verbose + if ~isempty(u1), disp(sprintf('u1 = %s', num2str(u1))); end + if ~isempty(u2) & ~isempty(Pn),disp(sprintf('u2 = %s',num2str(u2)));end + end + + % say (un)recovered in indices of whole M not only M(k2i(rows),cols) + r1 = rows(setdiff(1:length(rows),u1)); u1 = setdiff(1:m, r1); + r2 = cols(setdiff(1:length(cols),u2)); u2 = setdiff(1:n, r2); + P = normMback(Pn, T(r1,:,:)); R = P*X; + + % fill holes: use the original data when known as much as possible rather + % then the new data + if isempty(r1) | isempty(r2), fill = []; else, + fill = find(isnan(M(3*r1, r2)) ... % some depths (lambda) could not + ... % have been computed L2depths + | (~isnan(M(3*r1,r2)) & isnan(lambda))); end + M_ = M(k2i(r1),r2); M_(k2i(fill)) = R(k2i(fill)); M(k2i(r1),r2) = M_; + + added = length(fill); I = ~isnan(M(1:3:end,:)); + recoverable = sum(sum(I(:,find(sum(I) >= 2))==0)); + if opt.verbose, + disp(sprintf('%d points added, %d recoverable points remain.', ... + added, recoverable)); end + + info.err.no_fact = dist(M0(k2i(r1),r2), R, opt.metric); + if opt.verbose, + fprintf('Error (no factorization): %f\n', info.err.no_fact); + else, fprintf(' %f', info.err.no_fact); end + + end + end + + if ~added & sum(~I(:)) > 0 % impossible to recover anything + P=[];X=[]; u1=1:m; u2=1:nbeg; info.opt = opt; return; end +end + +if length(r1)<2, % rank cannot be 4 + P=[];X=[]; u1=1:m; u2=1:nbeg; info.opt = opt; return; end + +if ~opt.no_factorization + if opt.verbose + fprintf(1,'Factorization into structure and motion...'); tic; end + + % depths estimated from info.Mdepths (which is approximated by R) + Mdepths_un = normMback(info.Mdepths, T(r1,:,:)); + lambda(length(r1), length(r2)) = NaN; + for i = find(~isnan(M0(3*r1, r2)))', + lambda(i) = M0(k2i(i)) \ Mdepths_un(k2i(i)); end + for i = 1:length(r1), B(k2i(i),:) = M(k2i(i),:).*([1;1;1]*lambda(i,:)); end + fill = find(isnan(M0(3*r1, r2))); B(k2i(fill)) = R(k2i(fill)); + [Bn,T] = normM(B); + opt.info_separately = 0; Bn = balance_triplets(Bn, opt); + if opt.verbose, fprintf(1,'(running svd...'); tic; end + [u,s,v] = svd(Bn); s1 = sqrt(s(1:4,1:4)); P = u(:,1:4)*s1; X = s1*v(:,1:4)'; + if opt.verbose, disp([num2str(toc) ' sec)']); end + P = normMback(P, T); + + info.err.fact = dist(M0(k2i(r1),r2), P*X, opt.metric); + if opt.verbose, fprintf('Error (after factorization): %f\n', info.err.fact); + else, fprintf(' %f', info.err.fact); end +end + +u2 = union(u2beg, ptbeg(u2)); +info.opt = opt; + +function [S, F, strengths] = compute_predictions(Omega, I) +% compute predictor functions for all strategies + +for sg = 1:length(Omega) + switch Omega{sg}.name + case 'seq' + [b, len] = subseq_longest(I); + Isub = I; % for now, epipolar geometry is supposed to be computable + % through the whole sequence. See also lower. + F(sg) = sum(Isub(:) == 0); + S(sg) = sum(len); + case 'cent' + i = Omega{sg}.ind; + strengths{i} = strength(i, I, 1); + F(sg) = strengths{i}.strength(1); % or 2 + S(sg) = strengths{i}.num_scaled; + end +end + + +function result = strength(central, I, general) + +%strength Compute the central image strength of an image. +% +% result = strength(central, I, general) +% +% Parameters: +% `general' is set to logical true when generalized Jacobs' algorithm for +% unknown projective depths is supposed. +% +% `result' is a stucture containing following fields +% `strength' is computed strength +% `good_rows' are the useful rows for the used submatrix +% `good_cols' "" columns "" +% `Isub' is the "good" submatrix where ones are known points +% `num_scaled' is the number of scaled points + +if nargin<3, general=0; end + + +[m n] = size(I); +good_rows = []; + + %display(central); +% fundamental matrices + for i = setdiff(1:m, central) + common = find(I(i,:) & I(central,:)); + if length(common) >= 8 % 7 for 7-points algorithm + good_rows = [good_rows i]; + +% only common points + if ~general + I(i,setdiff(1:n,common)) = zeros(1,n-length(common)); end + else + I(i,:) = zeros(1,n); + end + end + +good_rows = union(good_rows,central); + +% Jacobs' algorithm needs at least 2 points in each column +% and its generalized form for unknown depths as well + present = sum(I); + good_cols = find(present >= 2); + +Isub = I(good_rows,good_cols); + +result.strength = [ sum(Isub(:)==0) size(Isub,1)*size(Isub,2) ]; + +% find scaled image points (those in the columns known in the +% central image) + scaled_cols = find(Isub(find(central==good_rows),:)==1); + num_scaled = 0; + for i=1:length(good_rows), + scaled = intersect(find(Isub(i,:)==1), scaled_cols); + num_scaled = num_scaled + length(scaled); + end + +result.good_rows = good_rows; +result.good_cols = good_cols; +result.Isub = Isub; +result.num_scaled = num_scaled; + + +function [rows, cols, central, info] = set_rows_cols(Omega, sg, F, S, ... + strengths, I,info, opt) + +[m n] = size(I); +switch Omega{sg}.name + case 'seq' + rows = 1:m; % for now, epipolar geometry is supposed to be computable + % through the whole sequence. See also above. + cols = 1:n; % "" + Isub = I(rows, cols); + central = 0; + case 'cent' + central = Omega{sg}.ind; + rows = strengths{central}.good_rows; + cols = strengths{central}.good_cols; + Isub = strengths{central}.Isub; +end + +info.omega = Omega{sg}; info.omega.F = F(sg); info.omega.S = S(sg); +sequence = set_sequence(central, S(sg), Isub, I, opt); +if ~isfield(info,'sequence'), info.sequence{1} = sequence; +else, info.sequence{end+1} = sequence; end + + +function sequence = set_sequence(central, num_scaled, Isub, I, opt) + +sequence.central = central; +if isempty(Isub) + sequence.scaled = 0; + sequence.missing = 0; + sequence.used_pts = 0; +else + sequence.scaled = num_scaled / sum(Isub(:)) * 100; + sz = size(Isub); + sequence.missing = sum(Isub(:) == 0) / (sz(1)*sz(2)) * 100; + sequence.used_pts = sum(Isub(:)) / sum(I(:)) * 100; +end +if opt.verbose + disp(sprintf(['Image %d is the central image, image points: %.2f %%' ... + ' scaled, %.2f %% missing, %.2f %% used.'], central, ... + sequence.scaled, sequence.missing, ... + sequence.used_pts)); end + +%normM Normalize the image coordinates by applying transformations normu. +function [Mr, T] = normM(M) + +for k = 1:size(M,1)/3 + Tk = normu(M(k2i(k),find(~isnan(M(3*k,:))))); + Mr(k2i(k),:) = Tk * M(k2i(k),:); T(k,1:3,1:3) = Tk; +end + +%normMback Adapt projective motion, to account for the normalization norm. +function P = normMback(P, T) + +for k = 1:size(P,1)/3 + Tk = reshape(T(k,1:3,1:3),3,3); + P(3*k-2:3*k,:) = inv(Tk)*P(3*k-2:3*k,:); +end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm_sub.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm_sub.m new file mode 100644 index 0000000..5895c73 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_mm_sub.m @@ -0,0 +1,53 @@ +%fill_mm_sub Proj. reconstruction of a normed sub-scene. +% +% When the central image concept is used, the information which image is +% the central image is passed to this function as input. +% +% Parameters: +% +% Mfull .. complete known parts of the problem, used here for the best +% estimate of the fundamental matrices + +function [P,X, lambda, u1,u2, info] = fill_mm_sub(Mfull, M, central,opt,info) + +I = ~isnan(M(1:3:end,:)); +[m n] = size(I); +if isempty(central), central = 0; end + +P=[]; X=[]; lambda=[]; u1=1:m; u2=1:n; + +%estimate the fundamental matrices and epipoles with the method of [Har95] + [F,ep,rows,nonrows] = M2Fe(Mfull, central); + + if ~isempty(nonrows), + disp(sprintf('Used local images:%s.', sprintf(' %d', rows))); end + if length(rows) < 2, return; end + +%determine scale faktors lambda_i_p + if ~central, rows_central = 0; else rows_central = find(rows == central); end + [lambda, Ilamb] = depth_estimation(M(k2i(rows),:),F,ep,rows, ... + rows_central); + + % prepare info.show_prmm - for show_prmm function + info.show_prmm.I = I; + info.show_prmm.Idepths = zeros(m,n); info.show_prmm.Idepths(rows,:)=Ilamb; + +%build the rescaled measurement matrix B + for i = 1:length(rows), B(k2i(i),:) = M(k2i(i),:).*([1;1;1]*lambda(i,:)); end + +%balance W by column-wise and "triplet-of-rows"-wise scalar multiplications + B = balance_triplets(B, opt); + +%fit holes in JIM by Jacobs' algorithm + [P,X, u1,u2, lambda1, info] = fill_prmm(B, Ilamb, central,opt,info); + +r1 = setdiff(1:length(rows),u1); r2 = setdiff(1:n,u2); + +lambda = lambda(r1,r2); % to fit P*X +if ~isempty(lambda1), + new = find(~Ilamb(r1,r2) & I(r1,r2)); lambda(new) = lambda1(new); end + +error = eucl_dist_only(B(k2i(r1), r2), P*X, ~isnan(B(3*r1,r2)), 3); +if opt.verbose, disp(sprintf('Error balanced: %f', error)); end + +u1 = union(nonrows, rows(u1)); \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_prmm.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_prmm.m new file mode 100644 index 0000000..5caf0d5 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/fill_prmm.m @@ -0,0 +1,101 @@ +%fill_prmm Compute the null-space and fill PRMM. +% +% Parameters: + +function [P,X, u1,u2, lambda, info] = fill_prmm(M, Idepths, central, ... + opt, info) + +[NULLSPACE, result] = create_nullspace(M, Idepths, central, ... + opt.create_nullspace); + +info.create_nullspace = opt.create_nullspace; +info.sequence{end}.tried = result.tried; +info.sequence{end}.tried_perc = result.tried/comb(size(M,2), 4) *100; +info.sequence{end}.used = result.used; +info.sequence{end}.used_perc = result.used/result.tried *100; +info.sequence{end}.failed = result.failed; +info.sequence{end}.size_nullspace = size(NULLSPACE); + +if opt.verbose + disp(sprintf('Tried/used: %d/%d (%.1e %%/ %2.1f %%)', result.tried, ... + result.used, info.sequence{end}.tried_perc, ... + info.sequence{end}.used_perc)); + disp(sprintf('%d x %d'' is size of the nullspace', size(NULLSPACE,2), ... + size(NULLSPACE,1))); end + +[m,n] = size(M); m = m/3; + +if size(NULLSPACE) == [0 0] + P = []; X = []; u1 = 1:m; u2 = 1:n; lambda=[]; +else + r = 4; + [L, S] = nullspace2L(NULLSPACE, r, opt); + clear NULLSPACE; + + if isempty(opt.create_nullspace), threshold = .01; + else, threshold = opt.create_nullspace.threshold; end + if svd_suff_data(S, r, threshold) + if opt.verbose + dS = diag(S); %disp(diag(S)); + fprintf(1,'Smallest 2r singular values:%s.\n', sprintf(' %f', ... + dS(end-2*r:end))); end + [Mdepths, lambda] = L2depths(L, M, Idepths, opt); info.Mdepths = Mdepths; + [P,X, u1b,u2] = approximate(Mdepths, r, L, opt); + u1 = union(ceil(u1b/3),[]); killb = setdiff(k2i(u1),u1b); + if ~isempty(killb), r1b = setdiff(1:3*m,u1b); kill = killb; + for ib = killb(1:end-1), lower = find(killb > ib); + if kill(lower(1)-1) < kill(lower(1)) - 1, + kill(lower) = kill(lower) -1; end; end + P = P(setdiff(1:length(r1b),kill),:);end + lambda = lambda(setdiff(1:m,u1),setdiff(1:n,u2)); % to fit P*X + else P=[]; X=[]; u1=1:m; u2=1:n; lambda=[]; end +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +function [L,S] = nullspace2L(NULLSPACE, r, opt) +% Compute the basis of MM (L) from the null-space. + +if opt.verbose, fprintf(1,'Computing the basis...'); tic; end +if size(NULLSPACE,2) < 10*size(NULLSPACE,1) % orig:[A,S,U] = svd(NULLSPACE',0); + [U,S,V] = svd(NULLSPACE); +else + [U,SS] = eig(NULLSPACE*NULLSPACE'); + dSS = diag(SS); l = length(dSS); + [sortdSS(l:-1:1),I] = sort(dSS); I(l:-1:1) = I; U = U(:,I); + sortdSS = max(sortdSS,zeros(1,l)); sortdSS=sqrt(sortdSS); + for i=1:l, S(i,i) = sortdSS(i); end +end +L = U(:,end+1-r:end); +if opt.verbose, disp(['(' num2str(toc) ' sec)']); end + +function r = comb(n,k) + +% returns combination number n over k + +r=1; +for i=1:k + r=r*(n-i+1)/i; +end + +function y = svd_suff_data(S,r, threshold) + +% S is the singular value part of the svd of the nullspaces of the column +% r-tuples. We'll want to be able to take the r least significant columns +% of U. This is right because the columns of U should span the whole space +% that M's columns might span. That is, M is FxP. The columns of U should +% span the F-dimensional Euclidean space, since U is FxF. However, we want +% to make sure that the F-r-1'th singular value of S isn't tiny. If it is, +% our answer is totally unreliable, because the nullspaces of the column +% r-tuples don't have sufficient rank. If this happens, it means that the +% intersection of the column cross-product spaces is bigger than r-dimensional, +% and randomly choosing an r-dimensional subspace of that isn't likely to +% give the right answer. + +Snumcols = size(S,2); +Snumrows = size(S,1); +if (Snumrows == 0 | Snumcols + r < Snumrows | Snumrows <= r) + y = 0; +else + y = S(Snumrows-r,Snumrows-r) > threshold; +end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/normu.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/normu.m new file mode 100644 index 0000000..0705f4e --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/normu.m @@ -0,0 +1,20 @@ +%normu Normalize image points to be used for LS estimation. +% A = normu(u) finds normalization matrix A so that mean(A*u)=0 and +% mean(||A*u||)=sqrt(2). (see Hartley: In Defence of 8-point Algorithm, +% ICCV`95). +% +% Parameters: +% u ... Size (2,N) or (3,N). Points to normalize. +% A ... Size (3,3). Normalization matrix. + +function A = normu(u) + +if size(u,1)==3, u = p2e(u); end + +m = mean(u')'; % <=> mean (u,2) +u = u - m*ones(1,size(u,2)); +distu = sqrt(sum(u.*u)); +r = mean(distu)/sqrt(2); +if ~r, A = []; return; end % one of degenarate configurations +A = diag([1/r 1/r 1]); +A(1:2,3) = -m/r; diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/spread_depths_col.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/spread_depths_col.m new file mode 100644 index 0000000..c34c4b3 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/spread_depths_col.m @@ -0,0 +1,19 @@ +% Mdepthcol is column of JIM with depths which is spread by this +% function to a submatrix with some zeros +function submatrix = spread_depths_col(Mdepthcol,depthsIcol) + +m = size(depthsIcol,1); +n = 1; + +known_depths = find(depthsIcol ~= 0); + +if ~isempty(known_depths) + rows = k2i(known_depths); + submatrix(rows,n) = Mdepthcol(rows); n=n+1; +end + +for i=setdiff(1:m, known_depths) + rows = k2i(i); + submatrix(rows,n) = Mdepthcol(rows); n=n+1; +end + diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/subseq_longest.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/subseq_longest.m new file mode 100644 index 0000000..020eae4 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/subseq_longest.m @@ -0,0 +1,34 @@ +%subseq_longest Find the longest continuous subsequences in columns of MM. +% +% Put the initial image of the longest continuous subsequence of known +% points in column ``p'' to ``b(p)''. +% +% [b, len] = subseq_longest(I) +% +% Parameters: +% len ... len(p) is length of the longest continuous +% subsequence of known points in column ``p'' + +function [b, len] = subseq_longest(I) + +[m n] = size(I); + +b(n) = 0; % memory allocation +len(n) = 0; % " " + +for p = 1:n + seq(1:m) = 0; + l = 1; + for i = 1:m + if I(i,p) + seq(l) = seq(l) +1; + else + l = i+1; + end + end + len(p) = max(seq); + best = find(seq == len(p)); + b(p) = best(1); +end + +%b(:) = 1; % debug \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m b/MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m new file mode 100644 index 0000000..0afcd5d --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm/u2FI.m @@ -0,0 +1,73 @@ +%u2FI Estimate fundamental matrix using ortogonal LS regression +% +% F = u2F(u) estimates F from u using NORMU +% F = u2F(u,'nonorm') disables normalization +% +% see also NORMU, U2FA +% +% Returns 0 if too few points are available + +function F = u2FI (u, str, A1, A2) + +sampcols = find(sum(~isnan(u(1:3:end,:))) == 2); +if length(sampcols) < 8 + F = 0; return +end + +if (nargin > 2) & ~strcmp(str, 'nonorm') & ~strcmp(str, 'usenorm') + donorm = 1; +else + donorm = 0; +end + +ptNum = size(sampcols,2); + +if donorm + A1 = normu(u(1:3,sampcols)); + A2 = normu(u(4:6,sampcols)); + if isempty(A1) | isempty(A2), F = 0; return; end + + u1 = A1*u(1:3,sampcols); %in u1, u2 there are only columns of sampcols + u2 = A2*u(4:6,sampcols); +else + u1 = u(1:3,sampcols); %" " + u2 = u(4:6,sampcols); +end + +for i = 1:ptNum + Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); +end + +M = Z'*Z; +V = seig(M); +F = reshape(V(:,1),3,3); + +%odrizneme nejmensi vlastni slozku, aby F melo hodnost 2 +[uu,us,uv] = svd(F); +%[y,i] = min (abs(diag(us))); +i = 3; +%if us(i,i) > 1e-12, disp('rank(F)>2'); end +us(i,i) = 0; +F = uu*us*uv'; + +if donorm | strcmp(str, 'usenorm') + F = A1'*F*A2; +end + +F1=F; + +F = F /norm(F,2); + +if rank(F) > 2 + %disp('!!! Error: u2FI: rank(F) > 2'); + % snizime hodnost, us(3,3) je stejne 1e-16 + [uu,us,uv] = svd(F); + us(3,3) = 0; + F = uu*us*uv'; +end + +%seig sorted eigenvalues +function [V,d] = seig(M) +[V,D] = eig(M); +[d,s] = sort(diag(D)); +V = V(:,s); diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm_test/bundle_PX_proj.m b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/bundle_PX_proj.m new file mode 100644 index 0000000..5eea542 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/bundle_PX_proj.m @@ -0,0 +1,358 @@ +% [P,X] = bundle_PX_proj(P0,X0,u,imsize [,opt]) Projective bundle adjustment. +% +% P0 ... double(3*K,4), joint camera matrix +% X0 ... double(4,N), scene points +% u ... double(2*K,N), joint image point matrix with nonhomogeneous image points +% imsize ... double(2,K), image sizes: imsize(:,k) is size of image k +% P, X ... bundled reconstruction +% +% P0, X0 and u need not be preconditioned - preconditioning is done inside +% this function. +% +% opt.verbose (default 1) .. whether display info +% .verbose_short(default 0) .. whether short info + +function [P,X] = bundle_PX_proj(P0,X0,q,imsize, opt) + +if nargin < 5 | isempty(opt) | ~isfield(opt,'verbose') + opt.verbose = 1; end +if ~isfield(opt,'verbose_short') + opt.verbose_short = 0; end + +RADIAL = 0; % Commented by DM (nargin > 4); % don't estimate/estimate radial distortion +[K,N] = size(q); K = K/2; + +% precondition +for k = 1:K + H{k} = vgg_conditioner_from_image(imsize(:,k)); + P0(k2i(k),:) = H{k}*P0(k2i(k),:); + q(k2i(k,2),:) = nhom(H{k}*hom(q(k2i(k,2),:))); +end + +P0 = normP(P0); +X0 = normx(X0); + +% form observation vector y +qq = reshape(q,[2 K*N]); +qivis = reshape(all(~isnan(qq)),[K N]); % visible image points +qq = qq(:,qivis); +y = qq(:); + + +% Compute matrices TP,TX, describing local coordinate systems tangent to P0(:), X0. +% Having homogeneous N-vector X, in the neighborhood of X0 it can be parameterized (minimaly) +% by inhomogeneous (N-1)-vector iX as X = X0 + TX*iX, where TX is a non-singular (N,N-1) matrix. + +for n = 1:N + [qX,dummy] = qr(X0(:,n)); + TX{n} = qX(2:end,:)'; +end +for k = 1:K + [qP,dummy] = qr(reshape(P0(k2i(k),:),[12 1])); + TP{k} = qP(2:end,:)'; +end + +% form initial parameter vector p0 +p0 = []; +for k = 1:K + p0 = [p0; zeros(11,1)]; + if RADIAL + p0 = [p0; radial(k).u0; radial(k).kappa]; + end +end +p0 = [p0; zeros(3*N,1)]; + +p = levmarq('F',...%@F, % commented by DM (perhaps Matlab version conflict) + p0,opt,P0,TP,X0,TX,y,qivis,RADIAL); + +% rearrange computed parameter vector p to P, X +for k = 1:K + P(k2i(k),:) = inv(H{k})*(P0(k2i(k),:) + reshape(TP{k}*p([1:11]+(k-1)*(11+RADIAL*3)),[3 4])); + if RADIAL + radial(k).u0 = p([12:13]+(k-1)*(11+RADIAL*3)); + radial(k).kappa = p([14]+(k-1)*(11+RADIAL*3)); + end +end +for n = 1:N + X(:,n) = X0(:,n) + TX{n}*p((11+RADIAL*3)*K+[1:3]+(n-1)*3); +end + +if opt.verbose_short, fprintf(')'); end +return + + +%% +% function F: (Np,1) --> (Ny,1)x(Ny,Np), [y,J]=F(p), value y and jacobian J=dF(p)/dp of the function. +% +% p ... size ((11+3)*K+3*N,1), p = [iP(1); u0(1); kappa(1); .. iP(K); u0(K); kappa(K); iX(1); .. iX(N)], +% where Np = (11+2+raddeg)*K + 3*N +% +% Optional parameters: +% TP{1:K} ... size (12,11), Pk(:) = P0k + TP{k}*iPk +% TX{1:N} ... size (4,3), Xn = X0n + TX{n}*iXn +% qivis ... size (K,N), has 1 in entries corresponding to visible image points +% y ... size (2*nnz(qivis),1), visible image observations, stacked as [q(1,1); q(2,1); .. q(2*K,N)], unobserved q are omitted +%% +% We consider the imaging model as +% u = hom(x) +% x = P*X +% P = P0 + TP*iP +% X = X0 + TX*iX +%% +function [y,J] = F(p,P0,TP,X0,TX,y,qivis,RADIAL) +% +[K,N] = size(qivis); +NR = RADIAL*3; % number of radial distortion paramters +% +% rearrange p to P, X [,radial] +for k = 1:K + P(k2i(k),:) = P0(k2i(k),:) + reshape(TP{k}*p([1:11]+(k-1)*(11+NR)),[3 4]); + if RADIAL + radial(k).u0 = p([12:13]+(k-1)*(11+NR)); + radial(k).kappa = p([14]+(k-1)*(11+NR)); + end +end +X = zeros(4,N); +for n = 1:N + X(:,n) = TX{n}*p((11+NR)*K+[1:3]+(n-1)*3); +end +X = X0 + X; +% +% compute retina points q +for k = 1:K + x(k2i(k),:) = P(k2i(k),:)*X; + q(k2i(k,2),:) = nhom(x(k2i(k),:)); + if RADIAL + q(k2i(k,2),:) = raddist(q(k2i(k,2),:),radial(k).u0,radial(k).kappa); + end +end +qq = reshape(q,[2 K*N]); +qq = qq(:,qivis); +y = qq(:)-y; % function value +% +% compute Jacobian J = dF(p)/dp +if nargout<2 + return +end +[kvis,nvis] = find(qivis); +Ji = zeros(2*length(kvis)*(11+NR+3),1); Jj = zeros(size(Ji)); Jv = zeros(size(Ji)); +cnt = 0; +for l = 1:length(kvis) % loop for all VISIBLE points in all cameras + k = kvis(l); + n = nvis(l); + xl = x(k2i(k),n); + ul = nhom(xl); + + % Compute derivatives (Jacobians). Notation: E.g., dudx = du(x)/dx, etc. + dxdP = kron(X(:,n)',eye(3))*TP{k}; % dx(iP,iX)/diP + dxdX = P(k2i(k),:)*TX{n}; % dx(iP,iX)/diX + dudx = [eye(2) -ul]/xl(3); + if RADIAL + [dqdu,dqdu0,dqdkappa] = raddist(ul,radial(k).u0,radial(k).kappa); + else + dqdu = eye(2); dqdu0 = []; dqdkappa = []; + end + dqdP = dqdu*dudx*dxdP; % dq(iP,iX)/diP + dqdX = dqdu*dudx*dxdX; % dq(iP,iX)/dX + c = cnt+[1:2*(11+NR+3)]; + [Ji(c),Jj(c),Jv(c)] = spidx([1:2]+(l-1)*2,[[1:(11+NR)]+(k-1)*(11+NR) (11+NR)*K+[1:3]+(n-1)*3],[dqdP dqdu0 dqdkappa dqdX]); + cnt = cnt + 2*(11+NR+3); +end +J = sparse(Ji,Jj,Jv,length(y),length(p)); +return + +function [i,j,v] = spidx(I,J,V) +% +i = I'*ones(1,length(J)); +j = ones(length(I),1)*J; +i = i(:); +j = j(:); +v = V(:); +return + + +%%%%%%%%%% local functions %%%%%%%%%%% + +% p = levmarq(F,p,opt,varargin) Minimize || F(p,varargin{:}) || over p. By +% Levenber-Marquardt. +% +% F ... handle to function [y,J] = F(p,varargin{:}), yielding value +% y=F(p,varargin{:}) and jacobian J. aux is typically a structure +% passing auxiliary arguments to F. +% p ... initial parameter vector +% opt ... options structure with possible fields :- +% - verbose ... printing residuals +% - res_scale ... residual scale, the prined residuals are multiplied by +% ressc before printing; useful if normalization were done before calling +% levmarq +% - max_niter ... maximum number of iterations +% - max_stepy ... maximal step of residual value (after multiplying by +% res_scale) for termination +% - lam_init ... initial value of lambda + +function p = levmarq(F,p,opt,varargin) + +% handle options +if ~isfield(opt,'verbose'), opt.verbose = 0; end +if ~isfield(opt,'res_scale'), opt.res_scale = 1; end +if ~isfield(opt,'max_niter'), opt.max_niter = 10000; end +if ~isfield(opt,'max_stepy'), max_stepy_undef = 1; opt.max_stepy = 100*eps*opt.res_scale; end +if ~isfield(opt,'lam_init'), opt.lam_init = 1e-4; end + +%OLDWARN = warning('off'); % commented by DM (perhaps Matlab version conflict) + +% Minimize || F(p) || over p. By Newton/Levenber-Marquardt. +lastFp = +Inf; +stepy = +Inf; +lam = opt.lam_init; +nfail = 0; +niter = 0; +[Fp,J] = feval(F,p,varargin{:}); +if issparse(J) + eyeJ = speye(size(J,2)); +else + eyeJ = eye(size(J,2)); +end + +% print out initial residuals +if opt.verbose | opt.verbose_short + if ~opt.verbose_short + fprintf(' %14.10g [rms] %14.10g [max]\n',... + opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp))); + else + fprintf('(rms/max/stepmax: %g/%g/', ... + opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp))); end +end + +while (nfail < 20) & (stepy*opt.res_scale > opt.max_stepy) & (niter < opt.max_niter) + + D = -(J'*J + lam*eyeJ) \ (J'*Fp); + if any( isnan(D) | abs(D)==inf ) + p = p*nan; + return + end + FpD = feval(F,p+D,varargin{:}); + + if sum((Fp).^2) > sum((FpD).^2) % success + p = p + D; + lam = max(lam/10,1e-15); + stepy = max(abs(Fp-lastFp)); + lastFp = Fp; + [Fp,J] = feval(F,p,varargin{:}); + nfail = 0; + niter = niter + 1; + else % failure + lam = min(lam*10,1e5); + nfail = nfail + 1; + end + + % if success, print out residuals + if (opt.verbose | opt.verbose_short) & nfail==0 + if ~opt.verbose_short + fprintf(' %7.2g [lam]: %14.10g [rms] %14.10g [max] %10.5g [stepmax]\n',lam,opt.res_scale*sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp)),opt.res_scale*stepy); + else fprintf(' %g/%g/%g', sqrt(mean((Fp).^2)),opt.res_scale*max(abs(Fp)),opt.res_scale*stepy); end + end +end + +if ~exist('max_stepy_undef') & stepy*opt.res_scale <= opt.max_stepy + fprintf('\n!!! finished because of high opt.max_stepy(=%f)', opt.max_stepy); end + +%warning(OLDWARN); % commented by DM (perhaps Matlab version conflict) +return + + +function [C,invC] = vgg_conditioner_from_image(c,r) +% +% function [C,invC] = vgg_conditioner_from_image(image_width, image_height) +% +% Makes a similarity metric for conditioning image points. +% +% Also can be called as vgg_conditioner_from_image([image_width image_height]) +% +% invC is inv(C), obtained more efficiently inside the function. + +if nargin<2 + r = c(2); + c = c(1); +end + +f = (c+r)/2; +C = [1/f 0 -c/(2*f) ; + 0 1/f -r/(2*f) ; + 0 0 1]; + +if nargout > 1 + invC = [f 0 c/2 ; + 0 f r/2 ; + 0 0 1]; +end + + +function x = nhom(x) +%nhom Projective to euclidean coordinates. +% x = nhom(x_) Computes euclidean coordinates by dividing +% all rows but last by the last row. +% x_ ... Size (dim+1,N) Projective coordinates. +% x ... Size (dim,N). Euclidean coordinates. +% (N can be arbitrary.) +if isempty(x) + x = []; + return; +end +d = size(x,1) - 1; +x = x(1:d,:)./(ones(d,1)*x(end,:)); +return + + +function x = hom(x) +%hom Euclidean to projective coordinates. +% x_ = hom(x) Adds the last row of ones. +% x ... Size (dim,N). Euclidean coordinates. +% x_ ... Size (dim+1,N) Projective coordinates. +% (N can be arbitrary.) + +if isempty(x), return, end +x(end+1,:) = 1; +return + + +% normP Normalize joint camera matrix so that norm(P(k2i(k),:),'fro') = 1 for each k. + +function P = normP(P) + +for k = 1:size(P,1)/3 + Pk = P(k2i(k),:); + P(k2i(k),:) = Pk/sqrt(sum(sum(Pk.*Pk))); +end + +return + + +% x = normx(x) Normalize MxN matrix so that norm of each its column is 1. + +function x = normx(x) + +if ~isempty(x) + x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); +end + +return + + +function i = k2i(k,step) + +%i = k2i(k [,step]) +% Computes indices of matrix rows corresponding to views k. If k is a scalar, +% it is i = [1:step]+step*(k-1). +% implicit: step = 3 +% E.g., for REC.u, REC.P, REC.X use k2i(k), +% for REC.q use k2i(k,2). + +if nargin<2 + step = 3; +end + +k = k(:)'; +i = [1:step]'*ones(size(k)) + step*(ones(step,1)*k-1); +i = i(:); +return \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm_test/fill_mm_bundle.m b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/fill_mm_bundle.m new file mode 100644 index 0000000..1a532c4 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/fill_mm_bundle.m @@ -0,0 +1,46 @@ +%fill_mm_bundle Proj. reconstruction from MM [with bundle adjustment]. +% +% Call fill_mm [and bundle adjustment]. +% +% [ P,X, u1,u2, info ] = fill_mm_bundle(M [,opt]) +% +% Parameters: +% M .. measurement matrix (MM) with homogeneous image points with NaNs +% standing for the unknown elements in all three coordinates +% imsize .. double(2,m), image sizes: imsize(:,i) is size of image i +% m .. No. of cameras +% opt .. options with default values in (): +% .no_BA(0) .. whether refine using bundle adjustment +% .verbose(1) .. whether display info +% .verbose_short .. see opt in bundle_PX_proj +% ... other options see in fill_mm +% +% Return parameters: +% info.R_lin .. linear estimation of filled M +% ... other parameters see in fill_mm + +function [ P,X, u1,u2, info ] = fill_mm_bundle(M, imsize, opt) + +if nargin < 3, opt = []; end +if ~isfield(opt, 'no_BA') + opt.no_BA = 0; end +if ~isfield(opt, 'verbose'), + opt.verbose = 1; end + +[P,X, u1,u2, info] = fill_mm(M, opt); + +info.R_lin = P*X; + +if ~opt.no_BA & length(u1) < size(M,1)/3 & length(u2) < size(M,2) + if opt.verbose, fprintf(1, 'Bundle adjustment...\n'); tic; end + + [m,n] = size(M); m = m/3; r1 = setdiff(1:m,u1); r2 = setdiff(1:n,u2); + [P,X] = bundle_PX_proj(P,X, normalize_cut(M(k2i(r1),r2)), imsize, opt); + % old bundler: + %[P,X] = qPXbundle_cmp(P,X, normalize_cut(M(k2i(r1),r2))); + + if opt.verbose, disp(['(' num2str(toc) ' sec)']); end + info.err.BA = dist(M(k2i(r1),r2), P*X, info.opt.metric); + if opt.verbose, fprintf('Error (after BA): %f\n', info.err.BA); + else, fprintf(' %f\n', info.err.BA); end +else, if ~opt.verbose, fprintf('\n'); end; end diff --git a/MultiCamSelfCal/MartinecPajdla/fill_mm_test/qPXbundle_cmp.m b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/qPXbundle_cmp.m new file mode 100644 index 0000000..c401d6b --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/fill_mm_test/qPXbundle_cmp.m @@ -0,0 +1,203 @@ +% [P,X] = qPXbundle_cmp(P0,X0,q) Bundle adjustment. +% +% P0 ... size (3*K,4), initial camera matrices, K = #images +% X0 ... size (4,N), initial scene points, N = #points +% q ... size (2*K,N), measured image points (in inhomogeneous coordinates) +% P, X ... bundled cameras and scene points, same size as P0, X0 + +% (c) Tom Werner 2001 + +function [P,X,radial] = qPXbundle_cmp(P0,X0,q) + +RADIAL = (nargin > 3); % don't estimate/estimate radial distortion +[K,N] = size(q); K = K/2; + +P0 = normP(P0); +X0 = normx(X0); + +% form observation vector y +qq = reshape(q,[2 K*N]); +aux.qivis = reshape(all(~isnan(qq)),[K N]); % visible image points +qq = qq(:,aux.qivis); +y = qq(:); + +% Choose matrices TP,TX, describing local coordinate systems tangent to P0(:), X0. +% Having homogeneous N-vector X, in the neighborhood of X0 it can be parameterized (minimaly) +% by inhomogeneous (N-1)-vector iX as X = X0 + TX*iX, where TX is a non-singular (N,N-1) matrix. + +for n = 1:N + [qX,dummy] = qr(X0(:,n)); + aux.TX{n} = qX(2:end,:)'; +end +for k = 1:K + [qP,dummy] = qr(reshape(P0(k2idx(k),:),[12 1])); + aux.TP{k} = qP(2:end,:)'; +end + +% form initial parameter vector p0 +p0 = []; +for k = 1:K + p0 = [p0; zeros(11,1)]; + if RADIAL + p0 = [p0; radial(k).u0; radial(k).kappa]; + end +end +p0 = [p0; zeros(3*N,1)]; + +% fill auxiliary structure for evaluating F(p) +aux.P0 = P0; +aux.X0 = X0; +aux.RADIAL = RADIAL; + +% Minimize || F(p) - y || over p. By Newton/Levenber-Marquardt. +p = p0; +lastFp = +Inf; +stepy = +Inf; +lam = .001; +fail_cnt = 0; +[Fp,J] = F(p,aux); +fprintf(' res (rms/max): %.10g / %.10g\n',sqrt(mean((y-Fp).^2)),max(abs(y-Fp))); +while (stepy > 100*eps) & (fail_cnt < 20) + D = (J'*J + lam*speye(size(J,2))) \ (J'*(y-Fp)); + FpD = F(p+D,aux); + if sum((y-Fp).^2) > sum((y-FpD).^2) + p = p + D; + lam = max(lam/10,1e-9); + stepy = max(abs(Fp-lastFp)); lastFp = Fp; + [Fp,J] = F(p,aux); + fail_cnt = 0; + fprintf([' res (rms/max), max_res_step, lam: %.10g / %.10g '... + '%.10g %g\n'],sqrt(mean((y-Fp).^2)),max(abs(y-Fp)),stepy,lam); + %figure(3); plot(abs(y-Fp)); drawnow + else + lam = min(lam*10,1e5); + fail_cnt = fail_cnt + 1; + end + stepy_DanielMartinec = .00005; % .005 0.1 .001 + if stepy < stepy_DanielMartinec + disp(['!!! ended by condition stepy < ' num2str(stepy_DanielMartinec) ... + '. Code modified by Daniel Martinec !!!']); + break; + end +end + +% rearrange computed parameter vector p to P, X +for k = 1:K + P(k2idx(k),:) = (P0(k2idx(k),:) + reshape(aux.TP{k}*p([1:11]+(k-1)*(11+RADIAL*3)),[3 4])); + if RADIAL + radial(k).u0 = p([12:13]+(k-1)*(11+RADIAL*3)); + radial(k).kappa = p([14]+(k-1)*(11+RADIAL*3)); + end +end +for n = 1:N + X(:,n) = X0(:,n) + aux.TX{n}*p((11+RADIAL*3)*K+[1:3]+(n-1)*3); +end + +return + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% +% function F: (Np,1) --> (Ny,1)x(Ny,Np), [y,J]=F(p), value y and jacobian J=dF(p)/dp of the function. +% +% p ... size ((11+3)*K+3*N,1), p = [iP(1); u0(1); kappa(1); .. iP(K); u0(K); kappa(K); iX(1); .. iX(N)], +% where Np = (11+2+raddeg)*K + 3*N +% aux ... auxiliary structure: +% aux.raddeg, aux.P0, aux.X0 ... see above +% aux.TP{1:K} ... size (12,11), Pk(:) = P0k+ aux.TP{k}*iPk +% aux.TX{1:N} ... size (4,3), Xn = X0n + aux.TX{n}*iXn +% aux.qivis ... size (K,N), has 1 in entries corresponding to visible image points +% y ... size (2*nnz(aux.qivis),1), visible image observations, stacked as [q(1,1); q(2,1); .. q(2*K,N)], unobserved q are omitted +%% +% We consider the imaging model as +% u = e2p(x) +% x = P*X +% P = P0 + TP*iP +% X = X0 + TX*iX +%% +function [y,J] = F(p,aux) +% +[K,N] = size(aux.qivis); +NR = aux.RADIAL*3; % number of radial distortion paramters +% +% rearrange p to P, X [,radial] +for k = 1:K + P(k2idx(k),:) = aux.P0(k2idx(k),:) + reshape(aux.TP{k}*p([1:11]+(k-1)*(11+NR)),[3 4]); + if aux.RADIAL + radial(k).u0 = p([12:13]+(k-1)*(11+NR)); + radial(k).kappa = p([14]+(k-1)*(11+NR)); + end +end +X = zeros(4,N); +for n = 1:N + X(:,n) = aux.TX{n}*p((11+NR)*K+[1:3]+(n-1)*3); +end +X = aux.X0 + X; +% +% compute retina points q +for k = 1:K + x(k2idx(k),:) = P(k2idx(k),:)*X; + q(k2idx(k,2),:) = p2e(x(k2idx(k),:)); + if aux.RADIAL + q(k2idx(k,2),:) = raddist(q(k2idx(k,2),:),radial(k).u0,radial(k).kappa); + end +end +qq = reshape(q,[2 K*N]); +qq = qq(:,aux.qivis); +y = qq(:); % function value +% +% compute Jacobian J = dF(p)/dp +if nargout<2, return, end +[kvis,nvis] = find(aux.qivis); +Ji = zeros(2*length(kvis)*(11+NR+3),1); Jj = zeros(size(Ji)); Jv = zeros(size(Ji)); +cnt = 0; +for l = 1:length(kvis) % loop for all VISIBLE points in all cameras + k = kvis(l); + n = nvis(l); + xl = x(k2idx(k),n); + ul = p2e(xl); + + % Compute derivatives (Jacobians). Notation: E.g., dudx = du(x)/dx, etc. + dxdP = kron(X(:,n)',eye(3))*aux.TP{k}; % dx(iP,iX)/diP + dxdX = P(k2idx(k),:)*aux.TX{n}; % dx(iP,iX)/diX + dudx = [eye(2) -ul]/xl(3); + if aux.RADIAL + [dqdu,dqdu0,dqdkappa] = raddist(ul,radial(k).u0,radial(k).kappa); + else + dqdu = eye(2); dqdu0 = []; dqdkappa = []; + end + dqdP = dqdu*dudx*dxdP; % dq(iP,iX)/diP + dqdX = dqdu*dudx*dxdX; % dq(iP,iX)/dX + + c = cnt+[1:2*(11+NR+3)]; + [Ji(c),Jj(c),Jv(c)] = spidx([1:2]+(l-1)*2,[[1:(11+NR)]+(k-1)*(11+NR) (11+NR)*K+[1:3]+(n-1)*3],[dqdP dqdu0 dqdkappa dqdX]); + cnt = cnt + 2*(11+NR+3); + +end +J = sparse(Ji,Jj,Jv); +return + + +function [i,j,v] = spidx(I,J,V) +% +i = I'*ones(1,length(J)); +j = ones(length(I),1)*J; +i = i(:); +j = j(:); +v = V(:); +return + + +function P = normP(P) +for k = 1:size(P,1)/3 + Pk = P(k2idx(k),:); + P(k2idx(k),:) = Pk/sqrt(sum(sum(Pk.*Pk))); +end +return + + +function x = normx(x) +x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); +return \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/setpaths.m b/MultiCamSelfCal/MartinecPajdla/setpaths.m new file mode 100644 index 0000000..7fc6474 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/setpaths.m @@ -0,0 +1,9 @@ +Start_path = './MartinecPajdla'; + +addpath ([Start_path '/fill_mm']) +addpath ([Start_path '/fill_mm_test']) +addpath ([Start_path '/utils']) + +global OutputDir Matlab_data; +Matlab_data = [ str_cut(Start_path) 'Matlab_data/' ]; +OutputDir = [ Matlab_data 'output/' ]; diff --git a/MultiCamSelfCal/MartinecPajdla/startup.m b/MultiCamSelfCal/MartinecPajdla/startup.m new file mode 100644 index 0000000..939c1ca --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/startup.m @@ -0,0 +1,18 @@ +MATLAB_PATH = '/usr/local/matlab/'; + +global Start_path; +Start_path = '/home.dokt/martid1/export/svoboda/Matlab'; +setpaths; + +screen = get(0, 'ScreenSize'); +xwidth = screen(4)/2; +ywidth = screen(4)/2; +set(0, 'defaultfigureposition', [screen(3)-xwidth*1.1 5 xwidth*1.1-4 ywidth*0.9]); +clear screen xwidth ywidth + +set(0,'FormatSpacing', 'compact',... + 'DefaultFigureColormap' , (0:1/63:1)'*[1 1 1],... + 'DefaultFigureMenu', 'none' ); +close + +flops(0); diff --git a/MultiCamSelfCal/MartinecPajdla/test_.m b/MultiCamSelfCal/MartinecPajdla/test_.m new file mode 100644 index 0000000..7d13ae5 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/test_.m @@ -0,0 +1,7 @@ +global scene; +scene.ID = 2; +M = load_scene; if isempty(M), return; end; + +options.no_factorization = 0; +options.create_nullspace.trial_coef = 10; +[ P,X, u1,u2, info ] = fill_mm(M, options); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/comb.m b/MultiCamSelfCal/MartinecPajdla/utils/comb.m new file mode 100644 index 0000000..65c2feb --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/comb.m @@ -0,0 +1,8 @@ +%returns combination number n over k +function r = comb(n,k) + +r=1; +for i=1:k + r=r*(n-i+1)/i; +end + diff --git a/MultiCamSelfCal/MartinecPajdla/utils/combfirst.m b/MultiCamSelfCal/MartinecPajdla/utils/combfirst.m new file mode 100644 index 0000000..3906aff --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/combfirst.m @@ -0,0 +1,12 @@ +%combfirst Returns the first combination in order of shifting the least left +%number to the right. +% +% function com=combfirst(n, k) +% +%n and k has the meaning of combination number. +function com=combfirst(n, k) + +com=find(1 == ones(1,k-1)); +com=[com k-1]; + +return \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/utils/combnext.m b/MultiCamSelfCal/MartinecPajdla/utils/combnext.m new file mode 100644 index 0000000..05511d4 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/combnext.m @@ -0,0 +1,28 @@ +%combnext Returns the next combination in order of shifting the least left +%number to the right. +% +% function next=combnext(n, k, com) +% +%n and k has the meaning of combination number. +function next=combnext(n, k, com) + + next=com; + move=k; moved=0; + while ~moved + if next(move) < n-k+move + next(move)=next(move)+1; + for i=move+1:k + next(i)=next(move)+i-move; + end + moved=1; + else + if move>1 + move=move-1; + else + disp('Error: this code should have never be called'); + end + end + end + +return + diff --git a/MultiCamSelfCal/MartinecPajdla/utils/diff_rand_ints.m b/MultiCamSelfCal/MartinecPajdla/utils/diff_rand_ints.m new file mode 100644 index 0000000..54c4001 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/diff_rand_ints.m @@ -0,0 +1,23 @@ +%diff_rand_ints Add n random ints in some scope to a given vector. +% +% Add n ints, randomly chosen between from and to inclusive to the list in +% ints. The resulting vector of ints should have all different numbers. +% +% y = diff_rand_ints(ints, n,from,to) + +function y = diff_rand_ints(ints, n,from,to) + +if (to + 1 - from) < n + size(ints,2) + error(sprintf('Not enough room for %d random numbers from %d to %d', n, from, to)); +end +for i = 1:n + x = random_int(from,to-size(ints,2)); + oldx = from - 1; + while oldx < x + temp = x; + x = x + sum((oldx < ints) & (ints <= x)); + oldx = temp; + end + ints = [ints,x]; +end +y = ints; \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/utils/dist.m b/MultiCamSelfCal/MartinecPajdla/utils/dist.m new file mode 100644 index 0000000..b9d8ab2 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/dist.m @@ -0,0 +1,22 @@ +%dist Return distance between image points in homog. coor. in specified metric. +% +% d = dist(M1, M2, metric) +% +% Parameters: +% metric .. x- and y-coordinates are associated with a single image point +% 1 .. square root of sum of squares of x- and y-coordinates +% 2 .. std of x- and y-coordinates +% <=> equivalent to noise type 2 in noise_add + +function d = dist(M1, M2, metric) + +if nargin < 3, metric = 2; end + +switch metric, + case 1, I = ~isnan(M1(1:3:end,:)) &~isnan(M2(1:3:end,:)); + d = eucl_dist(M1, M2, I) / sum(sum(I)); + case 2, D = normalize_cut(M1) - normalize_cut(M2); + i = find(~isnan(D(1:2:end))); + d = std([D(2*i-1) D(2*i)]); + otherwise, error('dist: unknown metric'); +end diff --git a/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist.m b/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist.m new file mode 100644 index 0000000..dad5b22 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist.m @@ -0,0 +1,11 @@ +%eucl_dist Return Eucledian norm of the difference between two scenes. +% +% er = eucl_dist(M0, M [,I]) +% +% Note: Perspective cameras are assumed. + +function er = eucl_dist(M0, M, I) + +if nargin < 3, I = ~isnan(M0(1:3:end,:)) & ~isnan(M(1:3:end,:)); end + +er = eucl_dist_only(normalize_cut(M0), normalize_cut(M), I); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist_only.m b/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist_only.m new file mode 100644 index 0000000..ade7a6d --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/eucl_dist_only.m @@ -0,0 +1,32 @@ +%eucl_dist_only Return Eucledian norm of the difference between two scenes. +% +% [e_norm, distances] = eucl_dist_only(M0, M [,I [,step]]) +% +% Parameters: +% step .. same as in k2i (see help k2i) +% default: step = 2 i.e. inhomogenous coordinates are assumed +% +% distances .. Eucledian distances between each known elements of M0 and M + +function [e_norm, distances] = eucl_dist_only(M0, M, I, step) + +if nargin < 3, I = ~isnan(M0(1:2:end,:)) & ~isnan(M(1:2:end,:)); end +if nargin < 4, step = 2; end + +if nargin >= 3 + m = size(M,1)/step; + if size(I,1) ~= m & ~isempty(M) + disp(sprintf(['!!! Warning: eucl_dist_only:' ' the height of I is' ... + ' bad, it should be equal to %d'], m)); keyboard;end +end + +known = find(I); +diff = (M0(k2i(known,step)) - M(k2i(known,step))) .^ 2; + +% sqrt must be performed on each point separately +for s = 1:step + B(s,:) = diff(s:step:end)'; % x-coordinates + B(s,:) = diff(s:step:end)'; % y-coordinates +end +distances = sqrt(sum(B)); +e_norm = sum(distances); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/file_exists.m b/MultiCamSelfCal/MartinecPajdla/utils/file_exists.m new file mode 100644 index 0000000..87640f6 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/file_exists.m @@ -0,0 +1,17 @@ +%file_exists Detection, whether given file exists. +% +% ex = file_exists(name) +% +% name .... file name +% ex .... logical value, if given file exist (0/1) + +function [ex] = file_exists(name); + +fid = fopen(name,'r'); +if fid < 0 + ex = 0; +else + fclose(fid); + ex = 1; +end + diff --git a/MultiCamSelfCal/MartinecPajdla/utils/k2i.m b/MultiCamSelfCal/MartinecPajdla/utils/k2i.m new file mode 100644 index 0000000..889319d --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/k2i.m @@ -0,0 +1,20 @@ +%k2i Compute indices of matrix rows corresponding to views k [with some step]. +% +% i = k2i(k [,step]) +% +% If k is scalar, it is i = [1:step]+step*(k-1). +% +% Default: step = 3 +% +% E.g., for REC.u, REC.P, REC.X use k2i(k), +% for REC.q use k2i(k,2). + +function i = k2i(k,step) + +if nargin<2 + step = 3; +end + +k = k(:)'; +i = [1:step]'*ones(size(k)) + step*(ones(step,1)*k-1); +i = i(:); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/kill_spaces.m b/MultiCamSelfCal/MartinecPajdla/utils/kill_spaces.m new file mode 100644 index 0000000..9d38d87 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/kill_spaces.m @@ -0,0 +1,5 @@ +%kill_spaces Remove all spaces from given string. + +function r = kill_spaces(s) + +r = s(setdiff(1:length(s),findstr(s,' '))); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/load_scene.m b/MultiCamSelfCal/MartinecPajdla/utils/load_scene.m new file mode 100644 index 0000000..211137c --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/load_scene.m @@ -0,0 +1,226 @@ +%load_scene Load data of a real scene. +% +% function [M,I] = load_scene([show[, load_all_images]]) +% +% global scene.ID ... contains the number of the scene to load + +function [M,I] = load_scene(show, load_all_images) + +if nargin < 1, show = 0; end + if nargin < 2, load_all_images = 0; end + +global scene Matlab_data; + +if scene.ID == 1 % artificial cubes + if ~isfield(scene,'to_print') + scene.to_print = 0; end % 0..little markers + thin lines, + % 1..big markers + thick lines + if ~isfield(scene,'load') | ~isfield(scene.load,'m') + scene.load.m = 5; end + if ~isfield(scene.load,'predef') + scene.load.predef = 0; end % 2 .. cubes like in the master thesis + if ~isfield(scene.load,'emptyness') + scene.load.emptyness = 0.05; end + [M,I,P0,X0,scene.lp,polys] = ... + create_cubes(2,scene.load.m,scene.load.predef,scene.load.emptyness,[],0); + M(k2idx(find(~I))) = NaN; return +end + +if 1, dir = [ Matlab_data 'cmp/' ]; % data in my directory +else dir = '/data/3DR/Projrec/'; end + +scene.data_type = 'cmp'; % data from Cmp +scene.detection = 'manual'; +switch scene.ID + case 2, scene.name = 'House'; + file='Domek/domek.cor'; %'OK.cor'; + picDir='Domek'; %'tif'; + case 3, scene.name = 'Corridor'; + file='Coridor-Cake/coridorC.cor'; picDir='Coridor-Cake'; + case 4, scene.name = 'Church'; + file='Plec/Plec.cor'; picDir='Plec'; + case 5, scene.name = 'Kampa'; + file='Kampa/Kampa.cor'; picDir='Kampa'; + case 6, scene.name = 'Cubes'; + file='Kostky/Kostky.cor'; picDir='Kostky'; + case 7, scene.name = 'puzzle'; + file='adam/hlavolam.cor'; picDir='adam'; + + otherwise % data from Oxford: + scene.data_type = 'oxford'; + scene.detection = 'Harris'' operator'; + dir = [ Matlab_data 'oxford/' ]; + decimals=3; + switch scene.ID + case 101, scene.name = 'House (Oxford)'; + file='house'; last_index=9; picDir='house/'; + case 102, scene.name = 'Corridor (Oxford)'; + file='bt'; last_index=10; picDir='corridor/'; + + otherwise % data from Oxford: + scene.data_type = 'oxford_'; + switch scene.ID + case 170, scene.name = 'Dinosaur (Oxford)'; + file='viff'; last_index=36; picDir='dinosaur/'; + + otherwise + scene.data_type = 'leuven'; + scene.detection = 'Harris'' operator'; + dir = [ Matlab_data 'leuven/' ]; + decimals=3; first_index = 0; ext=''; + switch scene.ID + case 201, scene.name = 'Castle (Leuven)'; + file='viff'; last_index=21; picDir='Castle/'; + case 202, scene.name = 'Temple (Leuven)'; + file='temple'; first_index=2; last_index=6; + decimals=2; ext='.pgm'; picDir='Temple/'; + otherwise + scene.data_type = 'boujou'; + scene.detection = 'boujou'; + dir = [ Matlab_data 'boujou/' ]; + switch scene.ID + case 301, scene.name = 'Road in Forest'; + file='road'; last_index=1; picDir='Road_in_Forest/'; + case 302, scene.name = 'Corridor (CMP)'; + file='corridorvideo'; last_index=1; picDir='Corridor_CMP/'; + case 303, scene.name = 'Fish eye round'; + file='fish_ray4in_im'; last_index=1; picDir='Fish_eye_round/fish_ray_4_in/'; + decimals=4; + otherwise + disp(sprintf('Error: undefined scene with ID %d.', scene.ID)); + M = []; I = []; return; + end + end + end + end +end + +switch scene.data_type + case 'cmp' + if ~exist('CORR'), load ([dir file],'-mat'); + CORR = CORR_EXCHANGE; + CORR.d.picturesDir=[dir picDir]; + end + x = CORR.d.corr(:,:,1)'; + y = CORR.d.corr(:,:,2)'; + [m n] = size(x); + I = x~=0; + M(1:3:3*m,1:n) = x; + M(2:3:3*m,1:n) = y; + M(3:3:3*m,1:n) = ones(size(I)); + + % kill unknown data + M(k2i(find(~I))) = NaN; + case 'oxford' + [M,I] = oxford2mm([dir picDir file], last_index); + M = e2p(M); % add ones as homogenous coordinates + [m n] = size(I); + case 'oxford_' + [M,I] = oxford_2mm([dir picDir file]); + M = e2p(M); % add ones as homogenous coordinates + [m n] = size(I); + case 'leuven' + scene.mm_file = [ dir picDir kill_spaces(scene.name) '.mat' ]; + convert = 1; to_file = 0; + if 1 %scene.ID == 201 % `Leuven's castle' is too big to compute always again + to_file = 1; + convert = ~file_exists(scene.mm_file); % whether file doesn't exist + end + if convert + [M,I] = leuven2mm([dir picDir], file, last_index, first_index, ... + decimals, ext); + if to_file + disp([ 'Saving to precompiled file ' scene.mm_file '...' ]); + a=[ 'save ' scene.mm_file ' M I']; eval(a); end + else + disp([ 'Loading from precompiled file ' scene.mm_file '...' ]); + a=[ 'load ' scene.mm_file ' M I']; eval(a); + end + M = e2p(M); % add ones as homogenous coordinates + [m n] = size(I); + + case 'boujou' + [M,I] = boujou2mm([dir picDir file '.txt']); + fprintf(1,'Converting from Eucledian to projective coordinates...'); tic; + M = e2p(M); % add ones as homogenous coordinates + disp(['(' num2str(toc) ' sec)']); + [m n] = size(I); +end + + +% take out some points and pictures + % take out some pictures + if scene.ID==3, % take out the first picture + I=I([2:8],:); M=M([2*3-2:3*8],:); m=m-1; + end + + % take out points which are just in two images or less + if 1 %scene.ID==3, + out = find(sum(I)<=1); %<=2 + stay = setdiff(1:n,out); M = M(:,stay); + I = I(:,stay); [m n] = size(I); + end + + % take out points which are not in the first image + %if scene.ID==2, stay=find(I(1,:) == 1); M=M(:,stay); I=I(:,stay); end + + +if strcmp(scene.data_type, 'cmp'), + decimals = ceil(log10(m + .1)); +end +format = sprintf('%%0%dd',decimals); + +% load images +changed = 0; +if show | load_all_images, to_load = m; else to_load = 1; end +if ~isfield(scene,'img') | size(scene.img,2)~=to_load,scene.img{to_load}=[];end +for k=1:to_load + switch scene.data_type + case 'cmp' + nos = sprintf(format,k); + InputFile = strcat(CORR.d.picturesDir,filesep,CORR.d.filePrefix,... + nos,'.', CORR.d.fileExt); + case 'oxford' + InputFile = [[dir picDir file] '.' num2str(k-1,format) '.png' ]; % '.pgm' + % doesn't work in Matlab + case 'oxford_' + InputFile = [[dir picDir file] '.' num2str(k-1,format) '.png' ]; % '.ppm' + % doesn't work in Matlab + case 'leuven' + InputFile = [[dir picDir file] '.' num2str(k-1+first_index,format) ... + '.png' ]; % '.pgm' doesn't work in Matlab + case 'boujou' + InputFile = [[dir picDir file] '.' num2str(k-1+first_index,format) ... + '.jpg' ]; + otherwise + disp('Unknown data type'); + end + + if isempty(scene.img{k}) | ~isfield(scene.img{k}, 'file') ... + | ~strcmp(scene.img{k}.file, InputFile) + disp(sprintf('Loading %s...', InputFile)); + scene.img{k}.file = InputFile; + scene.img{k}.data = imread(InputFile); + + scene.image_size = size(scene.img{1}.data); + if scene.ID == 2 % Domek + % pictures are too big therefore some smaller ones are taken + scene.image_size_show = scene.image_size; + scene.image_size(1:2) = [ 2003 2952 ]; + end + + changed=1; + end +end + +tiles_y=4; if m/tiles_y > 2, tiles_x = 6; else tiles_x = 4; end +global tiles; tiles_set(tiles_x, tiles_y); + +if show & changed % show + for k=1:m + fig = figure(10+k); + [x y] = tiles2xy(k); subfig(tiles.y, tiles.x, y*tiles.x+x+1, fig); + cla; image(scene.img{k}.data); hold on; + drawpointsI(M([k*3-2,k*3-1],:),[1;1]*I(k,:)); + end +end diff --git a/MultiCamSelfCal/MartinecPajdla/utils/normalize.m b/MultiCamSelfCal/MartinecPajdla/utils/normalize.m new file mode 100644 index 0000000..e323aaa --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/normalize.m @@ -0,0 +1,33 @@ +% +% Function Mnorm=normalize(M,I) +% +% normalizes M by dividing each point by its homogenous coordinate +% (these coordinates equal to ones afterwards). +% +% Parameter I can be omitted for complete scenes. + +function Mnorm=normalize(M,I) + +m=size(M,1)/3; +n=size(M,2); + +if nargin < 2 + I=ones(m,n); +end + +Mnorm(1:3*m, 1:n) = M; % There are two reasons for this. (i) Make NaN the + % unknown data thus the whole matrix has to be filled + % by NaNs. (ii) Sometimes it happens that when there + % is a missing data in the last column(s), the + % column(s) disapppears. + +known = find(I); +big_enough = known(find( abs(M(known*3)) > eps )); +if m == 1, big_enough = big_enough'; end + +if ~isempty(big_enough) + div_by = repmat(M(big_enough*3)',3,1); + Mnorm(k2i(big_enough)) = M(k2i(big_enough)) ./ div_by(:); +end + +Mnorm(setdiff(known, big_enough)*3) = 1; diff --git a/MultiCamSelfCal/MartinecPajdla/utils/normalize_cut.m b/MultiCamSelfCal/MartinecPajdla/utils/normalize_cut.m new file mode 100644 index 0000000..6df6ffd --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/normalize_cut.m @@ -0,0 +1,16 @@ +% +% Function Mnorm=normalize_cut(M,I) +% +% normalizes homogenous coordinates and cuts the last coordinate +% (which then equals to 1). + +function Mnorm=normalize_cut(M,I) + +m=size(M,1)/3; + +if nargin < 2, Mnorm=normalize(M); +else Mnorm=normalize(M,I); end + +Mnorm=Mnorm(union((1:m)*3-2,(1:m)*3-1),:); + +return diff --git a/MultiCamSelfCal/MartinecPajdla/utils/p2e.m b/MultiCamSelfCal/MartinecPajdla/utils/p2e.m new file mode 100644 index 0000000..2a02fc4 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/p2e.m @@ -0,0 +1,15 @@ +function x = p2e(x_) + +%P2E Projective to euclidean coordinates. +% x = p2e(x_) Computes euclidean coordinates by dividing +% all rows but last by the last row. +% x_ ... Size (dim+1,N) Projective coordinates. +% x ... Size (dim,N). Euclidean coordinates. +% (N can be arbitrary.) + +if isempty(x_) + x = []; + return; +end +dim = size(x_,1) - 1; +x = x_(1:dim,:) ./ (ones(dim,1)*x_(dim+1,:)); diff --git a/MultiCamSelfCal/MartinecPajdla/utils/random_int.m b/MultiCamSelfCal/MartinecPajdla/utils/random_int.m new file mode 100644 index 0000000..690fb1b --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/random_int.m @@ -0,0 +1,7 @@ +%random_int Returns random integer is specified range. +% +% y = random_int(from, to) + +function y = random_int(from, to) + +y = floor(from + (1 + to - from)*rand); \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/utils/str_cut.m b/MultiCamSelfCal/MartinecPajdla/utils/str_cut.m new file mode 100644 index 0000000..1db41d3 --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/str_cut.m @@ -0,0 +1,16 @@ +%str_cut Cut off last piece according to a delimeter from a string. +% +% [ head, tail ] = str_cut(s, delim) + +function [ head, tail ] = str_cut(s, delim) + +if nargin < 2, delim = '/'; end + +idcs = findstr(s,delim); +if isempty(idcs), + head = s; + tail = []; +else + head = s(1:idcs(end)); + tail = s(length(head)+1:end); +end \ No newline at end of file diff --git a/MultiCamSelfCal/MartinecPajdla/utils/tiles_set.m b/MultiCamSelfCal/MartinecPajdla/utils/tiles_set.m new file mode 100644 index 0000000..2d3aa7d --- /dev/null +++ b/MultiCamSelfCal/MartinecPajdla/utils/tiles_set.m @@ -0,0 +1,10 @@ +%tiles_set Set global tiles.x/y to the specified values. +% +% tiles_set(x, y) + +function tiles_set(x, y) + +global tiles; + +tiles.x = x; +tiles.y = y; diff --git a/MultiCamSelfCal/OutputFunctions/dispcamstats.m b/MultiCamSelfCal/OutputFunctions/dispcamstats.m new file mode 100644 index 0000000..6b85867 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/dispcamstats.m @@ -0,0 +1,13 @@ +function ret = dispcamstats(cam,inliers); +% DISPCAMSTATS display statistics about cameras to the command window +% +% auxiliary function for the main gocal script +% with a minor modification is may also write the statistics to a file +% I never needed it. +% +% $Id: dispcamstats.m,v 2.1 2005/05/23 16:21:50 svoboda Exp $ + +fprintf(1,'CamId std mean #inliers \n'); +for i=1:size(cam,2), + fprintf(1,'%3d %8.2f %8.2f %6d \n',cam(i).camId, cam(i).std2Derr, cam(i).mean2Derr, sum(inliers.IdMat(i,:))); +end diff --git a/MultiCamSelfCal/OutputFunctions/drawcloud.m b/MultiCamSelfCal/OutputFunctions/drawcloud.m new file mode 100644 index 0000000..bcfedb8 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/drawcloud.m @@ -0,0 +1,28 @@ +% drawcloud ... draw point cloud +% +% [fig] = drawcloud(X,fig,{color}) +% +% X ... 3xn matrix containing the points +% if X is 4xn only the first 3 rows are used +% fig . figure handle +% color color of plotting; defaults to blue +% +% fig . return the figure handle +% +% $Id: drawcloud.m,v 2.0 2003/06/19 12:07:02 svoboda Exp $ + +function [fig] = drawcloud(X,fig,color) +if nargin < 3 + color = 'b'; % default color +end + +figure(fig), hold on +plot3(X(1,:),X(2,:),X(3,:),[color,'o']) + + +view([1,1,1]); +axis('equal'); +grid on +rotate3d on +fig=fig; +return diff --git a/MultiCamSelfCal/OutputFunctions/drawobject.m b/MultiCamSelfCal/OutputFunctions/drawobject.m new file mode 100644 index 0000000..4199ad0 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/drawobject.m @@ -0,0 +1,37 @@ +% drawobject ... draws a (calibration) object of a specific type +% +% [fig] = drawobject(X, ctype, fig, {color}) +% +% X ...... 3xn matrix containg the object's corner points +% if X is 4xn only the first 3 rows are used +% ctype .. a string specifying the type of the object +% according to the type the appropriate drawing functions is called +% supported types are: +% 'cube' - a cube +% 'octagon' - a planar octagon +% 'cloud' - a point cloud +% fig .... figure handle +% color .. color of plotting; defaults to blue +% +% fig .... returns the figure handle +% +% $Id: drawobject.m,v 2.0 2003/06/19 12:07:02 svoboda Exp $ + +function [fig] = drawobject(X,ctype,fig,color) + +if nargin < 4 + color = 'b'; % default color +end + +switch ctype +case 'cube', + drawcube(X,fig,color); +case 'octagon', + drawoctagon(X,fig,color); +case 'cloud', + drawcloud(X,fig,color); +otherwise, + error('unknown object type: ', ctype) +end + +return diff --git a/MultiCamSelfCal/OutputFunctions/drawscene.m b/MultiCamSelfCal/OutputFunctions/drawscene.m new file mode 100644 index 0000000..d0f5fd2 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/drawscene.m @@ -0,0 +1,61 @@ +function drawscene(X,C,R,fig,ctypehint,scenetitle,camsId) + +% drawscene ... plots calibration points, cameras and their viewing axes +% +% drawscene(X,C,R,fig,ctypehint,scenetitle) +% +% X ............ 4xPOINTS matrix containg POINTS object points +% C ............ 3xCAMS matrix containing the camera centers (in world coord.) +% R ............ 3*CAMSx3 matrix containing camera rotation matrices +% (needed for drawing the viewing axes) +% fig .......... figure handle (defaults to 1) +% ctypehint .... calibration object type of X (defaults to 'cloud') +% scenetitle ... title of the plot (defaults to '') +% camsIs ....... 1xCAMS vector with cameas Id (default is 1:CAMS + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: drawscene.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ +% $State: Exp $ + +POINTS = size(X,2); +CAMS = size(C,2); + +if nargin < 7 + camsId = [1:CAMS]; +end + +if (nargin < 3) + error('not enough input arguments'); +end +if (nargin < 5) + scenetitle = ''; +end +if (nargin < 4) + ctypehint = 'cloud'; +end + +figure(fig); clf +title(scenetitle) +grid on +axis equal + +% plot camera positions (blue) +drawcloud(C,fig,'b'); + +% plot calibration object (red) +drawobject(X,ctypehint,fig,'r'); + +% Mean of all points +centroid = mean(X(1:3,:)'); + +% plot viewing axes +for i=1:CAMS + axis_dir = -R(3*i,:); % 3rd row of i-th rotation matrix + axis_len = 0.6*norm(C(1:3,i)-centroid'); + endpoint = C(1:3,i)+axis_len*axis_dir'; + line([C(1,i),endpoint(1)],[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]); + text(C(1,i),C(2,i),C(3,i),sprintf('%4d',camsId(i)),'Color','k'); +end + + diff --git a/MultiCamSelfCal/OutputFunctions/evalreprerror.m b/MultiCamSelfCal/OutputFunctions/evalreprerror.m new file mode 100644 index 0000000..bf252c3 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/evalreprerror.m @@ -0,0 +1,53 @@ +% evalreprerror ... computes and plots the final error statistics +% +% cam = evalreprerror(cam,config) +% cam, config ... see the main GOCAL script +% +% $Id: evalreprerror.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ + +function cam = evalreprerror(cam,config) + +disp('2D reprojection error') +disp(sprintf('All points: mean %2.2f pixels, std is %2.2f',mean([cam.err2d]), std([cam.err2d])')); +% disp(sprintf('Inliers: mean %2.2f pixels, std is %2.2f',mean([cam.inerr2d]), std([cam.inerr2d])')); +if mean([cam.err2d])>1.5 | std([cam.err2d])>1.5 + disp('***************************************************') + disp('W A R N I N G: the reprojection error is relatively high !') +end + +%%% +% evaluate the reprojection error for each camera separately to detect possible problems +%%% +for i=1:size(config.cal.cams2use,2), + cam(i).mean2Derr = mean(cam(i).err2d); + cam(i).std2Derr = std(cam(i).err2d); +end +% sort the values and print them to the 2D graphs + +figure(30), +clf +plot(config.cal.cams2use,[cam.mean2Derr],'bd'), +hold on, grid on, +plot(config.cal.cams2use,[cam.mean2Derr],'b-'), +plot(config.cal.cams2use,[cam.std2Derr],'rd') +plot(config.cal.cams2use,[cam.std2Derr],'r-') +xlabel('Id of the camera') +title('2D error: mean (blue), std (red)') +ylabel('pixels') + +figure(31) +clf +bar(config.cal.cams2use,[cam.mean2Derr;cam.std2Derr]',1.5) +grid on +xlabel('Id of the camera') +title('2D error: mean (blue), std (red)') +ylabel('pixels') + +figure(31), +eval(['print -depsc ', config.paths.data, 'reprerrors.eps']) + +figure(4), +eval(['print -depsc ', config.paths.data, 'reconstructedsetup.eps']) + +Ret = 1; +return diff --git a/MultiCamSelfCal/OutputFunctions/savecalpar.m b/MultiCamSelfCal/OutputFunctions/savecalpar.m new file mode 100644 index 0000000..e663232 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/savecalpar.m @@ -0,0 +1,101 @@ +% SaveCalPar save calibration parameters in different formats +% +% [Cst,Rot] = savecalpar(P,config) +% P ... 3*CAM x 4 matrix containing result of selfcalibration, see EUCLIDIZE +% config ... configuration structure, see CONFIGDATA +% +% +% Cst ... CAMSx3 matrix containing the camera centers (in world coord.) +% Rot ... 3*CAMSx3 matrix containing camera rotation matrices + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: savecalpar.m,v 2.0 2003/06/19 12:07:03 svoboda Exp $ +% $State: Exp $ + + +function [Cst,Rot] = savecalpar(P,config) + +idxused = config.cal.cams2use; + +CAMS = size(P,1)/3; + +Cst = zeros(CAMS,3); +Pst = zeros(3*CAMS,3); +Rot = []; +for i=1:CAMS, + % do not save P matrices in separate files + if 1 + Pmat = P(i*3-2:i*3,:); + save(sprintf(config.files.CalPmat,idxused(i)),'Pmat','-ASCII'); + end + sc = norm(P(i*3,1:3)); + % first normalize the Projection matrices to get normalized pixel points + P(i*3-2:i*3,:) = P(i*3-2:i*3,:)./sc; + % decompose the matrix by using rq decomposition + [K,R] = rq(P(i*3-2:i*3,1:3)); + tvec= inv(K)*P(i*3-2:i*3,4); % translation vector + C = -R'*tvec; % camera center + % Stephi calib params + Pstephi = R'*inv(K); + Pst(i*3-2:i*3,:) = Pstephi; + Cst(i,:) = C'; + % Stephi requires to save the pars in more "wordy" form + fid = fopen(sprintf(config.files.StCalPar,idxused(i)),'wt'); + if ~fid + error('SaveCalPar: The camera cal file cannot be opened'); + else + fprintf(fid,'C1 = %f \n', C(1)); + fprintf(fid,'C2 = %f \n', C(2)); + fprintf(fid,'C3 = %f \n', C(3)); + fprintf(fid,'\n'); + fprintf(fid,'P11 = %f \n', Pstephi(1,1)); + fprintf(fid,'P12 = %f \n', Pstephi(1,2)); + fprintf(fid,'P13 = %f \n', Pstephi(1,3)); + fprintf(fid,'P21 = %f \n', Pstephi(2,1)); + fprintf(fid,'P22 = %f \n', Pstephi(2,2)); + fprintf(fid,'P23 = %f \n', Pstephi(2,3)); + fprintf(fid,'P31 = %f \n', Pstephi(3,1)); + fprintf(fid,'P32 = %f \n', Pstephi(3,2)); + fprintf(fid,'P33 = %f \n', Pstephi(3,3)); + fclose(fid); + end + Rot = [Rot;R]; + % Prithwijit requires to save the pars in more "wordy" form + if 0 % do not save in the Prithwijit format + fid = fopen(sprintf(config.files.CalPar,idxused(i)),'wt'); + if ~fid + error('SaveCalPar: The camera cal file cannot be opened'); + else + fprintf(fid,'R11 = %f \n',R(1,1)); + fprintf(fid,'R12 = %f \n',R(1,2)); + fprintf(fid,'R13 = %f \n',R(1,3)); + fprintf(fid,'R21 = %f \n',R(2,1)); + fprintf(fid,'R22 = %f \n',R(2,2)); + fprintf(fid,'R23 = %f \n',R(2,3)); + fprintf(fid,'R31 = %f \n',R(3,1)); + fprintf(fid,'R32 = %f \n',R(3,2)); + fprintf(fid,'R33 = %f \n',R(3,3)); + fprintf(fid,'\n'); + fprintf(fid,'t11 = %f \n',tvec(1)); + fprintf(fid,'t21 = %f \n',tvec(2)); + fprintf(fid,'t31 = %f \n',tvec(3)); + fprintf(fid,'\n'); + fprintf(fid,'K11 = %f \n',K(1,1)); + fprintf(fid,'K12 = %f \n',K(1,2)); + fprintf(fid,'K13 = %f \n',K(1,3)); + fprintf(fid,'K21 = %f \n',K(2,1)); + fprintf(fid,'K22 = %f \n',K(2,2)); + fprintf(fid,'K23 = %f \n',K(2,3)); + fprintf(fid,'K31 = %f \n',K(3,1)); + fprintf(fid,'K32 = %f \n',K(3,2)); + fprintf(fid,'K33 = %f \n',K(3,3)); + fprintf(fid,'\n'); + fclose(fid); + end + end +end + +% save Stehpi params +save(config.files.Pst,'Pst','-ASCII'); +save(config.files.Cst,'Cst','-ASCII'); diff --git a/MultiCamSelfCal/OutputFunctions/showimg.m b/MultiCamSelfCal/OutputFunctions/showimg.m new file mode 100644 index 0000000..92c5748 --- /dev/null +++ b/MultiCamSelfCal/OutputFunctions/showimg.m @@ -0,0 +1,30 @@ +%SHOWIMG Pajdla: Shows scaled images in the gray palette +% +% +% function f = showimg(m,f) +% +% m = image matrix +% f = figure handle +% +% See also: IMAGE, IMAGESC, COLORMEN. + +% Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be +% pajdla@vision.felk.cvut.cz +% 03/06/95 ESAT-MI2, KU Leuven +% Documentation: +% Language: Matlab 4.2, (c) MathWorks +% +function f = showimg(m,f); + +if (nargin==2) + figure(f); +else + f=figure; +% colormen; + colormap('gray'); +end + +imagesc(m); +axis('image'); + +return diff --git a/MultiCamSelfCal/README.txt b/MultiCamSelfCal/README.txt new file mode 100644 index 0000000..638deaf --- /dev/null +++ b/MultiCamSelfCal/README.txt @@ -0,0 +1,435 @@ +% $Author: svoboda $ +% $Revision: 2.2 $ +% $Id: README.txt,v 2.2 2005/05/24 09:15:30 svoboda Exp $ +% $State: Exp $ + +WWW: +http://cmp.felk.cvut.cz/~svoboda/SelfCal/ +look at the home page to get the newest information, latest sources, +publications, sample data etc. + +Authors: +- Tomas Svoboda, svoboda@cmp.felk.cvut.cz, (design of the package, +most of the codes), corresponding author + +- Daniel Martinec and Tomas Pajdla, {martid1,pajdla}@cmp.felk.cvut.cz +(filling points) + +- Ondrej Chum, chum@fel.cvut.cz (RANSAC implementation) + +- Tomas Werner, werner@cmp.felk.cvut.cz (Projective Bundle Adjustment) + +- Jean-Yves Bouguet, jean-yves.bouguet@intel.com, (part of the Radial + distortion computation) + +Just a short how-to for multicamera selfcalibration: +---------------------------------------- +svoboda@vision.ee.ethz.ch, 08/2002 +updated 12/2002, 01/2003, 02/2003, 03/2003, 06/2003, 07/2003 + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% For BlueC users +%%% Very short how-to for them who know +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +(the steps denoted by * are typically needed to be performed just once +for each user) + +- Capture *.pvi files + +- convert *.pvi files to sequences of images + +* Go to */BlueCCal/BlueCFindingPoints and check all the three files + there. They have some local settings which has to be set for each + user differently. + +- run ./findpointsBlueC + This script starts finding process on each of the cluster machine + +- collect the data stored locally on the cluster machines. + +* Edit configdata.m and expname.m in */BlueCCal/CommonCfgAndIO + +- Go to */BlueCCal/MultiCamSelfCal, run matlab + +From now on in matlab window: +- >> gocal + Wait for the results. It may take several minutes if you have many + cameras and many points. + + Wait ...., and you are done if you are lucky ;-) + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Information relevant to the BlueC project +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +The CamSelfCal code is in /home/svoboda/Work/BlueCCal +The auxiliary scripts and acquisition SW are in /home/svoboda/Work/BlueCAcquire + +You will also need to set some local variables. +############################################################################ +# blue-c +if ( -f /opt/modules/modules/init/tcsh) then + source /opt/modules/modules/init/tcsh + if ( -d ${HOME}/lib/modulefiles ) then + module use ${HOME}/lib/modulefiles + endif + + # module add blue-c/linux-gcc3.2 # needed for 3DVideoRecorder + module add blue-c/linux # needed for the ClientTester + + setenv LD_LIBRARY_PATH $ACE_ROOT/ace/:/home/lamboray/lib/:$LD_LIBRARY_PATH + setenv LD_LIBRARY_PATH /pub/blue-c_lib/BCL/version_1.0/lib/linux-gcc3.2/:$LD_LIBRARY_PATH + # additional library I had to add + setenv LD_LIBRARY_PATH /pub/blue-c/development/pwes/lamboray/3DVideoRecorder/lib/:$LD_LIBRARY_PATH + setenv LD_LIBRARY_PATH /pub/blue-c_lib/BCL/version_1.0/lib/linux-gcc3.2/old/:$LD_LIBRARY_PATH +endif + +# Set local variabled for the auxiliary scripts +# machine name +set machine_basename = `echo $HOST | sed 's/[0-9]//g'` +setenv BlueC_MNAME "$machine_basename" +# local directory on the machines where the data is stored +setenv BlueC_LOCALDIR "/local/tomas" +# image base name +setenv BlueC_IMNAME "$machine_basename" +# basepath for binaries and auxiliary scripts +setenv BlueC_BASEPATH "/home/svoboda/Work/BlueCAcquire" +### +# full indexes and working machine to process the data +# need to be set differently for atlantics and arctics +if ( $machine_basename == "atlantic" ) then + # setenv BlueC_INDEXES `(seq 3 18)` + setenv BlueC_INDEXES "3 4 5 6 7 8 9 10 11 12 14 15 16 17 18" + setenv BlueC_WORKMACHINE "${machine_basename}2" +else if ($machine_basename == "arctic") then + setenv BlueC_INDEXES `(seq 1 16)` + # setenv BlueC_INDEXES "1 2 3 4 5 6 7 8 9 10 11 12 13 15 16" + setenv BlueC_WORKMACHINE "${machine_basename}19" +endif +############################################################################ + + +The scripts are in Perl or C-shell and are very simple and not very +robust. They use "convert" and "montage" tools from "ImageMagick" +package whis a standard part of most Linux distributions. + +BlueC acquisition software is mostly written by Edouard Carlo Lamboray, +lamboray@inf.ethz.ch. + +Run the administration script "firewire" written by Stephan Wuermlin, +wuermlin@inf.ethz.ch, if the 3DVideoRecorder does not work. +Try: +>> firewire stop +>> firewire reload +>> firewire chmod +This should help. + +Config files "configdata.m" and "expname.m" are in the sub-directory +Cfg. The configdata contains necessary paths to the data and the +expname determines the relevant subset of config data. + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +1) Image acqusition + +- Check if the video.cfg is OK. The shuter should be usualy at 590. It + very depends on the laser. The triggering should MUST be 1! + +- Switch off the triggering signal. + +- Start "./3DVideoRecorder -u -s -b 0 -n #number_of_images" to capture + sequences on each machine with camera attached to. The + 3DVideoRecorder expects the config files in ../Cfg/ + directory. #number_of_images should be between 300-700. More images + gives more robustness however, their processing takes accordingly + longer. The processing time is linear in terms of images. + +- Switch the triggering signal on and ... + +- Wave laser pointer. Try to fill the whole working volume. Try to + keep the laser pointer visible to as many cameras as + possible. Especially is necessary to fill the volume close to the + cave floor. + +- The 3DVideoRecorder stores big *.pvi files in /local/ directory on + each of the machine then ... + +- Run the "collectdata" to extract, transform and collect + images. Check if the paths are specified correctly. + +- You can use "createMontages" script to create 4x4 composed + images. Check the image quality and the VISIBILITY of points. + +- Change the config.files.idxcams accordingly if some cameras are + missing. + +2) Computing images statitistics and finding laser projections in + images. + +The codes are in the sub-directory FindingPoints + +Important note: To run the image processing in parallel we need: be +able to run "matlab" in each of the machines and have ssh access +without password (http://www.cs.umd.edu/~arun/misc/ssh.html). + + +Alternatively, you can run the script im2points in one matlab, which +is also much more certain if you are not sure what you are doing. It +runs accordingly longer, 16 cameras, 500 images each needs about 30 +minutes at PIII @ 1GHz (it also depends on the required +precision). The computation time is linear in terms of cameras and +images. + + +- Edit configdata.m and put correct paths and all config constants you + want here. + +- Important! Set the correct experiment name into expname.m. + +- Run "im2pmultiproc.pl", check if it uses the right *.pm config. This + perl script will create some temporary files in the working + directory. Also it needs access rights. + +- Warning! If you stop "im2pmultiproc.pl" by Ctrl^C, the matlab + processes will be still running. They have to be yet killed + manually. + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +3) Selfcalibration + +- Run "gocal" in Matlab. Again, be sure to have correct name of + the experiment in "expname.m" + +- Check the graphical output. Some inside check points are + applied. Nevertheless, it may happen that the reprojection error is + small and the results spoiled. The graphical outputs has to be + checked especially if only few points used for the + computation. Check the detected points in the graphical windows. The + points should ideally span the whole area of images. Some "holes" + may indicate bad camera setting or a bad movement of the calibrator. + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +4) What to do if the SelfCalibration crashes + +- the computation itself is rather robust. However, it may crash if + some cameras have really strange points. + +- Check visually the detected points by typing + "figure(100), imshow(IdMat.loaded)" + in the main Matlab command window. The frameIds are on the x-axis, + the cameraIds on the y-axis. Detected points are white, otherwise + black. The whiter image the better. Black lines signalize some + camera problems. + +- To check the quality and mainly the reliability of the point + detection you can use the script "showpoints". It plots graphical + information to the images. + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Explanation of the configuration variables +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +../CommonCfgAndIO/configdata.m + +- config.paths.data = ['/scratch/WorkingCalib/']; + + The main working directory containg all data. Some data may be in + subdirectories. + + +- config.files.basename = 'arctic'; + + Basename for all. This basename will be the main common identifier + in all exported files. + + +- config.paths.img = [config.paths.data,config.files.basename,'%d/']; + + Basename for image sub-directories. + + +- config.files.imnames = [config.files.basename,'%d.pvi.*.']; + + Basename for images. + + +- config.files.idxcams = [1:10,12:16]; % related to the imnames + + Numbers that index the image sub-directories and names of various + data files. These indexes must correspond to what is on the disk. + +- config.imgs.LEDsize = 7; % avg diameter of a LED in pixels + + Parameter used in the finding procedure. When unsure, better to make + it slightly larger. Rather robust value. "7" works well for both of + the BlueC installations. + + +- config.imgs.LEDcolor = 'green'; % color of the laser pointer + + Used in the finding points. Color of the laser pointer used. + + +- config.imgs.LEDthr = 100; + + Optional parameter for the finding points procedure. Default value + (hidden) is 70. Sometimes, it may help to resolve problems with + misdetection. The higher value the brighter points accepted as valid + projections. Useful only in really special cases. + +- config.imgs.subpix = 1/5; + + Used in the finding points. Required subpixels accuracy. Values + 1/3-1/5 give quite nice results. Higher values like 1 or 1/2 + increase the speed of the finding procedure significantly which may + be useful in some fast try-and-test experiments + + +- config.cal.INL_TOL = 7; + + Rather important value. Initial tolerance value for epipolar + geometry verification. It influences both the pair-wise point + validation through the epipolar geometry computation and the + iterative refinement at the end. It should correpond to the expected + radial distortion in the cameras. This value is iteratively + decreased during the optimization process. + + +- config.cal.NUM_CAMS_FILL = 10; + + How many camera may be filled by "artificial" points. This value + should depend to expected visibility of the calibration + points. Higher values are typically needed if the laser pointer is + not optimally visible in many cameras. Typically, the higher value + the slower run of the complete procedure. On the other hand, in case + of bad visibility, the high value may improve the robustness. From + the principle, this value can be maximally #CAMS-3. If a higher + value is set, automatic correction is applied. + + +- config.cal.DO_BA = 0; + + Do the Bundle Adjustment of the projective reconstruction a the end + of the all iterations. It is quite slow for many points and + cameras. It may improve the overall accuracy. Often not need at all. + +- config.cal.START_BA = 1; + + Optional parameters. When set, it does the Projective Bundle + Adjustment in each step in the final interation for removing + outliers. It may improves the performance for bad data sets. The + whole process it than accordingly slower. + + +- config.cal.UNDO_RADIAL= 1; + + Undo the radial distortion by using the paramaters from the CalTech + camera calibration toolbox? + + +- config.cal.UNDO_HEIKK = 0; + + Undo the radial distortion by using the parameters from the Jann + Heikkila calibration toolbox? + + +- config.cal.NTUPLES = 3; % currently, support for [2-5] implemented + + How many cameras are to be used for on sample of the reconstruction? + It turned out that "3" is optimal for most of the cases. "2" is + faster however, sometimes less robust. "4-5" more robust but slower. + + +- config.cal.MIN_PTS_VAL = 30; + + Used in the MultiCamera validation. How many points must be + simultaneously visible in config.cal.NTUPLES cameras to do the + reconstruction step? In fact, not an important value. It might be + useful if more points are required. This value must not be higher + than the total number of frames acquired for the particular + experiment. In practice, it should be below 1/2 if the theoretical + maximal value. + + +- config.cal.cams2use = [1:10,13:16]; + + Which cameras are to be used in the particular + experiments. Sometimes it is useful not to use all cameras specified + in config.files.idxcams. If not set, all cameras will be used. + + +- config.cal.nonlinpar = [70,0,1,0,0,0]; + + Default initial settings for the estimation of the nonlinear distortion + (1) ... camera view angle + (2) ... estimate principal point? + (3:4) ... estimate parameters of the radial distortion? + (5:6) ... estimate parameters of the tangential distortion? + + It is better to start with the default settings and leave the other + parameters to be estimated during the global optimization + + +- config.cal.NL_UPDATE = [1,1,1,1,1,1]; + + Which nonlinear parameteres would you like to update during the + global optimization. If you have noisy data with many outliers you + may want to stabilize the optimization by fixing some + parameteres. It also depends on what parameters are you actually + using for undoing distortions. + + +- config.cal.DO_GLOBAL_ITER = 1; + + Would you like to perform global optimization? If you already have + good parameters of the non-linear distortion you may want to disable + this. + + +- config.cal.GLOBAL_ITER_THR = 0.3; + + Rather important value. This is one of the stopping condition for + the global optimization. The process ends if the maximum of the + reprojection error (average in each of the cameras) is lower than + this threshold. Do not be too optimistic. The precision of the + complete camera model can be hardly better than the precision of the + finding points. + + +- config.cal.GLOBAL_ITER_MAX = 10; + + If the threshold above is set too optimistic, the optimization may + start to oscilate without actually reaching the desired + precision. The 10 iteration should be really enough for all + cases. If not than the data are simply worse than you think. + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Strategy +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +Having the images on a local disk speeds up the finiding procedeure +significantly. + + +You have to proceed the whole optimization process if you calibrate +the system for the first time or if you change the camera +linses. More often, however only the camera positions or orientations +change. You may then re-use the distortion parameters to speed up the +optimization process. Be sure you have the *.rad files in the working +directory and run the process with "conig.cal.UNDO_RADIAL=1" and +"config.cal.nonlinpar = [70,1,1,1,1,1]" config variables. + +Set "config.cal.DO_BA=1" if you have enough time and really insist on +the highest possible precision. Actually, this is mosty not need at +all. It typically improves the final numbers but not that much the +real camera models. + +Setting "config.cal.START_BA = 1" might help resolve problems with +really bad data (many outliers, bad sychronization etc.) The +projective Bundle Adjustment is then performed in each step. It is +really needed in only very special cases. It serves more as last +rescue if everything else fails. Do not expect too much :-) diff --git a/MultiCamSelfCal/Ransac/FDs.m b/MultiCamSelfCal/Ransac/FDs.m new file mode 100644 index 0000000..27297c2 --- /dev/null +++ b/MultiCamSelfCal/Ransac/FDs.m @@ -0,0 +1,11 @@ +%FDs error trem for fundamentel m. - squares of Sampson's distances +%function Ds = FDs(F,u) +%where u are corespondences and F is fundamental matrix + +function Ds = FDs(F,u) +rx1 = (F(:,1)' * u(1:3,:)).^2; +ry1 = (F(:,2)' * u(1:3,:)).^2; +rx2 = (F(1,:) * u(4:6,:)).^2; +ry2 = (F(2,:) * u(4:6,:)).^2; +r = Fr(F,u); +Ds = r ./ (rx1 + ry1 + rx2 + ry2); diff --git a/MultiCamSelfCal/Ransac/fFDs.c b/MultiCamSelfCal/Ransac/fFDs.c new file mode 100644 index 0000000..90dd385 --- /dev/null +++ b/MultiCamSelfCal/Ransac/fFDs.c @@ -0,0 +1,45 @@ +#include +#include "mex.h" +#include "matrix.h" + +#define f1 (*F) +#define f2 (*(F+1)) +#define f3 (*(F+2)) +#define f4 (*(F+3)) +#define f5 (*(F+4)) +#define f6 (*(F+5)) +#define f7 (*(F+6)) +#define f8 (*(F+7)) +#define f9 (*(F+8)) + +#define u1 (*(u)) +#define u2 (*(u+1)) +#define u4 (*(u+3)) +#define u5 (*(u+4)) + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *F = mxGetPr(aSrcP[0]); + double *u = mxGetPr(aSrcP[1]); + double *p; + double rx, ry, rwc, ryc, rxc, r; + int len = mxGetN(aSrcP[1]); + int i; + + aDstP[0] = mxCreateDoubleMatrix(1, len, mxREAL); + p = (double *)mxGetData(aDstP[0]); + + for (i=1; i<=len; i++) + { + rxc = f1 * u4 + f4 * u5 + f7; + ryc = f2 * u4 + f5 * u5 + f8; + rwc = f3 * u4 + f6 * u5 + f9; + r =(u1 * rxc + u2 * ryc + rwc); + rx = f1 * u1 + f2 * u2 + f3; + ry = f4 * u1 + f5 * u2 + f6; + + *p = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); + p ++; + u += 6; + } +} diff --git a/MultiCamSelfCal/Ransac/fFDs.mexglx b/MultiCamSelfCal/Ransac/fFDs.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..16e162eabb53a0c82bec156d4fdbd32e09ded9e1 GIT binary patch literal 6228 zcmeHLeQaA-6~DIAE|AcqB^%ksI(K6%1N3l07zIX^FFTFoe2^xkQ$t@J`z3Z|`$hIk zl@8E?9v=-MBVrY*#58p(NEJdN(k3KU1WB8%&G=CML29I=R0t#o(8R=q0t%Sl@4k1P z7bla(AM77E*73Q&bI$#~=idA7)8S~u=kp0g0TB>HlY50|hOB*$V`&yaSQ^CrqF&mS zKjo=+IlPCUR!IA4=uY43@KF?a8jK>P-49{GZoy720F!C20PhDs3?>}{zaRV{_#ik4 z_7bv7y5FOG0O^N3_?<}B?^4NqyXeYQ7hHui>O!P3PNaa~C<*agKnh?d@eK|TC?H2l zh~*NNqc18Q0lOS+aiWCplKC!{aHYh{C5#CHZItl4CFVLH1w@sUA;%54TH5bp13jmlpCCx46pMo9&j(cu?dx<2=JqVxD3t`P1WKXHrY`1)bL z@>wCA)w4CnK3IMPJLe<>&OeyK%HAWETwQG{g#}(AegjNmJ>P~@b2#+-g9wO#_^q@G zx%V-`5?CCRX;)sv`X#;qxfi23zCMe77A_)Ph@|TnQb8zFEgH# zOs?N4f-bXXNt)M}CFY1Fsa&6zm^)sQf9rD+bEK0&;!6@UnQS1QmG}e19NP6sU`R=2 z?Y)C2pSBOf^woLGUtC6Gr~WqUp)&tD_}f=E|Ahj_6ZX|+mT77B;+xauRK7<(Ic^uDk|bx^)xINWqW) z3%CEiR&UQ&+BZUNY|}Go%)yLNIC)CP;J@@iv{20al-oAvp)@p?adDWo1$;4dm?JJ5 zIj*%^!}m0NW?uXYeC(_IFIv^pdyC8VtDE+b;`o~ zf8%|CM>6jTJfU9)^L$}o@`k|k5g=a(A3AXmtbvb!d%&l_Sujc6BOHYP z&i3|C1sghh1|JJHX`lSK)*NgKg&G?}p~r&_9ma4fmJN>Oa2}?OSl)Q-f6kv9HHm8~ z)To6TA>Rp|(DLUpR%{s3$~iKrQps38DYV47EL z?j*+2me5k!6#gw^0{^%OrC>4>yrgK(zmarqLbAG8CKVT2+%j`{U`LFa$eAC`LnHs< zW+r1~q05?SJHCi#|&A!N)lRt|v~pNuQUEI97od+|`@#3Dv?_O0G zRI+W9&w!O5zlp}+r|P0zr7t4QF=2V~`xt%&7eh!tj^7JlRUW^ghA_C)D^lfMfkOH5 z+bZ!Eemr-SAJ339{CHJS#PY~5f$2x)x7ZawtTNL(aFj!hY=7 zL1luhz_&l=dCueiVPJmSk$iaT_$Z`$yYOMB_zZB;WA6f1`B}aWtlma^qDJI^54#JT zcD@}jZWuE@akqSH==0d0h5oz;KM%}rOcLAkLrC?u;=|1^NP|nNKX*zSMwN_Lg<1bH zd`~kNK+Mnj>KF_l+F9RL`}c`8V0F)B|NpqnUV}26y%qvgB{rOd{8#Pa+lWI|qV$0X zg|g3k+FOkR*Me@?I`F(Fe*&1)0OmO{<>E(?ehB;n7pu3W1sB7F(S4)%ii;n1{R_bCFQ=4m zMSpeeyi2hBDt8Z3_z__5+ft)zS8q$*E=E=n9psyNyKwj z-Wnen(c)qZuQs~gG0@wuN4t&<;310H(uRe6&3uJ2_0hCB982r+j-|)OC!h@<)g%3_ z-C?~g+}YKGOuVk?iSbP4oG^#a7;#H$Y!uG3m80d|&UF|ub8$nrOkLjUbiLhExwk@j zw{t6$KH;7Y`A{ZXtANuE2g#?YIN!9~J19D+Zlij~lRd57UG3~s-qe$^Y$AdTp6ZNjV%K-ImKNs?t@P}*>uX(0 zi+VB(xsP-^^6_2mEnWb(<=%R{b6@>-lqB^XxJ2)?c-{=RKR#|%!8g6KcglYQBq~hM literal 0 HcmV?d00001 diff --git a/MultiCamSelfCal/Ransac/fslcm.c b/MultiCamSelfCal/Ransac/fslcm.c new file mode 100644 index 0000000..80bcb1f --- /dev/null +++ b/MultiCamSelfCal/Ransac/fslcm.c @@ -0,0 +1,68 @@ +#include +#include "mex.h" +#include "matrix.h" + +#define a11 (*A) +#define a12 (*(A+1)) +#define a13 (*(A+2)) +#define a21 (*(A+3)) +#define a22 (*(A+4)) +#define a23 (*(A+5)) +#define a31 (*(A+6)) +#define a32 (*(A+7)) +#define a33 (*(A+8)) + +#define b11 (*B) +#define b12 (*(B+1)) +#define b13 (*(B+2)) +#define b21 (*(B+3)) +#define b22 (*(B+4)) +#define b23 (*(B+5)) +#define b31 (*(B+6)) +#define b32 (*(B+7)) +#define b33 (*(B+8)) + + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *A = mxGetPr(aSrcP[0]), *B = mxGetPr(aSrcP[1]); + double *p; + int i; + + aDstP[0] = mxCreateDoubleMatrix(4, 1, mxREAL); + p = mxGetPr(aDstP[0]); + + *p = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - + b11*b23*b32 - b12*b21*b33 + b11*b22*b33; + + *(p+1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - + a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + + a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + + 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - + a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - + 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + + (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - + 3*b11*b22)*b33; + + *(p+2) = -(a21*a33*b12) + a21*a32*b13 + + a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - + 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - + 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - + a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + + 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + + 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - + 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - + 3*b11*b23*b32 + a23* + (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - + a11*b32 + 2*b11*b32) + + (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - + 2*a11*b22 + 3*b11*b22)*b33 + + a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + + a11*b33 - 2*b11*b33); + + for (i=0; i < 9; i++) + B[i] = A[i] - B[i]; + + *(p+3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - + b11*b23*b32 - b12*b21*b33 + b11*b22*b33; + } diff --git a/MultiCamSelfCal/Ransac/fslcm.mexglx b/MultiCamSelfCal/Ransac/fslcm.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..b600a4b8f7345d7193fb014ce29da75d6512691c GIT binary patch literal 6744 zcmeHLdu*Fm6~B(NHqbWpM!RKOw|Nhkr&Y!&p#-!IC#hpHP^I+`gb*q!5|yY_qyZ{zCDJxY6ILKpXe5$tRntID+x3A8il%`1{qA?K z^EK_H0TM!j6P@qebMCq4o_p@YC+F*-)@GN>B{aB&Tafdt5TXvWZ>`-^C%owKi3dfE z?AQ9FCsx^b7a5O`{ih&1^`MPG80-PUr0lN%nL+;y`iVvGMD|w#?*={!Bs>Cy3T74J zA>hM6C)}@?2OPo;XnP&}e`E8Ya>|Z2;*9tZ+=9^&!g5Oj-fnDVU;u6zAt4Tyc^P$a zzRb%giw(9I`YYwQ9c8{s^4}=)ACdggGXGJ@bN>l$fsF#}wF&sul0Q`D*GQgwO>m29 zNza#g`v7sD32q(`z;SL8z%6%Mh_Nz%kCac9`Hx9{vCOYq#;=#W{n8CPy$0hdu_k~!u&gI75nWK z3h^rB9Iv((Ep;vpV<3@A8W}5+whTiUMszr0^v9BscZ>B72xB4^jXu^+i1My+e7U~h<)|SR5W0Swu|EY32pscmDcZIgLH?+!C z{xct*=n9EE`#gp&^;bXjCpUMSTYp5EWGpMtf8>io*e79W|6J(#KKdE|05J1N42SzJ z@e3vM#9}zCm0|)sf%Cius`qQhyAR`VyTxy1ul;uMwv6HIl1?dl1rCufg08@!xK4_` zo1{2A3#48leh<1557#Uy^m3%oyGV-1>|IjmcklaS_R6|#MVJ)ZUprfWA52@c|9Pyo zxE~?eK7AH^=}lhp(qYa`C>`cq9AtC;VhM1VD;Z?>{EXxop+O4g$0W~JbdaCFesj#KD9yl;eSbaRfMvda1^U^Oi+_i)p}y?NjWlWbv*R~M zvQMB{3KU<*MA>oA<@-l}&d!nS0+dHPEBC#@#)v4**rIH`71aQ z_z8T^WjO`U!pN74(`QF_FZcTeHXXbvDYp=<**B4IbS14(H;gXUD3-fc+a##@k zHTiImDpeQ(E8%)F9x&2MHT0kq4k)QtT7lGO8^I_a`+dp;=FW$`N;y!toDY{>EL_QF zXRpHR?gdOh6)%0A$cL94pUj8Qxx`ugd-;{@ z$MUHRE7fK8+}wF^7*bWZEcLMl8YrA*K)i)B{LJUt8I7JQOqA;g(g_3u&3ZV?6?$Y7 zgKU4PQd77jgExDcb1{I0aT!2cAcF|UF*3j@uI5+Ho@Zkc^UA=!hrrH`sdZ64E>(Sn zNvOg%U*W2)>MKmpa~bU`uxJnR+VN}lhujly=Qq6Fn0>$MYd0`J2MTll zi#MjHX4E{s{Uu}i?Bz9%*X#iAefer1Ln1no6?kv(l;Dk_qI?%TOV@t{I-_r1=c9I# z-;%X@WJPHvJGq#Bt~7LW`%AaKB|R8IgYx}b_Rc(mz5~S{y)UQaD`i7fu&tr1wV~0w zwRV#?8L?u!O>{8l8s`%ZIHK|~zKQ%zBi*m)K}Gr7$X^WRe$3kTVhn##*bkA+Ihpn` z7iAV$0MaafK$wLxCu4TXH<`aI@D#vX3oicC3gYizhF| zTosX$JYtq&Zi^MC=*x!%hMKyX>im+jGw|aA@EzcvffXv3)j2A4B!ruqKIip?+q<9g zZt{Qj)BZZ|ra+*!HW1kC^=&bGW09nHR~ork+>B(*r-b+))93yXc$NaS{y;70e?lhw znLPot`a?9ct@GsQHOUHCsxX+$NwjZywe)E zuLSO^zK^_%#74+CjzCI$4yf-j?=-OuoC$H5brrJcrV z*5m)f#9h$Uz!(!=0ct(IsYLZG%YHrYUZqFB4?&0MtXt^2^)02xc*zX1N#97n z4dlGk|p{M79sNDVz+KdVHWX%P7Ma6?iJ3VL-kpg_m@r{Fm+k^ko}@<+-6loL(>=?~YT^?2Sj=(Yb60zHO(wC8mQ zbaM)Qc6EeW*fW$ahV7>OYCZn8aZU{fq4yijfNDKb?yZLJ;YaWQoZ=n_f%K32$g}LU zw-Jlj=oqN&LG3o+n?U$~-H5w_vxoCSaLdfY&QWas->(}NpEGs#9EOCKc(!m|Eu76Z+K^1V!e1B^rEBx4e&MULQtRiB5NK5`g0Ui*X>-$_`K{; zpw|%7v1{dyR@&LLeD)#7RJx{g|bTU`sD@g-3I zS@7Dw3S{0*%le-Ouj`!De;NEb)P?kT6Fk8OWL~{r@lT@t81To6M-$P!Q97ph$5sCY z@XoqrUiEXywD%YA&bp;by+N<4@eeuPBrjxQAR66dAiLQWi<^CZ5sk#-j!LG8rmc)M z)Zg!qie0D>8pf8cj;%&(%eF2QIXNtDS{PTy3TMg~h^KlZaYN=%Mr3Fh($E)-=B*8F zA)_%AZfVCLrej9mP$IEMqH?*}haZ#C+F&Ihq#ZkkRNu^;R zAGFK7g7w{)FihLKaBD|nL#xrz+`KK+Wpp((wuVp#Hj{mKtlb8KcEz?_W4&W3I2hFR l*?_Fpen1`BsphB&tJ?8xQ$eL%U%s_ay<~{>;AJ)Ke*im*;D7)C literal 0 HcmV?d00001 diff --git a/MultiCamSelfCal/Ransac/fu2F7.m b/MultiCamSelfCal/Ransac/fu2F7.m new file mode 100644 index 0000000..0eed2cc --- /dev/null +++ b/MultiCamSelfCal/Ransac/fu2F7.m @@ -0,0 +1,28 @@ +function Fs = fu2F7(u) + +Z = lin_fm(u); + +NullSp = null(Z); +if size(NullSp,2) > 2 + Fs = []; + return; %degenerated sample +end + +F1 = reshape(NullSp(:,1),3,3); +F2 = reshape(NullSp(:,2),3,3); +p = fslcm(F1,F2); +aroots = rroots(p); + +%xr = o_fslcm(F1,F2) + +%aroots == xr + +for i = 1:length(aroots) + l = aroots(i); + Ft = F1 * l + F2 * (1-l); +% Ft = Ft /norm(Ft,2); + Fs(:,:,i) = Ft; +end + + + diff --git a/MultiCamSelfCal/Ransac/lin_fm.c b/MultiCamSelfCal/Ransac/lin_fm.c new file mode 100644 index 0000000..f5e2878 --- /dev/null +++ b/MultiCamSelfCal/Ransac/lin_fm.c @@ -0,0 +1,32 @@ +#include +#include "mex.h" +#include "matrix.h" + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *u = mxGetPr(aSrcP[0]); + double *p, *s; + int len = mxGetN(aSrcP[0]); + int i,k,l,pos; + + aDstP[0] = mxCreateDoubleMatrix(len, 9, mxREAL ); + p = (double *)mxGetData(aDstP[0]); + + s = u; + for (i = 0; i < len; i++) + { + pos = 0; + for (k = 0; k < 3; k++) + { + for (l = 0; l < 3; l++) + { + *(p+pos) = *(s+k+3) * (*(s+l)); + pos += len; + } + } + s += 6; + p++; + } +} + + diff --git a/MultiCamSelfCal/Ransac/lin_fm.mexglx b/MultiCamSelfCal/Ransac/lin_fm.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..1bddd4469b34110fac6df59890e2e1c97ce85cb7 GIT binary patch literal 6102 zcmeHLZ){sv6~DH#wxd~_m9BJK+1zf>3?^e|p>znU{BzQXBn@dYHd^}XI4|+5Y`@BW zuH3=4q9@cvUQo2{Lj@Bms*w6&5*3?}pdYBzh_nKs2u)%iS`<(xoUKTm6iS!^=J&hr z-j^4rlg0=31&(!m?(dv)|KD@(efNcAUw0@J5}G0+B8X0{6QUJzy3w<=iYP42;z6-p z+O7eSMd_Ic>$w|l%F1s(;XNNEp4EW*ABJGlx>ro91tFZdBKX(xCG_+juv z;3zmq_bc#VK>2B;cLngrkwiDv&;x5UYZb35l{e}_WHC;ph~Ov*@mfR*U@!3!2M83A zBPGOJK9-{|8a)BK9Bpyf$LnRj*L;i#0KMhoyCq)s@h2qaIv_>FMkz0N*?_%;@mn9? zBki1XQbcT$@|KS`OU(HuMYvEv*VF_?nJ|!|r(+tVdzohVPNUoQZ!XuEh&nirDuXw-4HAv#xr2lDf8~V>CZo&9G14-gY zG5>P}AdUNxK8tBM14(~x1cdlDFw0X@kCa;HhB1+|^G4B06&%A5hLJ9(jBzWU%39Bw zfMaICoUn?HSun;6shnw8`EeWiJ85%fxx184JC>anxpI%`3>G{&0MS`6Q;ykXmqxQ@ zf66IXWzW`?a#8|0pE3(YX3ScnxpJ&%%Y^Btpo#6FF0E5}V>}l_#))iM^e3MdJ$=0$ zokmM65qrW<$5nB?1H;Lof%ZPx@BieFF~B%!05d+q!dtu{j3j^Of3|l6R|)rzU#NmP z#$npNF4PKIndcBJKZTw15(gI_wh$C|iQgz)lVxF%H;6v~lUT>gka`{u-*^Z?5fOiq zR&VcPc-+96r@X1;cC2CIt&r=mcy>^tkx|N>LOe=|1+fI%dgNKA1PkcbgBvK}zkw2) zVk0FMXA>pzk4(Pf-rRbyiX7t5cP}?$4658`ss?&rJr;)c>Sf^C8{9l<=hzlf?Hpq| z!{owz4Rnq@%h0?qCoxAeL*>FHiP1Qb;orgqi8<1lDDhc|nang3PfPqLF^6{H6fk6c zTaO~jEACzxzIVw9*XGdB$=lNbs_R=E{_e%)f1$9_n0v96MLIEeX0hTvj$|!fJ&lUo znWp#dpZYnC6?X~FQ-ckYuP{*&wK-3PN=tusFS2pM*^CmV+y-PFtZu{qtjj7edZO~J z>eb6rC+gdM9`P>QTGzg5=-rDP0pV&O0eY4MBSO?W8&D|gVo(st%rHAnq_jfl3bomF0w`M zeQxr0;&$R{;`i6Pu57>7ICp)=-y@}^4)=rb^M7r3{}F!v9rse)o!wANE>=dCo*Y^H z&fJvw_MsyN8a(ypXITc??KkZl4(^I8FkTo6j1i|z_N!^7`7gj{>Wy2Rl#BeYIgOQd zwK@0K%kI8fY4OmJwPVqTEG$sYU)?yxVVkc0<)5<3{`TR%_KxUKq9vM7Io1gi?c;e= zW$b|TS;cvL8BF;@C4ZtM1(>#wcgQ5-06OF;OT&vkU$I%O$c`;2k&vA^7#26===l%zfY|BR_ z4ljMR)MqbsThNJJ;1+NPcnCZSE`Uk;9QP3ZdpbM69Bu9y7`lNyC|3ZBg9R3=r- zh}hW4JX}2K6hv$yUy7Yj=XnqqW2?j??A9TBtg7XBTxjDK82EST&uUJ_#7 zznSrFBC@(v&Pt0|+OZ2oU{6fg$XOgMLL>jub}nb;q08Hj$pQR`#QQ!%M8+Cpll->A z5He;ND?7oAPsSBvmK+CQtYT}BevDnLSgE5QW1P&`*2H-wab9&?GA7C0&@ms0lDrSB ztPC5b1iq!_eU}BJ*+&A-5?EAGr`)Adk&fPvV87W zte1XyF#E)C=Uzwt4APo(`z$E5-!gp2r?e9!{m2ej`|+EB%yHJlF(y3=)_&Yu$-Fga z()FH#f_^N5`!<>TrY71*FM^rocyZ5~ho7zsD%m#5?}N1;ze#SvPuE4ePM<-VW5V*} z_bL3U3PVUgsKk%Kx;%cvT*IJKuSu789t!QpZ<`w*;>UAG`|%992|r#HG_gGLn_&8p z`7PA!eWn3y1L3Z1+H8$Mlp+K-?r`)H*Mo^h;G z(H zep|~5)L%sItSg#`{6Y@m2jn{79^sMqEv#`EJ9bo?ARF-Q&3V2M@c%q8zpF?gyfu6Q zQokL9uv2^$xEiqc0_*%N-vHKc6Ctr#6oB`t1x`D^-5ED5A|&pU-}-|A`zz4D7{EUQ z=Jy?m?RgziziotY^NZ4;NcZQczhP{Y@v1TFpM&o__5l*}v%XiAKkck=jUw-%MhZzaIo zk4d5MCh<71erpONs#}0LzS0i=eZV{mNMYQ-yXx#;1J-XrEI$R@7RWyiOlk)6oTw=pJC_xP38VXdZC2q&l>fWH>@TN)Z#92YcHSje{yl0B(s(y8$M1~te?r;y z+s*-nVHQ#TO{CK;25vvct*ki~6X{eo8+h#z>4H;qO5@|Pv^b786T|2lJ}_kT^&T9? zLlCoNO$Yf}`3htk6IpvSl{MrY%Se^V&?fgA-9zpDNuwj#(>s7nysjBzrCjc$ut$%X zX(yIQ2=Cd*)ADYo9LDWJ+B6*7kheO+=nPaItWe(VRE5$fInX5^s$^?5@Y>;_tRy5z0XFmPe?-ogYQ!7O2zUh{hT4s^8l z83($%45lJZZEgC=Dz2W%*5u>6)`z^ot;)Uf ge5T&r)|4dmUARWCwFF-6RzLr(RKYj3x|hO#15eZ%x&QzG literal 0 HcmV?d00001 diff --git a/MultiCamSelfCal/Ransac/mfFDs.m b/MultiCamSelfCal/Ransac/mfFDs.m new file mode 100644 index 0000000..a6c5711 --- /dev/null +++ b/MultiCamSelfCal/Ransac/mfFDs.m @@ -0,0 +1,28 @@ +% mfFDs first order geometric error (Sampson distance) +% m-version of the original c-code (mex) which were +% causing some problems + +% $Author: svoboda $ +% $Revision: 2.2 $ +% $Id: mfFDs.m,v 2.2 2004/05/04 16:09:35 svoboda Exp $ +% $State: Exp $ + +function err = mfFDs(F,u); + +% disp('m-version of fFDs') + +Fu1 = F*u(4:6,:); +Fu2 = (F'*u(1:3,:)).^2; +Fu1pow = Fu1.^2; + +denom = Fu1pow(1,:)+Fu1pow(2,:)+Fu2(1,:)+Fu2(2,:); + +errvec = zeros(1,size(u,2)); +for i=1:size(u,2), + xFx = u(1:3,i)'*Fu1(:,i); + errvec(i) = xFx^2/denom(i); +end + +err = errvec; +return + diff --git a/MultiCamSelfCal/Ransac/normu.m b/MultiCamSelfCal/Ransac/normu.m new file mode 100644 index 0000000..914622b --- /dev/null +++ b/MultiCamSelfCal/Ransac/normu.m @@ -0,0 +1,19 @@ +function A = normu(u); + +% NORMU Normalizes image points to be used for LS estimation. +% A = NORMU(u) finds normalization matrix A so that mean(A*u)=0 and mean(||A*u||)=sqrt(2). +% (see Hartley: In Defence of 8-point Algorithm, ICCV`95). +% Parameters: +% u ... Size (2,N) or (3,N). Points to normalize. +% A ... Size (3,3). Normalization matrix. + +if size(u,1)==3, u = p2e(u); end + +m = mean(u')'; % <=> mean (u,2) +u = u - m*ones(1,size(u,2)); +distu = sqrt(sum(u.*u)); +r = mean(distu)/sqrt(2); +A = diag([1/r 1/r 1]); +A(1:2,3) = -m/r; + +return diff --git a/MultiCamSelfCal/Ransac/nsamples.m b/MultiCamSelfCal/Ransac/nsamples.m new file mode 100644 index 0000000..2b46243 --- /dev/null +++ b/MultiCamSelfCal/Ransac/nsamples.m @@ -0,0 +1,14 @@ +%SampleCnt calculates number of samples needed to be done + +function SampleCnt = nsamples(ni, ptNum, pf, conf) +q = prod ([(ni-pf+1) : ni] ./ [(ptNum-pf+1) : ptNum]); + +if (1 -q) < eps + SampleCnt = 1; +else + SampleCnt = log(1 - conf) / log(1 - q); +end + +if SampleCnt < 1 + SampleCnt = 1; +end diff --git a/MultiCamSelfCal/Ransac/p2e.m b/MultiCamSelfCal/Ransac/p2e.m new file mode 100644 index 0000000..440a7b8 --- /dev/null +++ b/MultiCamSelfCal/Ransac/p2e.m @@ -0,0 +1,3 @@ +function e = p2e (u) +e = u(1:2,:) ./ ([1;1] * u(3,:)); +return \ No newline at end of file diff --git a/MultiCamSelfCal/Ransac/rEG.m b/MultiCamSelfCal/Ransac/rEG.m new file mode 100644 index 0000000..67c2351 --- /dev/null +++ b/MultiCamSelfCal/Ransac/rEG.m @@ -0,0 +1,85 @@ +function [F, inls] = rEG(u, th, th7, conf) + +%rEG Robust computation of epipolar geometry based on RANSAC +% +% [F, inls] = rEG(u, th, th7, conf) +% u point correspondences (6xn), where n is the number of corrs. +% th threshold value for the Sampson's distance (see FDs) +% th7 threshold for inliers to iterate on F (default = th) +% conf confidence level of self-termination (default = .95) + +MAX_SAM = 100000; +iter_amount = .5; + +if nargin < 3 + th7 = th; +end; + +if nargin < 4 + conf = .95; +end; + +len = size(u,2); +ptr = 1:len; +max_i = 8; +max_sam = MAX_SAM; + +no_sam = 0; +no_mod = 0; + +while no_sam < max_sam + for pos = 1:7 + idx = pos + ceil(rand * (len-pos)); + ptr([pos, idx]) = ptr([idx, pos]); + end; + + no_sam = no_sam +1; + + aFs = fu2F7(u(:,ptr(1:7))); + + for i = 1:size(aFs,3) + no_mod = no_mod +1; + aF = aFs(:,:,i); + Ds = mfFDs(aF,u); + % Ds = fFDs(aF,u); + v = Ds < th; + v7 = Ds < th7; + no_i = sum(v); + + if max_i < no_i + inls = v; + F = aF; + max_i = no_i; + max_sam = min([max_sam,nsamples(max_i, len, 7, conf)]); + end + + if sum(v7) >= 8 + iter_amount*(max_i - 8) + aF = u2F(u(:,v7)); + Ds = mfFDs(aF,u); + % Ds = fFDs(aF,u); + v = Ds < th; + no_i = sum(v); + if max_i < no_i + inls = v; + F = aF; + max_i = no_i; + exp_sam = nsamples(max_i, len, 7, .95); + max_sam = min([max_sam,exp_sam]); + end + end + end +end + +if no_sam == MAX_SAM + warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); +end; + + + + + + + + + + diff --git a/MultiCamSelfCal/Ransac/rroots.c b/MultiCamSelfCal/Ransac/rroots.c new file mode 100644 index 0000000..0829b29 --- /dev/null +++ b/MultiCamSelfCal/Ransac/rroots.c @@ -0,0 +1,71 @@ +#include +#define rr_a (*po) +#define rr_d (*(po + 3)) +#define eps (2.2204e-016) + +#include "mex.h" +#include "matrix.h" + + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + + double *po = mxGetPr(aSrcP[0]); + + double b,c, b2, bt, v, pit, e, *r; + double p, q, D, A, cosphi, phit, R, _2R; + + b = *(po + 1) / rr_a; + c = *(po + 2) / rr_a; + b2 = b*b; + bt = b/3; + + p = (3*c - b2)/ 9; + q = ((2 * b2 * b)/27 - b*c/3 + rr_d/rr_a) / 2; + + D = q*q + p*p*p; + + if (D > 0) + { + aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); + r = mxGetPr(aDstP[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; + } + } else + { + /* if (p > -eps) + { + printf("%.17f\n", p); + aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); + r = mxGetPr(aDstP[0]); + *r = pow(q,1.0/3) - bt; + } + else */ + { + + aDstP[0] = mxCreateDoubleMatrix( 3, 1, mxREAL ); + r = mxGetPr(aDstP[0]); + 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; + } + } +} diff --git a/MultiCamSelfCal/Ransac/rroots.mexglx b/MultiCamSelfCal/Ransac/rroots.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..4ab396672979242d42960d169b422661ef33bc76 GIT binary patch literal 6903 zcmeHLZ){uD6~B%{n$gmx5THN@1_~6$Jg3VT14ffJZt0e^S!odn!mH!F#O@rwU_W=^ z&qOwZpvtmUf)CR^Y;0^&HB}l?&{Pp5!Wz+}i;yREl8yXA#+c;L4kc2BsxHI({qDQ> z^(7RI?E_!7W1V;I@1Ar2-}Bz_c{1r^{J=%cM zjUM{%3)d}cdIRwFs_t5!?u)+i=UH6u8*H5_U*)U2Q1L}o>7J$#6WD)JP;mE!XyitN zc;2KMd3eOGqTR!8;TaFhQxs=BjEe$lcB|38UgD=b`~``B?cs|h=KUfC1ug<;pDTb@ zNj&7?OC;u9A_c{2Da#&qFDUO3DaZ>5npB$>6l*0ec=%F@iyppA;`QoqgQ9U6zFcDc z2#PCoQOcmWQVgk@n<3eIOyP|Re_i2QAbGc*RJe#Uq5Xuy6l=voRo_S6ZQ4Hrp2xQF z%rC`$Jp@VO7P0*S0?-p+;tuctj^QyJHSOIUamQtUPlHLE2kq6>V3i2*VHkb=R@%tg zv5ajPfbqeY(VI-iQpqPwz#cPW_9e5nnK61Zv3}D?rh6^aU#Pa<9Nd;m$L*w*7X5?U zO}jG#xiw?PY_rYEb*IdZn4L)uitJ+++qvBTjOZa3 z;hxptS%Wf5b$D_g-CzB4zg^8+#4GlD)j9XL2KvL_f$yn0eeMBhj-Za`6ai+2!w@Um^71UFLR;;f_GAjJDv*b2OeDwMdCrzv4SPl>+HP{Q^sB`)(KWe6RkZUy?O zrNrgld+=}h(~b96pb-!M;8-U-tMWR^mFKWql}s&ar;Y(vM^O~jmpM05^<};z5-iTm zRzWXwr3tF%3KDaN6ReyYmzZzO1Rry+NQ?=IM2Pq$iCIi+Bt9fD7AX=D;vtEtO5902 zAo1X#lFlbVhgIdlhs_IavUb;(TNsk=JFGv(uc*8?l>2!j!t6|)zL{j z^QY^b^&2XEuy>-f#f6!7P^uLFq=&NCIz_i3Yji?T59M8`|BPS;Vn zkJHuMWaT>AbsUu|#)_bDqVr{QQq4+8^*Gg!)+=RHEC2crwsn;aBj(|UADudO>Jp!~ z9pb(8?%uly4Tkdq2ZO7Qy`iDJw+-j-=NN28M(62tV|-TaRl{}FLjJAA{JquO8IG|0 zu0M|^>`?mWGCxQ`cc`)gi+}~s#-;ieSKgrH-Ac}G-YepH@ewfJq>R-VW$i;7zG+_s zGrVJr$gqvEBcryzgV`-#to+_FhN^+Q4-7Y!k=B)vj0Ley86z^jXaO_Ist04`7Gq1? zSc`4p?iOQ9?5xE;vBwr;(Q}ZT%V#AX4Q;j5@gVD7!+cJF-vNIBJ_o)S@#odx+rW2$ zNjk=K5uV$(ZoMnCar@4_w}v)_zwxzjb7)f}($o}*+!5N?W_BlI>Ciw1u~5p4WzAdv z=j?f&B;KV+Q#jHD`FYfdaQ5+jJJt>75aFK3(=c(Roe|-_bS`{Ag_C|@ z^eoXCFvV%Ixmy@W*&>`wC-Jn+K|B!zC!w->Vs;GO%cq%eqhL8)tUnnS;ka#OvcRtB zv(Pfzokfj&#I62*GmW~mWt-f<&q#1@>6i2y`Y1Vq%_k<&U&z;k>8Grx?~*$K=wA$i zXhR>yjg@t@qpy?cmn2x-UrW`eaH-#z5}P{;oH-eP~WOM|&4sO3A4@4cH) z)}+TxqC(s8e=GSVZ3Ia>vJKXD{69->(49cXJtpygnYQCQmaJaWQm>l~DLc-42sUKy zvnK6Z&!d95@-;yHi+Sq|2&XxQsnB$W9KYcwvRz6}q=`}Ft z!*yso-gixU?Qft$x1o;xg?tsdI0>ElG|)`kgVe*IEV&PD$8R0S)buLs-qrw8+fnjt zHNB3HVgZxFUnhC5IX|8w@3OzYA7K&S^)=M}!R!;ncLM4Ec^{q%_E$MtP-Z&Zn1%I0 z8Iid6#Knyi^4_@k-{*Z~1N8{XH|zPDpv>0z@zwGk=)4!-&^&96Yk))N&2jG+%9V4$C&}DF4CSaXsr2W0X`@Hq75^c-$-v-usO4`SOn{hrQ z;$C3V&0xmaIfXYW{4<4b0lon|rm)UCo=_Mn?Dj(SZG~@C_6xxNyyK$MbIKgAQN5G+ zeIwog?9V$!l>SNRIDWei)BX{KQ4}FQ`r`3T2BM>ZWXkLbi+C)R@?|1LJY#3=TyJkU zE(Va&GmN%9yLKDxJMQ0uBqf`rOdIW*nW40dzLeD+OBpf-Gh(?x)JE?!w(V~1h#Fg> z+js0lBZhNEPp-fJabb1uH{*7=sY$pgO}Cb@pEBsRGI7(eEkj0qhOyN*xPL$y`>6q? zO>}3QOrpx!YT(YpMJjU{XVOzesaT?pSBw!Qp2AZnOWgwO}upS(!BI}E9QHdL_S{=!EBbKwEqP` CO^A#D literal 0 HcmV?d00001 diff --git a/MultiCamSelfCal/Ransac/seig.m b/MultiCamSelfCal/Ransac/seig.m new file mode 100644 index 0000000..6822a19 --- /dev/null +++ b/MultiCamSelfCal/Ransac/seig.m @@ -0,0 +1,5 @@ +%seig sorted eigenvalues +function [V,d] = seig(M) +[V,D] = eig(M); +[d,s] = sort(diag(D)); +V = V(:,s); diff --git a/MultiCamSelfCal/Ransac/u2F.m b/MultiCamSelfCal/Ransac/u2F.m new file mode 100644 index 0000000..f942863 --- /dev/null +++ b/MultiCamSelfCal/Ransac/u2F.m @@ -0,0 +1,42 @@ +%u2F estimates fundamental matrix using ortogonal LS regression +% F = u2F(u) estimates F from u using NORMU +% F = u2F(u,'nonorm') disables normalization +% see also NORMU, U2FA + + +function F = u2F (u, str) + +if (nargin > 1) & strcmp(str, 'nonorm') + donorm = 0; +else + donorm = 1; +end + +ptNum = size(u,2); + +if donorm + A1 = normu(u(1:3,:)); + A2 = normu(u(4:6,:)); + + u1 = A1*u(1:3,:); + u2 = A2*u(4:6,:); +end + +for i = 1:ptNum + Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); +end + +M = Z'*Z; +V = seig(M); +F = reshape(V(:,1),3,3); + +[uu,us,uv] = svd(F); +[y,i] = min (abs(diag(us))); +us(i,i) = 0; +F = uu*us*uv'; + +if donorm + F = A1'*F*A2; +end + +F = F /norm(F,2); diff --git a/MultiCamSelfCal/changes.txt b/MultiCamSelfCal/changes.txt new file mode 100644 index 0000000..9346a28 --- /dev/null +++ b/MultiCamSelfCal/changes.txt @@ -0,0 +1,56 @@ +$Id: changes.txt,v 2.2 2005/05/24 09:15:31 svoboda Exp $ + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +List of the most significant changes since the last public release +from 30th July 2003 + +- The RANSAC validation was rewritten as m-functions. Hence, no + re-compilation of the c-codes is necessary. The user can switch + between the mex- and m- version of the RANSAC by specifying the path + in gocal.m addpath ../RansacM; % ./Ransac for mex functions The + m-version can be slow for highly noisy data. + + The m-version of the RANSAC uses the 8-point algorithm not the + 7-point algorithm as the c-version. It is then less optimal. I was + just too lazy to rewrite the more complicated 7-point algorithm. I + did many experiments, the 8-point algorithm did well in all cases. + +- gocal.m ... code cleaned, output better formatted (new function + dispcamstats) + +- local alignments ... code cleaned, redundant I/O parameters + (possible source of errors) removed + +- getpoint.m ... code cleaned, made a bit faster. Made compatible with + the newest image processing toolbox, ver>5 + +- showpoints.m ... it possible to switch between single images and + image mosaics. The image mosaics may sometimes speed-up the + debugging process when something goes wrong with the point + detection. + +- The standard Matlab function quiver was copied to the BlueCCal + distribution since it has problems in the newest Matlab ver>7. + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +List of changes against to the last public release from 18th July 2003: + +Fixed Bugs in Code: + +- radial distorion ... proper handling of images with resolution + different than 640x480 + + +Functionality: + +- support for non-color images added. You may either specify + config.imgs.LEDcolor='intensity' and the color images will be + converted to a grayscale or the GETPOINT detects that the image is + only grayscale and it will be handled as such + +- main cycle in GOCAL. Some upgrade to make it more robust. I am + testing the upgrades on all datasets I have. It should be backward + compatible. Typically, new bad data discovers a small + functionality-bug in the main iterative cycle. diff --git a/MultiCamSelfCal/gocal.m b/MultiCamSelfCal/gocal.m new file mode 100644 index 0000000..9608616 --- /dev/null +++ b/MultiCamSelfCal/gocal.m @@ -0,0 +1,348 @@ +% The main script. Performes the self calibration +% The point coordinates are expected to be known +% see the directory FindingPoints +% +% $Author: svoboda $ +% $Revision: 2.7 $ +% $Id: gocal.m,v 2.7 2005/05/24 09:15:11 svoboda Exp $ +% $State: Exp $ + +clear all + +% add necessary paths +addpath ../CommonCfgAndIO +addpath ../RadialDistortions +addpath ./CoreFunctions +addpath ./OutputFunctions +addpath ./BlueCLocal +addpath ./LocalAlignments +addpath ../RansacM; % ./Ransac for mex functions (it is significantly faster for noisy data) +% get the configuration +config = configdata(expname); +disp('Multi-Camera Self-Calibration, Tomas Svoboda et al., 07/2003') +disp('************************************************************') +disp(sprintf('Experiment name: %s',expname)) + +%%% +% how many cameras to be filled +% if 0 then only points visible in all cameras will be used for selfcal +% the higher, the more points -> better against Gaussian noise +% however the higher probability of wrong filling +% Then the iterative search for outliers takes accordingly longer +% However, typically no more than 5-7 iterations are needed +% this number should correspond to the total number of the cameras +NUM_CAMS_FILL = config.cal.NUM_CAMS_FILL; +%%% +% tolerance for inliers. The higher uncorrected radial distortion +% the higher value. For BlueC cameras set to 2 for the ViRoom +% plastic cams, set to 4 (see FINDINL) +INL_TOL = config.cal.INL_TOL; +%%% +% Use Bundle Adjustment to refine the final (after removing outliers) results +% It is often not needed at all +DO_BA = config.cal.DO_BA; + +UNDO_RADIAL = config.cal.UNDO_RADIAL; % undo radial distortion, parameters are expected to be available +SAVE_STEPHI = 1; % save calibration parameters in Stephi's Carve/BlueC compatible form +SAVE_PGUHA = 1; % save calib pars in Prithwijit's compatible form +USED_MULTIPROC = 0; % was the multipropcessing used? + % if yes then multiple IdMat.dat and points.dat have to be loaded + % setting to 1 it forces to read the multiprocessor data against the + % monoprocessor see the IM2POINTS, IM2PMULTIPROC.PL + +%%% +% Data structures +% lin.* corrected values which obey linear model +% in.* inliers, detected by a chain application of Ransac + +if findstr(expname,'oscar') + % add a projector idx to the cameras + % they are handled the same + config.files.cams2use = [config.files.idxcams,config.files.idxproj]; +end + +selfcal.par2estimate = config.cal.nonlinpar; +selfcal.iterate = 1; +selfcal.count = 0; + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Main global cycle begins +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +while selfcal.iterate & selfcal.count < config.cal.GLOBAL_ITER_MAX, + % read the input data + loaded = loaddata(config); + linear = loaded; % initalize the linear structure + + CAMS = size(config.cal.cams2use,2); + FRAMES = size(loaded.IdMat,2); + + if CAMS < 3 | FRAMES < 20 + error('gocal: Not enough cameras or images -> Problem in loading data?') + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% correct the required amount of cameras to be filled + if CAMS-NUM_CAMS_FILL < 3 + NUM_CAMS_FILL = CAMS-3; + end + + config.cal.Res= loaded.Res; + config.cal.pp = reshape([loaded.Res./2,zeros(size(loaded.Res(:,1)))]',CAMS*3,1); + config.cal.pp = [loaded.Res./2,zeros(size(loaded.Res(:,1)))]; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % See the README how to compute data + % for undoing of the radial distortion + if UNDO_RADIAL + for i=1:CAMS, + [K,kc] = readradfile(sprintf(config.files.rad,config.cal.cams2use(i))); + xn = undoradial(loaded.Ws(i*3-2:i*3,:),K,[kc,0]); + linear.Ws(i*3-2:i*3,:) = xn; + end + linear.Ws = linear.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES); + elseif config.cal.UNDO_HEIKK, + for i=1:CAMS, + heikkpar = load(sprintf(config.files.heikkrad,config.cal.cams2use(i)),'-ASCII'); + xn = undoheikk(heikkpar(1:4),heikkpar(5:end),loaded.Ws(i*3-2:i*3-1,:)'); + linear.Ws(i*3-2:i*3-1,:) = xn'; + end + linear.Ws = linear.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES); + else + linear.Ws = loaded.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES); + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Detection of outliers + % RANSAC is pairwise applied + disp(' ') + disp(sprintf('********** After %d iteration *******************************************',selfcal.count)) + % disp('****************************************************************************') + disp(sprintf('RANSAC validation step running with tolerance threshold: %2.2f ...',INL_TOL)); + + inliers.IdMat = findinl(linear.Ws,linear.IdMat,INL_TOL); + + addpath ./MartinecPajdla; + setpaths; % set paths for M&P algorithms + + % remove zero-columns or just 1 point columns + % create packed represenatation + % it is still a bit tricky, the number of the minimum number of cameras + % are specified here, may be some automatic method would be useful + packed.idx = find(sum(inliers.IdMat)>=size(inliers.IdMat,1)-NUM_CAMS_FILL); + packed.IdMat = inliers.IdMat(:,packed.idx); + packed.Ws = linear.Ws(:,packed.idx); + + if size(packed.Ws,2)<20 + error(sprintf('Only %d points survived RANSAC validation and packing: probably not enough points for reliable selfcalibration',size(packed.Ws,2))); + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% fill cam(i) structures + for i=1:CAMS, + cam(i).camId = config.cal.cams2use(i); + cam(i).idlin = find(linear.IdMat(i,:)); % loaded structure + cam(i).idin = find(inliers.IdMat(i,:)); % survived initial pairwise validation + cam(i).xdist = loaded.Ws(3*i-2:3*i,cam(i).idlin); % original distorted coordinates + cam(i).xgt = linear.Ws(3*i-2:3*i,cam(i).idlin); + cam(i).xgtin = linear.Ws(3*i-2:3*i,cam(i).idin); + % convert the ground truth coordinates by using the known principal point + cam(i).xgt = cam(i).xgt + repmat(config.cal.pp(i,:)', 1, size(cam(i).xgt,2)); + cam(i).xgtin = cam(i).xgtin + repmat(config.cal.pp(i,:)', 1, size(cam(i).xgtin,2)); + end + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% options for the Martinec-Pajdla filling procedure + options.verbose = 0; + options.no_BA = 1; + options.iter = 5; + options.detection_accuracy = 2; + options.consistent_number = 9; + options.consistent_number_min = 6; + options.samples = 1000; %1000; %10000; + options.sequence = 0; + options.create_nullspace.trial_coef = 10; %20; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% start of the *compute and remove outliers cycle* + outliers = 1; + inliers.idx = packed.idx; + inliers.reprerr = [9e9]; + while outliers + disp(sprintf('%d points/frames have survived validations so far',size(inliers.idx,2))) + disp('Filling of missing points is running ...') + [P,X, u1,u2, info] = fill_mm_bundle(linear.Ws(:,inliers.idx),config.cal.pp(:,1:2)',options); + % + Rmat = P*X; + Lambda = Rmat(3:3:end,:); + % + [Pe,Xe,Ce,Re] = euclidize(Rmat,Lambda,P,X,config); + disp('************************************************************') + % + % compute reprojection errors + cam = reprerror(cam,Pe,Xe,FRAMES,inliers); + % + % detect outliers in cameras + [outliers,inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL); + % + disp(sprintf('Number of detected outliers: %3d',outliers)) + disp('About cameras (Id, 2D reprojection error, #inliers):') + dispcamstats(cam,inliers); + % [[cam(:).camId]',[cam(:).std2Derr]',[cam(:).mean2Derr]', sum(inliers.IdMat')'] + disp('***************************************************************') + % + % do BA after removing very bad outliers or if the process starts to diverge + % and only if required config.cal.START_BA + inliers.reprerr = [inliers.reprerr, mean([cam(:).mean2Derr])]; + if inliers.reprerr(end)<5*INL_TOL | inliers.reprerr(end-1)config.cal.GLOBAL_ITER_THR & config.cal.DO_GLOBAL_ITER & selfcal.count < config.cal.GLOBAL_ITER_MAX + % if the maximal reprojection error is still bigger + % than acceptable estimate radial distortion and + % iterate further + cd ../CalTechCal + selfcalib = goradf(config,selfcal.par2estimate,INL_TOL); + cd ../MultiCamSelfCal + selfcal.iterate = 1; + UNDO_RADIAL = 1; + if ~selfcalib.goradproblem; + % if all non-linear parameters estimated reliable + % we can reduce the tolerance threshold + INL_TOL = max([(2/3)*INL_TOL,config.cal.GLOBAL_ITER_THR]); + % add the second radial distortion parameter + if config.cal.NL_UPDATE(4), selfcal.par2estimate(4) = 1; end + % estimate also the principal point + if selfcal.count > 1 & config.cal.NL_UPDATE(2), selfcal.par2estimate(2) = 1; end + % estimate also the tangential distortion + if selfcal.count > 3 & all(config.cal.NL_UPDATE(5:6)), selfcal.par2estimate(5:6) = 1; end + else + INL_TOL = min([3/2*INL_TOL,config.cal.INL_TOL]); + end + else + % ends the iteration + % the last computed parameters will be taken as valid + selfcal.iterate = 0; + end +end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% End of the main global cycle +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + \ No newline at end of file diff --git a/MultiCamSelfCal/showpoints.m b/MultiCamSelfCal/showpoints.m new file mode 100644 index 0000000..893c6ea --- /dev/null +++ b/MultiCamSelfCal/showpoints.m @@ -0,0 +1,111 @@ +% script for demonstrating the localized points +% it load the input images and the matrix with found points +% and it denotes the position of the detected LED +% and it saves the augmented images on the disk if required + +% $Author: svoboda $ +% $Revision: 2.2 $ +% $Id: showpoints.m,v 2.2 2005/05/20 12:54:51 svoboda Exp $ +% $State: Exp $ + +clear all; +addpath ../CommonCfgAndIO + +SAVE_IMG = 1; % Do you want to save images? +COMPOSE_IMGS = 1; % Do you want to see composed images? + % the function getframe which is used here is not really robust + % no window can overlay the most active figure(1) + % otherwise it will not work correctly + +% load the confidata +config = configdata(expname); + +im.dir = config.paths.img; +im.ext = config.files.imgext; + +NoCams = size(config.files.idxcams,2); % number of cameras + +% load image names +for i=1:NoCams, + seq(i).camId = config.files.idxcams(i); + if seq(i).camId > -1 + if findstr(expname,'oscar') + seq(i).data = dir([sprintf(im.dir,seq(i).camId),config.files.imnames,'*.',im.ext]); + else + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + end + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + error('Not enough images found. Wrong image path or name pattern?'); + end +end + +% for i=1:NoCams, +% seq(i).camId = config.files.idxcams(i); +% if seq(i).camId > -1 +% seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); +% else +% seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); +% end +% seq(i).size = size(seq(i).data,1); +% if seq(i).size<4 +% error('Not enough images found. Wrong image path or name pattern?'); +% end +% end + +loaded = loaddata(config); + + +if ~COMPOSE_IMGS +% load images and show the results + for i=1:NoCams, + for j=1:size(loaded.IdMat,2), + IM= imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + figure(1), clf, axes('Position',[0 0 1 1]), axis off + imshow(IM), hold on, + text(15,20,sprintf('Camera: %0.2d Frame: %0.3d',config.files.idxcams(i),j),'Color','green','FontWeight','bold','FontSize',12,'EraseMode','back'); + if loaded.IdMat(i,j)>0 + plot(loaded.Ws(3*i-2,j),loaded.Ws(3*i-1,j),'go','MarkerSize',25,'LineWidth',2,'EraseMode','back'); + plot(loaded.Ws(3*i-2,j),loaded.Ws(3*i-1,j),'r+','MarkerSize',15,'LineWidth',1,'EraseMode','back'); + else + text(15,40,'No point found','Color','green','FontWeight','bold','FontSize',12,'EraseMode','back'); + end + if SAVE_IMG + eval(sprintf('print -djpeg -r72 %spoint.cam%d.%d.jpg',config.paths.data, config.files.idxcams(i),j)) + end + hold off + end + end +end + +% show the composed figures +if COMPOSE_IMGS + for j=1:size(loaded.IdMat,2), + % IMcomposed = uint8(zeros(size(IM,1),size(IM,2),3,NoCams)); + for i=1:NoCams, + IM= imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + fig1 = figure(1); clf, axes('Position',[0 0 1 1]), axis off + imshow(IM), hold on, + text(15,20,sprintf('Camera: %0.2d Frame: %0.3d',config.files.idxcams(i),j),'Color','green','FontWeight','bold','FontSize',12,'EraseMode','back'); + if loaded.IdMat(i,j)>0 + plot(loaded.Ws(3*i-2,j),loaded.Ws(3*i-1,j),'go','MarkerSize',25,'LineWidth',2,'EraseMode','back'); + plot(loaded.Ws(3*i-2,j),loaded.Ws(3*i-1,j),'r+','MarkerSize',15,'LineWidth',1,'EraseMode','back'); + else + text(15,40,'No point found','Color','green','FontWeight','bold','FontSize',12,'EraseMode','back'); + end + IMannotated = getframe(gcf); + IMcomposed(:,:,:,i) = IMannotated.cdata; + hold off + end + fig2 = figure(2); clf, axes('Position',[0 0 1 1]), axis off, + montage(IMcomposed); + if SAVE_IMG + % eval(sprintf('print -djpeg -r152 %spoints.composed.%d.jpg',config.paths.data, j)) + eval(sprintf('print -depsc %spoints.composed.%04d.eps',config.paths.data, j)) + end + end +end + diff --git a/MultiCamValidation/CoreFunctions/P2KRtC.m b/MultiCamValidation/CoreFunctions/P2KRtC.m new file mode 100644 index 0000000..a245d21 --- /dev/null +++ b/MultiCamValidation/CoreFunctions/P2KRtC.m @@ -0,0 +1,19 @@ +% [K,R,t,C] = P2KRtC(P) +% decompose the euclidean 3x4 projection matrix P into the +% +% K ... 3x3 upper triangular calibration matrix +% R ... 3x3 rotation matrix +% t ... 3x1 translation vector +% C ... 3x1 position od the camera center +% +% $Id: P2KRtC.m,v 2.0 2003/06/19 12:07:08 svoboda Exp $ + +function [K,R,t,C] = P2KRtC(P) + +P = P./norm(P(3,1:3)); + +[K,R] = rq(P(:,1:3)); +t = inv(K)*P(:,4); +C = -R'*t; + +return; diff --git a/MultiCamValidation/CoreFunctions/estimateX.m b/MultiCamValidation/CoreFunctions/estimateX.m new file mode 100644 index 0000000..d5536cb --- /dev/null +++ b/MultiCamValidation/CoreFunctions/estimateX.m @@ -0,0 +1,128 @@ +% estimateX ... estimate 3D points robustly +% +% reconstructed = estimateX(loaded,IdMat,cam) +% +% data ... data structure, see LOADDATA +% IdMat ... current point identification matrix +% cam ... array of camera structures, see the main script GO +% +% reconstructed.ptdIdx ... indexes->data of points used for the reconstruction +% .X ... reconstructed points, see u2PX +% .CamIdx ... indexes->data of cameras used for the reconstruction +% +% $Id: estimateX.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $ + +function reconstructed = estimateX(data,IdMat,cam,config) + +SS = config.cal.NTUPLES; % sample size +MIN_POINTS = config.cal.MIN_PTS_VAL; % minimal number of correnspondences in the sample + +Ws = data.Ws; +Pmat = data.Pmat; + +CAMS = size(IdMat,1); +FRAMES = size(IdMat,2); + +% create indexes for all possible SS-tuples +if 1 + count = 1; + if SS == 2; + for i=1:CAMS, + for j=(i+1):CAMS, + sample(count).CamIds = [i,j]; + count = count+1; + end + end + end + if SS == 3; + for i=1:CAMS, + for j=(i+1):CAMS, + for k=(j+1):CAMS, + sample(count).CamIds = [i,j,k]; + count = count+1; + end + end + end + end + if SS == 4; + for i=1:CAMS, + for j=(i+1):CAMS, + for k=(j+1):CAMS, + for l=(k+1):CAMS, + sample(count).CamIds = [i,j,k,l]; + count = count+1; + end + end + end + end + end + if SS == 5; + for i=1:CAMS, + for j=(i+1):CAMS, + for k=(j+1):CAMS, + for l=(k+1):CAMS, + for m=(l+1):CAMS, + sample(count).CamIds = [i,j,k,l,m]; + count = count+1; + end + end + end + end + end + end +else + sample(1).CamIds = [1:15]; + SS = size(sample(1).CamIds,2); +end + +disp(sprintf('Computing recontruction from all %d camera %d-tuples',size(sample,2), SS)); + +% create triple indexes +for i=1:CAMS, + tripleIdx{i} = [i*3-2:i*3]; +end + +%%% +% for all possible combination of SS-tuples of cameras +% do the linear 3D reconctruction if enough point avaialable +for i=1:size(sample,2), + ptsIdx = find(sum(IdMat([sample(i).CamIds],:))==SS); + if size(ptsIdx,2) > MIN_POINTS + X = uP2X(Ws([tripleIdx{[sample(i).CamIds]}],ptsIdx), [Pmat{[sample(i).CamIds]}]); + % compute the reprojections + for j=1:CAMS, + xe = Pmat{j}*X; + cam(j).xe = xe./repmat(xe(3,:),3,1); + % these points were the input into Martinec and Pajdla filling + mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far + mask.vis = zeros(1,FRAMES); % maks of visible points + mask.rec(ptsIdx) = 1; + mask.vis(cam(j).ptsLoaded) = 1; + mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera + unmask.rec = cumsum(mask.rec); + unmask.vis = cumsum(mask.vis); + cam(j).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); + cam(j).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); + cam(j).err2d = sum([cam(j).xe(1:2,cam(j).recandvis) - cam(j).xgt(1:2,cam(j).visandrec)].^2); + cam(j).mean2Derr = mean(cam(j).err2d); + cam(j).std2Derr = std(cam(j).err2d); + end + sample(i).mean2Derrs = [cam(:).mean2Derr]; + sample(i).std2Derrs = sum([cam(:).std2Derr]); + sample(i).mean2Derr = sum(sample(i).mean2Derrs); + else + sample(i).mean2Derr = 9e99; + sample(i).std2Derrs = 9e99; + end +end + +% find the best sample +[buff,idxBest] = min([sample(:).mean2Derr]+[sample(:).std2Derrs]); + +% and recompute it the best values +reconstructed.ptsIdx = find(sum(IdMat(sample(idxBest).CamIds,:))==SS); +reconstructed.X = uP2X(Ws([tripleIdx{[sample(idxBest).CamIds]}],reconstructed.ptsIdx), [Pmat{[sample(idxBest).CamIds]}]); +reconstructed.CamIdx = sample(idxBest).CamIds; + +return; + diff --git a/MultiCamValidation/CoreFunctions/findinl.m b/MultiCamValidation/CoreFunctions/findinl.m new file mode 100644 index 0000000..a31a367 --- /dev/null +++ b/MultiCamValidation/CoreFunctions/findinl.m @@ -0,0 +1,61 @@ +% FindInl find inliers in joint image matrix +% by pairwise epipolar geometry +% +% function IdMatIn = findinl(Ws,IdMat,tol) +% Ws ... 3MxN joint image matrix +% IdMat ... MxN ... 0 -> no point detected +% 1 -> point detected +% tol ... [pixels] tolerance for the epipolar geometry +% the point are accpted as outliers only if they +% are closer to the epipolar line than tol + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: findinl.m,v 2.0 2003/06/19 12:07:09 svoboda Exp $ +% $State: Exp $ + +function IdMatIn = findinl(Ws,IdMat,tol) + +NoCams = size(IdMat,1); + +% fill the array of structures not_used denoted as 0 +% allocate the array of structures for used +for i=1:NoCams, + not_used(i).pts = sum(IdMat(i,:)); + used(i).pts = -1; +end + +% allocate IdMat for outliers +IdMatIn = zeros(size(IdMat)); + +while (sum([not_used.pts])>1-NoCams), + [buff, id.cam_max] = max([not_used.pts]); + used = add(used, id.cam_max, not_used(id.cam_max).pts); + not_used = remove(not_used, id.cam_max); + Mask = repmat(IdMat(id.cam_max,:),NoCams,1); + Corresp = Mask & IdMat; + Corresp(id.cam_max,:) = 0; + [buff, id.cam_to_pair] = max(sum(Corresp')); % find the camera with most correspondences + idx.corr_to_pair = find(sum(IdMat([id.cam_max,id.cam_to_pair],:))==2); + % used = add(used, id.cam_to_pair, not_used(id.cam_to_pair).pts); + % not_used = remove(not_used, id.cam_to_pair); + if size(idx.corr_to_pair,2)<8, + error('Not enough points to compute epipolar geometry in RANSAC validation') + end + Wspair = []; + Wspair = Ws(id.cam_max*3-2:id.cam_max*3, idx.corr_to_pair); + Wspair = [Wspair; Ws(id.cam_to_pair*3-2:id.cam_to_pair*3, idx.corr_to_pair)]; + % id + [F, inls] = rEG(Wspair,tol,tol,0.99); + IdMatIn(id.cam_max, idx.corr_to_pair(inls)) = 1; + IdMat(id.cam_max, :) = 0; + IdMat(id.cam_max, idx.corr_to_pair(inls)) = 1; +end + +function list = add(list, id, value) +list(id).pts = value; +return + +function list = remove(list, id) +list(id).pts = -1; +return \ No newline at end of file diff --git a/MultiCamValidation/CoreFunctions/isptnorm.m b/MultiCamValidation/CoreFunctions/isptnorm.m new file mode 100644 index 0000000..7c22a29 --- /dev/null +++ b/MultiCamValidation/CoreFunctions/isptnorm.m @@ -0,0 +1,43 @@ +% isptnorm ISotropic PoinT NORMalization +% +% [xnorm,T] = isptnorm(x); +% x ... [N x dim] coordinates +% +% xnorm ... normalized coordinates +% T ... transformation matrix used +% +% T. Svoboda, 5/2001 + + +function [xnorm,T] = isptnorm(x); + + +% data dimension +dim = size(x,2); +N = size(x,1); + +% make homogenous coordinates +x(:,dim+1) = ones(N,1); + +% compute sum of square diferences +for i=1:dim, + ssd(:,i) = (x(:,i)-mean(x(:,i))).^2; +end + +scale = (sqrt(dim)*N) / (sum(sqrt(sum(ssd')))); + +T = zeros(dim+1); + +for i=1:dim, + T(i,i) = scale; + T(i,dim+1) = -scale*mean(x(:,i)); +end +T(dim+1,dim+1) = 1; + +xnorm = T*x'; +xnorm = xnorm'; +% return nonhomogenous part of the points coordinates +xnorm = xnorm(:,1:dim); + + + diff --git a/MultiCamValidation/CoreFunctions/rq.m b/MultiCamValidation/CoreFunctions/rq.m new file mode 100644 index 0000000..3176a2a --- /dev/null +++ b/MultiCamValidation/CoreFunctions/rq.m @@ -0,0 +1,33 @@ +%RQ Pajdla: Returns a 3x3 upper triangular R and a unitary Q so that X = R*Q +% +% function [R,Q] = rq(X) +% +% X = input matrix, +% Q = unitary matrix +% R = upper triangular matrix +% +% See also QR. + +% Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be +% pajdla@vision.felk.cvut.cz +% 05/28/94 ESAT-MI2, KU Leuven +% Documentation: +% Language: Matlab 4.1, (c) MathWorks +% +function [R,Q] = rq(X) + + [Qt,Rt] = qr(X'); + Rt = Rt'; + Qt = Qt'; + + Qu(1,:) = cross(Rt(2,:),Rt(3,:)); + Qu(1,:) = Qu(1,:)/norm(Qu(1,:)); + + Qu(2,:) = cross(Qu(1,:),Rt(3,:)); + Qu(2,:) = Qu(2,:)/norm(Qu(2,:)); + + Qu(3,:) = cross(Qu(1,:),Qu(2,:)); + + R = Rt * Qu'; + Q = Qu * Qt; + diff --git a/MultiCamValidation/CoreFunctions/uP2X.m b/MultiCamValidation/CoreFunctions/uP2X.m new file mode 100644 index 0000000..a21b7ac --- /dev/null +++ b/MultiCamValidation/CoreFunctions/uP2X.m @@ -0,0 +1,39 @@ +% uP2X ... linear reconstruction of 3D points +% from N-perspective views +% +% X = uP2X(Umat,Pmat); +% Umat ... 3*N x n matrix of n homogenous points +% Ps ... 3 x 4*N matrix of projection matrices +% +% X ... 4 x n matrix of homogenous 3D points +% +% Algorithm is based on: Hartley and Zisserman, Multiple +% View Geometry, 2000, pp 297-298 +% +% $Id: uP2X.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $ + +function X = uP2X(Umat,Ps); + +N = size(Umat,1)/3; +n = size(Umat,2); + +% reshuffle the Ps matrix +Pmat = []; +for i=1:N; + Pmat = [Pmat;Ps(:,i*4-3:i*4)]; +end + +X = []; +for i=1:n, % for all points + A = []; + for j=1:N, % for all cameras + % create the data matrix + A = [A; Umat(j*3-2,i)*Pmat(j*3,:) - Pmat(j*3-2,:); Umat(j*3-1,i)*Pmat(j*3,:) - Pmat(j*3-1,:)]; + end + [u,s,v] = svd(A); + X = [X,v(:,end)]; +end +% normalize reconstructed points +X = X./repmat(X(4,:),4,1); + +return; diff --git a/MultiCamValidation/CoreFunctions/workvolume.m b/MultiCamValidation/CoreFunctions/workvolume.m new file mode 100644 index 0000000..87436dc --- /dev/null +++ b/MultiCamValidation/CoreFunctions/workvolume.m @@ -0,0 +1,65 @@ +function [Xmat,idxisa] = workvolume(cam,room,imres,idxcams) + +STEP = 0.1; + +if nargin < 4 + idxcams = [1:size(cam,2)]; +end + +if nargin < 3 + imres = repmat([640 480],size(idxcams,2),1); +end +imres = imres(idxcams,:); + +if nargin < 2 + % room [x_min, x_max, y_min, y_max, z_min, z_max] + room = [-3 3 -3 3 0 3]; +end + +% compose Pmat containing all P matrices +Pmat = []; +for i=idxcams, + Pmat = [Pmat; cam(i).P]; +end + +% create points + +zcoor = room(5):STEP:room(6); +znum = size(zcoor,2); +ycoor = room(3):STEP:room(4); +ynum = size(ycoor,2); +xcoor = room(1):STEP:room(2); +xnum = size(xcoor,2); + + +buff = repmat(ycoor,znum,1); +yvec = reshape(buff,prod(size(buff)),1); +zvec = repmat(zcoor',ynum,1); + +buff = repmat(xcoor,znum*ynum,1); +xvec = reshape(buff,prod(size(buff)),1); + +Xmat = [xvec,repmat([yvec,zvec],xnum,1)]; +Xmat = [Xmat,ones(size(Xmat(:,1)))]; + +clear buff + +size(Xmat) +umat = Pmat*Xmat'; + +% normalize projected points in umat +scalemat = []; +for i=1:size(idxcams,2), + scalemat = [scalemat; repmat(umat(3*i,:),3,1)]; +end +umat = umat./scalemat; +clear scalemat; +mask = zeros(size(umat)); +mask(1:3:end,:) = umat(1:3:end,:)0); +idxisa = find(sum(mask)==size(mask,1)); + +return; + diff --git a/MultiCamValidation/FindingPoints/atlantic.pm b/MultiCamValidation/FindingPoints/atlantic.pm new file mode 100644 index 0000000..486f1f0 --- /dev/null +++ b/MultiCamValidation/FindingPoints/atlantic.pm @@ -0,0 +1,4 @@ +$process={"compname"=>"atlantic2","camIds"=>[3,4,5,6,7,8,9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"atlantic2","camIds"=>[11,12,13,14,15,16,17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamValidation/FindingPoints/getpoint.m b/MultiCamValidation/FindingPoints/getpoint.m new file mode 100644 index 0000000..37986d6 --- /dev/null +++ b/MultiCamValidation/FindingPoints/getpoint.m @@ -0,0 +1,261 @@ +% GETPOINT ... extracts position of an LED from an image +% only one or none LED is expected +% +% function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM) +% +% imname ... name of the image (full path should be specified) +% showfig .. show figures (1->on/0->off) +% imconfig . config.imgs, see CONFIGDATA +% avIM ... average image of the camera, see IM2POINTS +% stdIM ... image of standard deviations, see IM2POINTS +% +% pos ...... 2x1 vector containing (x,y)'-coordinates of the point +% if error then 0 is returned +% err ...... boolean, indicates an error (ambiguous blobs, point too +% eccentric, etc.) + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: getpoint.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $ +% $State: Exp $ + +function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM,subpix) + +err = 0; + +SHOW_WARN = 0; % show warnings? +BLK_PROC = 0; % blkproc may be faster for bigger LEDs +SUB_PIX = 1/imconfig.subpix; % required sub-pixel precision 3 -> 1/3 pixel + +TEST_ECC = 0; % perform the eccentricity check? +ECC_THR = 0.7; % eccentricity threshold (for validity check) + % this threshold is not usable in the current implementation + +LEDSIZE = imconfig.LEDsize; % avg diameter of a LED in pixels + +im.name = imname; + +%%% +% set figure handles +fig.im4thr = 1; % image used for thresholding +fig.imOrig = 2; % original image +fig.blob = 3; % ouput of bwlabel +fig.subI = 4; % subimage (local neighbourhood of est. LED pos.) + +im.info = imfinfo(im.name); +im.orig = imread(im.name); + +if findstr(im.info.ColorType,'grayscale'); + im.I = im.orig; +else + [im.r,im.c] = size(im.orig(:,:,1)); + im.R = im.orig(:,:,1); % Red component + im.G = im.orig(:,:,2); % Green component +end + +% find possible location of the point by thresholding +if strcmp(imconfig.LEDcolor,'green') + im.thr = uint8(abs(double(im.G(:,:))-double(avIM(:,:,2)))); % on which image the thresholding will be done + im.std = stdIM(:,:,2); % use green component + im.fit = im.G; % image for fitting of the PSF +elseif strcmp(imconfig.LEDcolor,'red') + im.thr = uint8(abs(double(im.R(:,:))-double(avIM(:,:,1)))); % on which image the thresholding will be done + im.std = stdIM(:,:,1); % use red component + im.fit = im.R; % image for fitting of the PSF +else + error('getpoint: no valid color of the laser pointer, see CONFIGDATA'); +end + +% show figures if required, may be useful when debugging +if showfig + figure(fig.imOrig), + clf + imshow(im.orig) + title(strcat(im.name, ' original')) + hold on + figure(fig.im4thr), + clf + imshow(im.thr); + title(strcat(im.name, ' image to be thresholded')) + drawnow + hold on +end + +% sortedInt = sort(double(im.thr(:))); % sort intensities +[maxint,idx] = max(im.thr(:)); +leds.thr = double(maxint)*4/5; +aboveThr = sum(sum(im.thr>leds.thr)); +% check how many pixels lie above the threshold +% if too many, there is probably no LED at all +% otherwise, take the position of the maximal intensity +% as the LED position +if aboveThr > (pi*LEDSIZE^2/2) + if SHOW_WARN + warning('Perhaps no LED in the image, detected blob is too large') + end + err=1; + pos=0; + return; +elseif ( (im.thr(idx) < 5*double(im.std(idx))) | ( im.thr(idx)< 70 )) + if SHOW_WARN + warning('Perhaps no LED in the image, detected maximum of image difference is too low') + end + err=1; + pos=0; + return; +else + rawpos = zeros(1,2); + [rawpos(1),rawpos(2)] = ind2sub(size(im.thr),idx); +end + +leds.size = round(LEDSIZE/1.2); % (2*leds.size+1)x(2*leds.size+1) is the area of interest around each detected LED +% check if the LED lies in the allowed position (not very close to the image border +% it is because of the implementation not because of principle +if rawpos(1)-leds.size < 1 | rawpos(1)+leds.size > size(im.thr,1) | ... + rawpos(2)-leds.size < 1 | rawpos(2)+leds.size > size(im.thr,2) + if SHOW_WARN + warning('LED position lies outside allowed boundary'); + end + err = 1; + pos = 0; + return +end + +leds.rows = (rawpos(1)-leds.size):(rawpos(1)+leds.size); +leds.cols = (rawpos(2)-leds.size):(rawpos(2)+leds.size); +[L,num] = bwlabel(im.thr(leds.rows,leds.cols)>leds.thr); +%%% +% define subimage as local neighbour of estimated LED position +if TEST_ECC + im.stats = imfeature(L,'Centroid','Eccentricity'); +else + im.stats = imfeature(L,'Centroid'); +end + +if size(im.stats,1)>1, + if SHOW_WARN + warning('More than one blob detected') + end + err=1; pos=0; + return; +end + +if TEST_ECC + if (im.stats.Eccentricity > ECC_THR) + warning('eccentricity treshold exceeded'); + err = 1; pos = 0; + end +end + +% Crop the sub-image of interest around found LED +IM = im.fit(leds.rows,leds.cols); + +% visual check of LED position +if showfig + figure(fig.subI) + imshow(IM) + plot(im.stats(1).Centroid(1),im.stats(1).Centroid(2),'g+','EraseMode','Back'); + drawnow + % pause +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% interpolate local neighborhood and find the maxima +% by correlation with the gaussian (see declaration of Gsize) +leds.scale = SUB_PIX; % the area of interest will be inreased by leds.scale using bilinear interpolation + % leds.size should be comparable to the leds.scale. If leds.size is assumed too small + % then the correlation based detection does not work + % properly +Gsize = round(leds.scale*LEDSIZE/2); % (2*Gsize+1)x(2*Gsize+1) is the dimension of the Gaussian mask that models PSF + +%activerows = (Gsize+1):(leds.scale*(leds.size*2+1)-Gsize-1); +%activecols = (Gsize+1):(leds.scale*(leds.size*2+1)-Gsize-1); + +% t1 = cputime; +IM2 = imresize(IM,leds.scale,'bicubic'); % zoom in +% disp(sprintf('elapsed for resize: %f',cputime-t1')) + +% Correlation mask that approximates point spread function (PSF) of a LED +Gaussian = fspecial('Gaussian',2*Gsize+1,leds.scale*LEDSIZE/3); + +activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2)); +activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2)); + +% check if leds.size and leds.scale have reasonable values +if (size(activerows,2)<5 | size(activerows,2)>50) + error('probably incorect setting of leds.size and leds.scale variables') +end + +corrcoefmat = zeros(size(IM2)); +% t1 = cputime; +if BLK_PROC % blkproc may be faster for big neighbourhoods + corrcoefmat(activerows,activecols) = blkproc(IM2(activerows,activecols),[1,1],[Gsize,Gsize],'corr2',Gaussian); +else + G = double(Gaussian(:)); + Gn = G-mean(G); + Gn2 = sum(Gn.^2); + B = im2col(double(IM2),size(Gaussian),'sliding'); + corrcoefmat(activerows,activecols) = col2im(mycorr2(B,G,Gn,Gn2), size(Gaussian), size(IM2),'sliding'); + % corrcoefmat(activerows,activecols) = colfilt(double(IM2(activerows,activecols)),size(Gaussian),'sliding','mycorr2',G,Gn,Gn2); +end +% disp(sprintf('elapsed for coorrelations: %f',cputime-t1')) + +[maxcorrcoef,idxmaxcorrcoef] = max(corrcoefmat(:)); +[rmax,cmax] = ind2sub(size(corrcoefmat),idxmaxcorrcoef); +finepos = rawpos+([rmax,cmax]-ceil(size(IM2)/2))./leds.scale; + +%%% +% plot the subimage with detected position of the maximal correlation +%%% + +if showfig + figure(5), + clf + subplot(2,2,4) + showimg(IM2,5); + % colormap('gray'); + hold on + axis on + plot(cmax,rmax,'g+','EraseMode','Back') + hold off + subplot(2,2,3) + mesh(corrcoefmat) + subplot(2,2,1) + mesh(double(IM2(activerows,activecols))) + subplot(2,2,2) + mesh(Gaussian) + drawnow + % pause +end + +%%% plot information about detected LED +if showfig + figure(fig.im4thr) + plot(finepos(2),finepos(1),'r+','EraseMode','Back'); + figure(fig.imOrig) + plot(finepos(2),finepos(1),'r+','EraseMode','Back','MarkerSize',10); + drawnow +end + +if showfig + pause +end + +pos = [finepos(2); finepos(1)]; + + + + + + + + + + + + + + + + + diff --git a/MultiCamValidation/FindingPoints/im2imstat.m b/MultiCamValidation/FindingPoints/im2imstat.m new file mode 100644 index 0000000..bfb4e76 --- /dev/null +++ b/MultiCamValidation/FindingPoints/im2imstat.m @@ -0,0 +1,140 @@ +% im2imstat computes image statistics from several images +% and finds the projections of laser points +% +% The computation core is taken from im2points.m +% computes average image and image of standard deviations +% requires configdata.m +% The name of the experiment has to be specified +% it determines the location of files etc ... +% +% the scripts serves as a template for a multiprocessing +% it assumes a vector of camera IDs CamIds to be known +% indexes in the CamIds are supposed to be correct +% +% It is used by im2pmultiproc.pl + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: im2imstat.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $ +% $State: Exp $ + +config = configdata(expname); + +STEP4STAT = 5; % step for computing average and std images, if 1 then all images taken + +im.dir = config.paths.img; +im.ext = config.files.imgext; + +% NoCams = size(config.files.idxcams,2); % number of cameras +% Use concrete CameraIds instead of all cameras +NoCams = size(CamsIds,2); +CamsIds + +% load image names +for i=1:NoCams, + seq(i).camId = CamsIds(i); + if seq(i).camId > -1 + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + [sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext] + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + error('Not enough images found. Wrong image path or name pattern?'); + end +end + +% Expected number of 3D points is equal to the number of frames. +% In fact, some frames might be without any calibration point + +% Because of non-consistent stopping of capturing, the sequences might +% have different number of images, select the minimal value as the right one +NoPoints = min([seq.size]); + +% compute the average images that will be used for finding LEDs +% if not already computed + +pointsIdx = [1:STEP4STAT:NoPoints]; + +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.avIM,seq(i).camId)), + disp(sprintf('The average image of the camera %d is being computed',seq(i).camId)); + avIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + avIM = avIM + double(IM); + end + avIM = uint8(round(avIM./size(pointsIdx,2))); + imwrite(avIM,sprintf(config.files.avIM,seq(i).camId)); + else disp('Average files already exist'); + end +end +disp(sprintf('Elapsed time for average images: %d [sec]',cputime-t)) +% compute the standard deviations images that will be used for finding LEDs +% if not already computed +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.stdIM,seq(i).camId)), + avIM = double(imread(sprintf(config.files.avIM,seq(i).camId))); + disp(sprintf('The image of standard deviations of the camera %d is being computed',seq(i).camId)); + stdIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + stdIM = stdIM + (double(IM)-avIM).^2; + end + stdIM = uint8(round(sqrt(stdIM./(size(pointsIdx,2)-1)))); + imwrite(stdIM,sprintf(config.files.stdIM,seq(i).camId)); + else + disp('Image of standard deviations already exist') + end +end +disp(sprintf('Elapsed time for computation of images [sec]: %d',cputime-t)) + +% find points in the images +Ws = []; +IdWs = []; +Res = []; + +IdMat = ones(NoCams,NoPoints); +% IdMat is very important for Martinec&Pajdla filling [ECCV2002] +% it is a NoCams x NoPoints matrix, +% IdMat(i,j) = 0 -> no j-th point in i-th +% IdMat(i,j) = 1 -> point successfully detected + +for i=1:NoCams, + Points = []; + avIM = imread(sprintf(config.files.avIM,seq(i).camId)); + stdIM = imread(sprintf(config.files.stdIM,seq(i).camId)); + for j=1:NoPoints, + [pos,err] = getpoint([sprintf(im.dir,seq(i).camId),seq(i).data(j).name], 0, config.imgs, avIM, stdIM); + if err + IdMat(i,j) = 0; + Points = [Points, [NaN; NaN; NaN]]; + else + Points = [Points, [pos; 1]]; + end + end + Ws = [Ws; Points]; + Res= [Res; size(avIM,2), size(avIM,1)]; +end + +idx = '.'; +for i=CamsIds, + idx = sprintf('%s%02d',idx,i); +end + +save([config.files.points,idx], 'Ws','-ASCII') +% save(config.files.IdPoints,'IdWs','-ASCII') +save([config.files.Res,idx], 'Res', '-ASCII') +save([config.files.IdMat,idx], 'IdMat', '-ASCII') + +% write auxiliary file that is done +done=1; +save(donefile,'done','-ascii'); + +% exit the Matlab +% this script is to be used in the batch mode +% hence exit at the end is necessary +exit; \ No newline at end of file diff --git a/MultiCamValidation/FindingPoints/im2pmultiproc.pl b/MultiCamValidation/FindingPoints/im2pmultiproc.pl new file mode 100755 index 0000000..77a3041 --- /dev/null +++ b/MultiCamValidation/FindingPoints/im2pmultiproc.pl @@ -0,0 +1,92 @@ +#! /usr/bin/perl -w + +# find projections of LEDs in images +# it spreads the job to all specified processors/machines +# It requires: +# - matlab +# - configdata.m +# - expname +# +# script is useful if more machines or more processors +# are available. +# automatic ssh authentication is necessary, see +# http://www.cs.umd.edu/~arun/misc/ssh.html +# http://www.cvm.uiuc.edu/~ms-drake/linux/ssh_nopw.html + +# $Author: svoboda $ +# $Revision: 2.0 $ +# $Id: im2pmultiproc.pl,v 2.0 2003/06/19 12:07:11 svoboda Exp $ +# $State: Exp $ + +use Env; +# use XML::Simple; +# use Data::Dumper; + +# name of the m-file template +$mfile="im2imstat"; +$mvariable="CamsIds"; +$donefile=".done"; + +# not yet ready +# $config = XMLin('multiprocAtlantic.xml'); +# print Dumper($config); +# @processes = @$config-> + +# load the info about local machines and parallel processes +use virooms; + +# just a debug cycle +foreach $process (@processes) { + print "$process->{'compname'} \n"; + foreach $camId (@{$process->{'camIds'}}) { + print "$camId \n"; + } +} + +# create temporary m-files with using the template m-file +# store their names for delete at the very end +# each process must have its own temporary m-file + +# compose the names and commands +foreach $process (@processes) { + $idfile=""; + $str4cmd=""; + foreach $camId (@{$process->{'camIds'}}) { + $idfile = "$idfile$camId"; + $str4cmd = "$str4cmd $camId"; + } + $process->{'scriptName'} = "$ENV{'PWD'}/$mfile${idfile}.m"; + $process->{'donefile'} = "$ENV{'PWD'}/${donefile}${idfile}"; + $process->{'catcmd'} = "echo \"${mvariable} = [${str4cmd}]; donefile='$process->{'donefile'}'; addpath $ENV{'PWD'}; addpath $ENV{'PWD'}/../Cfg; \" | cat - ${mfile}.m > $process->{'scriptName'}"; + $process->{'mcmd'} = "ssh $process->{'compname'} /pub/bin/matlab -nosplash -nojvm < $process->{'scriptName'} > $process->{'scriptName'}.out &"; +} + +print "*************************** \n"; + +# create the auxiliary scripts +foreach $process (@processes) { + system($process->{'catcmd'}); +} + +# run the parallel matlabs +foreach $process (@processes) { + system($process->{'mcmd'}); +} + +# wait until all processing is done +print "Image processing in progress. Plase be patient \n"; +do { + sleep 10; + @donefiles = glob("${donefile}*"); +} while (@donefiles < @processes); + +# copy the data files to a coomon disc space yet to be implemented + +# final cleaning of the auxiliary files +foreach $process (@processes) { + system("rm -f $process->{'donefile'}"); + system("rm -f $process->{'scriptName'}"); + system("rm -f $process->{'scriptName'}.out"); +} +# system("rm -f *.out"); + diff --git a/MultiCamValidation/FindingPoints/im2points.m b/MultiCamValidation/FindingPoints/im2points.m new file mode 100644 index 0000000..ec4845f --- /dev/null +++ b/MultiCamValidation/FindingPoints/im2points.m @@ -0,0 +1,159 @@ +% Read all images and extract point coordinates. +% +% All information needed are stored and retrieved +% from the function CONFIGDATA + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: im2points.m,v 2.0 2003/06/19 12:07:11 svoboda Exp $ +% $State: Exp $ + +clear all; + +% add path to config data +addpath ../Cfg +% add path for graphical output if needed +addpath ../OutputFunctions + +SHOWFIG = 0; % show images during point extraction +STEP4STAT = 5; % step for computing average and std images, if 1 then all images taken + +config = configdata(expname); + +im.dir = config.paths.img; +im.ext = config.files.imgext; + +NoCams = size(config.files.idxcams,2); % number of cameras + +% load image names +for i=1:NoCams, + seq(i).camId = config.files.idxcams(i); + if seq(i).camId > -1 + if findstr(expname,'oscar') + seq(i).data = dir([sprintf(im.dir,seq(i).camId),config.files.imnames,'*.',im.ext]); + else + seq(i).data = dir([sprintf(im.dir,seq(i).camId),sprintf(config.files.imnames,seq(i).camId),im.ext]); + end + else + seq(i).data = dir([im.dir,sprintf(config.files.imnames),im.ext]); + end + seq(i).size = size(seq(i).data,1); + if seq(i).size<4 + error('Not enough images found. Wrong image path or name pattern?'); + end +end + +% Expected number of 3D points is equal to the number of frames. +% In fact, some frames might be without any calibration point + +% Becouse of non-consistent stopping of capturing, the sequences might +% have different number of images, select the minimal value as the right one +NoPoints = min([seq.size]); +pointsIdx = [1:STEP4STAT:NoPoints]; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% beginning of the findings + +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.avIM,seq(i).camId)), + disp(sprintf('The average image of the camera %d is being computed',seq(i).camId)); + avIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + avIM = avIM + double(IM); + end + avIM = uint8(round(avIM./size(pointsIdx,2))); + imwrite(avIM,sprintf(config.files.avIM,seq(i).camId)); + else disp('Average files already exist'); + end +end +disp(sprintf('Elapsed time for average images: %d [sec]',cputime-t)) +% compute the standard deviations images that will be used for finding LEDs +% if not already computed +t = cputime; +for i=1:NoCams, + if ~exist(sprintf(config.files.stdIM,seq(i).camId)), + avIM = double(imread(sprintf(config.files.avIM,seq(i).camId))); + disp(sprintf('The image of standard deviations of the camera %d is being computed',seq(i).camId)); + stdIM = zeros(size(imread([sprintf(im.dir,seq(i).camId),seq(i).data(1).name]))); + for j=pointsIdx, + IM = imread([sprintf(im.dir,seq(i).camId),seq(i).data(j).name]); + stdIM = stdIM + (double(IM)-avIM).^2; + end + stdIM = uint8(round(sqrt(stdIM./(size(pointsIdx,2)-1)))); + imwrite(stdIM,sprintf(config.files.stdIM,seq(i).camId)); + else + disp('Image of standard deviations already exist') + end +end + +disp(sprintf('Elapsed time for computation of images [sec]: %d',cputime-t)) + + +% find points in the images +Ws = []; % joint image matrix +Res = []; % resolution of cameras +IdMat = ones(NoCams,NoPoints); +% IdMat is very important for Martinec&Pajdla filling [ECCV2002] +% it is a NoCams x NoPoints matrix, +% IdMat(i,j) = 0 -> no j-th point in i-th +% IdMat(i,j) = 1 -> point successfully detected + + +disp('*********************************************') +disp('Finding points (laser projections) in cameras') +disp(sprintf('Totally %d cameras, %d images for each cam', NoCams, NoPoints')) +disp('*********************************************') +for i=1:NoCams, + t1 = cputime; + disp(sprintf('Finding points in camera No: %0.2d',config.files.idxcams(i))) + Points = []; + avIM = imread(sprintf(config.files.avIM,seq(i).camId)); + stdIM = imread(sprintf(config.files.stdIM,seq(i).camId)); + for j=1:NoPoints, + [pos,err] = getpoint([sprintf(im.dir,seq(i).camId),seq(i).data(j).name], SHOWFIG, config.imgs, avIM, stdIM); + if err + IdMat(i,j) = 0; + Points = [Points, [NaN; NaN; NaN]]; + else + Points = [Points, [pos; 1]]; + end + end + Ws = [Ws; Points]; + Res= [Res; size(avIM,2), size(avIM,1)]; + t2 = cputime; + disp(sprintf('Elapsed time for finding points in one camera: %d minutes %d seconds',floor((t2-t1)/60), round(mod((t2-t1),60)))) +end + +%%% End of the findings +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +if findstr(expname,'oscar') + % needs special care for handling projector data + ProjPoints = load(config.files.projdata,'-ASCII'); + Ws = [Ws; ProjPoints(:,2:3)'; ones(size(ProjPoints(:,1)'))]; + IdMat = [IdMat; ones(size(ProjPoints(:,1)'))]; + Res = [Res; config.imgs.projres]; +end + +save(config.files.points, 'Ws','-ASCII') +save(config.files.Res, 'Res', '-ASCII') +save(config.files.IdMat, 'IdMat', '-ASCII') + +% display the overall statistics +disp('Overall statistics from im2points: ************************ ') +disp(sprintf('Total number of frames (possible 3D points): %d',NoPoints)) +disp(sprintf('Total number of cameras %d', NoCams)) +disp('More important statistics: ********************************* ') +disp(sprintf('Detected 3D points: %d', sum(sum(IdMat)>0))) +disp(sprintf('Detected 3D points in at least 3 cams: %d', sum(sum(IdMat)>2))) +disp(sprintf('Detected 3D points in ALL cameras: %d', sum(sum(IdMat)==NoCams))) + + + + + + + + diff --git a/MultiCamValidation/FindingPoints/mycorr2.m b/MultiCamValidation/FindingPoints/mycorr2.m new file mode 100644 index 0000000..1d55b6f --- /dev/null +++ b/MultiCamValidation/FindingPoints/mycorr2.m @@ -0,0 +1,24 @@ +% mycorr2 modified version of the 2D correlation +% for the use with im2col and col2im +% see GETPOINT +% +% +% $Id: mycorr2.m,v 2.0 2003/06/19 12:07:11 svoboda Exp $ + +% Note: It written in order to gain speed. The clarity of the code suffers accordingly + +function R = mycorr2(X,G,Gn,Gn2) + +% Gn = G-mean(G); +% Gn2 = sqrt(sum(Gn.^2)); + +mX = repmat(mean(X),size(X,1),1); +mXn = X - mX; +smX = sum(mXn.^2); + +numerator = (mXn'*Gn)'; +denominator = smX*Gn2; + +R = numerator./denominator; + +return \ No newline at end of file diff --git a/MultiCamValidation/FindingPoints/virooms.pm b/MultiCamValidation/FindingPoints/virooms.pm new file mode 100644 index 0000000..d490ba5 --- /dev/null +++ b/MultiCamValidation/FindingPoints/virooms.pm @@ -0,0 +1,16 @@ +$process={"compname"=>"viroom00","camIds"=>[3,4],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom00","camIds"=>[5,6],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom01","camIds"=>[7,8],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom01","camIds"=>[9,10],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom02","camIds"=>[11,12],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom02","camIds"=>[14],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[15,16],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[17,18],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamValidation/FindingPoints/virooms4.pm b/MultiCamValidation/FindingPoints/virooms4.pm new file mode 100644 index 0000000..d2a7b2f --- /dev/null +++ b/MultiCamValidation/FindingPoints/virooms4.pm @@ -0,0 +1,8 @@ +$process={"compname"=>"viroom00","camIds"=>[0],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom00","camIds"=>[1],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[2],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); +$process={"compname"=>"viroom03","camIds"=>[3],"scriptName"=>"","catcmd"=>"","mcmd"=>"","donefile"=>""}; +push(@processes,$process); diff --git a/MultiCamValidation/InputOutputFunctions/drawcloud.m b/MultiCamValidation/InputOutputFunctions/drawcloud.m new file mode 100644 index 0000000..07e6d0b --- /dev/null +++ b/MultiCamValidation/InputOutputFunctions/drawcloud.m @@ -0,0 +1,28 @@ +% drawcloud ... draw point cloud +% +% [fig] = drawcloud(X,fig,{color}) +% +% X ... 3xn matrix containing the points +% if X is 4xn only the first 3 rows are used +% fig . figure handle +% color color of plotting; defaults to blue +% +% fig . return the figure handle +% +% $Id: drawcloud.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ + +function [fig] = drawcloud(X,fig,color) +if nargin < 3 + color = 'b'; % default color +end + +figure(fig), hold on +plot3(X(1,:),X(2,:),X(3,:),[color,'o']) + + +view([1,1,1]); +axis('equal'); +grid on +rotate3d on +fig=fig; +return diff --git a/MultiCamValidation/InputOutputFunctions/drawobject.m b/MultiCamValidation/InputOutputFunctions/drawobject.m new file mode 100644 index 0000000..3d5834d --- /dev/null +++ b/MultiCamValidation/InputOutputFunctions/drawobject.m @@ -0,0 +1,37 @@ +% drawobject ... draws a (calibration) object of a specific type +% +% [fig] = drawobject(X, ctype, fig, {color}) +% +% X ...... 3xn matrix containg the object's corner points +% if X is 4xn only the first 3 rows are used +% ctype .. a string specifying the type of the object +% according to the type the appropriate drawing functions is called +% supported types are: +% 'cube' - a cube +% 'octagon' - a planar octagon +% 'cloud' - a point cloud +% fig .... figure handle +% color .. color of plotting; defaults to blue +% +% fig .... returns the figure handle +% +% $Id: drawobject.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ + +function [fig] = drawobject(X,ctype,fig,color) + +if nargin < 4 + color = 'b'; % default color +end + +switch ctype +case 'cube', + drawcube(X,fig,color); +case 'octagon', + drawoctagon(X,fig,color); +case 'cloud', + drawcloud(X,fig,color); +otherwise, + error('unknown object type: ', ctype) +end + +return diff --git a/MultiCamValidation/InputOutputFunctions/drawscene.m b/MultiCamValidation/InputOutputFunctions/drawscene.m new file mode 100644 index 0000000..9f922d1 --- /dev/null +++ b/MultiCamValidation/InputOutputFunctions/drawscene.m @@ -0,0 +1,61 @@ +function drawscene(X,C,R,fig,ctypehint,scenetitle,camsId) + +% drawscene ... plots calibration points, cameras and their viewing axes +% +% drawscene(X,C,R,fig,ctypehint,scenetitle) +% +% X ............ 4xPOINTS matrix containg POINTS object points +% C ............ 3xCAMS matrix containing the camera centers (in world coord.) +% R ............ 3*CAMSx3 matrix containing camera rotation matrices +% (needed for drawing the viewing axes) +% fig .......... figure handle (defaults to 1) +% ctypehint .... calibration object type of X (defaults to 'cloud') +% scenetitle ... title of the plot (defaults to '') +% camsIs ....... 1xCAMS vector with cameas Id (default is 1:CAMS + +% $Author: svoboda $ +% $Revision: 2.0 $ +% $Id: drawscene.m,v 2.0 2003/06/19 12:07:12 svoboda Exp $ +% $State: Exp $ + +POINTS = size(X,2); +CAMS = size(C,2); + +if nargin < 7 + camsId = [1:CAMS]; +end + +if (nargin < 3) + error('not enough input arguments'); +end +if (nargin < 5) + scenetitle = ''; +end +if (nargin < 4) + ctypehint = 'cloud'; +end + +figure(fig); clf +title(scenetitle) +grid on +axis equal + +% plot camera positions (blue) +drawcloud(C,fig,'b'); + +% plot calibration object (red) +drawobject(X,ctypehint,fig,'r'); + +% Mean of all points +centroid = mean(X(1:3,:)'); + +% plot viewing axes +for i=1:CAMS + axis_dir = -R(3*i,:); % 3rd row of i-th rotation matrix + axis_len = 0.6*norm(C(1:3,i)-centroid'); + endpoint = C(1:3,i)+axis_len*axis_dir'; + line([C(1,i),endpoint(1)],[C(2,i),endpoint(2)],[C(3,i),endpoint(3)]); + text(C(1,i),C(2,i),C(3,i),sprintf('%4d',camsId(i)),'Color','k'); +end + + diff --git a/MultiCamValidation/InputOutputFunctions/showimg.m b/MultiCamValidation/InputOutputFunctions/showimg.m new file mode 100644 index 0000000..92c5748 --- /dev/null +++ b/MultiCamValidation/InputOutputFunctions/showimg.m @@ -0,0 +1,30 @@ +%SHOWIMG Pajdla: Shows scaled images in the gray palette +% +% +% function f = showimg(m,f) +% +% m = image matrix +% f = figure handle +% +% See also: IMAGE, IMAGESC, COLORMEN. + +% Author: Tomas Pajdla, Tomas.Pajdla@esat.kuleuven.ac.be +% pajdla@vision.felk.cvut.cz +% 03/06/95 ESAT-MI2, KU Leuven +% Documentation: +% Language: Matlab 4.2, (c) MathWorks +% +function f = showimg(m,f); + +if (nargin==2) + figure(f); +else + f=figure; +% colormen; + colormap('gray'); +end + +imagesc(m); +axis('image'); + +return diff --git a/MultiCamValidation/Ransac/FDs.m b/MultiCamValidation/Ransac/FDs.m new file mode 100644 index 0000000..27297c2 --- /dev/null +++ b/MultiCamValidation/Ransac/FDs.m @@ -0,0 +1,11 @@ +%FDs error trem for fundamentel m. - squares of Sampson's distances +%function Ds = FDs(F,u) +%where u are corespondences and F is fundamental matrix + +function Ds = FDs(F,u) +rx1 = (F(:,1)' * u(1:3,:)).^2; +ry1 = (F(:,2)' * u(1:3,:)).^2; +rx2 = (F(1,:) * u(4:6,:)).^2; +ry2 = (F(2,:) * u(4:6,:)).^2; +r = Fr(F,u); +Ds = r ./ (rx1 + ry1 + rx2 + ry2); diff --git a/MultiCamValidation/Ransac/fFDs.c b/MultiCamValidation/Ransac/fFDs.c new file mode 100644 index 0000000..90dd385 --- /dev/null +++ b/MultiCamValidation/Ransac/fFDs.c @@ -0,0 +1,45 @@ +#include +#include "mex.h" +#include "matrix.h" + +#define f1 (*F) +#define f2 (*(F+1)) +#define f3 (*(F+2)) +#define f4 (*(F+3)) +#define f5 (*(F+4)) +#define f6 (*(F+5)) +#define f7 (*(F+6)) +#define f8 (*(F+7)) +#define f9 (*(F+8)) + +#define u1 (*(u)) +#define u2 (*(u+1)) +#define u4 (*(u+3)) +#define u5 (*(u+4)) + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *F = mxGetPr(aSrcP[0]); + double *u = mxGetPr(aSrcP[1]); + double *p; + double rx, ry, rwc, ryc, rxc, r; + int len = mxGetN(aSrcP[1]); + int i; + + aDstP[0] = mxCreateDoubleMatrix(1, len, mxREAL); + p = (double *)mxGetData(aDstP[0]); + + for (i=1; i<=len; i++) + { + rxc = f1 * u4 + f4 * u5 + f7; + ryc = f2 * u4 + f5 * u5 + f8; + rwc = f3 * u4 + f6 * u5 + f9; + r =(u1 * rxc + u2 * ryc + rwc); + rx = f1 * u1 + f2 * u2 + f3; + ry = f4 * u1 + f5 * u2 + f6; + + *p = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); + p ++; + u += 6; + } +} diff --git a/MultiCamValidation/Ransac/fFDs.mexglx b/MultiCamValidation/Ransac/fFDs.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..16e162eabb53a0c82bec156d4fdbd32e09ded9e1 GIT binary patch literal 6228 zcmeHLeQaA-6~DIAE|AcqB^%ksI(K6%1N3l07zIX^FFTFoe2^xkQ$t@J`z3Z|`$hIk zl@8E?9v=-MBVrY*#58p(NEJdN(k3KU1WB8%&G=CML29I=R0t#o(8R=q0t%Sl@4k1P z7bla(AM77E*73Q&bI$#~=idA7)8S~u=kp0g0TB>HlY50|hOB*$V`&yaSQ^CrqF&mS zKjo=+IlPCUR!IA4=uY43@KF?a8jK>P-49{GZoy720F!C20PhDs3?>}{zaRV{_#ik4 z_7bv7y5FOG0O^N3_?<}B?^4NqyXeYQ7hHui>O!P3PNaa~C<*agKnh?d@eK|TC?H2l zh~*NNqc18Q0lOS+aiWCplKC!{aHYh{C5#CHZItl4CFVLH1w@sUA;%54TH5bp13jmlpCCx46pMo9&j(cu?dx<2=JqVxD3t`P1WKXHrY`1)bL z@>wCA)w4CnK3IMPJLe<>&OeyK%HAWETwQG{g#}(AegjNmJ>P~@b2#+-g9wO#_^q@G zx%V-`5?CCRX;)sv`X#;qxfi23zCMe77A_)Ph@|TnQb8zFEgH# zOs?N4f-bXXNt)M}CFY1Fsa&6zm^)sQf9rD+bEK0&;!6@UnQS1QmG}e19NP6sU`R=2 z?Y)C2pSBOf^woLGUtC6Gr~WqUp)&tD_}f=E|Ahj_6ZX|+mT77B;+xauRK7<(Ic^uDk|bx^)xINWqW) z3%CEiR&UQ&+BZUNY|}Go%)yLNIC)CP;J@@iv{20al-oAvp)@p?adDWo1$;4dm?JJ5 zIj*%^!}m0NW?uXYeC(_IFIv^pdyC8VtDE+b;`o~ zf8%|CM>6jTJfU9)^L$}o@`k|k5g=a(A3AXmtbvb!d%&l_Sujc6BOHYP z&i3|C1sghh1|JJHX`lSK)*NgKg&G?}p~r&_9ma4fmJN>Oa2}?OSl)Q-f6kv9HHm8~ z)To6TA>Rp|(DLUpR%{s3$~iKrQps38DYV47EL z?j*+2me5k!6#gw^0{^%OrC>4>yrgK(zmarqLbAG8CKVT2+%j`{U`LFa$eAC`LnHs< zW+r1~q05?SJHCi#|&A!N)lRt|v~pNuQUEI97od+|`@#3Dv?_O0G zRI+W9&w!O5zlp}+r|P0zr7t4QF=2V~`xt%&7eh!tj^7JlRUW^ghA_C)D^lfMfkOH5 z+bZ!Eemr-SAJ339{CHJS#PY~5f$2x)x7ZawtTNL(aFj!hY=7 zL1luhz_&l=dCueiVPJmSk$iaT_$Z`$yYOMB_zZB;WA6f1`B}aWtlma^qDJI^54#JT zcD@}jZWuE@akqSH==0d0h5oz;KM%}rOcLAkLrC?u;=|1^NP|nNKX*zSMwN_Lg<1bH zd`~kNK+Mnj>KF_l+F9RL`}c`8V0F)B|NpqnUV}26y%qvgB{rOd{8#Pa+lWI|qV$0X zg|g3k+FOkR*Me@?I`F(Fe*&1)0OmO{<>E(?ehB;n7pu3W1sB7F(S4)%ii;n1{R_bCFQ=4m zMSpeeyi2hBDt8Z3_z__5+ft)zS8q$*E=E=n9psyNyKwj z-Wnen(c)qZuQs~gG0@wuN4t&<;310H(uRe6&3uJ2_0hCB982r+j-|)OC!h@<)g%3_ z-C?~g+}YKGOuVk?iSbP4oG^#a7;#H$Y!uG3m80d|&UF|ub8$nrOkLjUbiLhExwk@j zw{t6$KH;7Y`A{ZXtANuE2g#?YIN!9~J19D+Zlij~lRd57UG3~s-qe$^Y$AdTp6ZNjV%K-ImKNs?t@P}*>uX(0 zi+VB(xsP-^^6_2mEnWb(<=%R{b6@>-lqB^XxJ2)?c-{=RKR#|%!8g6KcglYQBq~hM literal 0 HcmV?d00001 diff --git a/MultiCamValidation/Ransac/fslcm.c b/MultiCamValidation/Ransac/fslcm.c new file mode 100644 index 0000000..80bcb1f --- /dev/null +++ b/MultiCamValidation/Ransac/fslcm.c @@ -0,0 +1,68 @@ +#include +#include "mex.h" +#include "matrix.h" + +#define a11 (*A) +#define a12 (*(A+1)) +#define a13 (*(A+2)) +#define a21 (*(A+3)) +#define a22 (*(A+4)) +#define a23 (*(A+5)) +#define a31 (*(A+6)) +#define a32 (*(A+7)) +#define a33 (*(A+8)) + +#define b11 (*B) +#define b12 (*(B+1)) +#define b13 (*(B+2)) +#define b21 (*(B+3)) +#define b22 (*(B+4)) +#define b23 (*(B+5)) +#define b31 (*(B+6)) +#define b32 (*(B+7)) +#define b33 (*(B+8)) + + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *A = mxGetPr(aSrcP[0]), *B = mxGetPr(aSrcP[1]); + double *p; + int i; + + aDstP[0] = mxCreateDoubleMatrix(4, 1, mxREAL); + p = mxGetPr(aDstP[0]); + + *p = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - + b11*b23*b32 - b12*b21*b33 + b11*b22*b33; + + *(p+1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - + a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + + a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + + 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - + a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - + 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + + (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - + 3*b11*b22)*b33; + + *(p+2) = -(a21*a33*b12) + a21*a32*b13 + + a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - + 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - + 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - + a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + + 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + + 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - + 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - + 3*b11*b23*b32 + a23* + (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - + a11*b32 + 2*b11*b32) + + (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - + 2*a11*b22 + 3*b11*b22)*b33 + + a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + + a11*b33 - 2*b11*b33); + + for (i=0; i < 9; i++) + B[i] = A[i] - B[i]; + + *(p+3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - + b11*b23*b32 - b12*b21*b33 + b11*b22*b33; + } diff --git a/MultiCamValidation/Ransac/fslcm.mexglx b/MultiCamValidation/Ransac/fslcm.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..b600a4b8f7345d7193fb014ce29da75d6512691c GIT binary patch literal 6744 zcmeHLdu*Fm6~B(NHqbWpM!RKOw|Nhkr&Y!&p#-!IC#hpHP^I+`gb*q!5|yY_qyZ{zCDJxY6ILKpXe5$tRntID+x3A8il%`1{qA?K z^EK_H0TM!j6P@qebMCq4o_p@YC+F*-)@GN>B{aB&Tafdt5TXvWZ>`-^C%owKi3dfE z?AQ9FCsx^b7a5O`{ih&1^`MPG80-PUr0lN%nL+;y`iVvGMD|w#?*={!Bs>Cy3T74J zA>hM6C)}@?2OPo;XnP&}e`E8Ya>|Z2;*9tZ+=9^&!g5Oj-fnDVU;u6zAt4Tyc^P$a zzRb%giw(9I`YYwQ9c8{s^4}=)ACdggGXGJ@bN>l$fsF#}wF&sul0Q`D*GQgwO>m29 zNza#g`v7sD32q(`z;SL8z%6%Mh_Nz%kCac9`Hx9{vCOYq#;=#W{n8CPy$0hdu_k~!u&gI75nWK z3h^rB9Iv((Ep;vpV<3@A8W}5+whTiUMszr0^v9BscZ>B72xB4^jXu^+i1My+e7U~h<)|SR5W0Swu|EY32pscmDcZIgLH?+!C z{xct*=n9EE`#gp&^;bXjCpUMSTYp5EWGpMtf8>io*e79W|6J(#KKdE|05J1N42SzJ z@e3vM#9}zCm0|)sf%Cius`qQhyAR`VyTxy1ul;uMwv6HIl1?dl1rCufg08@!xK4_` zo1{2A3#48leh<1557#Uy^m3%oyGV-1>|IjmcklaS_R6|#MVJ)ZUprfWA52@c|9Pyo zxE~?eK7AH^=}lhp(qYa`C>`cq9AtC;VhM1VD;Z?>{EXxop+O4g$0W~JbdaCFesj#KD9yl;eSbaRfMvda1^U^Oi+_i)p}y?NjWlWbv*R~M zvQMB{3KU<*MA>oA<@-l}&d!nS0+dHPEBC#@#)v4**rIH`71aQ z_z8T^WjO`U!pN74(`QF_FZcTeHXXbvDYp=<**B4IbS14(H;gXUD3-fc+a##@k zHTiImDpeQ(E8%)F9x&2MHT0kq4k)QtT7lGO8^I_a`+dp;=FW$`N;y!toDY{>EL_QF zXRpHR?gdOh6)%0A$cL94pUj8Qxx`ugd-;{@ z$MUHRE7fK8+}wF^7*bWZEcLMl8YrA*K)i)B{LJUt8I7JQOqA;g(g_3u&3ZV?6?$Y7 zgKU4PQd77jgExDcb1{I0aT!2cAcF|UF*3j@uI5+Ho@Zkc^UA=!hrrH`sdZ64E>(Sn zNvOg%U*W2)>MKmpa~bU`uxJnR+VN}lhujly=Qq6Fn0>$MYd0`J2MTll zi#MjHX4E{s{Uu}i?Bz9%*X#iAefer1Ln1no6?kv(l;Dk_qI?%TOV@t{I-_r1=c9I# z-;%X@WJPHvJGq#Bt~7LW`%AaKB|R8IgYx}b_Rc(mz5~S{y)UQaD`i7fu&tr1wV~0w zwRV#?8L?u!O>{8l8s`%ZIHK|~zKQ%zBi*m)K}Gr7$X^WRe$3kTVhn##*bkA+Ihpn` z7iAV$0MaafK$wLxCu4TXH<`aI@D#vX3oicC3gYizhF| zTosX$JYtq&Zi^MC=*x!%hMKyX>im+jGw|aA@EzcvffXv3)j2A4B!ruqKIip?+q<9g zZt{Qj)BZZ|ra+*!HW1kC^=&bGW09nHR~ork+>B(*r-b+))93yXc$NaS{y;70e?lhw znLPot`a?9ct@GsQHOUHCsxX+$NwjZywe)E zuLSO^zK^_%#74+CjzCI$4yf-j?=-OuoC$H5brrJcrV z*5m)f#9h$Uz!(!=0ct(IsYLZG%YHrYUZqFB4?&0MtXt^2^)02xc*zX1N#97n z4dlGk|p{M79sNDVz+KdVHWX%P7Ma6?iJ3VL-kpg_m@r{Fm+k^ko}@<+-6loL(>=?~YT^?2Sj=(Yb60zHO(wC8mQ zbaM)Qc6EeW*fW$ahV7>OYCZn8aZU{fq4yijfNDKb?yZLJ;YaWQoZ=n_f%K32$g}LU zw-Jlj=oqN&LG3o+n?U$~-H5w_vxoCSaLdfY&QWas->(}NpEGs#9EOCKc(!m|Eu76Z+K^1V!e1B^rEBx4e&MULQtRiB5NK5`g0Ui*X>-$_`K{; zpw|%7v1{dyR@&LLeD)#7RJx{g|bTU`sD@g-3I zS@7Dw3S{0*%le-Ouj`!De;NEb)P?kT6Fk8OWL~{r@lT@t81To6M-$P!Q97ph$5sCY z@XoqrUiEXywD%YA&bp;by+N<4@eeuPBrjxQAR66dAiLQWi<^CZ5sk#-j!LG8rmc)M z)Zg!qie0D>8pf8cj;%&(%eF2QIXNtDS{PTy3TMg~h^KlZaYN=%Mr3Fh($E)-=B*8F zA)_%AZfVCLrej9mP$IEMqH?*}haZ#C+F&Ihq#ZkkRNu^;R zAGFK7g7w{)FihLKaBD|nL#xrz+`KK+Wpp((wuVp#Hj{mKtlb8KcEz?_W4&W3I2hFR l*?_Fpen1`BsphB&tJ?8xQ$eL%U%s_ay<~{>;AJ)Ke*im*;D7)C literal 0 HcmV?d00001 diff --git a/MultiCamValidation/Ransac/fu2F7.m b/MultiCamValidation/Ransac/fu2F7.m new file mode 100644 index 0000000..7183863 --- /dev/null +++ b/MultiCamValidation/Ransac/fu2F7.m @@ -0,0 +1,33 @@ +function Fs = fu2F7(u) + +% Z = []; +% for j = 1:7 +% Z(j,:) = reshape(u(1:3,j)*u(4:6,j)',1,9); +% end + +Z = lin_fm(u); + +NullSp = null(Z); +if size(NullSp,2) > 2 + Fs = []; + return; %degenerated sample +end + +F1 = reshape(NullSp(:,1),3,3); +F2 = reshape(NullSp(:,2),3,3); +p = fslcm(F1,F2); +aroots = rroots(p); + +%xr = o_fslcm(F1,F2) + +%aroots == xr + +for i = 1:length(aroots) + l = aroots(i); + Ft = F1 * l + F2 * (1-l); +% Ft = Ft /norm(Ft,2); + Fs(:,:,i) = Ft; +end + + + diff --git a/MultiCamValidation/Ransac/lin_fm.c b/MultiCamValidation/Ransac/lin_fm.c new file mode 100644 index 0000000..f5e2878 --- /dev/null +++ b/MultiCamValidation/Ransac/lin_fm.c @@ -0,0 +1,32 @@ +#include +#include "mex.h" +#include "matrix.h" + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + double *u = mxGetPr(aSrcP[0]); + double *p, *s; + int len = mxGetN(aSrcP[0]); + int i,k,l,pos; + + aDstP[0] = mxCreateDoubleMatrix(len, 9, mxREAL ); + p = (double *)mxGetData(aDstP[0]); + + s = u; + for (i = 0; i < len; i++) + { + pos = 0; + for (k = 0; k < 3; k++) + { + for (l = 0; l < 3; l++) + { + *(p+pos) = *(s+k+3) * (*(s+l)); + pos += len; + } + } + s += 6; + p++; + } +} + + diff --git a/MultiCamValidation/Ransac/lin_fm.mexglx b/MultiCamValidation/Ransac/lin_fm.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..1bddd4469b34110fac6df59890e2e1c97ce85cb7 GIT binary patch literal 6102 zcmeHLZ){sv6~DH#wxd~_m9BJK+1zf>3?^e|p>znU{BzQXBn@dYHd^}XI4|+5Y`@BW zuH3=4q9@cvUQo2{Lj@Bms*w6&5*3?}pdYBzh_nKs2u)%iS`<(xoUKTm6iS!^=J&hr z-j^4rlg0=31&(!m?(dv)|KD@(efNcAUw0@J5}G0+B8X0{6QUJzy3w<=iYP42;z6-p z+O7eSMd_Ic>$w|l%F1s(;XNNEp4EW*ABJGlx>ro91tFZdBKX(xCG_+juv z;3zmq_bc#VK>2B;cLngrkwiDv&;x5UYZb35l{e}_WHC;ph~Ov*@mfR*U@!3!2M83A zBPGOJK9-{|8a)BK9Bpyf$LnRj*L;i#0KMhoyCq)s@h2qaIv_>FMkz0N*?_%;@mn9? zBki1XQbcT$@|KS`OU(HuMYvEv*VF_?nJ|!|r(+tVdzohVPNUoQZ!XuEh&nirDuXw-4HAv#xr2lDf8~V>CZo&9G14-gY zG5>P}AdUNxK8tBM14(~x1cdlDFw0X@kCa;HhB1+|^G4B06&%A5hLJ9(jBzWU%39Bw zfMaICoUn?HSun;6shnw8`EeWiJ85%fxx184JC>anxpI%`3>G{&0MS`6Q;ykXmqxQ@ zf66IXWzW`?a#8|0pE3(YX3ScnxpJ&%%Y^Btpo#6FF0E5}V>}l_#))iM^e3MdJ$=0$ zokmM65qrW<$5nB?1H;Lof%ZPx@BieFF~B%!05d+q!dtu{j3j^Of3|l6R|)rzU#NmP z#$npNF4PKIndcBJKZTw15(gI_wh$C|iQgz)lVxF%H;6v~lUT>gka`{u-*^Z?5fOiq zR&VcPc-+96r@X1;cC2CIt&r=mcy>^tkx|N>LOe=|1+fI%dgNKA1PkcbgBvK}zkw2) zVk0FMXA>pzk4(Pf-rRbyiX7t5cP}?$4658`ss?&rJr;)c>Sf^C8{9l<=hzlf?Hpq| z!{owz4Rnq@%h0?qCoxAeL*>FHiP1Qb;orgqi8<1lDDhc|nang3PfPqLF^6{H6fk6c zTaO~jEACzxzIVw9*XGdB$=lNbs_R=E{_e%)f1$9_n0v96MLIEeX0hTvj$|!fJ&lUo znWp#dpZYnC6?X~FQ-ckYuP{*&wK-3PN=tusFS2pM*^CmV+y-PFtZu{qtjj7edZO~J z>eb6rC+gdM9`P>QTGzg5=-rDP0pV&O0eY4MBSO?W8&D|gVo(st%rHAnq_jfl3bomF0w`M zeQxr0;&$R{;`i6Pu57>7ICp)=-y@}^4)=rb^M7r3{}F!v9rse)o!wANE>=dCo*Y^H z&fJvw_MsyN8a(ypXITc??KkZl4(^I8FkTo6j1i|z_N!^7`7gj{>Wy2Rl#BeYIgOQd zwK@0K%kI8fY4OmJwPVqTEG$sYU)?yxVVkc0<)5<3{`TR%_KxUKq9vM7Io1gi?c;e= zW$b|TS;cvL8BF;@C4ZtM1(>#wcgQ5-06OF;OT&vkU$I%O$c`;2k&vA^7#26===l%zfY|BR_ z4ljMR)MqbsThNJJ;1+NPcnCZSE`Uk;9QP3ZdpbM69Bu9y7`lNyC|3ZBg9R3=r- zh}hW4JX}2K6hv$yUy7Yj=XnqqW2?j??A9TBtg7XBTxjDK82EST&uUJ_#7 zznSrFBC@(v&Pt0|+OZ2oU{6fg$XOgMLL>jub}nb;q08Hj$pQR`#QQ!%M8+Cpll->A z5He;ND?7oAPsSBvmK+CQtYT}BevDnLSgE5QW1P&`*2H-wab9&?GA7C0&@ms0lDrSB ztPC5b1iq!_eU}BJ*+&A-5?EAGr`)Adk&fPvV87W zte1XyF#E)C=Uzwt4APo(`z$E5-!gp2r?e9!{m2ej`|+EB%yHJlF(y3=)_&Yu$-Fga z()FH#f_^N5`!<>TrY71*FM^rocyZ5~ho7zsD%m#5?}N1;ze#SvPuE4ePM<-VW5V*} z_bL3U3PVUgsKk%Kx;%cvT*IJKuSu789t!QpZ<`w*;>UAG`|%992|r#HG_gGLn_&8p z`7PA!eWn3y1L3Z1+H8$Mlp+K-?r`)H*Mo^h;G z(H zep|~5)L%sItSg#`{6Y@m2jn{79^sMqEv#`EJ9bo?ARF-Q&3V2M@c%q8zpF?gyfu6Q zQokL9uv2^$xEiqc0_*%N-vHKc6Ctr#6oB`t1x`D^-5ED5A|&pU-}-|A`zz4D7{EUQ z=Jy?m?RgziziotY^NZ4;NcZQczhP{Y@v1TFpM&o__5l*}v%XiAKkck=jUw-%MhZzaIo zk4d5MCh<71erpONs#}0LzS0i=eZV{mNMYQ-yXx#;1J-XrEI$R@7RWyiOlk)6oTw=pJC_xP38VXdZC2q&l>fWH>@TN)Z#92YcHSje{yl0B(s(y8$M1~te?r;y z+s*-nVHQ#TO{CK;25vvct*ki~6X{eo8+h#z>4H;qO5@|Pv^b786T|2lJ}_kT^&T9? zLlCoNO$Yf}`3htk6IpvSl{MrY%Se^V&?fgA-9zpDNuwj#(>s7nysjBzrCjc$ut$%X zX(yIQ2=Cd*)ADYo9LDWJ+B6*7kheO+=nPaItWe(VRE5$fInX5^s$^?5@Y>;_tRy5z0XFmPe?-ogYQ!7O2zUh{hT4s^8l z83($%45lJZZEgC=Dz2W%*5u>6)`z^ot;)Uf ge5T&r)|4dmUARWCwFF-6RzLr(RKYj3x|hO#15eZ%x&QzG literal 0 HcmV?d00001 diff --git a/MultiCamValidation/Ransac/normu.m b/MultiCamValidation/Ransac/normu.m new file mode 100644 index 0000000..914622b --- /dev/null +++ b/MultiCamValidation/Ransac/normu.m @@ -0,0 +1,19 @@ +function A = normu(u); + +% NORMU Normalizes image points to be used for LS estimation. +% A = NORMU(u) finds normalization matrix A so that mean(A*u)=0 and mean(||A*u||)=sqrt(2). +% (see Hartley: In Defence of 8-point Algorithm, ICCV`95). +% Parameters: +% u ... Size (2,N) or (3,N). Points to normalize. +% A ... Size (3,3). Normalization matrix. + +if size(u,1)==3, u = p2e(u); end + +m = mean(u')'; % <=> mean (u,2) +u = u - m*ones(1,size(u,2)); +distu = sqrt(sum(u.*u)); +r = mean(distu)/sqrt(2); +A = diag([1/r 1/r 1]); +A(1:2,3) = -m/r; + +return diff --git a/MultiCamValidation/Ransac/nsamples.m b/MultiCamValidation/Ransac/nsamples.m new file mode 100644 index 0000000..2b46243 --- /dev/null +++ b/MultiCamValidation/Ransac/nsamples.m @@ -0,0 +1,14 @@ +%SampleCnt calculates number of samples needed to be done + +function SampleCnt = nsamples(ni, ptNum, pf, conf) +q = prod ([(ni-pf+1) : ni] ./ [(ptNum-pf+1) : ptNum]); + +if (1 -q) < eps + SampleCnt = 1; +else + SampleCnt = log(1 - conf) / log(1 - q); +end + +if SampleCnt < 1 + SampleCnt = 1; +end diff --git a/MultiCamValidation/Ransac/p2e.m b/MultiCamValidation/Ransac/p2e.m new file mode 100644 index 0000000..440a7b8 --- /dev/null +++ b/MultiCamValidation/Ransac/p2e.m @@ -0,0 +1,3 @@ +function e = p2e (u) +e = u(1:2,:) ./ ([1;1] * u(3,:)); +return \ No newline at end of file diff --git a/MultiCamValidation/Ransac/rEG.m b/MultiCamValidation/Ransac/rEG.m new file mode 100644 index 0000000..a586813 --- /dev/null +++ b/MultiCamValidation/Ransac/rEG.m @@ -0,0 +1,83 @@ +function [F, inls] = rEG(u, th, th7, conf) + +%rEG Robust computation of epipolar geometry based on RANSAC +% +% [F, inls] = rEG(u, th, th7, conf) +% u point correspondences (6xn), where n is the number of corrs. +% th threshold value for the Sampson's distance (see FDs) +% th7 threshold for inliers to iterate on F (default = th) +% conf confidence level of self-termination (default = .95) + +MAX_SAM = 100000; +iter_amount = .5; + +if nargin < 3 + th7 = th; +end; + +if nargin < 4 + conf = .95; +end; + +len = size(u,2); +ptr = 1:len; +max_i = 8; +max_sam = MAX_SAM; + +no_sam = 0; +no_mod = 0; + +while no_sam < max_sam + for pos = 1:7 + idx = pos + ceil(rand * (len-pos)); + ptr([pos, idx]) = ptr([idx, pos]); + end; + + no_sam = no_sam +1; + + aFs = fu2F7(u(:,ptr(1:7))); + + for i = 1:size(aFs,3) + no_mod = no_mod +1; + aF = aFs(:,:,i); + Ds = fFDs(aF,u); + v = Ds < th; + v7 = Ds < th7; + no_i = sum(v); + + if max_i < no_i + inls = v; + F = aF; + max_i = no_i; + max_sam = min([max_sam,nsamples(max_i, len, 7, conf)]); + end + + if sum(v7) >= 8 + iter_amount*(max_i - 8) + aF = u2F(u(:,v7)); + Ds = fFDs(aF,u); + v = Ds < th; + no_i = sum(v); + if max_i < no_i + inls = v; + F = aF; + max_i = no_i; + exp_sam = nsamples(max_i, len, 7, .95); + max_sam = min([max_sam,exp_sam]); + end + end + end +end + +if no_sam == MAX_SAM + warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); +end; + + + + + + + + + + diff --git a/MultiCamValidation/Ransac/rroots.c b/MultiCamValidation/Ransac/rroots.c new file mode 100644 index 0000000..0829b29 --- /dev/null +++ b/MultiCamValidation/Ransac/rroots.c @@ -0,0 +1,71 @@ +#include +#define rr_a (*po) +#define rr_d (*(po + 3)) +#define eps (2.2204e-016) + +#include "mex.h" +#include "matrix.h" + + +void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) +{ + + double *po = mxGetPr(aSrcP[0]); + + double b,c, b2, bt, v, pit, e, *r; + double p, q, D, A, cosphi, phit, R, _2R; + + b = *(po + 1) / rr_a; + c = *(po + 2) / rr_a; + b2 = b*b; + bt = b/3; + + p = (3*c - b2)/ 9; + q = ((2 * b2 * b)/27 - b*c/3 + rr_d/rr_a) / 2; + + D = q*q + p*p*p; + + if (D > 0) + { + aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); + r = mxGetPr(aDstP[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; + } + } else + { + /* if (p > -eps) + { + printf("%.17f\n", p); + aDstP[0] = mxCreateDoubleMatrix( 1, 1, mxREAL ); + r = mxGetPr(aDstP[0]); + *r = pow(q,1.0/3) - bt; + } + else */ + { + + aDstP[0] = mxCreateDoubleMatrix( 3, 1, mxREAL ); + r = mxGetPr(aDstP[0]); + 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; + } + } +} diff --git a/MultiCamValidation/Ransac/rroots.mexglx b/MultiCamValidation/Ransac/rroots.mexglx new file mode 100755 index 0000000000000000000000000000000000000000..4ab396672979242d42960d169b422661ef33bc76 GIT binary patch literal 6903 zcmeHLZ){uD6~B%{n$gmx5THN@1_~6$Jg3VT14ffJZt0e^S!odn!mH!F#O@rwU_W=^ z&qOwZpvtmUf)CR^Y;0^&HB}l?&{Pp5!Wz+}i;yREl8yXA#+c;L4kc2BsxHI({qDQ> z^(7RI?E_!7W1V;I@1Ar2-}Bz_c{1r^{J=%cM zjUM{%3)d}cdIRwFs_t5!?u)+i=UH6u8*H5_U*)U2Q1L}o>7J$#6WD)JP;mE!XyitN zc;2KMd3eOGqTR!8;TaFhQxs=BjEe$lcB|38UgD=b`~``B?cs|h=KUfC1ug<;pDTb@ zNj&7?OC;u9A_c{2Da#&qFDUO3DaZ>5npB$>6l*0ec=%F@iyppA;`QoqgQ9U6zFcDc z2#PCoQOcmWQVgk@n<3eIOyP|Re_i2QAbGc*RJe#Uq5Xuy6l=voRo_S6ZQ4Hrp2xQF z%rC`$Jp@VO7P0*S0?-p+;tuctj^QyJHSOIUamQtUPlHLE2kq6>V3i2*VHkb=R@%tg zv5ajPfbqeY(VI-iQpqPwz#cPW_9e5nnK61Zv3}D?rh6^aU#Pa<9Nd;m$L*w*7X5?U zO}jG#xiw?PY_rYEb*IdZn4L)uitJ+++qvBTjOZa3 z;hxptS%Wf5b$D_g-CzB4zg^8+#4GlD)j9XL2KvL_f$yn0eeMBhj-Za`6ai+2!w@Um^71UFLR;;f_GAjJDv*b2OeDwMdCrzv4SPl>+HP{Q^sB`)(KWe6RkZUy?O zrNrgld+=}h(~b96pb-!M;8-U-tMWR^mFKWql}s&ar;Y(vM^O~jmpM05^<};z5-iTm zRzWXwr3tF%3KDaN6ReyYmzZzO1Rry+NQ?=IM2Pq$iCIi+Bt9fD7AX=D;vtEtO5902 zAo1X#lFlbVhgIdlhs_IavUb;(TNsk=JFGv(uc*8?l>2!j!t6|)zL{j z^QY^b^&2XEuy>-f#f6!7P^uLFq=&NCIz_i3Yji?T59M8`|BPS;Vn zkJHuMWaT>AbsUu|#)_bDqVr{QQq4+8^*Gg!)+=RHEC2crwsn;aBj(|UADudO>Jp!~ z9pb(8?%uly4Tkdq2ZO7Qy`iDJw+-j-=NN28M(62tV|-TaRl{}FLjJAA{JquO8IG|0 zu0M|^>`?mWGCxQ`cc`)gi+}~s#-;ieSKgrH-Ac}G-YepH@ewfJq>R-VW$i;7zG+_s zGrVJr$gqvEBcryzgV`-#to+_FhN^+Q4-7Y!k=B)vj0Ley86z^jXaO_Ist04`7Gq1? zSc`4p?iOQ9?5xE;vBwr;(Q}ZT%V#AX4Q;j5@gVD7!+cJF-vNIBJ_o)S@#odx+rW2$ zNjk=K5uV$(ZoMnCar@4_w}v)_zwxzjb7)f}($o}*+!5N?W_BlI>Ciw1u~5p4WzAdv z=j?f&B;KV+Q#jHD`FYfdaQ5+jJJt>75aFK3(=c(Roe|-_bS`{Ag_C|@ z^eoXCFvV%Ixmy@W*&>`wC-Jn+K|B!zC!w->Vs;GO%cq%eqhL8)tUnnS;ka#OvcRtB zv(Pfzokfj&#I62*GmW~mWt-f<&q#1@>6i2y`Y1Vq%_k<&U&z;k>8Grx?~*$K=wA$i zXhR>yjg@t@qpy?cmn2x-UrW`eaH-#z5}P{;oH-eP~WOM|&4sO3A4@4cH) z)}+TxqC(s8e=GSVZ3Ia>vJKXD{69->(49cXJtpygnYQCQmaJaWQm>l~DLc-42sUKy zvnK6Z&!d95@-;yHi+Sq|2&XxQsnB$W9KYcwvRz6}q=`}Ft z!*yso-gixU?Qft$x1o;xg?tsdI0>ElG|)`kgVe*IEV&PD$8R0S)buLs-qrw8+fnjt zHNB3HVgZxFUnhC5IX|8w@3OzYA7K&S^)=M}!R!;ncLM4Ec^{q%_E$MtP-Z&Zn1%I0 z8Iid6#Knyi^4_@k-{*Z~1N8{XH|zPDpv>0z@zwGk=)4!-&^&96Yk))N&2jG+%9V4$C&}DF4CSaXsr2W0X`@Hq75^c-$-v-usO4`SOn{hrQ z;$C3V&0xmaIfXYW{4<4b0lon|rm)UCo=_Mn?Dj(SZG~@C_6xxNyyK$MbIKgAQN5G+ zeIwog?9V$!l>SNRIDWei)BX{KQ4}FQ`r`3T2BM>ZWXkLbi+C)R@?|1LJY#3=TyJkU zE(Va&GmN%9yLKDxJMQ0uBqf`rOdIW*nW40dzLeD+OBpf-Gh(?x)JE?!w(V~1h#Fg> z+js0lBZhNEPp-fJabb1uH{*7=sY$pgO}Cb@pEBsRGI7(eEkj0qhOyN*xPL$y`>6q? zO>}3QOrpx!YT(YpMJjU{XVOzesaT?pSBw!Qp2AZnOWgwO}upS(!BI}E9QHdL_S{=!EBbKwEqP` CO^A#D literal 0 HcmV?d00001 diff --git a/MultiCamValidation/Ransac/seig.m b/MultiCamValidation/Ransac/seig.m new file mode 100644 index 0000000..6822a19 --- /dev/null +++ b/MultiCamValidation/Ransac/seig.m @@ -0,0 +1,5 @@ +%seig sorted eigenvalues +function [V,d] = seig(M) +[V,D] = eig(M); +[d,s] = sort(diag(D)); +V = V(:,s); diff --git a/MultiCamValidation/Ransac/u2F.m b/MultiCamValidation/Ransac/u2F.m new file mode 100644 index 0000000..f942863 --- /dev/null +++ b/MultiCamValidation/Ransac/u2F.m @@ -0,0 +1,42 @@ +%u2F estimates fundamental matrix using ortogonal LS regression +% F = u2F(u) estimates F from u using NORMU +% F = u2F(u,'nonorm') disables normalization +% see also NORMU, U2FA + + +function F = u2F (u, str) + +if (nargin > 1) & strcmp(str, 'nonorm') + donorm = 0; +else + donorm = 1; +end + +ptNum = size(u,2); + +if donorm + A1 = normu(u(1:3,:)); + A2 = normu(u(4:6,:)); + + u1 = A1*u(1:3,:); + u2 = A2*u(4:6,:); +end + +for i = 1:ptNum + Z(i,:) = reshape(u1(:,i)*u2(:,i)',1,9); +end + +M = Z'*Z; +V = seig(M); +F = reshape(V(:,1),3,3); + +[uu,us,uv] = svd(F); +[y,i] = min (abs(diag(us))); +us(i,i) = 0; +F = uu*us*uv'; + +if donorm + F = A1'*F*A2; +end + +F = F /norm(F,2); diff --git a/MultiCamValidation/gorec.m b/MultiCamValidation/gorec.m new file mode 100644 index 0000000..46ca393 --- /dev/null +++ b/MultiCamValidation/gorec.m @@ -0,0 +1,173 @@ +% the main script for the multicamera validation +% reads the points and the camera matrices +% and does 3D reconstructions +% evaluates the reprojection errors +% to check whether the P matrices still hold or not +% +% $Id: gorec.m,v 2.1 2005/05/23 16:22:51 svoboda Exp $ + +clear all; + +% add necessary paths +addpath ../CommonCfgAndIO +addpath ./CoreFunctions +addpath ./InputOutputFunctions +addpath ../RansacM; % ./Ransac for mex functions (it is significantly faster for noisy data) + +% get the configuration +config = configdata(expname); + +UNDO_RADIAL = logical(config.cal.UNDO_RADIAL | config.cal.UNDO_HEIKK); + +if UNDO_RADIAL + % add functions dealing with radial distortion + addpath ../RadialDistortions +end + +% read the input data +loaded = loaddata(config); +linear = loaded; % initalize the linear structure + +CAMS = size(config.cal.cams2use,2); +FRAMES = size(loaded.IdMat,2); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% See the README how to compute data +% for undoing of the radial distortion +if config.cal.UNDO_RADIAL + for i=1:CAMS, + [K,kc] = readradfile(sprintf(config.files.rad,config.cal.cams2use(i))); + xn = undoradial(loaded.Ws(i*3-2:i*3,:),K,[kc,0]); + linear.Ws(i*3-2:i*3,:) = xn; + end +elseif config.cal.UNDO_HEIKK, + for i=1:CAMS, + heikkpar = load(sprintf(config.files.heikkrad,config.cal.cams2use(i)),'-ASCII'); + xn = undoheikk(heikkpar(1:4),heikkpar(5:end),loaded.Ws(i*3-2:i*3-1,:)'); + linear.Ws(i*3-2:i*3-1,:) = xn'; + end +end + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Detection of outliers +% RANSAC is pairwise applied +disp('RANSAC validation step running ...'); + +inl.IdMat = findinl(linear.Ws,linear.IdMat,config.cal.INL_TOL); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% fill cam(i) structures +for i=1:CAMS, + cam(i).camId = config.cal.cams2use(i); + cam(i).ptsLoaded = find(loaded.IdMat(i,:)); % loaded structure + cam(i).ptsInl = find(inl.IdMat(i,:)); % survived initial pairwise validation + cam(i).xgt = loaded.Ws(3*i-2:3*i,cam(i).ptsLoaded); + cam(i).xlin = linear.Ws(3*i-2:3*i,cam(i).ptsLoaded); + cam(i).xgtin = loaded.Ws(3*i-2:3*i,cam(i).ptsInl); + cam(i).P = loaded.Pmat{i}; + [cam(i).K, cam(i).R, cam(i).t, cam(i).C] = P2KRtC(cam(i).P); +end + +% estimate the working volume which is +% the intersection of the view cones +disp('Computing maximal possible working volume') +tic, +[workingvolume.Xmat,workingvolume.idxisa] = workvolume(cam); +toc +% plot3(workingvolume.Xmat(workingvolume.idxisa,1),workingvolume.Xmat(workingvolume.idxisa,2),workingvolume.Xmat(workingvolume.idxisa,3),'.') +Rmat =[]; +for i=1:CAMS + Rmat = [Rmat;cam(i).R]; +end +drawscene(workingvolume.Xmat(workingvolume.idxisa,:)',[cam(:).C],Rmat,3,'cloud','Maximal working volume',[cam(:).camId]); +drawnow + + +disp('***********************************************************') +disp('Computing a robust 3D reconstruction via camera sampling ...') +% compute a reconstruction robustly + +t1 = cputime; +reconstructed = estimateX(linear,inl.IdMat,cam,config); +reconstructed.CamIds = config.cal.cams2use(reconstructed.CamIdx); +t2 = cputime; +disp(sprintf('Elapsed time for 3D computation: %d minutes %d seconds',floor((t2-t1)/60), round(mod((t2-t1),60)))) + +% compute reprojections +for i=1:CAMS, + xe = linear.Pmat{i}*reconstructed.X; + cam(i).xe = xe./repmat(xe(3,:),3,1); + + % these points were the input into Martinec and Pajdla filling + mask.rec = zeros(1,FRAMES); % mask of points that survived validation so far + mask.vis = zeros(1,FRAMES); % maks of visible points + mask.rec(reconstructed.ptsIdx) = 1; + mask.vis(cam(i).ptsLoaded) = 1; + mask.both = mask.vis & mask.rec; % which points are visible and reconstructed for a particular camera + unmask.rec = cumsum(mask.rec); + unmask.vis = cumsum(mask.vis); + cam(i).recandvis = unmask.rec(~xor(mask.rec,mask.both) & mask.rec); + cam(i).visandrec = unmask.vis(~xor(mask.rec,mask.both) & mask.rec); + cam(i).err2d = sqrt(sum([cam(i).xe(1:2,cam(i).recandvis) - cam(i).xlin(1:2,cam(i).visandrec)].^2)); + cam(i).mean2Derr = mean(cam(i).err2d); + cam(i).std2Derr = std(cam(i).err2d); +end + +% plot measured and reprojected 2D points +for i=1:CAMS + figure(i+10) + clf + plot(cam(i).xgt(1,:),cam(i).xgt(2,:),'ro'); + hold on, grid on + plot(cam(i).xgtin(1,:),cam(i).xgtin(2,:),'bo'); + plot(cam(i).xlin(1,:),cam(i).xlin(2,:),'go'); + plot(cam(i).xe(1,:),cam(i).xe(2,:),'k+') + title(sprintf('measured, o, vs reprojected, +, 2D points (camera: %d)',config.cal.cams2use(i))); + for j=1:size(cam(i).visandrec,2); % plot the reprojection errors + line([cam(i).xlin(1,cam(i).visandrec(j)),cam(i).xe(1,cam(i).recandvis(j))],[cam(i).xlin(2,cam(i).visandrec(j)),cam(i).xe(2,cam(i).recandvis(j))],'Color','g'); + end + % draw the image boarder + line([0 0 0 loaded.Res(i,1) loaded.Res(i,1) loaded.Res(i,1) loaded.Res(i,1) 0],[0 loaded.Res(i,2) loaded.Res(i,2) loaded.Res(i,2) loaded.Res(i,2) 0 0 0],'Color','k','LineWidth',2,'LineStyle','--') + axis('equal') +end + +% plot the 3D points +figure(100), +clf +plot3(reconstructed.X(1,:),reconstructed.X(2,:),reconstructed.X(3,:),'*'); +grid on + +figure(31) +clf +bar(config.cal.cams2use,[cam.mean2Derr;cam.std2Derr]',1.5) +grid on +xlabel('Id of the camera') +title('2D error: mean (blue), std (red)') +ylabel('pixels') + +%%% +% print the results in a text form +reconstructed + +%%% +% save the data for non-linear estimation +% the idea is to apply the caltech non-linear optimization +% or any other alternative traditional calibration method to the +% robustly reconstructed points. These 3D points will play the role +% of a calibration grid. +% +% It is also assumed that the majority of the cameras are good to produce +% acceptable 3D points. Othewise, we are trying to perform the calibration by using +% a bad calibration grid +% +% This assumes sufficient overlap between cameras. No point filling applied +% +% 3D-2D correspondences is needed for each camera +for i=1:CAMS, + xe = loaded.Ws(i*3-2:i*3, reconstructed.ptsIdx(logical(loaded.IdMat(i,reconstructed.ptsIdx)))); + Xe = reconstructed.X(:, logical(loaded.IdMat(i,reconstructed.ptsIdx))); + corresp = [Xe',xe']; + save(sprintf(config.files.points4cal,config.cal.cams2use(i)),'corresp','-ASCII'); +end + diff --git a/RadialDistortions/comp_distortion_oulu.m b/RadialDistortions/comp_distortion_oulu.m new file mode 100644 index 0000000..c2253af --- /dev/null +++ b/RadialDistortions/comp_distortion_oulu.m @@ -0,0 +1,48 @@ +function [x] = comp_distortion_oulu(xd,k); + +%comp_distortion_oulu.m +% +%[x] = comp_distortion_oulu(xd,k) +% +%Compensates for radial and tangential distortion. Model From Oulu university. +%For more informatino about the distortion model, check the forward projection mapping function: +%project_points.m +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + + +if length(k) == 1, + + [x] = comp_distortion(xd,k); + +else + + k1 = k(1); + k2 = k(2); + k3 = k(5); + p1 = k(3); + p2 = k(4); + + x = xd; % initial guess + + for kk=1:20, + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (xd - delta_x)./(ones(2,1)*k_radial); + + end; + +end; + + \ No newline at end of file diff --git a/RadialDistortions/isptnorm.m b/RadialDistortions/isptnorm.m new file mode 100644 index 0000000..7c22a29 --- /dev/null +++ b/RadialDistortions/isptnorm.m @@ -0,0 +1,43 @@ +% isptnorm ISotropic PoinT NORMalization +% +% [xnorm,T] = isptnorm(x); +% x ... [N x dim] coordinates +% +% xnorm ... normalized coordinates +% T ... transformation matrix used +% +% T. Svoboda, 5/2001 + + +function [xnorm,T] = isptnorm(x); + + +% data dimension +dim = size(x,2); +N = size(x,1); + +% make homogenous coordinates +x(:,dim+1) = ones(N,1); + +% compute sum of square diferences +for i=1:dim, + ssd(:,i) = (x(:,i)-mean(x(:,i))).^2; +end + +scale = (sqrt(dim)*N) / (sum(sqrt(sum(ssd')))); + +T = zeros(dim+1); + +for i=1:dim, + T(i,i) = scale; + T(i,dim+1) = -scale*mean(x(:,i)); +end +T(dim+1,dim+1) = 1; + +xnorm = T*x'; +xnorm = xnorm'; +% return nonhomogenous part of the points coordinates +xnorm = xnorm(:,1:dim); + + + diff --git a/RadialDistortions/readradfile.m b/RadialDistortions/readradfile.m new file mode 100644 index 0000000..6e88609 --- /dev/null +++ b/RadialDistortions/readradfile.m @@ -0,0 +1,35 @@ +% readradfiles reads the BlueC *.rad files +% +% *.rad files contain paprameters of the radial distortion +% [K,kc] = readradfiles(name) +% name ... name of the *.rad file with its full path +% +% K ... 3x3 calibration matrix +% kc ... 4x1 vector of distortion parameters +% +% $Id: readradfile.m,v 2.0 2003/06/19 12:07:16 svoboda Exp $ +function [K,kc] = readradfiles(name); + +fid = fopen(name,'r'); +if fid<0 + error(sprintf('Could not open %s. Missing rad files?',name')) +end + +for i=1:3, + for j=1:3, + buff = fgetl(fid); + K(i,j) = str2num(buff(7:end)); + end +end + +buff = fgetl(fid); +for i=1:4, + buff = fgetl(fid); + kc(i) = str2num(buff(7:end)); +end + +fclose(fid); + +return; + + diff --git a/RadialDistortions/undoheikk.m b/RadialDistortions/undoheikk.m new file mode 100644 index 0000000..8c37115 --- /dev/null +++ b/RadialDistortions/undoheikk.m @@ -0,0 +1,36 @@ +function p=imcorr(sys,par,dp) +%IMCORR corrects image coordinates, which are contaminated by radial +%and tangential distortion. +% +%Usage: +% p=imcorr(name,par2,dp) +% +%where +% name = string that is specific to the camera and the framegrabber. +% This string must be defined in configc.m +% par2 = camera intrinsic parameters for correcting the coordinates. +% these parameters are computed by using invmodel.m. +% dp = distorted image coordinates in pixels (n x 2 matrix) +% p = corrected image coordinates + +% Version 3.0 10-17-00 +% Janne Heikkila, University of Oulu, Finland + +NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4); +Asp=par(1); Foc=par(2); +Cpx=par(3); Cpy=par(4); +Rad1=par(5); Rad2=par(6); +Tan1=par(7); Tan2=par(8); + + +dx=(dp(:,1)-Cpx)*Sx/NDX/Asp; +dy=(dp(:,2)-Cpy)*Sy/NDY; + +r2=dx.*dx+dy.*dy; +delta=Rad1*r2+Rad2*r2.*r2; + +cx=dx.*(1+delta)+2*Tan1*dx.*dy+Tan2*(r2+2*dx.*dx); +cy=dy.*(1+delta)+Tan1*(r2+2*dy.*dy)+2*Tan2*dx.*dy; + +p=NDX*Asp*cx/Sx+Cpx; +p(:,2)=NDY*cy/Sy+Cpy; diff --git a/RadialDistortions/undoradial.m b/RadialDistortions/undoradial.m new file mode 100644 index 0000000..2107e30 --- /dev/null +++ b/RadialDistortions/undoradial.m @@ -0,0 +1,33 @@ +% undoradial remove radial distortion +% +% [xl] = undoradial(x,K,kc) +% +% x ... 3xN coordinates of the distorted pixel points +% K ... 3x3 camera calibration matrix +% kc ... 4x1 vector of distortion parameters +% +% xl ... linearized pixel coordinates +% these coordinates should obey the linear pinhole model +% +% It calls comp_distortion_oulu: undistort pixel coordinates. +% function taken from the CalTech camera calibration toolbox + +function [xl] = undoradial(x_kk,K,kc) + +cc(1) = K(1,3); +cc(2) = K(2,3); +fc(1) = K(1,1); +fc(2) = K(2,2); + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +if norm(kc) ~= 0, + % Third: Compensate for lens distortion: + xn = comp_distortion_oulu(x_distort,kc); +else + xn = x_distort; +end; + +% back to the linear pixel coordinates +xl = K*[xn;ones(size(xn(1,:)))]; diff --git a/RansacM/Fsampson.m b/RansacM/Fsampson.m new file mode 100644 index 0000000..08aab42 --- /dev/null +++ b/RansacM/Fsampson.m @@ -0,0 +1,21 @@ +function errs = Fsampson(F,u); +% FSAMPSON ... first order geometrical error (Sampson Distance) +% errs = Fsampson(F,u); +% F ... 3x3 Fundamental matrix +% u ... 6xN point pairs homogenous +% +% errs ... 1xN error for each point pair +% +% $Id: Fsampson.m,v 1.1 2005/05/23 16:15:59 svoboda Exp $ + +N = size(u,2); + +u1 = u(1:3,:); +u2 = u(4:6,:); + +errs = zeros(1,N); +for i=1:N + Fu1 = F*u1(:,i); + Fu2 = F'*u1(:,i); + errs(i) = (u2(:,i)'*F*u1(:,i))^2 / (sum([Fu1(1:2)'.^2,Fu2(1:2)'.^2])); +end diff --git a/RansacM/nsamples.m b/RansacM/nsamples.m new file mode 100644 index 0000000..6157544 --- /dev/null +++ b/RansacM/nsamples.m @@ -0,0 +1,16 @@ +% nsamples calculate the number of samples yet needed +% +% N = nsamples(no_i,ptNum,s,conf) +% no_i ... current number of inliers +% ptNum ... total number of points +% s ... sample size +% conf ... confidence value +% +% $Id: nsamples.m,v 1.1 2005/05/23 16:15:59 svoboda Exp $ + +function N = nsamples(no_i,ptNum,s,conf) + +outl = 1-no_i/ptNum; +N = log(1-conf) / log(1-(1-outl)^s+eps); + +return; \ No newline at end of file diff --git a/RansacM/pointnormiso.m b/RansacM/pointnormiso.m new file mode 100644 index 0000000..18a2237 --- /dev/null +++ b/RansacM/pointnormiso.m @@ -0,0 +1,28 @@ +function [u2,T] = pointnormiso(u); +% pointnormiso Isotropic point normalization +% +% [u2,T] = pointnormiso(u); +% u ... 3xN input data homogenous +% +% u2 ... 3xN normalized data homogenous +% T ... 3x3 transformation matrix the does the tranformation +% +% $Id: pointnormiso.m,v 1.1 2005/05/23 16:16:00 svoboda Exp $ + +n=size(u,2); + +xmean = mean(u(1,:)); +ymean = mean(u(2,:)); + +u2 = u; +u2(1:2,:) = u(1:2,:) - repmat([xmean;ymean],1,n); + +scale = sqrt(2)/mean(sqrt(sum(u2(1:2,:).^2))); + +u2(1:2,:) = scale*u2(1:2,:); + +T = diag([scale,scale,1]); +T(1,3) = -scale*xmean; +T(2,3) = -scale*ymean; + +return; diff --git a/RansacM/rEG.m b/RansacM/rEG.m new file mode 100644 index 0000000..277eb3a --- /dev/null +++ b/RansacM/rEG.m @@ -0,0 +1,75 @@ +function [F, inls] = rEG(u,th,th4,conf,ss) +% REG robust estimation of the epipolar geometry via RANSAC +% +% [H, inls] = rRG(u,th,{th4=th,conf=0.99,ss=8}) +% u ... 6xN pairs homogenous coordinates +% th ... inlier tolerance in pixels +% th4 ... currently not used +% conf ... confidence, the higher -> more samples +% ss ... sample size +% +% F ... 3x3 fundamental matrix +% inls ... 1xN logical 1->inlier. 0->outlier +% +% $Id: rEG.m,v 1.1 2005/05/23 16:16:00 svoboda Exp $ + + +MAX_SAM = 100000; % maimal number of random samples + +len = size(u,2); + +% parsing the inputs +if nargin < 3 + th4 = th; +end + +if nargin < 4 + conf = 0.99; +end + +if nargin < 5 + ss = 8; % sample size +end + +len = size(u,2); +ptr = 1:len; +max_i = 5; +max_sam = MAX_SAM; + +no_sam = 0; +no_mod = 0; + +th = 2*th^2; + +while no_sam < max_sam + for pos = 1:ss + idx = pos + ceil(rand * (len-pos)); + ptr([pos, idx]) = ptr([idx, pos]); + end; + + no_sam = no_sam +1; + + sF = u2Fdlt(u(:,ptr(1:ss)),0); + errs = Fsampson(sF,u); + v = errs < th; + no_i = sum(v); + + if max_i < no_i + inls = v; + F = sF; + max_i = no_i; + max_sam = min([max_sam,nsamples(max_i, len, ss, conf)]); + end +end + +%%% +% refine the F by using all detected outliers and with point normalization +F = u2Fdlt(u(:,inls),1); + +if no_sam == MAX_SAM + warning(sprintf('RANSAC - termination forced after %d samples expected number of samples is %d', no_sam, exp_sam)); +else + disp(sprintf('RANSAC: %d samples, %d inliers out of %d points',no_sam,sum(inls),len)) +end; + +return; \ No newline at end of file diff --git a/RansacM/u2Fdlt.m b/RansacM/u2Fdlt.m new file mode 100644 index 0000000..98f3dfa --- /dev/null +++ b/RansacM/u2Fdlt.m @@ -0,0 +1,59 @@ +function F = u2Fdlt(u,do_norm) +% u2Fdlt linear estimation of the Fundamental matrix +% from point correspondences +% +% H = u2Fdlt(u,{do_norm=1}) +% u ... {4|6}xN corresponding coordinates +% do_norm .. do isotropic normalization of points? +% +% F ... 3x3 fundamental matrix +% +% $Id: u2Fdlt.m,v 1.1 2005/05/23 16:16:01 svoboda Exp $ + +NoPoints = size(u,2); + +if nargin < 2 + do_norm=1; +end + +u = u'; + +% parse the input parameters +if NoPoints<8 + error('Too few correspondences') +end + +if size(u,2) == 4, + % make the homogenous coordinates + u = [u(:,1:2), ones(size(u(:,1))), u(:,3:4), ones(size(u(:,1)))]; +end + +u1 = u(:,1:3); +u2 = u(:,4:6); + +if do_norm + [u1,T1] = pointnormiso(u1'); + u1 = u1'; + [u2,T2] = pointnormiso(u2'); + u2 = u2'; +end + +% create the data matrix +A = zeros(NoPoints,9); +for i=1:NoPoints % create equations + for j=1:3 + for k=1:3 + A(i,(j-1)*3+k)=u2(i,j)*u1(i,k); + end + end +end + +[U,S,V] = svd(A); +f = V(:,size(V,2)); +F = reshape(f,3,3)'; + +if do_norm + F = inv(T2)*F*T1; +end + +return;