From 83d8b7cdb2590d104c09390bf2ddfd6c33552457 Mon Sep 17 00:00:00 2001 From: Daniel Mirota Date: Fri, 21 Sep 2018 16:17:07 -0700 Subject: [PATCH] Updates to Calibu --- CMakeLists.txt | 50 +- cmake_modules/PackageConfig.cmake.in | 4 + cmake_modules/install_package.cmake | 17 +- include/calibu/cam/rectify_crtp.h | 6 +- include/calibu/conics/Conic.h | 2 + include/calibu/conics/ConicFinder.h | 6 +- include/calibu/image/ImageProcessing.h | 4 +- include/calibu/pcalib/base64.h | 1 + include/calibu/pose/Pnp.h | 6 +- include/calibu/target/LineGroup.h | 12 +- include/calibu/target/TargetGridDot.h | 14 +- src/conics/ConicFinder.cpp | 61 ++- src/conics/FindConics.cpp | 2 +- src/image/ImageProcessing.cpp | 79 +++- src/pose/Pnp.cpp | 161 ++++++- src/target/TargetGridDot.cpp | 623 +++++++++++++++++++++++-- 16 files changed, 936 insertions(+), 112 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5693cc8..3bd8831 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,8 +26,8 @@ if(BUILD_MATLAB) endif() endif() -set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}") -if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") +set(CMAKE_CXX_FLAGS "-std=c++14 -Wall ${CMAKE_CXX_FLAGS}") +if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" AND NOT ANDROID) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") endif() @@ -118,11 +118,12 @@ SET(SOURCES ## Find required dependencies find_package( Eigen3 REQUIRED ) find_package( Sophus REQUIRED ) -find_package( GLog REQUIRED ) +find_package( glog REQUIRED CONFIG) +set(HAVE_GLOG 1) +list( APPEND LINK_LIBS glog::glog ) -find_package(TinyXML2 REQUIRED) -list(APPEND CALIBU_INC ${TinyXML2_INCLUDE_DIRS} ) -list(APPEND LINK_LIBS ${TinyXML2_LIBRARIES} ) +find_package(tinyxml2 REQUIRED CONFIG) +list(APPEND LINK_LIBS tinyxml2 ) ## Apply project include directories @@ -139,7 +140,7 @@ list( APPEND USER_INC ${Sophus_INCLUDE_DIRS} ) ## Find optional dependencies # OpenCV is required for PNP methods and calibration application -find_package( OpenCV REQUIRED core calib3d ) +find_package( OpenCV REQUIRED calib3d imgproc core ) if( OpenCV_FOUND ) set( HAVE_OPENCV 1 ) list( APPEND LINK_LIBS ${OpenCV_LIBS}) @@ -148,20 +149,6 @@ if( OpenCV_FOUND ) list( APPEND SOURCES ${SRC_DIR}/pose/Pnp.cpp ${SRC_DIR}/pose/Tracker.cpp ) endif() -####################################################### -## Setup and configure library -## Generate symbol export helper header on MSVC -IF(MSVC) - string(TOUPPER ${LIBRARY_NAME} LIBRARY_NAME_CAPS) - include(GenerateExportHeader) - GENERATE_EXPORT_HEADER( ${LIBRARY_NAME} - BASE_NAME ${LIBRARY_NAME_CAPS} - EXPORT_MACRO_NAME ${LIBRARY_NAME_CAPS}_EXPORT - EXPORT_FILE_NAME "${CMAKE_CURRENT_BINARY_DIR}/include/${LIBRARY_NAME}/${LIBRARY_NAME}_export.h" - STATIC_DEFINE ${LIBRARY_NAME_CAPS}_BUILT_AS_STATIC - ) -ENDIF() - ####################################################### ## Optionally create unit tests @@ -194,6 +181,22 @@ endif() add_library( calibu ${SOURCES} ) target_link_libraries( calibu ${LINK_LIBS} ) +####################################################### +## Setup and configure library +## Generate symbol export helper header on MSVC +SET(EXPORT_HEADER) +IF(MSVC) + SET(EXPORT_HEADER "${CMAKE_CURRENT_BINARY_DIR}/include/${LIBRARY_NAME}/${LIBRARY_NAME}_export.h") + string(TOUPPER ${LIBRARY_NAME} LIBRARY_NAME_CAPS) + include(GenerateExportHeader) + GENERATE_EXPORT_HEADER( ${LIBRARY_NAME} + BASE_NAME ${LIBRARY_NAME_CAPS} + EXPORT_MACRO_NAME ${LIBRARY_NAME_CAPS}_EXPORT + EXPORT_FILE_NAME ${EXPORT_HEADER} + STATIC_DEFINE ${LIBRARY_NAME_CAPS}_BUILT_AS_STATIC + ) +ENDIF() + # install everything install_package( PKG_NAME Calibu @@ -202,10 +205,11 @@ install_package( DESCRIPTION "Computer vision camera model library." # INSTALL_HEADERS ${HEADERS} INSTALL_INCLUDE_DIR true - INSTALL_GENERATED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/include/calibu/config.h + INSTALL_GENERATED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/include/calibu/config.h ${EXPORT_HEADER} DESTINATION ${CMAKE_INSTALL_PREFIX} - INCLUDE_DIRS ${USER_INC} + INCLUDE_DIRS ${USER_INC} ${CMAKE_INSTALL_PREFIX}/include LINK_LIBS ${LINK_LIBS} + LINK_DIRS ${CMAKE_INSTALL_PREFIX}/lib ) if( BUILD_APPLICATIONS ) diff --git a/cmake_modules/PackageConfig.cmake.in b/cmake_modules/PackageConfig.cmake.in index 56a2ad9..3a331da 100644 --- a/cmake_modules/PackageConfig.cmake.in +++ b/cmake_modules/PackageConfig.cmake.in @@ -1,3 +1,7 @@ +include (CMakeFindDependencyMacro) + +find_dependency(tinyxml2 @tinyxml2_VERSION@) + SET( @PACKAGE_PKG_NAME@_LIBRARIES "@PACKAGE_LINK_LIBS@" CACHE INTERNAL "@PACKAGE_PKG_NAME@ libraries" FORCE ) SET( @PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ include directories" FORCE ) SET( @PACKAGE_PKG_NAME@_LIBRARY_DIRS @PACKAGE_LINK_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ library directories" FORCE ) diff --git a/cmake_modules/install_package.cmake b/cmake_modules/install_package.cmake index 682cc7e..5d33de9 100644 --- a/cmake_modules/install_package.cmake +++ b/cmake_modules/install_package.cmake @@ -102,7 +102,7 @@ function(install_package) endif() get_target_property( _target_library ${PACKAGE_LIB_NAME} LOCATION ) get_filename_component( _lib ${_target_library} NAME ) - list( APPEND PACKAGE_LINK_LIBS ${PACKAGE_LIB_NAME} ) + list( INSERT PACKAGE_LINK_LIBS 0 ${PACKAGE_LIB_NAME} ) endif() if( PACKAGE_INSTALL_HEADER_DIRS ) @@ -141,10 +141,19 @@ function(install_package) # install library itself if( PACKAGE_LIB_NAME ) - install( FILES ${_target_library} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) + + if(MSVC) + string(REPLACE "$(Configuration)" "\${BUILD_TYPE}" _target_library ${_target_library}) + endif() + + install( TARGETS ${PACKAGE_LIB_NAME} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + ) set( PACKAGE_LIB_LINK "-l${PACKAGE_LIB_NAME}" ) endif() - + # build pkg-config file if( PACKAGE_PKG_NAME ) configure_file( ${modules_dir}/PkgConfig.pc.in ${PACKAGE_PKG_NAME}.pc @ONLY ) @@ -195,7 +204,7 @@ function(install_package) cmake_policy( SET CMP0026 OLD ) endif() get_target_property( _target_library ${PACKAGE_LIB_NAME} LOCATION ) - list( APPEND PACKAGE_LINK_LIBS ${_target_library} ) + list( INSERT PACKAGE_LINK_LIBS 0 ${_target_library} ) endif() if( PACKAGE_INSTALL_HEADER_DIRS ) diff --git a/include/calibu/cam/rectify_crtp.h b/include/calibu/cam/rectify_crtp.h index dfeb2b5..f2b12b5 100644 --- a/include/calibu/cam/rectify_crtp.h +++ b/include/calibu/cam/rectify_crtp.h @@ -61,8 +61,8 @@ namespace calibu }; /////////////////////////////////////////////////////////////////////////////// - CALIBU_EXPORT - struct LookupTable + + struct CALIBU_EXPORT LookupTable { inline LookupTable():m_vLutPixels(std::vector()),m_nWidth(0){}; inline LookupTable( int nWidth, int nHeight ) @@ -129,7 +129,7 @@ namespace calibu /// Rectify image pInputImageData using lookup table generated by /// 'CreateLookupTable' to output image pOutputRectImageData. template - CALIBU_EXPORT void Rectify( + void Rectify( const LookupTable& lut, const scalar* pInputImageData, scalar* pOutputRectImageData, diff --git a/include/calibu/conics/Conic.h b/include/calibu/conics/Conic.h index 8d9c87b..89351bf 100644 --- a/include/calibu/conics/Conic.h +++ b/include/calibu/conics/Conic.h @@ -46,6 +46,8 @@ struct Conic { // center (c1,c2) Eigen::Vector2d center; + Eigen::Vector3d center_undistorted; + double radius; }; CALIBU_EXPORT diff --git a/include/calibu/conics/ConicFinder.h b/include/calibu/conics/ConicFinder.h index 0bb1c62..71493ff 100644 --- a/include/calibu/conics/ConicFinder.h +++ b/include/calibu/conics/ConicFinder.h @@ -47,12 +47,12 @@ struct ParamsConicFinder float conic_min_aspect; }; -CALIBU_EXPORT -class ConicFinder + +class CALIBU_EXPORT ConicFinder { public: ConicFinder(); - void Find(const ImageProcessing& imgs); + void Find(const ImageProcessing& imgs, const std::shared_ptr> camera = nullptr); inline const std::vector >& Conics() const { diff --git a/include/calibu/image/ImageProcessing.h b/include/calibu/image/ImageProcessing.h index 1132b84..5028d09 100644 --- a/include/calibu/image/ImageProcessing.h +++ b/include/calibu/image/ImageProcessing.h @@ -37,8 +37,8 @@ struct ParamsImageProcessing { bool black_on_white; }; -CALIBU_EXPORT -class ImageProcessing { + +class CALIBU_EXPORT ImageProcessing { public: ImageProcessing(int maxWidth, int maxHeight); ~ImageProcessing(); diff --git a/include/calibu/pcalib/base64.h b/include/calibu/pcalib/base64.h index 4443c4d..a17fd68 100644 --- a/include/calibu/pcalib/base64.h +++ b/include/calibu/pcalib/base64.h @@ -2,6 +2,7 @@ #include #include +#include #include #include diff --git a/include/calibu/pose/Pnp.h b/include/calibu/pose/Pnp.h index d5f55d5..1664d66 100644 --- a/include/calibu/pose/Pnp.h +++ b/include/calibu/pose/Pnp.h @@ -23,18 +23,20 @@ #include #include +#include #include namespace calibu { - std::vector PosePnPRansac( + std::vector CALIBU_EXPORT PosePnPRansac( const std::shared_ptr> cam, const std::vector > & img_pts, const std::vector > & ideal_pts, const std::vector & candidate_map, int robust_3pt_its, float robust_3pt_tol, - Sophus::SE3d * T + Sophus::SE3d * T, + bool calibrate = false ); double ReprojectionErrorRMS( diff --git a/include/calibu/target/LineGroup.h b/include/calibu/target/LineGroup.h index 2a5ba53..e80e221 100644 --- a/include/calibu/target/LineGroup.h +++ b/include/calibu/target/LineGroup.h @@ -41,7 +41,7 @@ struct Vertex { EIGEN_MAKE_ALIGNED_OPERATOR_NEW; inline Vertex(size_t id, const Conic& c) - : id(id), conic(c), pc(c.center), pg(GRID_INVALID,GRID_INVALID), area(0.0), value(-1) + : id(id), conic(c), pc(c.center), pg(GRID_INVALID,GRID_INVALID), area(0.0), value(-1), pc_u(c.center_undistorted) { } @@ -53,6 +53,7 @@ struct Vertex size_t id; Conic conic; Eigen::Vector2d pc; + Eigen::Vector3d pc_u; Eigen::Vector2i pg; std::vector triples; std::set neighbours; @@ -62,9 +63,10 @@ struct Vertex struct Triple { - inline Triple(Vertex& o1, Vertex& c, Vertex& o2) + inline Triple(Vertex& o1, Vertex& c, Vertex& o2, Eigen::Vector2d angles = Eigen::Vector2d()) { vs = {&o1, &c, &o2}; + m_angles = angles; } Triple(const Triple& triple) = default; @@ -104,6 +106,7 @@ struct Triple // Colinear sequence of vertices, v[0], v[1], v[2]. v[1] is center std::vector vs; + Eigen::Vector2d m_angles; }; inline bool operator==(const Vertex& lhs, const Vertex& rhs) @@ -121,6 +124,11 @@ inline double Distance(const Vertex& v1, const Vertex& v2) return (v2.pc - v1.pc).norm(); } +inline double DistanceUndistorted(const Vertex& v1, const Vertex& v2) +{ + return (v2.pc_u - v1.pc_u).squaredNorm(); +} + inline std::ostream& operator<<(std::ostream& os, const Vertex& v) { os << "(" << v.pg.transpose() << ")"; diff --git a/include/calibu/target/TargetGridDot.h b/include/calibu/target/TargetGridDot.h index e226e65..6ff23b6 100644 --- a/include/calibu/target/TargetGridDot.h +++ b/include/calibu/target/TargetGridDot.h @@ -49,8 +49,8 @@ inline bool operator<(const Dist& lhs, const Dist& rhs) { return lhs.dist < rhs. struct ParamsGridDot { ParamsGridDot() : - max_line_dist_ratio(0.3), - max_norm_triple_area(0.03), + max_line_dist_ratio(0.4), + max_norm_triple_area(0.05), min_cross_area(1.5), max_cross_area(9.0), cross_radius_ratio(0.058), @@ -65,8 +65,8 @@ struct ParamsGridDot double cross_line_ratio; }; -CALIBU_EXPORT -class TargetGridDot + +class CALIBU_EXPORT TargetGridDot : public TargetInterface { public: @@ -166,9 +166,9 @@ class TargetGridDot void Clear(); void SetGrid(Vertex& v, const Eigen::Vector2i& g); bool Match(std::map, - Eigen::aligned_allocator > >& obs, - const std::array& PG); + std::less, + Eigen::aligned_allocator > >& obs, + const std::array& PG); std::vector > tpts2d; std::vector tpts2d_radius; diff --git a/src/conics/ConicFinder.cpp b/src/conics/ConicFinder.cpp index 82fc4d6..1a88aa7 100644 --- a/src/conics/ConicFinder.cpp +++ b/src/conics/ConicFinder.cpp @@ -1,4 +1,4 @@ -/* +/* This file is part of the Calibu Project. https://github.com/gwu-robotics/Calibu @@ -24,27 +24,74 @@ #include #include +#include + +#include + namespace calibu { ConicFinder::ConicFinder() { } -void ConicFinder::Find(const ImageProcessing& imgs) +void ConicFinder::Find(const ImageProcessing& imgs, const std::shared_ptr> camera) { candidates.clear(); conics.clear(); - + // Find candidate regions for conics - FindCandidateConicsFromLabels( + /*FindCandidateConicsFromLabels( imgs.Width(), imgs.Height(), imgs.Labels(), candidates, params.conic_min_area, params.conic_max_area, params.conic_min_density, params.conic_min_aspect - ); - + );*/ + // Find conic parameters - FindConics(imgs.Width(), imgs.Height(), candidates, imgs.ImgDeriv(), conics ); + //FindConics(imgs.Width(), imgs.Height(), candidates, imgs.ImgDeriv(), conics ); + + cv::SimpleBlobDetector::Params params; + + params.filterByConvexity = true; + params.filterByInertia = false; + params.minArea = 3.7; + params.minConvexity = 0.89375; + params.maxThreshold = 90; + params.minDistBetweenBlobs = 2.5; + params.minThreshold = 0; + params.thresholdStep = 45; + + auto detector = cv::SimpleBlobDetector::create(params); + + const cv::Mat im(imgs.Height(), imgs.Width(), cv::DataType::type, const_cast(imgs.ImgThresh())); + + std::vector keypoints; + detector->detect(im, keypoints); + + for (auto & keypoint : keypoints) + { + + unsigned char intensity = im.at(keypoint.pt); + + //LOG(INFO) << "intensity: " << intensity; + + Conic conic; + conic.center = Eigen::Vector2d(keypoint.pt.x, keypoint.pt.y); + conic.radius = keypoint.size/2; + conic.bbox.x1 = keypoint.pt.x - keypoint.size/2; + conic.bbox.y1 = keypoint.pt.y - keypoint.size/2; + conic.bbox.x2 = keypoint.pt.x + keypoint.size/2 + 1; + conic.bbox.y2 = keypoint.pt.y + keypoint.size/2 + 1; + conics.push_back(conic); + } + + + if (camera != nullptr) + { + for (auto & cone : conics) { + cone.center_undistorted = camera->Unproject(cone.center); + } + } } } diff --git a/src/conics/FindConics.cpp b/src/conics/FindConics.cpp index 54c5d36..016f4df 100644 --- a/src/conics/FindConics.cpp +++ b/src/conics/FindConics.cpp @@ -158,7 +158,7 @@ void FindCandidateConicsFromLabels( if( min_aspect < aspect && aspect < 1.0 / min_aspect ) { const double density = (double)labels[i].size / (double)area; - if( min_density <= density ) + //if( min_density <= density ) { PixelClass candidate = labels[i]; candidate.bbox = r.Grow(2).Clamp(border,border,w-(1+border),h-(1+border)); diff --git a/src/image/ImageProcessing.cpp b/src/image/ImageProcessing.cpp index ee8cc57..3284261 100644 --- a/src/image/ImageProcessing.cpp +++ b/src/image/ImageProcessing.cpp @@ -24,6 +24,8 @@ #include #include +#include + namespace calibu { ImageProcessing::ImageProcessing(int maxWidth, int maxHeight) @@ -65,9 +67,14 @@ void ImageProcessing::Process(const unsigned char* greyscale_image, memcpy(&I[0], greyscale_image, img_size); } + + cv::Mat dst(h, w, cv::DataType::type, &tI[0]); + + cv::Mat input_test(h, w, cv::DataType::type, &I[0]); + // Process image gradient<>(width, height, &I[0], &dI[0]); - integral_image(width, height, &I[0], &intI[0] ); + /*integral_image(width, height, &I[0], &intI[0] ); // Threshold image AdaptiveThreshold( @@ -76,10 +83,76 @@ void ImageProcessing::Process(const unsigned char* greyscale_image, (unsigned char)0, (unsigned char)255 ); + cv::Mat dst2; + + cv::Mat dst3(h, w, cv::DataType::type); + + auto clahe = cv::createCLAHE(); + clahe->setClipLimit(8); + clahe->setTilesGridSize(cv::Size(2, 2)); + + clahe->apply(input_test, input_test); + + cv::Mat dst_intI(h, w, cv::DataType::type); + + cv::Mat dst_intI_test(h, w, cv::DataType::type, &intI[0]); + + integral_image(width, height, &I[0], &intI[0]); + + AdaptiveThreshold( + width, height, &I[0], &intI[0], dst3.data, params.at_threshold, + width / params.at_window_ratio, 20, + (unsigned char)0, (unsigned char)255 + );*/ + + cv::adaptiveThreshold(input_test, dst, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 127, 4); + + //gradient<>(width, height, dst.data, &dI[0]); + + cv::Mat bw2 = dst < 128; + cv::Mat labelImage2(bw2.size(), CV_16U); + + + cv::Mat output2; + cv::Mat centroids2; + cv::Mat stats2; + cv::connectedComponentsWithStats(bw2, labelImage2, stats2, centroids2, 8, CV_16U); + + // Label image (connected components) labels.clear(); - Label(width, height, &tI[0], &lI[0], labels, - params.black_on_white ? 0 : 255 ); + //Label(width, height, &tI[0], &lI[0], labels, + // params.black_on_white ? 0 : 255); + + + + /*cv::Mat input(h, w, cv::DataType::type, &tI[0]); + cv::Mat bw = input < 128; + cv::Mat labelImage(bw.size(), CV_16U); + + + cv::Mat output; + cv::Mat centroids; + cv::Mat stats; + + cv::connectedComponentsWithStats(bw, labelImage, stats, centroids, 8, CV_16U);*/ + //std::vector labels2; + for (int i = 1; i < stats2.size().height; i++) + { + //populate labels from stats + PixelClass current; + + current.bbox.x1 = stats2.at(i, cv::CC_STAT_LEFT); + current.bbox.y1 = stats2.at(i, cv::CC_STAT_TOP); + current.bbox.x2 = current.bbox.x1 + stats2.at(i, cv::CC_STAT_WIDTH) - 1; + current.bbox.y2 = current.bbox.y1 + stats2.at(i, cv::CC_STAT_HEIGHT) - 1; + current.equiv = -1; + current.size = stats2.at(i, cv::CC_STAT_AREA); + labels.push_back(current); + } + + + } } diff --git a/src/pose/Pnp.cpp b/src/pose/Pnp.cpp index e1a8ac6..5acd2a5 100644 --- a/src/pose/Pnp.cpp +++ b/src/pose/Pnp.cpp @@ -21,7 +21,7 @@ #include -#include +#include #include #include @@ -37,19 +37,74 @@ vector PosePnPRansac( const vector & candidate_map, int robust_3pt_its, float robust_3pt_tol, - Sophus::SE3d * T) { + Sophus::SE3d * T, + bool calibrate) { vector inlier_map(candidate_map.size(), -1); + + cv::Mat cv_coeff_start(4, 1, CV_64F); + cv::Mat cv_K_start(3, 3, CV_64F); + std::vector cv_obj; std::vector cv_img; + std::vector cv_img3; + + std::vector > cv_obj2(1); + std::vector > cv_img2(1); + std::vector idx_vec; cv::Mat cv_coeff; cv::Mat cv_rot(3,1,CV_64F); cv::Mat cv_trans(3,1,CV_64F); + cv::Mat cv_rot3(3, 1, CV_64F); + cv::Mat cv_trans3(3, 1, CV_64F); + + std::vector cv_rot2; + std::vector cv_trans2; + cv::Mat cv_K(3,3,CV_64F); - // cv::eigen2cv(cam.K(), cv_K); + cv::Mat cv_K2(3, 3, CV_64F); + + cv::eigen2cv(cam->K(), cv_K2); cv::setIdentity(cv_K); + + cv::eigen2cv(cam->K(), cv_K_start); + cv::eigen2cv(cam->GetParams().tail<4>(), cv_coeff_start); + + for (size_t i = 0; i= 0) + { + const Eigen::Vector3d & c3d = ideal_pts[ideal_point_id]; + cv_img2[0].push_back(cv::Point2f(img_pts[i].x(), img_pts[i].y())); + cv_obj2[0].push_back(cv::Point3f(c3d.x(), c3d.y(), c3d.z())); + idx_vec.push_back(i); + } + } + + //assuming KB4 for the time being + cv::Mat cv_coeff2(4, 1, CV_64F); + cv::eigen2cv(cam->GetParams().tail<4>(), cv_coeff2); + + int flags = cv::fisheye::CALIB_USE_INTRINSIC_GUESS | cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC | cv::fisheye::CALIB_FIX_SKEW; + + double reproject_error = std::numeric_limits::max(); + try { + if(calibrate) + reproject_error = cv::fisheye::calibrate(cv_obj2, cv_img2, cv::Size(cam->Width(), cam->Height()), cv_K2, cv_coeff2, cv_rot2, cv_trans2, flags); + } + catch (...) + { + } + + //cv::cv2eigen(cv_coeff2, cam->GetParams().tail<4>()); + + + + //cv::cv2eigen(cv_K2, cam->K()); + for (size_t i = 0; i PosePnPRansac( { // TODO: This is really bad for cameras > 180 FOV // Replace with PNP for general camera. + + cam->GetParams().tail<4>()[0] = cv_coeff2.at(0, 0); + cam->GetParams().tail<4>()[1] = cv_coeff2.at(1, 0); + cam->GetParams().tail<4>()[2] = cv_coeff2.at(2, 0); + cam->GetParams().tail<4>()[3] = cv_coeff2.at(3, 0); + + cam->GetParams().head<4>()[0] = cv_K2.at(0, 0); + cam->GetParams().head<4>()[1] = cv_K2.at(1, 1); + cam->GetParams().head<4>()[2] = cv_K2.at(0, 2); + cam->GetParams().head<4>()[3] = cv_K2.at(1, 2); + const Eigen::Vector3d img_center_pts = cam->Unproject(img_pts[i]); Eigen::Vector2d center; center << img_center_pts[0]/img_center_pts[2], img_center_pts[1]/img_center_pts[2]; // const Eigen::Vector2d center = cam.Unmap(img_pts[i]); - const Eigen::Vector3d & c3d = ideal_pts[ideal_point_id]; cv_img.push_back(cv::Point2f(center.x(), center.y())); + + + cam->GetParams().tail<4>()[0] = cv_coeff_start.at(0, 0); + cam->GetParams().tail<4>()[1] = cv_coeff_start.at(1, 0); + cam->GetParams().tail<4>()[2] = cv_coeff_start.at(2, 0); + cam->GetParams().tail<4>()[3] = cv_coeff_start.at(3, 0); + + cam->GetParams().head<4>()[0] = cv_K_start.at(0, 0); + cam->GetParams().head<4>()[1] = cv_K_start.at(1, 1); + cam->GetParams().head<4>()[2] = cv_K_start.at(0, 2); + cam->GetParams().head<4>()[3] = cv_K_start.at(1, 2); + + const Eigen::Vector3d img_center_pts2 = cam->Unproject(img_pts[i]); + Eigen::Vector2d center2; + center2 << img_center_pts2[0] / img_center_pts2[2], img_center_pts2[1] / img_center_pts2[2]; + cv_img3.push_back(cv::Point2f(center2.x(), center2.y())); + + + const Eigen::Vector3d & c3d = ideal_pts[ideal_point_id]; cv_obj.push_back(cv::Point3f(c3d.x(), c3d.y(), c3d.z())); idx_vec.push_back(i); } } + //if the calibration pose and pnp pose are close there are few outliers if they are not close there are outliers + + + std::vector cv_inliers; + std::vector cv_inliers3; if(cv_img.size() < 4) return cv_inliers; if(robust_3pt_its > 0) { cv::solvePnPRansac(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans, - false, robust_3pt_its, robust_3pt_tol / cam->K()(0,0), 60, cv_inliers); + false, robust_3pt_its/2, robust_3pt_tol / cam->K()(0,0), 0.99, cv_inliers); + cv::solvePnPRansac(cv_obj, cv_img3, cv_K, cv_coeff, cv_rot3, cv_trans3, + false, robust_3pt_its/2, robust_3pt_tol / cam->K()(0, 0), 0.99, cv_inliers3); }else{ cv::solvePnP(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans, false); } + Vector3d rot, trans; - cv::cv2eigen(cv_rot, rot); - cv::cv2eigen(cv_trans, trans); + + std::vector * current_inliers = nullptr; + + /*auto angle = acos(cv_rot3.dot(cv_rot) / (cv::norm(cv_rot3) * cv::norm(cv_rot))); + auto angle2 = acos(cv_rot2[0].dot(cv_rot) / (cv::norm(cv_rot2[0]) * cv::norm(cv_rot))); + auto angle3 = acos(cv_rot2[0].dot(cv_rot3) / (cv::norm(cv_rot2[0]) * cv::norm(cv_rot3)));*/ + + if (cv_inliers.size() > cv_inliers3.size() && reproject_error < 0.025) { + cam->GetParams().tail<4>()[0] = cv_coeff2.at(0, 0); + cam->GetParams().tail<4>()[1] = cv_coeff2.at(1, 0); + cam->GetParams().tail<4>()[2] = cv_coeff2.at(2, 0); + cam->GetParams().tail<4>()[3] = cv_coeff2.at(3, 0); + + cam->GetParams().head<4>()[0] = cv_K2.at(0, 0); + cam->GetParams().head<4>()[1] = cv_K2.at(1, 1); + cam->GetParams().head<4>()[2] = cv_K2.at(0, 2); + cam->GetParams().head<4>()[3] = cv_K2.at(1, 2); + + cv::cv2eigen(cv_rot, rot); + cv::cv2eigen(cv_trans, trans); + + current_inliers = &cv_inliers; + } + else + { + cv::cv2eigen(cv_rot3, rot); + cv::cv2eigen(cv_trans3, trans); + + current_inliers = &cv_inliers3; + } + + + + std::vector cv_obj_inliers; + std::vector cv_img_inliers; + + + for (auto it = current_inliers->begin(); it != current_inliers->end(); ++it) + { + cv_obj_inliers.push_back(cv_obj[*it]); + cv_img_inliers.push_back(cv_img[*it]); + } + + cv::Mat cv_rot_inliers(3, 1, CV_64F); + cv::Mat cv_trans_inliers(3, 1, CV_64F); + + + cv::solvePnP(cv_obj_inliers, cv_img_inliers, cv_K, cv_coeff, cv_rot_inliers, cv_trans_inliers, false); + + cv::cv2eigen(cv_rot_inliers, rot); + cv::cv2eigen(cv_trans_inliers, trans); + + if(std::isnan(rot[0]) || std::isnan(rot[1]) || std::isnan(rot[2])) return inlier_map; - for (size_t i = 0; i #include +#define _USE_MATH_DEFINES + +#include + #include #include #include #include #include +#include #include +#include +#include +#include + +#include + +#include +#include + + +#define SCALE_FACTOR 6 + +//#define OPENCV_DEBUG(arg) {arg} +#define OPENCV_DEBUG(arg) + +#include namespace calibu { @@ -51,10 +72,10 @@ TargetGridDot::TargetGridDot(double grid_spacing, const Eigen::MatrixXi& grid) TargetGridDot::TargetGridDot( const std::string& preset ) { - Eigen::MatrixXi grid; + Eigen::MatrixXi grid; double large_dot_radius; double small_dot_radius; - calibu::LoadGridFromPreset( preset, grid, grid_spacing_, + calibu::LoadGridFromPreset( preset, grid, grid_spacing_, large_dot_radius, small_dot_radius ); grid_size_(0) = grid.cols(); @@ -65,7 +86,7 @@ TargetGridDot::TargetGridDot( const std::string& preset ) } -void TargetGridDot::Init() +void TargetGridDot::Init() { // Create cached grid coordinates tpts2d.resize(grid_size_(0) * grid_size_(1)); @@ -107,7 +128,7 @@ std::vector > ClosestPoints( // Distance relation is symmetric for(size_t p2=p1+1; p2 < pts.size(); ++p2 ) { - const double dist = Distance(pts[p1], pts[p2]); + const double dist = DistanceUndistorted(pts[p1], pts[p2]); ret[p1][p2] = Dist{ &pts[p2], dist}; ret[p2][p1] = Dist{ &pts[p1], dist}; } @@ -121,7 +142,7 @@ std::vector > ClosestPoints( return ret; } -std::vector MostCentral( std::vector >& distances ) +std::vector MostCentral( const std::vector >& distances, std::vector & indices_out) { std::vector sum_sq; @@ -134,6 +155,12 @@ std::vector MostCentral( std::vector >& distances ) sum_sq.push_back( dist ); } + indices_out.resize(distances.size()); + std::iota(indices_out.begin(), indices_out.end(), 0); + + sort(indices_out.begin(), indices_out.end(), + [&sum_sq](size_t index1, size_t index2) {return sum_sq[index1] < sum_sq[index2]; }); + std::sort(sum_sq.begin(), sum_sq.end()); return sum_sq; } @@ -198,19 +225,19 @@ double Area(const Conic& c) // http://en.wikipedia.org/wiki/Matrix_representation_of_conic_sections const Eigen::Matrix2d A33 = c.C.block<2,2>(0,0); Eigen::SelfAdjointEigenSolver eigensolver(A33); - if (eigensolver.info() == Eigen::Success) { + /*if (eigensolver.info() == Eigen::Success) { const double detA = c.C.determinant(); const double detA33 = A33.determinant(); const double fac = -detA / (detA33); const Eigen::Vector2d l = eigensolver.eigenvalues(); const double a = sqrt(fac/l[0]); - const double b = sqrt(fac/l[1]); + const double b = sqrt(fac/l[1]);*/ - return M_PI * a * b; - }else{ + return M_PI * c.radius * c.radius;//* a * b; + /*}else{ return 0.0; - } + }*/ } std::set Neighbours(std::map Neighbours(std::map& closest, double thresh_dist, double thresh_area) +void FindTriples( Vertex& v, std::vector& closest, double thresh_dist, double thresh_area, const ImageProcessing& images, cv::Mat & debug_image) { // Consider 9 closests points (including itself) - const size_t NEIGHBOURS = 9; + const size_t NEIGHBOURS = 14; const size_t max_neigh = std::min(closest.size(), NEIGHBOURS); // We need at least 3 points for a single collinear triple. if(max_neigh < 3) return; + float color = 42.5; + + for (size_t n1 = 0; n1 < max_neigh; n1++) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, color * 5);) + } + // Ignore points too much furthar than closest. // We are interested in 8-connected region const double max_dist = 5 * closest[1].dist; @@ -247,13 +282,226 @@ void FindTriples( Vertex& v, std::vector& closest, double thresh_dist, dou std::array used; used.fill(false); + std::array direction_bins; + direction_bins.fill(false); + + std::array direction_bins_scores; + direction_bins_scores.fill(std::numeric_limits::max()); + + std::set angles; + + std::vector angles_diffs; + + + //compute the angle for possible pairs + std::vector > angles_and_indices; + for (size_t n1 = 1; n1 < max_neigh; ++n1) { + //Eigen::Vector2d diff = Eigen::CwiseBinaryOp, Eigen::Vector2d, Eigen::Vector2d>(closest[n1].v->pc,v.pc); + auto diff = closest[n1].v->pc - v.pc; + angles_and_indices.push_back(std::pair(std::atan2(diff.y(), diff.x()), n1)); + } + + std::sort(angles_and_indices.begin(), angles_and_indices.end(), [](std::pair &a, std::pair &b) { return a.first < b.first; }); + + bool erase_front = false; + std::vector > pairs; + for (auto front = angles_and_indices.begin(); front != angles_and_indices.end(); /*front++*/) { + for (auto rear = angles_and_indices.rbegin(); rear != std::make_reverse_iterator(front+1); /*rear++*/) + { + auto n1 = front->second; + auto n2 = rear->second; + Vertex& c1 = *closest[n1].v; + Vertex& c2 = *closest[n2].v; + + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + auto pt2 = cv::Point2d(v.pc.x(), v.pc.y()); + auto pt3 = cv::Point2d(c2.pc.x(), c2.pc.y()); + + for (size_t n1 = 0; n1 < max_neigh; n1++) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, color * 5);) + } + + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, color);) + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt2, 1, color * 2);) + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt3, 1, color * 3);) + + if (std::abs(front->first - rear->first + M_PI) < 0.1) + { + const double d1 = closest[n1].dist; + const double d2 = closest[n2].dist; + if (2.0 * fabs(d2 - d1) / (fabs(d1) + fabs(d2)) < thresh_dist) + { + + auto candidate = std::pair(Eigen::Vector2i(front->second, rear->second), Eigen::Vector2d(front->first, rear->first)); + + bool good_candidate = true; + if (!pairs.empty()) { + auto angle_distance = (pairs.back().second - candidate.second).squaredNorm(); + + //LOG(INFO) << angle_distance; + + if (angle_distance < 0.05) { + //collision + //keep the closer pair + if (closest[pairs.back().first.x()].dist > closest[candidate.first.x()].dist || closest[pairs.back().first.y()].dist > closest[candidate.first.y()].dist) { + pairs.pop_back(); + } + else { + good_candidate = false; + } + } + } + + if(good_candidate) + pairs.push_back(candidate); + + //erase both case + + erase_front = true; + //erase rear + //no need to clean up the loop is broken out of + angles_and_indices.erase((rear+1).base()); + //front = angles_and_indices.erase(front); + //front--; + break; + } + else + { + if (d1 > d2) + { + //erase front case + front = angles_and_indices.erase(front); + //reset rear because iterators are invalidated after erase + rear = angles_and_indices.rbegin(); + //rear--; + } + else + { + //erase rear case + angles_and_indices.erase((rear+1).base()); + //reset front and rear because iterators are invalidated after erase + front = angles_and_indices.begin(); + rear = angles_and_indices.rbegin(); + } + } + } + else { + rear++; + } + } + + if (erase_front) + { + erase_front = false; + front = angles_and_indices.erase(front); + } + else { + ++front; + } + } + + if (pairs.size() > 4) + { + //check for neighbor and choose consistant 4 + //LOG(INFO) << "4"; + + //check if one of the pair has contains a neighbor with triples + auto max_triples = 0; + auto max_triples_index = 0; + for (auto & pair : pairs) + { + //search for the neighbor with the most triples + if (closest[pair.first.x()].v->triples.size() > max_triples) { + max_triples = closest[pair.first.x()].v->triples.size(); + max_triples_index = pair.first.x(); + } + + if (closest[pair.first.y()].v->triples.size() > max_triples) { + max_triples = closest[pair.first.y()].v->triples.size(); + max_triples_index = pair.first.y(); + } + } + + if (max_triples > 0) { + auto neighboring_triples = closest[max_triples_index].v->triples; + + std::vector >::iterator > > distances(neighboring_triples.size()); + std::vector used2(neighboring_triples.size(), false); + for (auto iter = pairs.begin(); iter != pairs.end(); iter++) { + + Eigen::VectorXd current_distances(neighboring_triples.size()); + for (int j = 0; j < neighboring_triples.size(); j++) + { + current_distances[j] = (neighboring_triples[j].m_angles - iter->second).squaredNorm(); + } + int index = 0; + auto current_distance = current_distances.minCoeff(&index); + + if (!used2[index]) { + used2[index] = true; + distances[index] = std::pair >::iterator>(current_distance, iter); + } + else { + //collision + //compare distances + if (distances[index].first < current_distance) + { + iter = pairs.erase(iter); + iter--; + } + else + { + auto dist = std::distance(pairs.begin(), iter); + dist -= 1; + pairs.erase(distances[index].second); + iter = pairs.begin() + dist; + distances[index] = std::pair >::iterator>(current_distance, iter); + } + } + } + } + } + + if (pairs.size() > 4) + { + LOG(INFO) << "double check"; + } + // Filter possible pairs - for(size_t n1 = 1; n1 < max_neigh; ++n1 ) { + //for(size_t n1 = 1; n1 < max_neigh; ++n1 ) { + for(auto &pair : pairs){ + auto n1 = pair.first.x(); + auto n2 = pair.first.y(); const double d1 = closest[n1].dist; - for(size_t n2 = n1+1; n2 < max_neigh; ++n2 ) { + if (used[n1]) + continue; + + //for(size_t n2 = n1+1; n2 < max_neigh; ++n2 ) { const double d2 = closest[n2].dist; + if (used[n2]) + continue; + + Vertex& c1 = *closest[n1].v; + Vertex& c2 = *closest[n2].v; + + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + auto pt2 = cv::Point2d(v.pc.x(), v.pc.y()); + auto pt3 = cv::Point2d(c2.pc.x(), c2.pc.y()); + + for (size_t n1 = 0; n1 < max_neigh; n1++) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, color * 5);) + } + + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, color);) + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt2, 1, color * 2);) + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt3, 1, color * 3);) + // Check distances aren't much further than closest if( d1 < max_dist && d2 < max_dist ) { @@ -261,28 +509,208 @@ void FindTriples( Vertex& v, std::vector& closest, double thresh_dist, dou if( 2.0 * fabs(d2-d1) / (fabs(d1)+fabs(d2)) < thresh_dist ) { // Check points are colinear with center - Vertex& c1 = *closest[n1].v; - Vertex& c2 = *closest[n2].v; + if( NormArea(c1.pc,v.pc,c2.pc) < thresh_area ) { + + + // Check no aliasing exists between matched if(used[n1] || used[n2]) { v.triples.clear(); v.neighbours.clear(); + OPENCV_DEBUG(cv::line(debug_image, SCALE_FACTOR*pt1, SCALE_FACTOR*pt2, 255);) + OPENCV_DEBUG(cv::line(debug_image, SCALE_FACTOR*pt2, SCALE_FACTOR*pt3, 255);) + for (size_t n1 = 0; n1 < max_neigh; ++n1) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, 0);) + } return; }else{ - used[n1] = true; - used[n2] = true; - v.neighbours.insert(&c1); - v.neighbours.insert(&c2); - v.triples.push_back( Triple(c1, v, c2) ); + //check line iterator + const cv::Mat input(images.Height(), images.Width(), cv::DataType::type, const_cast(images.ImgThresh())); + /*auto pt1 = cv::Point(c1.pc.x(), c1.pc.y()); + auto pt2 = cv::Point(c2.pc.x(), c2.pc.y()); + cv::LineIterator it(input, pt1, pt2); + std::vector buf(it.count); + std::vector buf_positions(it.count); + for (int i = 0; i < it.count; i++, ++it) + { + buf[i] = *(*it); + buf_positions[i] = it.pos(); + } + buf[0] = 0; + buf[it.count - 1] = 0; + std::vector buf2 = buf; + + auto last = std::unique(buf.begin(), buf.end()); + + buf.erase(last, buf.end()); + + std::vector check = { 0, 255, 0, 255, 0 }; + + if (buf == check){*/ + + + + cv::LineIterator it(input, pt1, pt2); + std::vector buf(it.count); + std::vector buf_positions(it.count); + for (int i = 0; i < it.count; i++, ++it) + { + buf[i] = *(*it); + buf_positions[i] = it.pos(); + } + buf[0] = 0; + buf[it.count - 1] = 0; + std::vector buf2 = buf; + + auto last = std::unique(buf.begin(), buf.end()); + + buf.erase(last, buf.end()); + + + + + cv::LineIterator it2(input, pt2, pt3); + std::vector buf3(it2.count); + std::vector buf_positions3(it2.count); + for (int i = 0; i < it2.count; i++, ++it2) + { + buf3[i] = *(*it2); + buf_positions3[i] = it2.pos(); + } + buf3[0] = 0; + buf3[it2.count - 1] = 0; + std::vector buf4 = buf3; + + auto last2 = std::unique(buf3.begin(), buf3.end()); + + buf3.erase(last2, buf3.end()); + + + std::vector check = { 0, 255, 0 }; + + cv::Point2d diff = pt3 - pt1; + + auto angle = atan2(diff.y, diff.x); + + if (angle < 0) + angle += M_PI; + + //LOG(INFO) << "angle: " << angle; + + angles_diffs.clear(); + + if (angles.size() >= 2) { + std::set::iterator end = angles.end(); + end--; + for (std::set::iterator it = angles.begin(); it != end; it++) + { + std::set::iterator it2 = it; + it2++; + angles_diffs.push_back(*it2 - *it); + } + angles_diffs.push_back((*(angles.begin()) + M_PI) - *(end)); + } + else { + angles_diffs.push_back( M_PI ); + } + + Eigen::Map angles_diffs2(angles_diffs.data(), angles_diffs.size()); + + int angles_diffs2_index = -1; + angles_diffs2.maxCoeff(&angles_diffs2_index); + + + auto ret = angles.insert(angle); + + auto dist = std::distance(angles.begin(), ret.first); + + //LOG(INFO) << "distance: " << dist; + + //if (dist == 0 || dist == (angles.count() - 1)) + + /*if (dist != 0 && dist != (angles.size() - 1)) + { + std::set::iterator before = ret.first; + before--; + std::set::iterator after = ret.first; + after++; + if ( (*(ret.first) - *before) < (M_PI / 5) && (*after - *(ret.first)) < (M_PI / 5)) + { + angles.erase(ret.first); + break; + } + + if (dist-1 != angles_diffs2_index) + { + break; + } + + }*/ + + Eigen::VectorXd bins(9); + Eigen::VectorXi indices(9); + bins << 0.0, M_PI, M_PI/4, M_PI/2, (M_PI/4 + M_PI/2), -M_PI, -M_PI/4, -M_PI/2, -(M_PI/4 + M_PI/2); + indices << 0, 0, 1, 2, 3, 0, 3, 2, 1; + + int index = -1; + auto absdiff = (bins.array() - angle).abs(); + auto score = absdiff.minCoeff(&index); + + if (buf == check && buf3 == check /*&& direction_bins_scores[indices[index]] > score*/) { + used[n1] = true; + used[n2] = true; + direction_bins[indices[index]] = true; + direction_bins_scores[indices[index]] = score; + v.neighbours.insert(&c1); + v.neighbours.insert(&c2); + v.triples.push_back(Triple(c1, v, c2, pair.second)); + OPENCV_DEBUG(cv::line(debug_image, SCALE_FACTOR*pt1, SCALE_FACTOR*pt2, color *4);) + OPENCV_DEBUG(cv::line(debug_image, SCALE_FACTOR*pt2, SCALE_FACTOR*pt3, color * 4);) + + if (false)//(v.triples.size() == 4) + { + for (size_t n1 = 0; n1 < max_neigh; ++n1) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, 0);) + } + return; + } + + //break; + } else { + check[0] = 0; + } } + + + + + } + } } + + /*cv::circle(debug_image, pt1, 1, 0); + cv::circle(debug_image, pt2, 1, 0); + cv::circle(debug_image, pt3, 1, 0);*/ } + //} + + //LOG(INFO) << "here: "; + + for (size_t n1 = 0; n1 < max_neigh; ++n1) { + Vertex& c1 = *closest[n1].v; + auto pt1 = cv::Point2d(c1.pc.x(), c1.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, 0);) } + } bool TargetGridDot::FindTarget( @@ -317,8 +745,26 @@ void TargetGridDot::Clear() void TargetGridDot::SetGrid(Vertex& v, const Eigen::Vector2i& g) { - v.pg = g; - map_grid_ellipse_[g] = &v; + if (g(0) == GRID_INVALID) + { + auto current_item = map_grid_ellipse_.find(v.pg); + if (current_item != map_grid_ellipse_.end()) + { + map_grid_ellipse_.erase(current_item); + } + v.pg = g; + return; + } + + if (map_grid_ellipse_.find(g) == map_grid_ellipse_.end()) + { + v.pg = g; + map_grid_ellipse_[g] = &v; + } + else + { + LOG(INFO) << "Collision"; + } } bool TargetGridDot::Match(std::map::type); // Generate vertex and point structures + // Create OpenCV structure and compute centroid for( size_t i=0; i < conics.size(); ++i ) { Vertex v(i, conics[i]); vs_.push_back(v); + //centroid += (v.pc_u - centroid) / (i + 1); + points_mat.at(i, 0) = v.pc_u[0]; + points_mat.at(i, 1) = v.pc_u[1]; + points_mat.at(i, 2) = v.pc_u[2]; } // Compute closest points for each ellipse - std::vector > vs_distance = ClosestPoints(vs_); - std::vector vs_central = MostCentral(vs_distance); + // Replace with kdtree + cvflann::Matrix points(points_mat.ptr(),vs_.size(), 3 ); + auto kdtree = cvflann::KDTreeSingleIndex >(points,cvflann::KDTreeSingleIndexParams()); + kdtree.buildIndex(); + + std::vector > vs_distance(conics.size()); + const size_t number_of_neighbors = 14; + std::vector indices(number_of_neighbors); + std::vector distances(number_of_neighbors); + for (size_t i = 0; i < conics.size(); i++) { + vs_distance[i].resize(number_of_neighbors); + cvflann::Matrix point(vs_[i].pc_u.data(), 1, 3); + cvflann::Matrix indices_mat(indices.data(), number_of_neighbors, 1); + cvflann::Matrix distances_mat(distances.data(), number_of_neighbors, 1); + kdtree.knnSearch(point, indices_mat, distances_mat, number_of_neighbors, cvflann::SearchParams()); + for (size_t j = 0; j < number_of_neighbors; j++) { + vs_distance[i][j] = Dist{ &vs_[indices[j]], distances[j] }; + } + } + + std::vector vs_central; + + const size_t number_of_neighbors2 = vs_.size(); + vs_central.resize(number_of_neighbors2); + indices.resize(number_of_neighbors2); + distances.resize(number_of_neighbors2); + cvflann::Matrix point(centroid.data(), 1, 3); + cvflann::Matrix indices_mat(indices.data(), number_of_neighbors2, 1); + cvflann::Matrix distances_mat(distances.data(), number_of_neighbors2, 1); + kdtree.knnSearch(point, indices_mat, distances_mat, number_of_neighbors2, cvflann::SearchParams()); + for (size_t j = 0; j < number_of_neighbors2; j++) { + vs_central[j] = Dist{ &vs_[indices[j]], distances[j] }; + } + + cv::Mat debug_image; + OPENCV_DEBUG(debug_image.create(SCALE_FACTOR*images.Height(), SCALE_FACTOR*images.Width(), cv::DataType::type);) + const cv::Mat dst(images.Height(), images.Width(), cv::DataType::type, const_cast(images.ImgThresh())); + + //dst.copyTo(debug_image); + OPENCV_DEBUG(cv::resize(dst, debug_image, debug_image.size(), 0, 0, cv::INTER_NEAREST);) + + // Find colinear neighbours for each ellipse for(size_t i=0; i < vs_.size(); ++i) { - FindTriples(vs_[i], vs_distance[i], params_.max_line_dist_ratio, params_.max_norm_triple_area ); - for(Triple& t : vs_[i].triples) line_groups_.push_back( LineGroup(t) ); + FindTriples(vs_[indices[i]], vs_distance[indices[i]], params_.max_line_dist_ratio, params_.max_norm_triple_area, images, debug_image); + for(Triple& t : vs_[indices[i]].triples) line_groups_.push_back( LineGroup(t) ); } // Find central, well connected vertex @@ -427,7 +920,7 @@ bool TargetGridDot::FindTarget( } if(!central) { -// std::cerr << "No central point found" << std::endl; + LOG(INFO) << "No central point found" << std::endl; return false; } @@ -444,6 +937,9 @@ bool TargetGridDot::FindTarget( // Setup central as center of grid SetGrid(*central, Eigen::Vector2i(0,0)); + auto pt1 = cv::Point2d(central->pc.x(), central->pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, SCALE_FACTOR*5, 100);) + OPENCV_DEBUG(cv::putText(debug_image, "0, 0", SCALE_FACTOR*pt1, cv::FONT_HERSHEY_PLAIN, 0.75, 100);) available.erase(std::find(available.begin(), available.end(), central)); // add neighbours of central to form basis @@ -456,6 +952,11 @@ bool TargetGridDot::FindTarget( Vertex& n = t.Neighbour(j); g[i] = 2*j-1; SetGrid(n, g); + auto pt1 = cv::Point2d(n.pc.x(), n.pc.y()); + OPENCV_DEBUG(cv::circle(debug_image, SCALE_FACTOR*pt1, 1, 100);) + std::stringstream output; + output << g[0] << ", " << g[1]; + OPENCV_DEBUG(cv::putText(debug_image, output.str(), SCALE_FACTOR*pt1, cv::FONT_HERSHEY_PLAIN, 0.75, 100);) available.erase(std::find(available.begin(), available.end(), &n)); fringe.push_back(&n); } @@ -465,9 +966,12 @@ bool TargetGridDot::FindTarget( // depth first search extending 'fringe' set by adding colinear vertices while(fringe.size() > 0) { Vertex& f = *fringe.front(); - for(size_t i=0; i