From 116e692571ba23deae16672b3808bf430bd36f6f Mon Sep 17 00:00:00 2001 From: Dorian Galvez-Lopez Date: Tue, 22 Jul 2014 11:34:49 -0400 Subject: [PATCH 1/3] Support for prefabricated patterns Arguments -grid-rows and -grid-cols added for consistency with calibgrid. When a known configuration is selected, a known dot grid is loaded instead of being randomly generated. This avoid issues between gcc/clang builds. --- applications/calib-hal/main.cpp | 100 ++++++++++++++++++++++++++++---- 1 file changed, 90 insertions(+), 10 deletions(-) diff --git a/applications/calib-hal/main.cpp b/applications/calib-hal/main.cpp index d934bb9..0c0f705 100644 --- a/applications/calib-hal/main.cpp +++ b/applications/calib-hal/main.cpp @@ -31,6 +31,8 @@ const char* sUriInfo = "\t-grid-seed Random seed used when creating grid (=71)\n" "\t-fix-intrinsics,-f Fix camera intrinsics during optimisation.\n" "\t-paused,-p Start video paused.\n" +"\t-grid-rows Number of rows in the grid pattern.\n" +"\t-grid-cols Number of cols in the grid pattern.\n" "e.g.:\n" "\tcalibgrid -c leftcam.xml -c rightcaml.xml video_uri\n\n" @@ -70,7 +72,8 @@ const char* sUriInfo = "log - run google protobuf data log from HAL (see https://github.com/gwu-robotics):\n" " e.g. \"log://~/Data/calib.log\"\n\n"; - +static inline Eigen::MatrixXi GWUSmallGrid(); +static inline Eigen::MatrixXi GoogleLargeGrid(); int main( int argc, char** argv) { @@ -89,9 +92,16 @@ int main( int argc, char** argv) // Default grid printed on US Letter double grid_spacing = 0.254 / (19-1); - const Eigen::Vector2i grid_size(19,10); + int grid_rows = 10; + int grid_cols = 19; uint32_t grid_seed = 71; + // defaults for large A0 grid + //double grid_spacing = 0.03156; + //int grid_rows = 36; + //int grid_cols = 25; + //uint32_t grid_seed = 71; + // Use no input cameras by default std::vector input_cameras; @@ -135,9 +145,12 @@ int main( int argc, char** argv) grid_spacing = cl.follow(grid_spacing,"-grid-spacing"); grid_seed = cl.follow((int)grid_seed,"-grid-seed"); + grid_cols = cl.follow((int)grid_cols,"-grid-cols"); + grid_rows = cl.follow((int)grid_rows,"-grid-rows"); fix_intrinsics = cl.search(2, "-fix-intrinsics", "-f"); start_paused = cl.search(2, "-paused", "-p"); output_filename = cl.follow(output_filename.c_str(), 2, "-output", "-o"); + const Eigen::Vector2i grid_size(grid_cols, grid_rows); // Load camera hints from command line cl.disable_loop(); @@ -199,7 +212,15 @@ int main( int argc, char** argv) conic_finder.Params().conic_min_density = 0.6; conic_finder.Params().conic_min_aspect = 0.2; - TargetGridDot target(grid_spacing, grid_size, grid_seed); + std::unique_ptr target; + if(grid_seed == 71 && grid_size[0] == 25 && grid_size[1] == 36) + // known large pattern + target.reset(new TargetGridDot(grid_spacing, GoogleLargeGrid())); + else if(grid_seed == 71 && grid_size[0] == 19 && grid_size[1] == 10) + // known small pattern + target.reset(new TargetGridDot(grid_spacing, GWUSmallGrid())); + else + target.reset(new TargetGridDot(grid_spacing, grid_size, grid_seed)); //////////////////////////////////////////////////////////////////// // Initialize Calibration object and tracking params @@ -337,7 +358,7 @@ int main( int argc, char** argv) conic_finder.Conics(); std::vector ellipse_target_map; - tracking_good[iI] = target.FindTarget( + tracking_good[iI] = target->FindTarget( image_processing, conic_finder.Conics(), ellipse_target_map ); @@ -352,7 +373,7 @@ int main( int argc, char** argv) // find camera pose given intrinsics PosePnPRansac( - calibrator.GetCamera(iI).camera, ellipses, target.Circles3D(), + calibrator.GetCamera(iI).camera, ellipses, target->Circles3D(), ellipse_target_map, 0, 0, &T_hw[iI] ); @@ -365,7 +386,7 @@ int main( int argc, char** argv) for(size_t p=0; p < ellipses.size(); ++p) { const Eigen::Vector2d pc = ellipses[p]; - const Eigen::Vector2i pg = target.Map()[p].pg; + const Eigen::Vector2i pg = target->Map()[p].pg; if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) ) { @@ -398,7 +419,7 @@ int main( int argc, char** argv) glMatrixMode(GL_MODELVIEW); if(disp_lines) { - for(std::list::const_iterator i = target.LineGroups().begin(); i != target.LineGroups().end(); ++i) + for(std::list::const_iterator i = target->LineGroups().begin(); i != target->LineGroups().end(); ++i) { glColor3f(0.5,0.5,0.5); glBegin(GL_LINE_STRIP); @@ -414,14 +435,14 @@ int main( int argc, char** argv) if(disp_cross) { for( size_t i=0; i < conics.size(); ++i ) { const Eigen::Vector2d pc = conics[i].center; - pangolin::glColorBin( target.Map()[i].value, 2); + pangolin::glColorBin( target->Map()[i].value, 2); pangolin::glDrawCross(pc, conics[i].bbox.Width()*0.75 ); } } if(disp_bbox) { for( size_t i=0; i < conics.size(); ++i ) { - const Eigen::Vector2i pg = tracking_good[iI] ? target.Map()[i].pg : Eigen::Vector2i(0,0); + const Eigen::Vector2i pg = tracking_good[iI] ? target->Map()[i].pg : Eigen::Vector2i(0,0); if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) ) { pangolin::glColorBin(pg(1)*grid_size(0)+pg(0), grid_size(0)*grid_size(1)); glDrawRectPerimeter(conics[i].bbox); @@ -434,7 +455,7 @@ int main( int argc, char** argv) if(v3D.IsShown()) { v3D.ActivateScissorAndClear(stacks); - calibu::glDrawTarget(target, Eigen::Vector2d(0,0), 1.0, 0.8, 1.0); + calibu::glDrawTarget(*target, Eigen::Vector2d(0,0), 1.0, 0.8, 1.0); for(size_t c=0; c< calibrator.NumCameras(); ++c) { const Eigen::Matrix3d Kinv = calibrator.GetCamera(c).camera.Kinv(); @@ -468,3 +489,62 @@ int main( int argc, char** argv) calibrator.WriteCameraModels(output_filename); } + +inline Eigen::MatrixXi GWUSmallGrid() { + Eigen::MatrixXi m(10, 19); + m << + 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, + 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, + 1, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, + 0, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, + 0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, + 1, 1, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, 1, 0, + 0, 1, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, + 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0; + return m; +} + +// Large google pattern +inline Eigen::MatrixXi GoogleLargeGrid() { + Eigen::MatrixXi m(36, 25); + m << + 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, + 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, + 1, 0, 0, 0, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0, + 0, 0, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, + 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, + 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, + 0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 0, 0, + 1, 1, 1, 1, 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 1, 0, 1, 0, + 1, 1, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, + 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, + 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 1, + 0, 1, 0, 0, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, + 1, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 0, + 0, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1, 1, 1, 1, + 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, + 1, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 1, + 1, 0, 1, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, + 1, 0, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, + 0, 1, 0, 0, 0, 0, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 1, + 0, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, + 1, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, + 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 1, 0, 1, + 0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 1, 0, 1, 1, 0, + 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, 0, 0, 0, 0, 0, 1, 0, + 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 0, 0, 1, 0, 1, 0, 1, 1, + 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1, 0, + 1, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 1, 0, 0, + 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, + 1, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 1, + 1, 1, 0, 1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, + 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, + 0, 0, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0; + return m; +} From e5a1e761a126ba58f5e3913d5f5b7fb37985ab78 Mon Sep 17 00:00:00 2001 From: Dorian Galvez-Lopez Date: Tue, 22 Jul 2014 12:01:04 -0400 Subject: [PATCH 2/3] Notation consistency --- applications/calib-hal/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/applications/calib-hal/main.cpp b/applications/calib-hal/main.cpp index 0c0f705..9e5890d 100644 --- a/applications/calib-hal/main.cpp +++ b/applications/calib-hal/main.cpp @@ -213,10 +213,10 @@ int main( int argc, char** argv) conic_finder.Params().conic_min_aspect = 0.2; std::unique_ptr target; - if(grid_seed == 71 && grid_size[0] == 25 && grid_size[1] == 36) + if(grid_seed == 71 && grid_size(0) == 25 && grid_size(1) == 36) // known large pattern target.reset(new TargetGridDot(grid_spacing, GoogleLargeGrid())); - else if(grid_seed == 71 && grid_size[0] == 19 && grid_size[1] == 10) + else if(grid_seed == 71 && grid_size(0) == 19 && grid_size(1) == 10) // known small pattern target.reset(new TargetGridDot(grid_spacing, GWUSmallGrid())); else From ba712c27645c6980e7261a30ac788081b6ea1786 Mon Sep 17 00:00:00 2001 From: Dorian Galvez-Lopez Date: Tue, 22 Jul 2014 13:58:43 -0400 Subject: [PATCH 3/3] Example of grid sized added --- applications/calib-hal/main.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/applications/calib-hal/main.cpp b/applications/calib-hal/main.cpp index 9e5890d..85042e3 100644 --- a/applications/calib-hal/main.cpp +++ b/applications/calib-hal/main.cpp @@ -35,7 +35,9 @@ const char* sUriInfo = "\t-grid-cols Number of cols in the grid pattern.\n" "e.g.:\n" -"\tcalibgrid -c leftcam.xml -c rightcaml.xml video_uri\n\n" +"\tcalibgrid-hal -c leftcam.xml -c rightcaml.xml video_uri\n" +"\tcalibgrid-hal -grid-spacing 0.03156 -grid-rows 36 -grid-cols 25 video_uri (large grid)\n" +"\tcalibgrid-hal -grid-spacing 0.01411 -grid-rows 10 -grid-cols 19 video_uri (default)\n\n" "Video URI's take the following form:\n" " scheme:[param1=value1,param2=value2,...]//device\n" "\n" @@ -96,12 +98,6 @@ int main( int argc, char** argv) int grid_cols = 19; uint32_t grid_seed = 71; - // defaults for large A0 grid - //double grid_spacing = 0.03156; - //int grid_rows = 36; - //int grid_cols = 25; - //uint32_t grid_seed = 71; - // Use no input cameras by default std::vector input_cameras;